/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/bmp085.c |
---|
0,0 → 1,224 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY bmp085.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für BMP085/BMP180 Luftdrucksensor, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "bmp085.h" |
//path to i2c fleury lib |
#include BMP085_I2CFLEURYPATH |
/* |
* i2c write |
*/ |
void bmp085_writemem(uint8_t reg, uint8_t value) { |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_write(value); |
i2c_stop(); |
} |
/* |
* i2c read |
*/ |
void bmp085_readmem(uint8_t reg, uint8_t buff[], uint8_t bytes) { |
uint8_t i =0; |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_rep_start(BMP085_ADDR | I2C_READ); |
for(i=0; i<bytes; i++) { |
if(i==bytes-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
#if BMP085_FILTERPRESSURE == 1 |
#define BMP085_AVARAGECOEF 21 |
static long k[BMP085_AVARAGECOEF]; |
long bmp085_avaragefilter(long input) { |
uint8_t i=0; |
long sum=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
k[i] = k[i+1]; |
} |
k[BMP085_AVARAGECOEF-1] = input; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
sum += k[i]; |
} |
return (sum /BMP085_AVARAGECOEF) ; |
} |
#endif |
/* |
* read calibration registers |
*/ |
void bmp085_getcalibration() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
bmp085_readmem(BMP085_REGAC1, buff, 2); |
bmp085_regac1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC2, buff, 2); |
bmp085_regac2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC3, buff, 2); |
bmp085_regac3 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC4, buff, 2); |
bmp085_regac4 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC5, buff, 2); |
bmp085_regac5 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC6, buff, 2); |
bmp085_regac6 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGB1, buff, 2); |
bmp085_regb1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGB2, buff, 2); |
bmp085_regb2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMB, buff, 2); |
bmp085_regmb = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMC, buff, 2); |
bmp085_regmc = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMD, buff, 2); |
bmp085_regmd = ((int)buff[0] <<8 | ((int)buff[1])); |
} |
/* |
* get raw temperature as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawtemperature() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
long ut,x1,x2; |
//read raw temperature |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADTEMPERATURE); |
_delay_ms(5); // min. 4.5ms read Temp delay |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 2); |
ut = ((long)buff[0] << 8 | ((long)buff[1])); //uncompensated temperature value |
//calculate raw temperature |
x1 = ((long)ut - bmp085_regac6) * bmp085_regac5 >> 15; |
x2 = ((long)bmp085_regmc << 11) / (x1 + bmp085_regmd); |
bmp085_rawtemperature = x1 + x2; |
} |
/* |
* get raw pressure as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawpressure() { |
uint8_t buff[3]; |
memset(buff, 0, sizeof(buff)); |
long up,x1,x2,x3,b3,b6,p; |
unsigned long b4,b7; |
#if BMP085_AUTOUPDATETEMP == 1 |
bmp085_getrawtemperature(); |
#endif |
//read raw pressure |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADPRESSURE+(BMP085_MODE << 6)); |
_delay_ms(2 + (3<<BMP085_MODE)); |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 3); |
up = ((((long)buff[0] <<16) | ((long)buff[1] <<8) | ((long)buff[2])) >> (8-BMP085_MODE)); // uncompensated pressure value |
//calculate raw pressure |
b6 = bmp085_rawtemperature - 4000; |
x1 = (bmp085_regb2* (b6 * b6) >> 12) >> 11; |
x2 = (bmp085_regac2 * b6) >> 11; |
x3 = x1 + x2; |
b3 = (((((long)bmp085_regac1) * 4 + x3) << BMP085_MODE) + 2) >> 2; |
x1 = (bmp085_regac3 * b6) >> 13; |
x2 = (bmp085_regb1 * ((b6 * b6) >> 12)) >> 16; |
x3 = ((x1 + x2) + 2) >> 2; |
b4 = (bmp085_regac4 * (uint32_t)(x3 + 32768)) >> 15; |
b7 = ((uint32_t)up - b3) * (50000 >> BMP085_MODE); |
p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; |
x1 = (p >> 8) * (p >> 8); |
x1 = (x1 * 3038) >> 16; |
x2 = (-7357 * p) >> 16; |
bmp085_rawpressure = p + ((x1 + x2 + 3791) >> 4); |
#if BMP085_FILTERPRESSURE == 1 |
bmp085_rawpressure = bmp085_avaragefilter(bmp085_rawpressure); |
#endif |
} |
/* |
* get celsius temperature |
*/ |
double bmp085_gettemperature() { |
bmp085_getrawtemperature(); |
double temperature = ((bmp085_rawtemperature + 8)>>4); |
temperature = temperature /10; |
return temperature; |
} |
/* |
* get pressure |
*/ |
int32_t bmp085_getpressure() { |
bmp085_getrawpressure(); |
return bmp085_rawpressure + BMP085_UNITPAOFFSET; |
} |
/* |
* get altitude |
*/ |
double bmp085_getaltitude() { |
bmp085_getrawpressure(); |
return ((1 - pow(bmp085_rawpressure/(double)101325, 0.1903 )) / 0.0000225577) + BMP085_UNITMOFFSET; |
} |
/* |
* init bmp085 |
*/ |
void bmp085_init() { |
#if BMP085_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
bmp085_getcalibration(); //get calibration data |
bmp085_getrawtemperature(); //update raw temperature, at least the first time |
#if BMP085_FILTERPRESSURE == 1 |
//initialize the avarage filter |
uint8_t i=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
bmp085_getrawpressure(); |
} |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/bmp085.h |
---|
0,0 → 1,74 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- this library is a porting of the bmp085driver 0.4 ardunio library |
http://code.google.com/p/bmp085driver/ |
Notes: |
- to compile with avrgcc you may define -lm flag for some math functions |
*/ |
#ifndef BMP085_H_ |
#define BMP085_H_ |
#include <stdio.h> |
#include <avr/io.h> |
#define BMP085_ADDR (0x77<<1) //0x77 default I2C address |
#define BMP085_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define BMP085_I2CINIT 1 //init i2c |
//registers |
#define BMP085_REGAC1 0xAA |
#define BMP085_REGAC2 0xAC |
#define BMP085_REGAC3 0xAE |
#define BMP085_REGAC4 0xB0 |
#define BMP085_REGAC5 0xB2 |
#define BMP085_REGAC6 0xB4 |
#define BMP085_REGB1 0xB6 |
#define BMP085_REGB2 0xB8 |
#define BMP085_REGMB 0xBA |
#define BMP085_REGMC 0xBC |
#define BMP085_REGMD 0xBE |
#define BMP085_REGCONTROL 0xF4 //control |
#define BMP085_REGCONTROLOUTPUT 0xF6 //output 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB |
#define BMP085_REGREADTEMPERATURE 0x2E //read temperature |
#define BMP085_REGREADPRESSURE 0x34 //read pressure |
//modes |
#define BMP085_MODEULTRALOWPOWER 0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5 |
#define BMP085_MODESTANDARD 1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4 |
#define BMP085_MODEHIGHRES 2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3 |
#define BMP085_MODEULTRAHIGHRES 3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25 |
//autoupdate temperature enabled |
#define BMP085_AUTOUPDATETEMP 1 //autoupdate temperature every read |
//setup parameters |
#define BMP085_MODE BMP085_MODEULTRAHIGHRES //define a mode |
#define BMP085_UNITPAOFFSET 0 //define a unit offset (pa) |
#define BMP085_UNITMOFFSET 0 //define a unit offset (m) |
//avarage filter |
#define BMP085_FILTERPRESSURE 1 //avarage filter for pressure |
//variables |
int bmp085_regac1, bmp085_regac2, bmp085_regac3, bmp085_regb1, bmp085_regb2, bmp085_regmb, bmp085_regmc, bmp085_regmd; |
unsigned int bmp085_regac4, bmp085_regac5, bmp085_regac6; |
long bmp085_rawtemperature, bmp085_rawpressure; |
//functions |
extern void bmp085_init(); |
extern int32_t bmp085_getpressure(); |
extern double bmp085_getaltitude(); |
extern double bmp085_gettemperature(); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/hmc5883l.c |
---|
0,0 → 1,131 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY hmc5883l.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für HMC5883L Compass, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "hmc5883l.h" |
//path to i2c fleury lib |
#include HMC5883L_I2CFLEURYPATH |
static double hmc5883l_scale = 0; |
/* |
* init the hmc5883l |
*/ |
void hmc5883l_init() { |
#if HMC5883L_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
//set scale |
hmc5883l_scale = 0; |
uint8_t regValue = 0x00; |
#if HMC5883L_SCALE == HMC5883L_SCALE088 |
regValue = 0x00; |
hmc5883l_scale = 0.73; |
#elif HMC5883L_SCALE == HMC5883L_SCALE13 |
regValue = 0x01; |
hmc5883l_scale = 0.92; |
#elif HMC5883L_SCALE == HMC5883L_SCALE19 |
regValue = 0x02; |
hmc5883l_scale = 1.22; |
#elif HMC5883L_SCALE == HMC5883L_SCALE25 |
regValue = 0x03; |
hmc5883l_scale = 1.52; |
#elif HMC5883L_SCALE == HMC5883L_SCALE40 |
regValue = 0x04; |
hmc5883l_scale = 2.27; |
#elif HMC5883L_SCALE == HMC5883L_SCALE47 |
regValue = 0x05; |
hmc5883l_scale = 2.56; |
#elif HMC5883L_SCALE == HMC5883L_SCALE56 |
regValue = 0x06; |
hmc5883l_scale = 3.03; |
#elif HMC5883L_SCALE == HMC5883L_SCALE81 |
regValue = 0x07; |
hmc5883l_scale = 4.35; |
#endif |
//setting is in the top 3 bits of the register. |
regValue = regValue << 5; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_CONFREGB); |
i2c_write(regValue); |
i2c_stop(); |
//set measurement mode |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_MODEREG); |
i2c_write(HMC5883L_MEASUREMODE); |
i2c_stop(); |
} |
/* |
* get raw data |
*/ |
void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) { |
uint8_t i = 0; |
uint8_t buff[6]; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_DATAREGBEGIN); |
i2c_stop(); |
i2c_start_wait(HMC5883L_ADDR | I2C_READ); |
for(i=0; i<6; i++) { |
if(i==6-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
*mxraw = (int16_t)((buff[0] << 8) | buff[1]); |
*mzraw = (int16_t)((buff[2] << 8) | buff[3]); |
*myraw = (int16_t)((buff[4] << 8) | buff[5]); |
} |
/* |
* get scaled data |
*/ |
void hmc5883l_getdata(double *mx, double *my, double *mz) { |
int16_t mxraw = 0; |
int16_t myraw = 0; |
int16_t mzraw = 0; |
hmc5883l_getrawdata(&mxraw, &myraw, &mzraw); |
#if HMC5883L_CALIBRATED == 1 |
float mxt = mxraw - HMC5883L_OFFSETX; |
float myt = myraw - HMC5883L_OFFSETY; |
float mzt = mzraw - HMC5883L_OFFSETZ; |
*mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt; |
*my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt; |
*mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt; |
#else |
*mx = mxraw * hmc5883l_scale; |
*my = myraw * hmc5883l_scale; |
*mz = mzraw * hmc5883l_scale; |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/hmc5883l.h |
---|
0,0 → 1,71 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- HMC5883L Triple Axis Magnetometer Arduino Library |
http://bildr.org/2012/02/hmc5883l_arduino/ |
*/ |
#ifndef HMC5883L_H_ |
#define HMC5883L_H_ |
//definitions |
#define HMC5883L_ADDR (0x1E<<1) //device address |
//i2c settings |
#define HMC5883L_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define HMC5883L_I2CINIT 1 //init i2c |
//registers |
#define HMC5883L_CONFREGA 0x00 |
#define HMC5883L_CONFREGB 0x01 |
#define HMC5883L_MODEREG 0x02 |
#define HMC5883L_DATAREGBEGIN 0x03 |
//setup measurement mode |
#define HMC5883L_MEASURECONTINOUS 0x00 |
#define HMC5883L_MEASURESINGLESHOT 0x01 |
#define HMC5883L_MEASUREIDLE 0x03 |
#define HMC5883L_MEASUREMODE HMC5883L_MEASURECONTINOUS |
//setup scale |
#define HMC5883L_SCALE088 1 //0.88 |
#define HMC5883L_SCALE13 2 //1.3 |
#define HMC5883L_SCALE19 3 //1.9 |
#define HMC5883L_SCALE25 4 //2.5 |
#define HMC5883L_SCALE40 5 //4.0 |
#define HMC5883L_SCALE47 6 //4.7 |
#define HMC5883L_SCALE56 7 //5.6 |
#define HMC5883L_SCALE81 8 //8.1 |
#define HMC5883L_SCALE HMC5883L_SCALE13 |
#define HMC5883L_CALIBRATED 1 //enable this if this magn is calibrated |
//calibration values |
#if HMC5883L_CALIBRATED == 1 |
#define HMC5883L_OFFSETX -99.7 |
#define HMC5883L_OFFSETY -154.0 |
#define HMC5883L_OFFSETZ -22.7 |
#define HMC5883L_GAINX1 0.952017 |
#define HMC5883L_GAINX2 0.00195895 |
#define HMC5883L_GAINX3 0.0139661 |
#define HMC5883L_GAINY1 0.00195895 |
#define HMC5883L_GAINY2 0.882824 |
#define HMC5883L_GAINY3 0.00760243 |
#define HMC5883L_GAINZ1 0.0139661 |
#define HMC5883L_GAINZ2 0.00760243 |
#define HMC5883L_GAINZ3 0.995365 |
#endif |
//functions |
extern void hmc5883l_init(); |
extern void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw); |
extern void hmc5883l_getdata(double *mx, double *my, double *mz); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050.c |
---|
0,0 → 1,694 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 |
#include <math.h> //include libm |
#endif |
//path to i2c fleury lib |
#include "i2cmaster.h" |
volatile uint8_t buffer[14]; |
/* |
* read bytes from chip register |
*/ |
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data) { |
uint8_t i = 0; |
int8_t count = 0; |
if(length > 0) { |
//request register |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); |
_delay_us(10); |
//read data |
i2c_start(MPU6050_ADDR | I2C_READ); |
for(i=0; i<length; i++) { |
count++; |
if(i==length-1) |
data[i] = i2c_readNak(); |
else |
data[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
return count; |
} |
/* |
* read 1 byte from chip register |
*/ |
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) { |
return mpu6050_readBytes(regAddr, 1, data); |
} |
/* |
* write bytes to chip register |
*/ |
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data) { |
if(length > 0) { |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (uint8_t i = 0; i < length; i++) { |
i2c_write((uint8_t) data[i]); |
} |
i2c_stop(); |
} |
} |
/* |
* write 1 byte to chip register |
*/ |
void mpu6050_writeByte(uint8_t regAddr, uint8_t data) { |
return mpu6050_writeBytes(regAddr, 1, &data); |
} |
/* |
* read bits from chip register |
*/ |
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { |
// 01101001 read byte |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 010 masked |
// -> 010 shifted |
int8_t count = 0; |
if(length > 0) { |
uint8_t b; |
if ((count = mpu6050_readByte(regAddr, &b)) != 0) { |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
b &= mask; |
b >>= (bitStart - length + 1); |
*data = b; |
} |
} |
return count; |
} |
/* |
* read 1 bit from chip register |
*/ |
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data) { |
uint8_t b; |
uint8_t count = mpu6050_readByte(regAddr, &b); |
*data = b & (1 << bitNum); |
return count; |
} |
/* |
* write bit/bits to chip register |
*/ |
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { |
// 010 value to write |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 00011100 mask byte |
// 10101111 original value (sample) |
// 10100011 original & ~mask |
// 10101011 masked | value |
if(length > 0) { |
uint8_t b = 0; |
if (mpu6050_readByte(regAddr, &b) != 0) { //get current data |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
data <<= (bitStart - length + 1); // shift data into correct position |
data &= mask; // zero all non-important bits in data |
b &= ~(mask); // zero all important bits in existing byte |
b |= data; // combine data with existing byte |
mpu6050_writeByte(regAddr, b); |
} |
} |
} |
/* |
* write one bit to chip register |
*/ |
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data) { |
uint8_t b; |
mpu6050_readByte(regAddr, &b); |
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); |
mpu6050_writeByte(regAddr, b); |
} |
#if MPU6050_GETATTITUDE == 2 |
/* |
* write word/words to chip register |
*/ |
void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data) { |
if(length > 0) { |
uint8_t i = 0; |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (i = 0; i < length * 2; i++) { |
i2c_write((uint8_t)(data[i++] >> 8)); // send MSB |
i2c_write((uint8_t)data[i]); // send LSB |
} |
i2c_stop(); |
} |
} |
/* |
* set a chip memory bank |
*/ |
void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank) { |
bank &= 0x1F; |
if (userBank) bank |= 0x20; |
if (prefetchEnabled) bank |= 0x40; |
mpu6050_writeByte(MPU6050_RA_BANK_SEL, bank); |
} |
/* |
* set memory start address |
*/ |
void mpu6050_setMemoryStartAddress(uint8_t address) { |
mpu6050_writeByte(MPU6050_RA_MEM_START_ADDR, address); |
} |
/* |
* read a memory block |
*/ |
void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
for (uint16_t i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
// read the chunk of data as specified |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i); |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
} |
/* |
* write a memory block |
*/ |
uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
uint8_t *verifyBuffer = 0; |
uint8_t *progBuffer = 0; |
uint16_t i; |
uint8_t j; |
if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
for (i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
if (useProgMem) { |
// write the chunk of data as specified |
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
// write the chunk of data as specified |
progBuffer = (uint8_t *)data + i; |
} |
mpu6050_writeBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer); |
// verify data if needed |
if (verify && verifyBuffer) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); |
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { |
free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 0; // uh oh. |
} |
} |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
if (verify) free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* write a dmp configuration set |
*/ |
uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem) { |
uint8_t *progBuffer = 0; |
uint8_t success, special; |
uint16_t i, j; |
if (useProgMem) { |
progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary |
} |
// config set data is a long string of blocks with the following structure: |
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] |
uint8_t bank, offset, length; |
for (i = 0; i < dataSize;) { |
if (useProgMem) { |
bank = pgm_read_byte(data + i++); |
offset = pgm_read_byte(data + i++); |
length = pgm_read_byte(data + i++); |
} else { |
bank = data[i++]; |
offset = data[i++]; |
length = data[i++]; |
} |
// write data or perform special action |
if (length > 0) { |
// regular block of data to write |
if (useProgMem) { |
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); |
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
progBuffer = (uint8_t *)data + i; |
} |
success = mpu6050_writeMemoryBlock(progBuffer, length, bank, offset, 1, 0); |
i += length; |
} else { |
// special instruction |
// NOTE: this kind of behavior (what and when to do certain things) |
// is totally undocumented. This code is in here based on observed |
// behavior only, and exactly why (or even whether) it has to be here |
// is anybody's guess for now. |
if (useProgMem) { |
special = pgm_read_byte(data + i++); |
} else { |
special = data[i++]; |
} |
if (special == 0x01) { |
// enable DMP-related interrupts |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, 1); //setIntZeroMotionEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, 1); //setIntFIFOBufferOverflowEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, 1); //setIntDMPEnabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x32); // single operation |
success = 1; |
} else { |
// unknown special command |
success = 0; |
} |
} |
if (!success) { |
if (useProgMem) free(progBuffer); |
return 0; // uh oh |
} |
} |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* get the fifo count |
*/ |
uint16_t mpu6050_getFIFOCount(void) { |
mpu6050_readBytes(MPU6050_RA_FIFO_COUNTH, 2, (uint8_t *)buffer); |
return (((uint16_t)buffer[0]) << 8) | buffer[1]; |
} |
/* |
* read fifo bytes |
*/ |
void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length) { |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, length, data); |
} |
/* |
* get the interrupt status |
*/ |
uint8_t mpu6050_getIntStatus(void) { |
mpu6050_readByte(MPU6050_RA_INT_STATUS, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* reset fifo |
*/ |
void mpu6050_resetFIFO(void) { |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} |
/* |
* get gyro offset X |
*/ |
int8_t mpu6050_getXGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset X |
*/ |
void mpu6050_setXGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Y |
*/ |
int8_t mpu6050_getYGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Y |
*/ |
void mpu6050_setYGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Z |
*/ |
int8_t mpu6050_getZGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Z |
*/ |
void mpu6050_setZGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
#endif |
/* |
* set sleep disabled |
*/ |
void mpu6050_setSleepDisabled() { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0); |
} |
/* |
* set sleep enabled |
*/ |
void mpu6050_setSleepEnabled(void) { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1); |
} |
/* |
* test connectino to chip |
*/ |
uint8_t mpu6050_testConnection(void) { |
mpu6050_readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, (uint8_t *)buffer); |
if(buffer[0] == 0x34) |
return 1; |
else |
return 0; |
} |
/* |
* initialize the accel and gyro |
*/ |
void mpu6050_init(void) { |
//allow mpu6050 chip clocks to start up |
_delay_ms(100); |
//set sleep disabled |
mpu6050_setSleepDisabled(); |
//wake up delay needed sleep disabled |
_delay_ms(10); |
//set clock source |
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) |
// as the clock reference for improved stability |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set sampe rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz |
//set gyro range |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS); |
//set accel range |
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS); |
#if MPU6050_GETATTITUDE == 1 |
#error "Do not enable timer 0 it is in use elsewhere!" |
//MPU6050_TIMER0INIT |
#endif |
} |
//can not accept many request if we alreay have getattitude requests |
/* |
* get raw data |
*/ |
void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
*ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
*ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
*az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
*gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
*gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
*gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
} |
/* |
* get raw data converted to g and deg/sec values |
*/ |
void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
*axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
*ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
*azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
*gxds = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN; |
*gyds = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN; |
*gzds = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN; |
#else |
*axg = (double)(ax)/MPU6050_AGAIN; |
*ayg = (double)(ay)/MPU6050_AGAIN; |
*azg = (double)(az)/MPU6050_AGAIN; |
*gxds = (double)(gx)/MPU6050_GGAIN; |
*gyds = (double)(gy)/MPU6050_GGAIN; |
*gzds = (double)(gz)/MPU6050_GGAIN; |
#endif |
} |
#if MPU6050_GETATTITUDE == 1 |
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; |
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
/* |
* Mahony update function (for 6DOF) |
*/ |
void mpu6050_mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) { |
float norm; |
float halfvx, halfvy, halfvz; |
float halfex, halfey, halfez; |
float qa, qb, qc; |
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
// Normalise accelerometer measurement |
norm = sqrt(ax * ax + ay * ay + az * az); |
ax /= norm; |
ay /= norm; |
az /= norm; |
// Estimated direction of gravity and vector perpendicular to magnetic flux |
halfvx = q1 * q3 - q0 * q2; |
halfvy = q0 * q1 + q2 * q3; |
halfvz = q0 * q0 - 0.5f + q3 * q3; |
// Error is sum of cross product between estimated and measured direction of gravity |
halfex = (ay * halfvz - az * halfvy); |
halfey = (az * halfvx - ax * halfvz); |
halfez = (ax * halfvy - ay * halfvx); |
// Compute and apply integral feedback if enabled |
if(mpu6050_mahonytwoKiDef > 0.0f) { |
integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq); // integral error scaled by Ki |
integralFBy += mpu6050_mahonytwoKiDef * halfey * (1.0f / mpu6050_mahonysampleFreq); |
integralFBz += mpu6050_mahonytwoKiDef * halfez * (1.0f / mpu6050_mahonysampleFreq); |
gx += integralFBx; // apply integral feedback |
gy += integralFBy; |
gz += integralFBz; |
} else { |
integralFBx = 0.0f; // prevent integral windup |
integralFBy = 0.0f; |
integralFBz = 0.0f; |
} |
// Apply proportional feedback |
gx += mpu6050_mahonytwoKpDef * halfex; |
gy += mpu6050_mahonytwoKpDef * halfey; |
gz += mpu6050_mahonytwoKpDef * halfez; |
} |
// Integrate rate of change of quaternion |
gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); // pre-multiply common factors |
gy *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
gz *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
qa = q0; |
qb = q1; |
qc = q2; |
q0 += (-qb * gx - qc * gy - q3 * gz); |
q1 += (qa * gx + qc * gz - q3 * gy); |
q2 += (qa * gy - qb * gz + q3 * gx); |
q3 += (qa * gz + qb * gy - qc * gx); |
// Normalise quaternion |
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
q0 /= norm; |
q1 /= norm; |
q2 /= norm; |
q3 /= norm; |
} |
/* |
* update quaternion |
*/ |
void mpu6050_updateQuaternion(void) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
double axg = 0; |
double ayg = 0; |
double azg = 0; |
double gxrs = 0; |
double gyrs = 0; |
double gzrs = 0; |
//get raw data |
while(1) { |
mpu6050_readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, (uint8_t *)buffer); |
if(buffer[0]) |
break; |
_delay_us(10); |
} |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
gxrs = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN*0.01745329; //degree to radians |
#else |
axg = (double)(ax)/MPU6050_AGAIN; |
ayg = (double)(ay)/MPU6050_AGAIN; |
azg = (double)(az)/MPU6050_AGAIN; |
gxrs = (double)(gx)/MPU6050_GGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy)/MPU6050_GGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz)/MPU6050_GGAIN*0.01745329; //degree to radians |
#endif |
//compute data |
mpu6050_mahonyUpdate(gxrs, gyrs, gzrs, axg, ayg, azg); |
} |
/* |
* update timer for attitude |
*/ |
ISR(TIMER0_OVF_vect) { |
mpu6050_updateQuaternion(); |
} |
/* |
* get quaternion |
*/ |
void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz) { |
*qw = q0; |
*qx = q1; |
*qy = q2; |
*qz = q3; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1); |
*pitch = -asin(2*q1*q3 + 2*q0*q2); |
*roll = atan2(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 - 1); |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050.h |
---|
0,0 → 1,174 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- most of the code is a port of the arduino mpu6050 library by Jeff Rowberg |
https://github.com/jrowberg/i2cdevlib |
- Mahony complementary filter for attitude estimation |
http://www.x-io.co.uk |
*/ |
#ifndef MPU6050_H_ |
#define MPU6050_H_ |
#include <avr/io.h> |
#include "mpu6050registers.h" |
#ifdef __cplusplus |
extern "C" { |
#endif |
//i2c settings |
#define MPU6050_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define MPU6050_I2CINIT 0 //init i2c |
//definitions |
#define MPU6050_ADDR (0x68 <<1) //device address - 0x68 pin low (GND), 0x69 pin high (VCC) |
//enable the getattitude functions |
//because we do not have a magnetometer, we have to start the chip always in the same position |
//then to obtain your object attitude you have to apply the aerospace sequence |
//0 disabled |
//1 mahony filter |
//2 dmp chip processor |
#define MPU6050_GETATTITUDE 2 |
//definitions for raw data |
//gyro and acc scale |
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 |
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 |
#define MPU6050_GYRO_LSB_250 131.0 |
#define MPU6050_GYRO_LSB_500 65.5 |
#define MPU6050_GYRO_LSB_1000 32.8 |
#define MPU6050_GYRO_LSB_2000 16.4 |
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000 |
#endif |
#define MPU6050_ACCEL_LSB_2 16384.0 |
#define MPU6050_ACCEL_LSB_4 8192.0 |
#define MPU6050_ACCEL_LSB_8 4096.0 |
#define MPU6050_ACCEL_LSB_16 2048.0 |
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16 |
#endif |
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
#define MPU6050_AXOFFSET 0 |
#define MPU6050_AYOFFSET 0 |
#define MPU6050_AZOFFSET 0 |
#define MPU6050_AXGAIN 16384.0 |
#define MPU6050_AYGAIN 16384.0 |
#define MPU6050_AZGAIN 16384.0 |
#define MPU6050_GXOFFSET -42 |
#define MPU6050_GYOFFSET 9 |
#define MPU6050_GZOFFSET -29 |
#define MPU6050_GXGAIN 16.4 |
#define MPU6050_GYGAIN 16.4 |
#define MPU6050_GZGAIN 16.4 |
#endif |
//definitions for attitude 1 function estimation |
#if MPU6050_GETATTITUDE == 1 |
#error "GETATTITUDE == 1 is not supported!" |
//setup timer0 overflow event and define madgwickAHRSsampleFreq equal to timer0 frequency |
//timerfreq = (FCPU / prescaler) / timerscale |
// timerscale 8-bit = 256 |
// es. 61 = (16000000 / 1024) / 256 |
#define MPU6050_TIMER0INIT TCCR0B |=(1<<CS02)|(1<<CS00); TIMSK0 |=(1<<TOIE0); |
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz |
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain |
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain |
#endif |
#if MPU6050_GETATTITUDE == 2 |
//dmp definitions |
//packet size |
#define MPU6050_DMP_dmpPacketSize 42 |
//define INT0 rise edge interrupt |
#define MPU6050_DMP_INT0SETUP EICRA |= (1<<ISC01) | (1<<ISC00) |
//define enable and disable INT0 rise edge interrupt |
#define MPU6050_DMP_INT0DISABLE EIMSK &= ~(1<<INT0) |
#define MPU6050_DMP_INT0ENABLE EIMSK |= (1<<INT0) |
extern volatile uint8_t mpu6050_mpuInterrupt; |
#endif |
//functions |
extern void mpu6050_init(void); |
extern uint8_t mpu6050_testConnection(void); |
#if MPU6050_GETATTITUDE == 0 |
extern void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); |
extern void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds); |
#endif |
extern void mpu6050_setSleepDisabled(void); |
extern void mpu6050_setSleepEnabled(void); |
extern int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data); |
extern void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data); |
extern void mpu6050_writeByte(uint8_t regAddr, uint8_t data); |
extern int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data); |
extern void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); |
extern void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data); |
#if MPU6050_GETATTITUDE == 1 |
extern void mpu6050_updateQuaternion(void); |
extern void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double *pitch, double *roll, double *yaw); |
#endif |
#if MPU6050_GETATTITUDE == 2 |
extern void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data); |
extern void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank); |
extern void mpu6050_setMemoryStartAddress(uint8_t address); |
extern void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address); |
extern uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem); |
extern uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem); |
extern uint16_t mpu6050_getFIFOCount(void); |
extern void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length); |
extern uint8_t mpu6050_getIntStatus(void); |
extern void mpu6050_resetFIFO(); |
extern int8_t mpu6050_getXGyroOffset(void); |
extern void mpu6050_setXGyroOffset(int8_t offset); |
extern int8_t mpu6050_getYGyroOffset(void); |
extern void mpu6050_setYGyroOffset(int8_t offset); |
extern int8_t mpu6050_getZGyroOffset(void); |
extern void mpu6050_setZGyroOffset(int8_t offset); |
//base dmp |
extern uint8_t mpu6050_dmpInitialize(void); |
extern void mpu6050_dmpEnable(void); |
extern void mpu6050_dmpDisable(void); |
extern void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw); |
extern uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz); |
#endif |
#ifdef __cplusplus |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050dmp6.c |
---|
0,0 → 1,495 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050dmp6.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, DMP-Routinen für IMU, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
/* |
* This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
*/ |
//to enable get roll, pitch and yaw function we include the math function |
//if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
//${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 2 |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include <math.h> //include libm |
#define MPU6050_DMP_CODE_SIZE 1929 |
#define MPU6050_DMP_CONFIG_SIZE 192 |
#define MPU6050_DMP_UPDATES_SIZE 47 |
volatile uint8_t mpu6050_mpuInterrupt = 0; |
uint8_t *dmpPacketBuffer; |
uint16_t mpu6050_fifoCount = 0; |
uint8_t mpu6050_mpuIntStatus = 0; |
uint8_t mpu6050_fifoBuffer[64]; |
/* ================================================================================================ * |
| Default MotionApps v2.0 42-byte FIFO packet structure: | |
| | |
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| | |
| [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
* ================================================================================================ */ |
// this block of memory gets written to the MPU on start-up, and it seems |
// to be volatile memory, so it has to be done each time (it only takes ~1 |
// second though) |
const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
// bank 0, 256 bytes |
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
// bank 1, 256 bytes |
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
// bank 2, 256 bytes |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
// bank 3, 256 bytes |
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
// bank 4, 256 bytes |
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
// bank 5, 256 bytes |
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
// bank 6, 256 bytes |
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
// bank 7, 138 bytes (remainder) |
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
}; |
const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
// BANK OFFSET LENGTH [DATA] |
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
// SPECIAL 0x01 = enable interrupts |
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate |
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
// It is important to make sure the host processor can keep up with reading and processing |
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
}; |
const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
0x01, 0xB2, 0x02, 0xFF, 0xFF, |
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
0x01, 0x6A, 0x02, 0x06, 0x00, |
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
0x01, 0x62, 0x02, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
}; |
/* |
* initialize mpu6050 dmp |
*/ |
uint8_t mpu6050_dmpInitialize() { |
//setup interrupt |
MPU6050_DMP_INT0SETUP; |
//reset |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
_delay_ms(30);//wait after reset |
//disable sleep mode |
mpu6050_setSleepDisabled(); |
//set memorybank to 0 |
mpu6050_setMemoryBank(0, 0, 0); |
//get X/Y/Z gyro offsets |
int8_t xgOffset = mpu6050_getXGyroOffset(); |
int8_t ygOffset = mpu6050_getYGyroOffset(); |
int8_t zgOffset = mpu6050_getZGyroOffset(); |
//setting slave 0 address to 0x7F |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
//disabling I2C Master mode |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
//setting slave 0 address to 0x68 (self) |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
//resetting I2C Master control |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
_delay_ms(20); |
//load DMP code into memory banks |
if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
//set clock source |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
//set DMP and FIFO_OFLOW interrupts enabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
//set sample rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
//set external frame sync to TEMP_OUT_L[0] |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set gyro sensitivity to +/- 2000 deg/sec |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
//set DMP configuration bytes (function unknown) |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
//clear OTP Bank flag |
mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
//set X/Y/Z gyro offsets to previous values |
//xgOffset = 0; |
//ygOffset = 0; |
zgOffset = 90; |
mpu6050_setXGyroOffset(xgOffset); |
mpu6050_setYGyroOffset(ygOffset); |
mpu6050_setZGyroOffset(zgOffset); |
//set X/Y/Z gyro user offsets to zero |
mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
//writing final memory update 1/7 (function unknown) |
uint8_t dmpUpdate[16], j; |
uint16_t pos = 0; |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 2/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reset FIFO |
mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
//reading FIFO count |
uint8_t fifoCount = mpu6050_getFIFOCount(); |
uint8_t fifoBuffer[128]; |
//current FIFO count |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
//setting motion detection threshold to 2 |
mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
//setting zero-motion detection threshold to 156 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
//setting motion detection duration to 80 |
mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
//setting zero-motion detection duration to 0 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
//reset FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
//enabling FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
//enabling DMP |
mpu6050_dmpEnable(); |
//resetting DMP |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//writing final memory update 3/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 4/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 5/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reading FIFO data...")); |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//reading final memory update 6/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//reading FIFO data |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//writing final memory update 7/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//disabling DMP (you turn it on later) |
mpu6050_dmpDisable(); |
//resetting FIFO and clearing INT status one last time |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} else { |
return 2; // configuration block loading failed |
} |
} else { |
return 1; // main binary block loading failed |
} |
return 0; // success |
} |
/* |
* enable dmp |
*/ |
void mpu6050_dmpEnable() { |
MPU6050_DMP_INT0ENABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
} |
/* |
* disable dmp |
*/ |
void mpu6050_dmpDisable() { |
MPU6050_DMP_INT0DISABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
} |
/* |
* get quaternion from packet |
*/ |
void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
if (packet == 0) packet = dmpPacketBuffer; |
*qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
*qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
*qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
*qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
*pitch = -asin(2*qx*qz + 2*qw*qy); |
*roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
} |
/* |
* get quaternion and wait |
*/ |
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
//reset interrupt |
mpu6050_mpuInterrupt = 0; |
//check for overflow |
mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
//reset |
mpu6050_resetFIFO(); |
} else if (mpu6050_mpuIntStatus & 0x02) { |
//wait for correct available data length, should be a VERY short wait |
while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
//read a packet from FIFO |
mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
//get quaternion |
mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
return 1; |
} |
return 0; |
} |
/* |
* on interrupt set data availabe |
*/ |
ISR (INT0_vect) { |
mpu6050_mpuInterrupt = 1; |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050registers.h |
---|
0,0 → 1,365 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#ifndef MPU6050REGISTERS_H_ |
#define MPU6050REGISTERS_H_ |
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN |
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN |
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN |
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS |
#define MPU6050_RA_XA_OFFS_L_TC 0x07 |
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS |
#define MPU6050_RA_YA_OFFS_L_TC 0x09 |
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS |
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B |
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR |
#define MPU6050_RA_XG_OFFS_USRL 0x14 |
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR |
#define MPU6050_RA_YG_OFFS_USRL 0x16 |
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR |
#define MPU6050_RA_ZG_OFFS_USRL 0x18 |
#define MPU6050_RA_SMPLRT_DIV 0x19 |
#define MPU6050_RA_CONFIG 0x1A |
#define MPU6050_RA_GYRO_CONFIG 0x1B |
#define MPU6050_RA_ACCEL_CONFIG 0x1C |
#define MPU6050_RA_FF_THR 0x1D |
#define MPU6050_RA_FF_DUR 0x1E |
#define MPU6050_RA_MOT_THR 0x1F |
#define MPU6050_RA_MOT_DUR 0x20 |
#define MPU6050_RA_ZRMOT_THR 0x21 |
#define MPU6050_RA_ZRMOT_DUR 0x22 |
#define MPU6050_RA_FIFO_EN 0x23 |
#define MPU6050_RA_I2C_MST_CTRL 0x24 |
#define MPU6050_RA_I2C_SLV0_ADDR 0x25 |
#define MPU6050_RA_I2C_SLV0_REG 0x26 |
#define MPU6050_RA_I2C_SLV0_CTRL 0x27 |
#define MPU6050_RA_I2C_SLV1_ADDR 0x28 |
#define MPU6050_RA_I2C_SLV1_REG 0x29 |
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A |
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B |
#define MPU6050_RA_I2C_SLV2_REG 0x2C |
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D |
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E |
#define MPU6050_RA_I2C_SLV3_REG 0x2F |
#define MPU6050_RA_I2C_SLV3_CTRL 0x30 |
#define MPU6050_RA_I2C_SLV4_ADDR 0x31 |
#define MPU6050_RA_I2C_SLV4_REG 0x32 |
#define MPU6050_RA_I2C_SLV4_DO 0x33 |
#define MPU6050_RA_I2C_SLV4_CTRL 0x34 |
#define MPU6050_RA_I2C_SLV4_DI 0x35 |
#define MPU6050_RA_I2C_MST_STATUS 0x36 |
#define MPU6050_RA_INT_PIN_CFG 0x37 |
#define MPU6050_RA_INT_ENABLE 0x38 |
#define MPU6050_RA_DMP_INT_STATUS 0x39 |
#define MPU6050_RA_INT_STATUS 0x3A |
#define MPU6050_RA_ACCEL_XOUT_H 0x3B |
#define MPU6050_RA_ACCEL_XOUT_L 0x3C |
#define MPU6050_RA_ACCEL_YOUT_H 0x3D |
#define MPU6050_RA_ACCEL_YOUT_L 0x3E |
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F |
#define MPU6050_RA_ACCEL_ZOUT_L 0x40 |
#define MPU6050_RA_TEMP_OUT_H 0x41 |
#define MPU6050_RA_TEMP_OUT_L 0x42 |
#define MPU6050_RA_GYRO_XOUT_H 0x43 |
#define MPU6050_RA_GYRO_XOUT_L 0x44 |
#define MPU6050_RA_GYRO_YOUT_H 0x45 |
#define MPU6050_RA_GYRO_YOUT_L 0x46 |
#define MPU6050_RA_GYRO_ZOUT_H 0x47 |
#define MPU6050_RA_GYRO_ZOUT_L 0x48 |
#define MPU6050_RA_EXT_SENS_DATA_00 0x49 |
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A |
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B |
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C |
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D |
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E |
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F |
#define MPU6050_RA_EXT_SENS_DATA_07 0x50 |
#define MPU6050_RA_EXT_SENS_DATA_08 0x51 |
#define MPU6050_RA_EXT_SENS_DATA_09 0x52 |
#define MPU6050_RA_EXT_SENS_DATA_10 0x53 |
#define MPU6050_RA_EXT_SENS_DATA_11 0x54 |
#define MPU6050_RA_EXT_SENS_DATA_12 0x55 |
#define MPU6050_RA_EXT_SENS_DATA_13 0x56 |
#define MPU6050_RA_EXT_SENS_DATA_14 0x57 |
#define MPU6050_RA_EXT_SENS_DATA_15 0x58 |
#define MPU6050_RA_EXT_SENS_DATA_16 0x59 |
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A |
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B |
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C |
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D |
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E |
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F |
#define MPU6050_RA_EXT_SENS_DATA_23 0x60 |
#define MPU6050_RA_MOT_DETECT_STATUS 0x61 |
#define MPU6050_RA_I2C_SLV0_DO 0x63 |
#define MPU6050_RA_I2C_SLV1_DO 0x64 |
#define MPU6050_RA_I2C_SLV2_DO 0x65 |
#define MPU6050_RA_I2C_SLV3_DO 0x66 |
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 |
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 |
#define MPU6050_RA_MOT_DETECT_CTRL 0x69 |
#define MPU6050_RA_USER_CTRL 0x6A |
#define MPU6050_RA_PWR_MGMT_1 0x6B |
#define MPU6050_RA_PWR_MGMT_2 0x6C |
#define MPU6050_RA_BANK_SEL 0x6D |
#define MPU6050_RA_MEM_START_ADDR 0x6E |
#define MPU6050_RA_MEM_R_W 0x6F |
#define MPU6050_RA_DMP_CFG_1 0x70 |
#define MPU6050_RA_DMP_CFG_2 0x71 |
#define MPU6050_RA_FIFO_COUNTH 0x72 |
#define MPU6050_RA_FIFO_COUNTL 0x73 |
#define MPU6050_RA_FIFO_R_W 0x74 |
#define MPU6050_RA_WHO_AM_I 0x75 |
#define MPU6050_TC_PWR_MODE_BIT 7 |
#define MPU6050_TC_OFFSET_BIT 6 |
#define MPU6050_TC_OFFSET_LENGTH 6 |
#define MPU6050_TC_OTP_BNK_VLD_BIT 0 |
#define MPU6050_VDDIO_LEVEL_VLOGIC 0 |
#define MPU6050_VDDIO_LEVEL_VDD 1 |
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 |
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 |
#define MPU6050_CFG_DLPF_CFG_BIT 2 |
#define MPU6050_CFG_DLPF_CFG_LENGTH 3 |
#define MPU6050_EXT_SYNC_DISABLED 0x0 |
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 |
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 |
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 |
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 |
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 |
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 |
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 |
#define MPU6050_DLPF_BW_256 0x00 |
#define MPU6050_DLPF_BW_188 0x01 |
#define MPU6050_DLPF_BW_98 0x02 |
#define MPU6050_DLPF_BW_42 0x03 |
#define MPU6050_DLPF_BW_20 0x04 |
#define MPU6050_DLPF_BW_10 0x05 |
#define MPU6050_DLPF_BW_5 0x06 |
#define MPU6050_GCONFIG_FS_SEL_BIT 4 |
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 |
#define MPU6050_GYRO_FS_250 0x00 |
#define MPU6050_GYRO_FS_500 0x01 |
#define MPU6050_GYRO_FS_1000 0x02 |
#define MPU6050_GYRO_FS_2000 0x03 |
#define MPU6050_ACONFIG_XA_ST_BIT 7 |
#define MPU6050_ACONFIG_YA_ST_BIT 6 |
#define MPU6050_ACONFIG_ZA_ST_BIT 5 |
#define MPU6050_ACONFIG_AFS_SEL_BIT 4 |
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 |
#define MPU6050_ACCEL_FS_2 0x00 |
#define MPU6050_ACCEL_FS_4 0x01 |
#define MPU6050_ACCEL_FS_8 0x02 |
#define MPU6050_ACCEL_FS_16 0x03 |
#define MPU6050_DHPF_RESET 0x00 |
#define MPU6050_DHPF_5 0x01 |
#define MPU6050_DHPF_2P5 0x02 |
#define MPU6050_DHPF_1P25 0x03 |
#define MPU6050_DHPF_0P63 0x04 |
#define MPU6050_DHPF_HOLD 0x07 |
#define MPU6050_TEMP_FIFO_EN_BIT 7 |
#define MPU6050_XG_FIFO_EN_BIT 6 |
#define MPU6050_YG_FIFO_EN_BIT 5 |
#define MPU6050_ZG_FIFO_EN_BIT 4 |
#define MPU6050_ACCEL_FIFO_EN_BIT 3 |
#define MPU6050_SLV2_FIFO_EN_BIT 2 |
#define MPU6050_SLV1_FIFO_EN_BIT 1 |
#define MPU6050_SLV0_FIFO_EN_BIT 0 |
#define MPU6050_MULT_MST_EN_BIT 7 |
#define MPU6050_WAIT_FOR_ES_BIT 6 |
#define MPU6050_SLV_3_FIFO_EN_BIT 5 |
#define MPU6050_I2C_MST_P_NSR_BIT 4 |
#define MPU6050_I2C_MST_CLK_BIT 3 |
#define MPU6050_I2C_MST_CLK_LENGTH 4 |
#define MPU6050_CLOCK_DIV_348 0x0 |
#define MPU6050_CLOCK_DIV_333 0x1 |
#define MPU6050_CLOCK_DIV_320 0x2 |
#define MPU6050_CLOCK_DIV_308 0x3 |
#define MPU6050_CLOCK_DIV_296 0x4 |
#define MPU6050_CLOCK_DIV_286 0x5 |
#define MPU6050_CLOCK_DIV_276 0x6 |
#define MPU6050_CLOCK_DIV_267 0x7 |
#define MPU6050_CLOCK_DIV_258 0x8 |
#define MPU6050_CLOCK_DIV_500 0x9 |
#define MPU6050_CLOCK_DIV_471 0xA |
#define MPU6050_CLOCK_DIV_444 0xB |
#define MPU6050_CLOCK_DIV_421 0xC |
#define MPU6050_CLOCK_DIV_400 0xD |
#define MPU6050_CLOCK_DIV_381 0xE |
#define MPU6050_CLOCK_DIV_364 0xF |
#define MPU6050_I2C_SLV_RW_BIT 7 |
#define MPU6050_I2C_SLV_ADDR_BIT 6 |
#define MPU6050_I2C_SLV_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV_EN_BIT 7 |
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 |
#define MPU6050_I2C_SLV_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV_GRP_BIT 4 |
#define MPU6050_I2C_SLV_LEN_BIT 3 |
#define MPU6050_I2C_SLV_LEN_LENGTH 4 |
#define MPU6050_I2C_SLV4_RW_BIT 7 |
#define MPU6050_I2C_SLV4_ADDR_BIT 6 |
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV4_EN_BIT 7 |
#define MPU6050_I2C_SLV4_INT_EN_BIT 6 |
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 |
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 |
#define MPU6050_MST_PASS_THROUGH_BIT 7 |
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 |
#define MPU6050_MST_I2C_LOST_ARB_BIT 5 |
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 |
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 |
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 |
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 |
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 |
#define MPU6050_INTCFG_INT_LEVEL_BIT 7 |
#define MPU6050_INTCFG_INT_OPEN_BIT 6 |
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 |
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 |
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 |
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 |
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 |
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 |
#define MPU6050_INTMODE_ACTIVEHIGH 0x00 |
#define MPU6050_INTMODE_ACTIVELOW 0x01 |
#define MPU6050_INTDRV_PUSHPULL 0x00 |
#define MPU6050_INTDRV_OPENDRAIN 0x01 |
#define MPU6050_INTLATCH_50USPULSE 0x00 |
#define MPU6050_INTLATCH_WAITCLEAR 0x01 |
#define MPU6050_INTCLEAR_STATUSREAD 0x00 |
#define MPU6050_INTCLEAR_ANYREAD 0x01 |
#define MPU6050_INTERRUPT_FF_BIT 7 |
#define MPU6050_INTERRUPT_MOT_BIT 6 |
#define MPU6050_INTERRUPT_ZMOT_BIT 5 |
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 |
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 |
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 |
#define MPU6050_INTERRUPT_DMP_INT_BIT 1 |
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 |
// TODO: figure out what these actually do |
// UMPL source code is not very obivous |
#define MPU6050_DMPINT_5_BIT 5 |
#define MPU6050_DMPINT_4_BIT 4 |
#define MPU6050_DMPINT_3_BIT 3 |
#define MPU6050_DMPINT_2_BIT 2 |
#define MPU6050_DMPINT_1_BIT 1 |
#define MPU6050_DMPINT_0_BIT 0 |
#define MPU6050_MOTION_MOT_XNEG_BIT 7 |
#define MPU6050_MOTION_MOT_XPOS_BIT 6 |
#define MPU6050_MOTION_MOT_YNEG_BIT 5 |
#define MPU6050_MOTION_MOT_YPOS_BIT 4 |
#define MPU6050_MOTION_MOT_ZNEG_BIT 3 |
#define MPU6050_MOTION_MOT_ZPOS_BIT 2 |
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 |
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 |
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 |
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 |
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 |
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 |
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 |
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 |
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 |
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 |
#define MPU6050_DETECT_FF_COUNT_BIT 3 |
#define MPU6050_DETECT_FF_COUNT_LENGTH 2 |
#define MPU6050_DETECT_MOT_COUNT_BIT 1 |
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 |
#define MPU6050_DETECT_DECREMENT_RESET 0x0 |
#define MPU6050_DETECT_DECREMENT_1 0x1 |
#define MPU6050_DETECT_DECREMENT_2 0x2 |
#define MPU6050_DETECT_DECREMENT_4 0x3 |
#define MPU6050_USERCTRL_DMP_EN_BIT 7 |
#define MPU6050_USERCTRL_FIFO_EN_BIT 6 |
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 |
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 |
#define MPU6050_USERCTRL_DMP_RESET_BIT 3 |
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 |
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 |
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 |
#define MPU6050_PWR1_DEVICE_RESET_BIT 7 |
#define MPU6050_PWR1_SLEEP_BIT 6 |
#define MPU6050_PWR1_CYCLE_BIT 5 |
#define MPU6050_PWR1_TEMP_DIS_BIT 3 |
#define MPU6050_PWR1_CLKSEL_BIT 2 |
#define MPU6050_PWR1_CLKSEL_LENGTH 3 |
#define MPU6050_CLOCK_INTERNAL 0x00 |
#define MPU6050_CLOCK_PLL_XGYRO 0x01 |
#define MPU6050_CLOCK_PLL_YGYRO 0x02 |
#define MPU6050_CLOCK_PLL_ZGYRO 0x03 |
#define MPU6050_CLOCK_PLL_EXT32K 0x04 |
#define MPU6050_CLOCK_PLL_EXT19M 0x05 |
#define MPU6050_CLOCK_KEEP_RESET 0x07 |
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 |
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 |
#define MPU6050_PWR2_STBY_XA_BIT 5 |
#define MPU6050_PWR2_STBY_YA_BIT 4 |
#define MPU6050_PWR2_STBY_ZA_BIT 3 |
#define MPU6050_PWR2_STBY_XG_BIT 2 |
#define MPU6050_PWR2_STBY_YG_BIT 1 |
#define MPU6050_PWR2_STBY_ZG_BIT 0 |
#define MPU6050_WAKE_FREQ_1P25 0x0 |
#define MPU6050_WAKE_FREQ_2P5 0x1 |
#define MPU6050_WAKE_FREQ_5 0x2 |
#define MPU6050_WAKE_FREQ_10 0x3 |
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 |
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 |
#define MPU6050_BANKSEL_MEM_SEL_BIT 4 |
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 |
#define MPU6050_WHO_AM_I_BIT 6 |
#define MPU6050_WHO_AM_I_LENGTH 6 |
#define MPU6050_DMP_MEMORY_BANKS 8 |
#define MPU6050_DMP_MEMORY_BANK_SIZE 256 |
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/sensor.c |
---|
0,0 → 1,30 |
/* |
* sensor.c |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY sensor.c |
//# |
//# 19.09.2015 cebra |
//# - add: Routinen für das GY-87 Sensorboard zur Berechnung der Kompasswerte etc. |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include "mpu6050.h" |
#include "sensor.h" |
void Init_Sensors(void) |
{ |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/sensor.h |
---|
0,0 → 1,15 |
/* |
* sensor.h |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
#ifndef SENSOR_H_ |
#define SENSOR_H_ |
#endif /* 10DOF_SENSOR_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/HAL_HW3_9.c |
---|
0,0 → 1,562 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.c |
//# |
//# |
//# 10.11.2014 cebra |
//# - chg: Displaybeleuchtung aktiv einschalten beim Start |
//# |
//# 14.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf PKT_TitlePKTlipo(false) da eine |
//# richtige LiPo-Anzeige im Startbildschirm zu diesem Zeitpunkt noch |
//# nicht moeglich ist |
//# - chg: InitHWPorts() - bt_init() umgestellt auf BTM222_Initalize() |
//# |
//# 11.06.2014 OG |
//# - fix: InitHWPorts() einstellen der Sprache geht jetzt wieder |
//# |
//# 10.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - chg: InitHWPorts() - Aufruf von InitWi232() geaendert |
//# |
//# 28.05.2014 OG |
//# - chg: InitHWPorts() - Bearbeiten der Sprache auskommentiert weil sich |
//# Edit_generic() geaendert hat und notwendige Texte aus einem Menue |
//# bezieht welches hier in der HAL nicht vorhanden ist |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 24.03.2014 OG |
//# - chg: InitHWPorts(): den Begriff "WiFly" bzw. "WiFly Patch" geaendert |
//# zu "SV2 Patch" |
//# |
//# 21.02.2014 OG |
//# - chg: InitHWPorts() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: InitHWPorts() Anzeige vom Startscreen ueberarbeitet |
//# - chg: Code-Formatierung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING vor Servo_init |
//# |
//# 07.07.2013 OG |
//# - add: DEBUG_SV2_EXTENSION in InitHWPorts() bzgl. Anzeige von "A:.. B:.." usw. |
//# |
//# 26.06.2013 OG |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 26.06.2013 Cebra |
//# - add: Modulumschaltung für WiFlypatch umgebaut |
//# - chg: Wartezeit nach Einschalten von BT auf 3 Sekunden verlängert |
//# |
//# 22.06.2013 cebra |
//# - chg: Fehler in CheckPCB führte zu fehlerhafter Lipomessung, alle Pins von Port A waren auf Eingang |
//# |
//# 13.06.2013 cebra |
//# - chg: Displaybeleuchtung ohne Helligkeitssteuerung |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 28.04.2013 OG |
//# - add: Unterstuetzung von define USE_FONT_BIG (Layoutanpassung) |
//# |
//# 12.04.2013 Cebra |
//# - chg: Fehler bei der Umschaltung Wi232/Kabelverbindung beim Start des PKT |
//########################################################################### |
#ifndef HAL_HW3_9_C_ |
#define HAL_HW3_9_C_ |
#include "cpu.h" |
#include <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include "main.h" |
//#if defined HWVERSION3_9 |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "uart/uart1.h" |
#include "timer/timer.h" |
#include "wi232/Wi232.h" |
//#include "i2cmaster.h" |
#include "bluetooth/bluetooth.h" |
#include "bluetooth/error.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "pkt/pkt.h" |
#include "setup/setup.h" |
#include "osd/osd.h" |
#include "sound/pwmsine8bit.h" |
#include "tracking/ng_servo.h" |
volatile uint8_t USBBT; |
volatile uint8_t U02SV2; |
volatile uint8_t PCB_Version; |
volatile uint8_t PCB_SV2RxTxPatch; |
uint8_t PortA; |
uint8_t PortB; |
uint8_t PortC; |
uint8_t PortD; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void CheckPCB(void) |
{ |
// |
DDRA = 0x00; |
DDRB = 0x00; |
DDRC = 0x00; |
DDRD = 0x00; |
PortA = PINA; |
PortB = PINB; |
PortC = PINC; |
PortD = PIND; |
DDRC = 0x00; |
PORTC &= ~(1<<PORTC7); // Pullup setzen |
if (PINC & (1<<PINC7) ) PCB_Version = PKT39m; else PCB_Version = PKT39x; |
PORTC &= ~(1<<PORTC0); // Pullup setzen |
if (PINC & (1<<PINC0) ) PCB_SV2RxTxPatch = false; else PCB_SV2RxTxPatch = true; |
PORTC |= (1<<PORTC0); // Pullup aus |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void InitHWPorts( void ) // Initialisierung der Hardware für die jeweilige Leiterplattenversion |
{ |
int8_t yoffs; |
CheckPCB(); // Version der Leiterplatte prüfen |
if( PCB_SV2RxTxPatch ) |
{ |
DDRC |= (1<<DDC0)|(1<<DDC1); // Ausgang: C0 Reset BT, C1 Reset WiFly |
PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
} |
PORTA |= (1<<PORTA4)|(1<<PORTA5)|(1<<PORTA6)|(1<<PORTA7); // Enable Pull Up for the 4 keys |
DDRA &= ~(1<<DDA4); // Eingang: A4 auf Low setzen (Power On) |
if( Config.Lipomessung == true ) |
{ |
DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
} |
DDRB = 0xFF; // alles Ausgaenge |
PORTC |= (1<<PORTC4)|(1<<PORTC7); // Enable Pull Up for LBO + Summer |
DDRC |= (1<<DDC2)|(1<<DDC3)|(1<<DDC5)|(1<<DDC6|(1<<DDC7)); // Ausgang: Led2,Rs232Switch,Summer |
DDRC &= ~(1<<DDC4); // Eingang: LowBat LTC1308 |
if( PCB_SV2RxTxPatch ) clr_USBOn(); |
else _BTOn(); // Erstmal USB dektivieren, damit beim versehentlichen Einschalten USB im PC ruhig bleibt |
DDRD |= (1<<DDD4)|(1<<DDD5)|(1<<DDD6)|(1<<DDD7); // Ausgang: PiepserTest, Servo, Wi232-CMD und Beleuchtung |
PORTD |= (1<<PORTD6); // Wi232-CMD auf High schalten |
set_D_LIGHT(); // Displaybeleuchtung an |
set_V_On(); // Spannung mit T3 halten |
Timer0_Init(); // system |
Display_on = 1; |
USART_Init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
uart1_init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); // USB |
// I2C_Init(1); |
// i2c_init(); |
LCD_Init(0); // muss vor "ReadParameter" stehen |
ReadParameter(); // aktuelle Werte aus EEProm auslesen |
#ifdef USE_TRACKING |
servoInit(SERVO_PERIODE); |
#endif |
//servoSetDefaultPos(); |
sei(); |
Old_Baudrate = Config.PKT_Baudrate; |
SetBaudUart1( Config.PKT_Baudrate ); |
SetBaudUart0( Config.PKT_Baudrate ); |
// 10.11.2015 cebra |
// TODO cebra: diese Stelle muss bei HW_Sound überarbeitet werden |
// if( (Config.HWSound==0) && (PCB_Version == PKT39m) ) |
// { |
// Timer2_Init(); // Displaybeleuchtung oder Soundmodul |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
// } |
#ifdef USE_SOUND |
if (Config.HWSound==1) InitSound(); |
#endif |
LCD_Init(1); |
set_beep ( 400, 0x0080, BeepNormal); |
OSD_active = false; // keine OSD Ausgabe |
ADC_Init(); // ADC für Lipomessung// MartinR: verschoben |
//------------------------- |
// beim ersten Start (oder nach Reset) Sprache abfragen |
//------------------------- |
if( Config.DisplayLanguage > NUM_LANG ) |
{ |
Config.DisplayLanguage = 1; |
//Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage, 0, 1, Language,1, NULL,NULL); // OG: ?? hierbei wird die falsche Sprache angezeigt - warum? |
Config.DisplayLanguage = Edit_generic( 2, 0, 1, Language,1, NULL,NULL); |
WriteParameter(); |
} |
//------------------------- |
// Anzeige Start-Screen |
//------------------------- |
PKT_TitlePKTlipo( false ); // Titel mit PKT-Version anzeigen (und clearscreen) |
yoffs = 11; |
if( PCB_SV2RxTxPatch ) |
{ |
lcdx_printp_center( 2, PSTR("SV2 Patch"), MNORMAL, 0,7); // "SV2 Patch" (ehemals "Wifly Patch") |
yoffs = 5; |
} |
if( PCB_Version==PKT39m ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9m (09.2011)"), MNORMAL, 0,yoffs); |
} |
if( PCB_Version==PKT39x ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9x (11.2012)"), MNORMAL, 0,yoffs); |
} |
lcdx_printp_center( 5, strGet(BOOT1), MNORMAL, 0,2); // "Taste 1 Sekunde" |
lcdx_printp_center( 6, strGet(BOOT2), MNORMAL, 0,4); // "festhalten!" |
lcd_rect_round( 0, 5*8-2, 127, 2*8+8, 1, R2); // Rahmen |
//------------------------- |
//------------------------- |
#ifdef DEBUG_SV2_EXTENSION |
lcd_printpns_at (0, 7, PSTR("A:"),0); |
lcd_print_hex(PortA,0); |
lcd_printpns_at (5, 7, PSTR("B:"),0); |
lcd_print_hex(PortB,0); |
lcd_printpns_at (10, 7, PSTR("C:"),0); |
lcd_print_hex(PortC,0); |
lcd_printpns_at (15, 7, PSTR("D:"),0); |
lcd_print_hex(PortD,0); |
#endif |
//_delay_ms(800); |
_delay_ms(900); |
if (PINA & (1<<PINA7)) // Spannung eingeschaltet lassen |
clr_V_On(); |
_delay_ms(100); |
set_beep ( 400, 0x0080, BeepNormal); |
get_key_press(KEY_ALL); |
#ifdef USE_SOUND |
if (Config.HWSound==1); |
{ |
playTone(505,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(535,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(505,100,Config.Volume); |
} |
#endif |
//lcd_cls(); // 20.02.2014 OG |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
// _delay_ms(250); |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
set_BTOff(); |
//set_Modul_On(USB); |
if( (Config.UseWi == true) && (Config.WiIsSet == false) ) |
{ |
Wi232_Initalize(); // wenn Wi232 nicht initialisiert ist, dann jetzt tun |
} |
#ifdef USE_BLUETOOTH |
if ((Config.UseBT == true) && (Config.BTIsSet == false)) // BT ausschalten |
{ |
//lcd_cls(); |
BTM222_Initalize(); |
// set_USBOn(); |
} |
bt_start(); |
#endif // end: #ifdef USE_BLUETOOTH |
if( (Config.UseWi == true) && (Config.U02SV2 == 0) ) |
{ |
Change_Output(Uart02Wi); // Verbindung zu Wi232 herstellen |
} |
else |
{ |
Change_Output(Uart02FC); // Verbindung zu SV" (Kabel) herstellen |
} |
} // InitHWPorts() |
//#define set_BEEP() (PORTC &= ~(1 << Summer)) // Summer |
//#define clr_BEEP() (PORTC |= (1 << Summer)) |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC &= ~(1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD &= ~(1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC |= (1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD |= (1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_D_LIGHT(void) /* Displaybeleuchtung ein*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHTV2(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_D_LIGHT(void) /* Displaybeleuchtung aus*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHTV2(); |
} |
} |
uint8_t BTIsOn=0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_Modul_On(uint8_t Modul) // Umschaltfunktionen für Leiterplatte 3.9x mit WiFlypatch |
{ |
if (PCB_SV2RxTxPatch) PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
switch (Modul) |
{ |
case USB: if (PCB_SV2RxTxPatch) set_USBOn(); |
else set_USBOn(); |
BTIsOn = 0; |
break; |
case Bluetooth: if (PCB_SV2RxTxPatch) set_BTM222On(); |
else _BTOn(); |
if (BTIsOn == 0) |
{ |
BTIsOn = 1; |
_delay_ms(3000); // wait for BTM222 initialisation |
} |
break; |
case Wlan: set_WiFlyOn(); |
BTIsOn = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOn(void) |
{ |
if (BTIsOn == 0) |
{ |
if (PCB_SV2RxTxPatch) set_Modul_On(Bluetooth); |
else |
{ |
_BTOn(); |
BTIsOn = 1; |
_delay_ms(3000); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOff(void) |
{ |
if( PCB_SV2RxTxPatch ) |
set_Modul_On(USB); |
else |
{ |
set_USBOn(); |
BTIsOn = 0; |
} |
} |
#endif |
//#endif // HAL_HW3_9_C_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/HAL_HW3_9.h |
---|
0,0 → 1,194 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.h |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 26.06.2013 Cebra |
//# - add: defines für WyFlypatch Port C0 + C1 |
//# |
//# 17.05.2013 OG |
//# - add: defines: KEY_ENTER_LONG, KEY_ESC_LONG, KEY_PLUS_LONG, KEY_MINUS_LONG |
//############################################################################ |
#ifndef HAL_HW3_9_H_ |
#define HAL_HW3_9_H_ |
//#define PIEPSER_NERVT // Summer zu Testzwecken ganz Ausschalten |
#define PKT39m 1 |
#define PKT39x 2 |
#define USB 1 |
#define Bluetooth 2 |
#define Wlan 3 |
// Hardware 3.9 Portbelegung |
#define KEY_PIN PINA // Port A als Input |
#define Touch0 PA0 // Pin 37 |
#define Touch1 PA1 // Pin 36 |
#define Touch2 PA2 // Pin 35 |
#define Touch3 PA3 // Pin 34 |
#define KEY_EXT PINA3 // MartinR |
#define Key1 PA4 // Pin 33 |
#define Key2 PA5 // Pin 32 |
#define Key3 PA6 // Pin 31 |
#define Key4 PA7 // Pin 30 |
#define USB2Wi PB0 // Pin 40 aktiv low > IC5 |
#define VoltageHold PB1 // Pin 41 High = Spannung T3 halten |
#define Display_Reset PB2 // Pin 42 |
#define Display_A0 PB3 // Pin 43 |
#define Display_CS PB4 // Pin 44 |
#define Display_SI PB5 // Pin 1 |
#define LED1 PB6 // Pin 2 Low = LED1 (nicht benutzbar wegen SCL |
#define Display_SCL PB7 // Pin 3 |
#define I2C_SCL PC0 // Pin 19 SCL |
#define I2C_CDA PC1 // Pin 20 SDA |
#define BT_Reset PC0 // Pin 19 Reseteingang BTM222 / WiyFlypatch |
#define WiFly_Reset PC1 // Pin 20 Reseteingang WyFly / WiyFlypatch |
#define USB2FC PC2 // Pin 21 aktiv low > IC5 |
#define USB_BT PC3 // Pin 22 high = USB, Low = Bluetooth, LED2 |
#define USB_Reset PC3 // Pin 22 Reseteingang USB FT232 / WiFlypatch |
#define LowBat PC4 // Pin 23 Low Bat Warning Lipo PKT,Input |
#define Uart02Wi PC5 // Pin 24 aktiv Low > IC4 |
#define Uart02FC PC6 // Pin 25 aktiv Low > IC4 |
#define Summer PC7 // Pin 26 Low = Summer |
#define DisplaybeleuchtungV2 PC7 // Pin 26 High = Display-LED PCB 3.9x |
#define Uart0RxD PD0 // Pin 9 über IC4 =Wi | SV2 |
#define Uart0TxD PD1 // Pin 10 über IC4 =Wi | SV2 |
#define Uart1RxD PD2 // Pin 11 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define Uart1TxD PD3 // Pin 12 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define SERVO2 PD4 // Pin 13 PWM Servo 2 |
#define SERVO1 PD5 // Pin 14 PWM Servo 1 |
#define Wi232_CMD PD6 // Pin 15 aktiv Low = Wi232 CMD |
#define Displaybeleuchtung PD7 // Pin 16 High = Display-LED PCB 3.9m |
#define SummerV2 PD7 // Pin 16 Low = Summer, PCB 3.9x |
#define KEY_ENTER Key1 |
#define KEY_ESC Key2 |
#define KEY_PLUS Key3 |
#define KEY_MINUS Key4 |
#define KEY_NONE 0 // keine Taste (menuctrl) |
#define KEY_ENTER_LONG KEY_ENTER+100 // for long press keys used in scrollbox & menuctrl |
#define KEY_ESC_LONG KEY_ESC+100 |
#define KEY_PLUS_LONG KEY_PLUS+100 |
#define KEY_MINUS_LONG KEY_MINUS+100 |
// |= schaltet Ausgang auf HIGH |
// &= ~ schaltet Ausgang auf LOW |
#define set_reset() (PORTB |= (1 << Display_Reset)) |
#define clr_reset() (PORTB &= ~(1 << Display_Reset)) |
#define set_A0() (PORTB |= (1 << Display_A0)) |
#define clr_A0() (PORTB &= ~(1 << Display_A0)) |
#define set_cs() (PORTB |= (1 << Display_CS)) |
#define clr_cs() (PORTB &= ~(1 << Display_CS)) |
#define set_si() (PORTB |= (1 << Display_SI)) |
#define clr_si() (PORTB &= ~(1 << Display_SI)) |
#define set_scl() (PORTB |= (1 << Display_SCL)) |
#define clr_scl() (PORTB &= ~(1 << Display_SCL)) |
#define _BTOn() (PORTC &= ~(1 << USB_BT)) // Bluetooth ein |
#define set_USBOn() (PORTC |= (1 << USB_Reset)) // USB ein |
#define clr_USBOn() (PORTC &= ~(1 << USB_Reset)) // USB aus |
#define set_BTM222On() (PORTC |= (1 << BT_Reset)) // BT ein |
#define clr_BTM222On() (PORTC &= ~(1 << BT_Reset)) // BT aus |
#define set_WiFlyOn() (PORTC |= (1 << WiFly_Reset)) // WiFly ein |
#define clr_WiFlyOn() (PORTC &= ~(1 << WiFly_Reset)) // WiFly aus |
#define clr_V_On() (PORTB &= ~(1 << VoltageHold)) // Spannung mit T3 halten |
#define set_V_On() (PORTB |= (1 << VoltageHold)) |
#define set_USB2FC() (PORTC &= ~(1 << USB2FC)) // USB mit FC-Kabel verbinden |
#define clr_USB2FC() (PORTC |= (1 << USB2FC)) |
#define set_USB2Wi() (PORTB &= ~(1 << USB2Wi)) // USB mit Wi232 verbinden |
#define clr_USB2Wi() (PORTB |= (1 << USB2Wi)) |
#define set_Uart02FC() (PORTC &= ~(1 << Uart02FC)) // Uart0 mit FC-Kabel verbinden |
#define clr_Uart02FC() (PORTC |= (1 << Uart02FC)) |
#define set_Uart02Wi() (PORTC &= ~(1 << Uart02Wi)) // Uart0 mit Wi232 verbinden |
#define clr_Uart02Wi() (PORTC |= (1 << Uart02Wi)) |
#define set_DISPLAYLIGHT() (PORTD &= ~(1 << Displaybeleuchtung)) // Displaybeleuchtung PCB3.9m |
#define clr_DISPLAYLIGHT() (PORTD |= (1 << Displaybeleuchtung)) |
#define set_DISPLAYLIGHTV2() (PORTC &= ~(1 << DisplaybeleuchtungV2)) // Displaybeleuchtung PCB3.9x |
#define clr_DISPLAYLIGHTV2() (PORTC |= (1 << DisplaybeleuchtungV2)) |
#define set_WI232CMD() (PORTD &= ~(1 << Wi232_CMD)) // Wi232 Programmierpin |
#define clr_WI232CMD() (PORTD |= (1 << Wi232_CMD)) |
void set_D_LIGHT(void); /* Displaybeleuchtung ein*/ |
void clr_D_LIGHT(void); /* Displaybeleuchtung aus */ |
void set_BEEP(void); /* Beeper ein*/ |
void clr_BEEP(void); /* Beeper aus*/ |
void InitHWPorts(void); |
void set_BTOn(void); /* Bluetooth einschalten*/ |
void set_BTOff(void); /* Bluetooth einschalten*/ |
void set_Modul_On(uint8_t Modul); |
extern volatile uint8_t PCB_Version; |
extern volatile uint8_t PCB_SV2RxTxPatch; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/avr-nmea-gps-library/nmea.c |
---|
0,0 → 1,525 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY nmea.c |
//# |
//# 08.08.2015 CB |
//# - add: Einführung eines neuen Interruptbasierten NMEA Parsers |
//# cpp-Code auf c geändert und an PKT angepasst |
//# |
//############################################################################ |
/* URSPRUNG: |
File: nmea.cpp |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include "nmea.h" |
#include "../timer/timer.h" |
//#include <stdio.h> |
//#include <stdlib.h> |
//#include <util/delay.h> |
#include <stdbool.h> |
#define MAX_POWER 10 |
#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
static const int32_t powers[MAX_POWER] PROGMEM = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
uint8_t m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
m_bFlagDataReady = false; // valid GPS fix and data available, user can call reader functions |
volatile uint32_t res_nNMEAcounter = 0; // NMEA Datensatzzähler |
volatile uint32_t res_nCRCerror = 0; // zählt die CRC Errors beim dekodieren |
uint8_t receiveNMEA = false; |
char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// will be set to true for characters between $ and * only |
bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
int m_nChecksum ; // numeric checksum, computed for a given sentence |
bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
int index_received_checksum ; // used to parse received checksum |
// word cutting variables |
int m_nWordIdx , // the current word in a sentence |
m_nPrevIdx, // last character index where we did a cut |
m_nNowIdx ; // current character index |
// globals to store parser results |
int32_t res_fLongitude; // GPRMC and GPGGA |
int32_t res_fLatitude; // GPRMC and GPGGA |
unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
char Time[9]; // GPRMC and GPGGA |
uint8_t res_nSatellitesUsed; // GPGGA |
uint8_t res_nGPSfix; // GPGGA |
int16_t res_nHDOP; // GPGGA |
int16_t res_fAltitude; // GPGGA |
float res_fSpeed; // GPRMC |
int16_t res_fBearing; // GPRMC |
NMEA_GPGGA_t NMEA; // Variable mit der Struktur für die NMEA Daten für PKT |
// the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// aux functions |
int digit2dec(char hexdigit); |
float string2float(char* s); |
int mstrcmp(const char *s1, const char *s2); |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c) |
{ |
if (c == '$') { |
m_bFlagRead = true; |
// init parser vars |
m_bFlagComputedCks = false; |
m_nChecksum = 0; |
// after getting * we start cuttings the received m_nChecksum |
m_bFlagReceivedCks = false; |
index_received_checksum = 0; |
// word cutting variables |
m_nWordIdx = 0; m_nPrevIdx = 0; m_nNowIdx = 0; |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
} |
if (m_bFlagRead) { |
// check ending |
if (c == '\r' || c== '\n') { |
// catch last ending item too |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
// cut received m_nChecksum |
tmp_szChecksum[index_received_checksum] = 0; |
// sentence complete, read done |
m_bFlagRead = false; |
// parse |
parsedata(); |
} else { |
// computed m_nChecksum logic: count all chars between $ and * exclusively |
if (m_bFlagComputedCks && c == '*') m_bFlagComputedCks = false; |
if (m_bFlagComputedCks) m_nChecksum ^= c; |
if (c == '$') m_bFlagComputedCks = true; |
// received m_nChecksum |
if (m_bFlagReceivedCks) { |
tmp_szChecksum[index_received_checksum] = c; |
index_received_checksum++; |
} |
if (c == '*') m_bFlagReceivedCks = true; |
// build a word |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = c; |
if (c == ',') { |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
m_nPrevIdx = m_nNowIdx; |
} |
else m_nNowIdx++; |
} |
} |
return m_nWordIdx; |
} |
/* |
* parse internal tmp_ structures, fused by pushdata, and set the data flag when done |
*/ |
void parsedata(void) { |
int received_cks = 16*digit2dec(tmp_szChecksum[0]) + digit2dec(tmp_szChecksum[1]); |
//uart1.Send("seq: [cc:%X][words:%d][rc:%s:%d]\r\n", m_nChecksum,m_nWordIdx, tmp_szChecksum, received_cks); |
// check checksum, and return if invalid! |
if (m_nChecksum != received_cks) { |
m_bFlagDataReady = false; |
res_nCRCerror = res_nCRCerror+1; |
return; |
} |
/* $GPGGA |
* $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh |
* ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A, |
* |
* WORDS: |
* 1 = UTC of Position |
* 2 = Latitude |
* 3 = N or S |
* 4 = Longitude |
* 5 = E or W |
* 6 = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix) |
* 7 = Number of satellites in use [not those in view] |
* 8 = Horizontal dilution of position |
* 9 = Antenna altitude above/below mean sea level (geoid) |
* 10 = Meters (Antenna height unit) |
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. |
* -geoid is below WGS-84 ellipsoid) |
* 12 = Meters (Units of geoidal separation) |
* 13 = Age in seconds since last update from diff. reference station |
* 14 = Diff. reference station ID# |
* 15 = Checksum |
*/ |
if (mstrcmp(tmp_words[0], "$GPGGA") == 0) { |
// Check GPS Fix: 0=no fix, 1=GPS fix, 2=Dif. GPS fix |
res_nGPSfix = atoi(&tmp_words[6][0]); |
NMEA.SatFix = res_nGPSfix; |
if (tmp_words[6][0] == '0') { |
// clear data |
res_fLatitude = 0; |
res_fLongitude = 0; |
// m_bFlagDataReady = false; |
// return; |
} |
// parse time |
res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
NMEA_getTime (tmp_words[1]); |
// parse latitude and longitude in NMEA format |
// res_fLatitude = string2float(tmp_words[2]); |
// res_fLongitude = string2float(tmp_words[4]); |
// get decimal format |
// if (tmp_words[3][0] == 'S') res_fLatitude *= -1.0; |
// if (tmp_words[5][0] == 'W') res_fLongitude *= -1.0; |
// float degrees = trunc(res_fLatitude / 100.0f); |
// float minutes = res_fLatitude - (degrees * 100.0f); |
// res_fLatitude = degrees + minutes / 60.0f; |
// degrees = trunc(res_fLongitude / 100.0f); |
// minutes = res_fLongitude - (degrees * 100.0f); |
// res_fLongitude = degrees + minutes / 60.0f; |
res_fLatitude = NMEA_calcLatitude(tmp_words[2],tmp_words[3]); |
res_fLongitude = NMEA_calcLongitude(tmp_words[4],tmp_words[5]); |
NMEA.Latitude = res_fLatitude; |
NMEA.Longitude = res_fLongitude; |
// parse number of satellites |
res_nSatellitesUsed = (int)string2float(tmp_words[7]); |
NMEA.SatsInUse = atoi(tmp_words[7]); |
// parse HDOP |
res_nHDOP = NMEA_floatStrToInt(tmp_words[8],1); |
NMEA.HDOP = res_nHDOP; |
// parse altitude |
// res_fAltitude = string2float(tmp_words[9]); |
res_fAltitude = NMEA_floatStrToInt( tmp_words[9], 1); |
NMEA.Altitude = res_fAltitude; |
// data ready |
m_bFlagDataReady = true; |
res_nNMEAcounter = res_nNMEAcounter +1; |
NMEA.Counter = res_nNMEAcounter; |
} |
// 8.8.2015 CB wird zur Zeit nicht benötigt |
/* $GPRMC |
* note: a siRF chipset will not support magnetic headers. |
* $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh |
* ex: $GPRMC,230558.501,A,4543.8901,N,02112.7219,E,1.50,181.47,230213,,,A*66, |
* |
* WORDS: |
* 1 = UTC of position fix |
* 2 = Data status (V=navigation receiver warning) |
* 3 = Latitude of fix |
* 4 = N or S |
* 5 = Longitude of fix |
* 6 = E or W |
* 7 = Speed over ground in knots |
* 8 = Track made good in degrees True, Bearing This indicates the direction that the device is currently moving in, |
* from 0 to 360, measured in azimuth. |
* 9 = UT date |
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course) |
* 11 = E or W |
* 12 = Checksum |
*/ |
// if (mstrcmp(tmp_words[0], "$GPRMC") == 0) { |
// // Check data status: A-ok, V-invalid |
// if (tmp_words[2][0] == 'V') { |
// // clear data |
// res_fLatitude = 0; |
// res_fLongitude = 0; |
//// m_bFlagDataReady = false; |
// return; |
// } |
//// // parse time |
//// res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
//// res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
//// res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
//// // parse latitude and longitude in NMEA format |
//// res_fLatitude = string2float(tmp_words[3]); |
//// res_fLongitude = string2float(tmp_words[5]); |
//// // get decimal format |
//// if (tmp_words[4][0] == 'S') res_fLatitude *= -1.0; |
//// if (tmp_words[6][0] == 'W') res_fLongitude *= -1.0; |
//// float degrees = trunc(res_fLatitude / 100.0f); |
//// float minutes = res_fLatitude - (degrees * 100.0f); |
//// res_fLatitude = degrees + minutes / 60.0f; |
//// degrees = trunc(res_fLongitude / 100.0f); |
//// minutes = res_fLongitude - (degrees * 100.0f); |
//// res_fLongitude = degrees + minutes / 60.0f; |
// //parse speed |
// // The knot (pronounced not) is a unit of speed equal to one nautical mile (1.852 km) per hour |
// res_fSpeed = NMEA_floatStrToInt(tmp_words[7],1); |
// res_fSpeed /= 1.852; // convert to km/h |
// // parse bearing |
// res_fBearing = NMEA_floatStrToInt(tmp_words[8],1); |
//// // parse UTC date |
//// res_nUTCDay = digit2dec(tmp_words[9][0]) * 10 + digit2dec(tmp_words[9][1]); |
//// res_nUTCMonth = digit2dec(tmp_words[9][2]) * 10 + digit2dec(tmp_words[9][3]); |
//// res_nUTCYear = digit2dec(tmp_words[9][4]) * 10 + digit2dec(tmp_words[9][5]); |
// |
// // data ready |
// res_nNMEAcounter = res_nNMEAcounter +1; |
// NMEA.Counter = res_nNMEAcounter; |
// m_bFlagDataReady = true; |
// } |
} |
//-------------------------------------------------------------- |
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLatitude( const char *s, const char *NS) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(0,5,NS,0,0,0); |
deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
min = NMEA_floatStrToInt( &s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *NS == 'S' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLongitude( const char *s, const char *WE) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(10,5,WE,0,0,0); |
deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
min = NMEA_floatStrToInt( &s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *WE == 'W' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
void NMEA_getTime( const char *s) |
{ |
uint8_t sem = 0; |
uint8_t i; |
for( i=0; i<6; i++ ) |
{ |
NMEA.Time[sem++] = s[i]; |
if( i==1 || i==3) |
NMEA.Time[sem++] = ':'; |
} |
NMEA.Time[sem] = '\0'; |
} |
//-------------------------------------------------------------- |
/* |
* returns base-16 value of chars '0'-'9' and 'A'-'F'; |
* does not trap invalid chars! |
*/ |
int digit2dec(char digit) |
{ |
if ((int)digit >= 65) |
return ((int)digit - 55); |
else |
return ((int)digit - 48); |
} |
//-------------------------------------------------------------- |
/* returns base-10 value of zero-terminated string |
* that contains only chars '+','-','0'-'9','.'; |
* does not trap invalid strings! |
*/ |
float string2float(char* s) { |
long integer_part = 0; |
float decimal_part = 0.0; |
float decimal_pivot = 0.1; |
bool isdecimal = false, isnegative = false; |
char c; |
while ( ( c = *s++) ) { |
// skip special/sign chars |
if (c == '-') { isnegative = true; continue; } |
if (c == '+') continue; |
if (c == '.') { isdecimal = true; continue; } |
if (!isdecimal) { |
integer_part = (10 * integer_part) + (c - 48); |
} |
else { |
decimal_part += decimal_pivot * (float)(c - 48); |
decimal_pivot /= 10.0; |
} |
} |
// add integer part |
decimal_part += (float)integer_part; |
// check negative |
if (isnegative) decimal_part = - decimal_part; |
return decimal_part; |
} |
//-------------------------------------------------------------- |
// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
//-------------------------------------------------------------- |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ) |
{ |
char *endPtr; |
int32_t v; |
v = strtol(s, &endPtr, 10); |
if( *endPtr == '.' ) |
{ |
for (s = endPtr + 1; *s && power1; s++) |
{ |
v = v * 10 + (*s - '0'); |
--power1; |
} |
} |
if( power1 ) |
{ |
// Table to avoid multiple multiplications |
v = v * getPower(power1); |
} |
return v; |
} |
//-------------------------------------------------------------- |
int mstrcmp(const char *s1, const char *s2) |
{ |
while((*s1 && *s2) && (*s1 == *s2)) |
s1++,s2++; |
return *s1 - *s2; |
} |
//-------------------------------------------------------------- |
bool NMEA_isdataready() { |
return m_bFlagDataReady; |
} |
int NMEA_getHour() { |
return res_nUTCHour; |
} |
int NMEA_getMinute() { |
return res_nUTCMin; |
} |
int NMEA_getSecond() { |
return res_nUTCSec; |
} |
int NMEA_getDay() { |
return res_nUTCDay; |
} |
int NMEA_getMonth() { |
return res_nUTCMonth; |
} |
int NMEA_getYear() { |
return res_nUTCYear; |
} |
int32_t NMEA_getLatitude() { |
return res_fLatitude; |
} |
int32_t NMEA_getLongitude() { |
return res_fLongitude; |
} |
uint8_t NMEA_getSatellites() { |
return res_nSatellitesUsed; |
} |
uint8_t NMEA_getGPSfix() { |
return res_nGPSfix; |
} |
int16_t NMEA_getHDOP() { |
return res_nHDOP; |
} |
int16_t NMEA_getAltitude() { |
return res_fAltitude; |
} |
float NMEA_getSpeed() { |
return res_fSpeed; |
} |
int16_t NMEA_getBearing() { |
return res_fBearing; |
} |
uint32_t NMEA_getNMEAcounter() { |
return res_nNMEAcounter; |
} |
uint32_t NMEA_getCRCerror() { |
return res_nCRCerror; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/avr-nmea-gps-library/nmea.h |
---|
0,0 → 1,135 |
/* |
File: nmea.h |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include <string.h> |
//#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <math.h> |
#include <stdio.h> |
//--------------------------------- |
#define NMEA_TIMEOUT 500 // Timeout 5 sek. |
//--------------------------------- |
typedef struct |
{ |
uint32_t Counter; // Zaehler empfangene GPGGA Pakete |
int32_t Latitude; // in 1E-7 deg |
int32_t Longitude; // in 1E-7 deg |
int16_t Altitude; // Hoehe in Meter |
uint8_t SatFix; // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix, 6=Estimated) |
uint8_t SatsInUse; // Number of satelites currently in use |
int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser |
char Time[9]; // "hh:mm:ss" |
} NMEA_GPGGA_t; |
//--------------------------------- |
// export vars |
//--------------------------------- |
extern NMEA_GPGGA_t NMEA; |
extern uint8_t receiveNMEA; |
extern volatile uint32_t res_nNMEAcounter; |
// /*class NMEA { |
// |
// private: |
// bool m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
// m_bFlagDataReady; // valid GPS fix and data available, user can call reader functions |
// char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
// tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// |
// // will be set to true for characters between $ and * only |
// bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
// int m_nChecksum ; // numeric checksum, computed for a given sentence |
// bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
// int index_received_checksum ; // used to parse received checksum |
// |
// // word cutting variables |
// int m_nWordIdx , // the current word in a sentence |
// m_nPrevIdx, // last character index where we did a cut |
// m_nNowIdx ; // current character index |
// |
// // globals to store parser results |
// float res_fLongitude; // GPRMC and GPGGA |
// float res_fLatitude; // GPRMC and GPGGA |
// unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
// res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
// int res_nSatellitesUsed; // GPGGA |
// float res_fAltitude; // GPGGA |
// float res_fSpeed; // GPRMC |
// float res_fBearing; // GPRMC |
// |
// // the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// // aux functions |
// int digit2dec(char hexdigit); |
// float string2float(char* s); |
// int mstrcmp(const char *s1, const char *s2); |
// |
// public: |
// // constructor, initing a few variables |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c); |
void parsedata(void); |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ); |
int32_t NMEA_calcLatitude( const char *s, const char *NS); |
int32_t NMEA_calcLongitude( const char *s, const char *WE); |
void NMEA_getTime( const char *s); |
// READER functions: retrieving results, call isdataready() first |
bool NMEA_isdataready(); |
int NMEA_getHour(); |
int NMEA_getMinute(); |
int NMEA_getSecond(); |
int NMEA_getDay(); |
int NMEA_getMonth(); |
int NMEA_getYear(); |
int32_t NMEA_getLatitude(); |
int32_t NMEA_getLongitude(); |
uint8_t NMEA_getSatellites(); |
uint8_t NMEA_getGPSfix(); |
int16_t NMEA_getHDOP(); |
int16_t NMEA_getAltitude(); |
float NMEA_getSpeed(); |
int16_t NMEA_getBearing(); |
uint32_t NMEA_getNMEAcounter(); |
uint32_t NMEA_getCRCerror(); |
// }; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/bluetooth.c |
---|
0,0 → 1,1760 |
/** |
* source for the Bluetooth driver |
* @file bluetooth.c |
* @author Linus Lotz<lotz@in.tum.de> |
* @author Salomon Sickert |
*/ |
//############################################################################ |
//# HISTORY bluetooth.c |
//# |
//# |
//# 02.08.2015 CB |
//# - chg: bt_showsettings(), angepasst und vollendet, zur Zeit nur im Mastermodus des BTM222 nutzbar |
//# |
//# 30.06.2014 OG |
//# - chg: bt_discover() wieder auf (berechnete) Progressbar umgestellt |
//# |
//# 26.06.2014 OG |
//# - del: bt_receiveNMEA() - jetzt in gps/nmea.c |
//# - chg: bt_discover(), bt_searchDevice() - stark geaendert; Timeout geht jetzt |
//# nach Zeit und Abbruch durch Benutzer moeglich |
//# - fix: BT-Devicesuche - wenn bei einer Suche Devices gefunden wurden und bei |
//# einer nachfolgenden Suche weniger (z.B. weil ein Device ausgeschaltet wurde) |
//# dann wurden zuviele Devices angezeigt; fixed |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover(), bt_searchDevice() |
//# |
//# 22.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 21.06.2014 CB |
//# - fix: Fehler in "copy_DevName", zwei Leerzeichen am Anfang beendeten sofort den Namen, Namen war dann leer |
//# - fix: Umbau der Timeoutfunktion bei der Devicesuche, wenn kein BT Device in Reichweite wurde die Suche nicht beendet |
//# - add: Progressbar bei BT Device Suche |
//# |
//# 19.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - etliche Aenderungen und Funktionsumbenennungen - vorallem rund um den |
//# Bereich BT-Init |
//# - viel Code-Layout (noch nicht abgeschlossen) |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 2013 Cebra |
//# - Erweiterung um BT Local-ID Abfrage |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung fuer WiFlypatch geaendert |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() - korrigiert ggf. fehlehaften BT-Namen in Config.bt_name |
//# |
//# 24.06.2013 OG |
//# - chg: bt_init(): Code-Layout, Code-Struktur, ggf. Debug-Ausgaben |
//# - chg: bt_searchmodul(): Code-Layout und Ausgabe |
//# - chg: send_cmd(): Handling von BT_SET_NAME & Code-Layout |
//# - add: define DEBUG_BT |
//# |
//# 23.05.2013 OG |
//# - chg: Timeout reduziert in bt_disconnect() (trennen der BT-Verbindung) |
//# - fix: copy_DevName() Range angepasst und 0 Terminierung |
//# |
//# 24.04.2013 OG |
//# - chg: Auskommentiert: #define __DELAY_BACKWARD_COMPATIBLE__ |
//# das wird bereits vom Compiler-Symbol dieses PKT-Projektes definiert |
//# und erzeugt nur zusaetzliche Warnings |
//# |
//# 03.04.2013 Cebra |
//# - chg: Init Routinen BT überarbeitet, es kam zu falschen Baudrateneinstellungen |
//# beim BTM-222 ohne Hinweis, mehr Displayinformationen beim Init |
//############################################################################ |
#include <string.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdlib.h> |
#include "bluetooth.h" |
#include "../main.h" |
//---------------------------------------------------------------------------- |
#ifdef USE_BLUETOOTH // 30.01.2014 OG |
// brutal hier den Source ausklammern, damit man an anderen Stellen des PKT |
// den BT-Code durch den Compiler entdeckt |
// ansonsten kann es passieren, dass einiges an BT-Code unenddeckt einkompiliert wird |
// -> bei den ersten Test's waren das immerhin ca. 10 KByte durch unenddeckten Code... |
//---------------------------------------------------------------------------- |
//#define DEBUG_BT |
//#ifdef HWVERSION3_9 |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "fifo.h" |
#include "error.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../utils/xutils.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "../utils/scrollbox.h" |
//############################################################################ |
//############################################################################ |
//#define BT_CMD_TIMEOUT_MS 2000 |
#define BT_CMD_TIMEOUT_MS 500 // 15.06.2014 OG: Timeout reduziert von 2000 auf 500 |
static const char STR_BTM222INIT[] PROGMEM = "BTM222 Init"; |
//#define SaveMem |
// |
// Baudrate for the UART-connection to the BTM-222 on SQUIRREL |
// |
#define SQUIRREL |
#ifdef SQUIRREL |
#define UART_BAUD_RATE 19200 |
#endif |
#ifdef NUT |
#define UART_BAUD_RATE 19200 |
#endif |
typedef enum { |
BT_RAW, |
BT_DATA, |
BT_CMD, |
BT_NOECHO, |
BT_NOANSWER |
} communication_mode_t; |
typedef enum { |
BT_TEST, // AT |
BT_CONNECT, // ATA |
BT_DISCONNECT, // ATH |
BT_CLEAR_ADDRESS, // ATD0 |
BT_SET_ADDRESS, // ATD=_____ |
BT_GET_LOCALID, // ATB? Inquire the Local BD address |
BT_FIND_DEVICES, // ATF? |
BT_DISABLE_AUTOCONNECT, // ATO1 |
BT_ENABLE_AUTOCONNECT, // ATO0 |
BT_SET_MASTER, // ATR0 |
BT_SET_SLAVE, // ATR1 |
BT_SET_PIN, // ATP=1234 |
BT_SET_2400, // ATL* Baudrate 2400 |
BT_SET_4800, // ATL0 Baudrate 4800 |
BT_SET_9600, // ATL1 Baudrate 9600 |
BT_SET_19200, // ATL2 Baudrate 19200 |
BT_SET_38400, // ATL3 Baudrate 38400 |
BT_SET_57600, // ATL4 Baudrate 57600 |
BT_SET_115200, // ATL5 Baudrate 115200 |
BT_SET_NOANSWER, // ATQ1 Rückmeldungen aus |
BT_SET_NOECHO, // ATE0 ECHO deaktivieren |
BT_SET_ANSWER, // ATQ0 Rückmeldungen |
BT_SET_ECHO, // ATE1 ECHO aktivieren |
BT_SET_DEFAULT, // Defaultwerte setzen |
BT_SET_NAME, // Devicename |
BT_SET_DISPWRDOWN, // disable auto Powerdown |
BT_SHOW_SETTINGS // ATI1 Listing all setting value |
} bt_cmd_t; |
//TODO: FIFO Groesse |
#define IN_FIFO_SIZE 512 |
char localID[15]="12345678901234"; |
static uint8_t bt_buffer[IN_FIFO_SIZE]; |
fifo_t in_fifo; |
char bt_rx_buffer[RXD_BUFFER_SIZE]; |
volatile uint8_t bt_rx_len; |
volatile uint8_t bt_rx_ready = 0; |
uint8_t EchoAnswerOn=false; //Merkzelle |
static bt_mode_t bt_mode = BLUETOOTH_SLAVE; |
static communication_mode_t comm_mode = BT_CMD; |
uint8_t i = 0; |
uint8_t bt_devicecount = 0; |
uint8_t bt_rxerror = 0; |
uint8_t bt_frameerror = 0; |
uint8_t bt_overrun = 0; |
uint8_t bt_bufferoverflow = 0; |
device_info device_list[NUTS_LIST]; |
// Set a timeout of Y ms and a Conditon X, which have to be true while timeout |
#define while_timeout(X, Y) for(uint16_t __timeout = 0; __timeout++ <= Y && (X); Delay_MS(Y ? 1 : 0)) |
//-------------------------------------------------------------- |
// entfernt ungueltige Zeichen und Leerzeichen (rechts) aus dem |
// BT-Namen die evtl. durch Default's reingekommen sind. |
// Falls ungueltige Zeichen ('.') wird der BT-Name auf "PKT" |
// gesetzt. |
//-------------------------------------------------------------- |
void bt_fixname(void) |
{ |
char *p; |
p = Config.bt_name; |
while( *p!=0 && *p!='.' ) p++; // suche '.' |
if( *p=='.' || Config.bt_name[0]==' ' || Config.bt_name[0]==0) // wenn '.' gefunden oder Anfang mit ' ' oder kein String |
strcpy_P(Config.bt_name, PSTR("PKT")); // -> dann BT-Name auf "PKT" setzen |
strrtrim( Config.bt_name ); // ggf. Leerzeichen rechts loeschen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_MS(int count) |
{ |
for (int i = 0; i < count; i++) |
_delay_ms(1); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_start(void) |
{ |
if( Config.BTIsSlave == true ) EchoAnswerOn = false; |
else EchoAnswerOn = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void uart_receive(void) |
{ |
unsigned int uart_data; |
while( !fifo_is_full(&in_fifo) ) |
{ |
uart_data = uart1_getc(); |
//USART_puts("."); |
if (!(uart_data==UART_NO_DATA)) USART_putc(uart_data); |
switch( uart_data & 0xFF00 ) |
{ |
// Framing Error detected, i.e no stop bit detected |
case UART_FRAME_ERROR: |
bt_rxerror++; |
bt_frameerror++; |
return; |
// Overrun, a character already presend in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped |
// |
case UART_OVERRUN_ERROR: |
bt_rxerror++; |
bt_overrun++; |
return; |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// |
case UART_BUFFER_OVERFLOW: |
bt_rxerror++; |
bt_bufferoverflow++; |
return; |
// UART Inputbuffer empty, nothing to do |
case UART_NO_DATA: |
return; |
default: |
fifo_write( &in_fifo, uart_data); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void uart_send(const char *data, const uint8_t length) |
{ |
#ifdef SND_DEBUG |
debug_pgm(PSTR("bt_uart_send")); |
if (comm_mode == BT_CMD) debug_pgm(PSTR("bt_uart_send:BT_CMD")); else debug_pgm(PSTR("bt_uart_send:Wrong comm-mode")); |
if (EchoAnswerOn == true) debug_pgm(PSTR("bt_uart_send:EchoAnswer ON")); else debug_pgm(PSTR("bt_uart_send:EchoAnswer OFF")); |
#endif |
char echo; |
// lcd_printp_at (i++, 1, PSTR("."), 0); |
for (uint8_t i = 0; i < length; i++) |
{ |
#ifdef SND_DEBUG |
USART_putc((data[i])); //test |
#endif |
// debug_pgm(PSTR("bt_init_S")); |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR("UART: Remote not ready")); |
#endif |
return; |
} |
if (comm_mode == BT_RAW) |
_delay_ms(50); |
if (comm_mode == BT_DATA) |
_delay_ms(1); |
if ((comm_mode == BT_CMD) && (EchoAnswerOn == true)) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: get Echo")); |
#endif |
uint8_t x = 0; |
for (; x < 3; x++) |
{ |
while_timeout(fifo_is_empty(&in_fifo), 200) |
// for(uint16_t __timeout = 0; __timeout++ <= 200 && (fifo_is_empty(&in_fifo)); _delay_ms(200 ? 1 : 0)) |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
if (echo != data[i]) { |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UART: Remote not ready")); |
#endif |
return; |
} |
} |
else |
break; |
} |
#ifdef DEBUG |
if (x == 3) |
{ |
error_putc(data[i]); |
error_pgm(PSTR("BT: WRONG ECHO")); |
//_delay_ms(2000); |
} |
#endif |
} |
else |
{ |
{ |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
} |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: skip Echo")); |
#endif |
} |
} |
} |
//-------------------------------------------------------------- |
// send_cmd( command, *data) |
//-------------------------------------------------------------- |
static uint16_t send_cmd( const bt_cmd_t command, const char *data) |
{ |
uint16_t CommandDelay; // nach BTM222 Kommandos verschiedene Verzögerungszeiten bevor es weitergehen kann |
uint8_t i; |
char full_command[20]; // Maximum command size |
CommandDelay = 100; // Default = 100ms |
switch( command ) |
{ |
case BT_SET_PIN: strcpy_P( full_command, PSTR("ATP=")); |
for( i = 0; i < bt_pin_length; i++) |
{ |
full_command[i+4] = Config.bt_pin[i]; |
} |
full_command[(bt_pin_length+4)] = 0; |
break; |
case BT_SET_DEFAULT: strcpy_P( full_command, PSTR("ATZ0")); |
CommandDelay = 3000; |
break; |
case BT_SET_2400: strcpy_P( full_command, PSTR("ATL*")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL*")); |
#endif |
break; |
case BT_SET_4800: strcpy_P( full_command, PSTR("ATL0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL0")); |
#endif |
break; |
case BT_SET_9600: strcpy_P( full_command, PSTR("ATL1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL1")); |
#endif |
break; |
case BT_SET_19200: strcpy_P( full_command, PSTR("ATL2")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL2")); |
#endif |
break; |
case BT_SET_38400: strcpy_P( full_command, PSTR("ATL3")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL3")); |
#endif |
break; |
case BT_SET_57600: strcpy_P( full_command, PSTR("ATL4")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL4")); |
#endif |
break; |
case BT_SET_115200: strcpy_P( full_command, PSTR("ATL5")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL5")); |
#endif |
break; |
case BT_SET_NOANSWER: strcpy_P( full_command, PSTR("ATQ1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ1")); |
#endif |
break; |
case BT_SET_NOECHO: strcpy_P( full_command, PSTR("ATE0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE0")); |
#endif |
break; |
case BT_SET_ANSWER: strcpy_P( full_command, PSTR("ATQ0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ0")); |
#endif |
break; |
case BT_SET_ECHO: strcpy_P( full_command, PSTR("ATE1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE1")); |
#endif |
break; |
case BT_TEST: strcpy_P( full_command, PSTR("AT")); |
#ifdef DEBUG |
debug_pgm(PSTR("AT")); |
#endif |
break; |
case BT_CONNECT: strcpy_P( full_command, PSTR("ATA")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATA")); |
#endif |
break; |
case BT_DISCONNECT: strcpy_P( full_command, PSTR("ATH")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATH")); |
#endif |
break; |
case BT_CLEAR_ADDRESS: strcpy_P( full_command, PSTR("ATD0")); |
break; |
case BT_SET_ADDRESS: strcpy_P( full_command, PSTR("ATD=")); |
memcpy( (full_command + strlen(full_command)), data, 12); |
full_command[16] = 0; |
#ifdef DEBUG |
debug_pgm(PSTR("ATLD=")); |
#endif |
break; |
case BT_FIND_DEVICES: strcpy_P( full_command, PSTR("ATF?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATF?")); |
#endif |
break; |
case BT_DISABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO1")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO1")); |
#endif |
break; |
case BT_ENABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO0")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO0")); |
#endif |
break; |
case BT_SET_MASTER: strcpy_P( full_command, PSTR("ATR0")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR0")); |
#endif |
break; |
case BT_SET_SLAVE: strcpy_P( full_command, PSTR("ATR1")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR1")); |
#endif |
break; |
case BT_SET_NAME: strcpy_P( full_command, PSTR("ATN=") ); |
strcat( full_command, Config.bt_name); |
//lcd_print_at( 0, 7, full_command, 0); // DEBUG |
//lcdx_printf_at( 17, 7, MNORMAL, 0,0, "%3d", strlen(full_command) ); |
//_delay_ms(6000); |
#ifdef DEBUG |
debug_pgm(PSTR("ATN=")); |
#endif |
break; |
case BT_SET_DISPWRDOWN: strcpy_P(full_command, PSTR("ATS1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATS1")); |
#endif |
break; |
case BT_GET_LOCALID: strcpy_P(full_command, PSTR("ATB?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATB?")); |
#endif |
break; |
case BT_SHOW_SETTINGS: strcpy_P(full_command, PSTR("ATI1")); |
break; |
default: warn_pgm(PSTR("CMD UNK")); |
CommandDelay = 0; |
return false; |
} |
strcat_P( full_command, PSTR("\r") ); |
// throw away your television |
uart_receive(); |
fifo_clear( &in_fifo ); |
// send command |
uart_send( full_command, strlen(full_command)); |
// get response |
#ifdef DEBUG |
debug_pgm(PSTR("send_cmd:get Response")); |
#endif |
if( !EchoAnswerOn ) |
{ |
_delay_ms(CommandDelay); |
uart1_flush(); |
fifo_clear(&in_fifo); |
return true; |
} |
else |
{ |
while_timeout(true, BT_CMD_TIMEOUT_MS) |
{ |
uart_receive(); |
if( command == BT_GET_LOCALID ) |
{ |
_delay_ms(CommandDelay); |
return bt_getID(); |
} |
if( fifo_strstr_pgm( &in_fifo, PSTR("OK\r\n")) ) |
{ |
_delay_ms(CommandDelay); |
return true; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("ERROR\r\n")) ) |
{ |
return false; |
} |
} // end: while_timeout() |
} |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void clean_line(void) |
{ |
while_timeout(true, 50) |
uart_receive(); |
fifo_strstr_pgm( &in_fifo, PSTR("\r\n") ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static communication_mode_t update_comm_mode( uint16_t timeout_ms ) |
{ |
while_timeout(true, timeout_ms) |
{ |
uart_receive(); |
if( fifo_strstr_pgm(&in_fifo, PSTR("DISCONNECT")) ) |
{ |
clean_line(); |
comm_mode = BT_CMD; |
send_cmd(BT_TEST, NULL); |
return comm_mode; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("CONNECT")) ) |
{ |
_delay_ms(100); // don't delete this, else there will be no success!!!!!!!!! |
comm_mode = BT_DATA; |
return comm_mode; |
} |
if( fifo_strstr_pgm (&in_fifo, PSTR("Time out,Fail to connect!")) ) |
{ |
clean_line(); |
#ifdef DEBUG |
debug_pgm(PSTR("CONNECT FAILED\n")); |
#endif |
send_cmd(BT_TEST, NULL); |
comm_mode = BT_CMD; |
return comm_mode; |
} |
} |
return comm_mode; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_set_EchoAnswer( uint8_t on ) |
{ |
comm_mode = BT_CMD; // Set comm_mode to CMD |
EchoAnswerOn = false; |
if( on ) |
{ |
send_cmd( BT_SET_ECHO , NULL); |
send_cmd( BT_SET_ANSWER , NULL); |
send_cmd( BT_TEST , NULL); |
// EchoAnswerOn = true; |
/* |
if( !send_cmd( BT_TEST , NULL) ) |
{ |
//lcd_printp_at (0, 6, PSTR("Error set Echo"), MNORMAL); |
//_delay_ms(3000); |
} |
*/ |
} |
else |
{ |
send_cmd( BT_SET_NOECHO , NULL); |
send_cmd( BT_SET_NOANSWER, NULL); |
// EchoAnswerOn = false; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t BTM222_SetBaud( uint8_t baudrate ) |
{ |
bt_cmd_t btcommand = BT_SET_57600; |
uint8_t lOk = true; |
comm_mode = BT_CMD; // Set comm_mode to CMD |
uart1_flush(); |
fifo_clear( &in_fifo ); |
bt_set_EchoAnswer(true); |
if( !send_cmd(BT_TEST, NULL) ) // Test mit PKT_Baudrate |
{ |
return false; |
} |
else |
{ |
switch( baudrate ) |
{ |
case Baud_2400: btcommand = BT_SET_2400; break; |
case Baud_4800: btcommand = BT_SET_4800; break; |
case Baud_9600: btcommand = BT_SET_9600; break; |
case Baud_19200: btcommand = BT_SET_19200; break; |
case Baud_38400: btcommand = BT_SET_38400; break; |
case Baud_57600: btcommand = BT_SET_57600; break; |
case Baud_115200: btcommand = BT_SET_115200; break; |
} |
if( !(send_cmd( btcommand, NULL)) ) lOk = false; // Fehler |
SetBaudUart1( baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
send_cmd( BT_TEST, NULL); |
if( !(send_cmd(BT_TEST, NULL)) ) lOk = false; // Fehler |
bt_set_EchoAnswer(false); |
} |
return lOk; |
} |
//-------------------------------------------------------------- |
// foundbaud = BTM222_CheckBaud( Baudrate ) |
// |
// suche BTM222 mit entsprechender Baudrate |
// |
// RUECKGABE: |
// =0: Error (BTM222 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint8_t BTM222_CheckBaud( uint8_t Baudrate ) |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
comm_mode = BT_CMD; // Set comm_mode to CMD |
SetBaudUart1( Baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
EchoAnswerOn = false; |
send_cmd( BT_TEST , NULL); |
send_cmd( BT_SET_ANSWER, NULL); |
send_cmd( BT_SET_ECHO , NULL); |
EchoAnswerOn = true; |
if( send_cmd(BT_TEST, NULL) ) |
{ |
return Baudrate; // BTM222 Baudrate gefunden |
} |
return 0; // 0 = nicht gefunden |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void BTM222_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
// ggf. rechte Leerzeichen oder ungueltige Zeichen |
// von bt_name entfernen |
bt_fixname(); |
//---------------------------- |
// 1. Frage: initialisieren? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, PSTR("initialisieren?"), NULL, STR_BTM222INIT, STR_BTM222INIT) ) |
{ |
return; |
} |
//------------------------------------ |
// 2. BTM222 suchen |
//------------------------------------ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Old_Baudrate) ); // "suche nnn Baud" |
set_Modul_On( Bluetooth ); |
EchoAnswerOn = false; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_115200 ); |
if( !foundbaud ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 not found!"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
Config.UseBT = false; // Modul in Konfig sperren, verhindert Init beim Start |
} |
//------------------------------------ |
// 2. Initialisieren |
//------------------------------------ |
if( foundbaud > 0 ) |
{ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel: mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_BTM222INIT ); // Headline |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 7, 0, 0,0); // 7 Progress Steps |
comm_mode = BT_CMD; // Set comm_mode to CMD |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_CLEAR_ADDRESS, NULL) ) nError = 1; // Clear remote address |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_SLAVE, NULL) ) nError = 2; // Set BTM to SLAVE |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_PIN, NULL) ) nError = 3; // Set BTM PIN |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_NAME, NULL) ) nError = 4; // Set BTM Name |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_DISPWRDOWN, NULL) ) nError = 5; |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_GET_LOCALID, NULL) ) nError = 6; // MAC ermitteln |
//if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_ANSWER, NULL) ) nError = 7; |
if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Config.PKT_Baudrate)) nError = 8; // Set BTM Baudrate |
//if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Baud_38400)) nError = 8; // * TEST/DEBUG * Set BTM Baudrate |
SetBaudUart1( Config.PKT_Baudrate ); // PKT wieder auf Original Baudrate setzen |
if( !nError ) |
{ |
bt_set_EchoAnswer( false ); |
bt_mode = BLUETOOTH_SLAVE; |
WriteBTInitFlag(); // Init merken |
WriteBTSlaveFlag(); |
bt_start(); |
set_Modul_On( USB ); |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
return; |
} |
else |
{ |
set_Modul_On(USB); |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("BTM Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
return; |
} |
} |
SetBaudUart1( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
set_Modul_On( USB ); |
clear_key_all(); |
return; |
} |
//###################################################################################### |
//###################################################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_set_mode( const bt_mode_t mode ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: set Mode")); |
#endif |
//if (update_comm_mode(0) == BT_DATA) // 30.1.2012 CB |
// return false; |
if( mode == bt_mode ) |
return true; |
if( mode == BLUETOOTH_MASTER ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_SET_MASTER, NULL) ) |
{ |
bt_mode = BLUETOOTH_MASTER; |
send_cmd(BT_DISABLE_AUTOCONNECT, NULL); |
WriteBTMasterFlag(); |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Master is set")); |
#endif |
} |
} |
if( mode == BLUETOOTH_SLAVE ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_ENABLE_AUTOCONNECT, NULL) ) |
{ |
bt_mode = BLUETOOTH_SLAVE; |
send_cmd(BT_SET_SLAVE, NULL); |
WriteBTSlaveFlag(); |
bt_set_EchoAnswer(false); |
comm_mode = BT_CMD; |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Slave is set")); |
#endif |
} |
} |
// if (bt_mode == BLUETOOTH_MASTER) debug_pgm(PSTR("bt_mode:BLUETOOTH_MASTER ")); |
// if (bt_mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("bt_mode:BLUETOOTH_SLAVE")); |
// if (mode == BLUETOOTH_MASTER) debug_pgm(PSTR("mode:BLUETOOTH_MASTER ")); |
// if (mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("mode:BLUETOOTH_SLAVE")); |
return (mode == bt_mode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_receive( void *data, uint8_t length, uint16_t timeout_ms ) |
{ |
uint8_t rec_length = 0; |
uint8_t i = 0; |
//while_timeout(true, timeout_ms); |
for( uint16_t __timeout = 0; __timeout++ <= true && (timeout_ms); _delay_ms(true ? 1 : 0)) |
{ |
if( i == length ) |
return true; |
uart_receive(); |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( update_comm_mode(0) != BT_DATA) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("not connected")); |
#endif |
return false; |
} |
if( timeout_ms == 0 ) // We have a connection |
timeout_ms += 2000; |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( !rec_length ) // Find starting point of packet |
{ |
fifo_read( &in_fifo, (char *)&rec_length); |
if( rec_length != length ) |
{ |
rec_length = 0; |
} |
else |
{ |
timeout_ms += 2000; |
} |
} |
else |
{ |
fifo_read( &in_fifo, (char *)data + i); |
i++; |
} |
} |
return false; |
} |
#ifndef SaveMem |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_send( void *data, const uint8_t length ) |
{ |
if (update_comm_mode(0) == BT_CMD) |
return false; |
uart_send((const char *)&length, 1); |
uart_send((char *)data, length); |
return (update_comm_mode(0) == BT_DATA); |
} |
#ifdef SQUIRREL |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_connect( const char *address ) |
{ |
fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart_receive(); |
fifo_clear(&in_fifo); |
// Maybe we already disconnected??? |
if (BT_DATA == update_comm_mode(0)) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("We are still connected...")); |
#endif |
return false; |
} |
// send_cmd(BT_TEST, NULL); |
/* |
if (!send_cmd(BT_DISABLE_AUTOCONNECT, address)) |
return false; |
*/ |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("SET_ADD")); |
#endif |
if (!send_cmd(BT_SET_ADDRESS, address)) |
return false; |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("CONNECT")); |
#endif |
if (!send_cmd(BT_CONNECT, NULL)) |
return false; |
#ifdef DEBUG |
debug_pgm (PSTR ("WAIT FOR COMM")); |
#endif |
return (BT_DATA == update_comm_mode(20000)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_disconnect(void) |
{ |
uint8_t RetVal = false; |
for( uint8_t i = 0; i < 4; i++) // Switch to online cmd mode |
{ |
const char plus = '+'; |
uart_send(&plus, 1); |
_delay_ms(1000); |
} |
comm_mode = BT_CMD; |
EchoAnswerOn = true; |
uart1_flush(); // noch nicht verarbeitete Daten löschen |
fifo_clear(&in_fifo); |
if( !send_cmd(BT_DISCONNECT, NULL) ) |
RetVal = false; |
if( BT_CMD == update_comm_mode(2000) ) // neue Position um Disconnect-Antwort zu finden |
{ |
fifo_clear(&in_fifo); |
RetVal = true; |
} |
if( !send_cmd(BT_CLEAR_ADDRESS, NULL) ) |
RetVal = false; |
// if( BT_CMD == update_comm_mode(2000) ) // 31.7.2015 CB ursprüngliche Position, wurde verschoben |
// { |
// fifo_clear(&in_fifo); |
// return true; |
// } |
#ifdef DEBUG |
debug_pgm(PSTR("Still in DATA??")); |
#endif |
return RetVal; |
} |
/* |
BTM-222 Softwareversion 4.35 |
Inquiry Results: |
111111111222222222233333333334 |
01234567890123456789012345678901234567890 |
1 LE091452 0024-2C-BEB0CA |
2 E71 c 0024-7C-3EC9B9 |
BTM-222 Softwareversion 6.26 |
Inquiry Results: |
1 E71 c 0024-7C-3EC9B9 N.A. |
2 LE091452 0024-2C-BEB0CA N.A. |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_mac(const char *src, char *dst) |
{ |
uint8_t off = 0; |
for (uint8_t i = 0; i < 40; i++) |
{ |
if (src[i] == '-') |
if (src[i+3] == '-')// MAC Adresse suchen |
{ |
off = i-4; |
break; |
} |
} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_localID(const char *src, char *dst) |
{ |
uint8_t off = 0; |
//for (uint8_t i = 0; i < 40; i++) |
//{ |
// if (src[i] == '-') if (src[i+3] == '-')// MAC Adresse suchen |
// { |
// off = i-4; |
// break; |
// } |
//} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_DevName(const char *src, char *dst) |
{ |
uint8_t off = 0; |
uint8_t i = 0; |
for (i = 0; i < 18; i++) |
{ |
if (src[i] == ' ' && src[i+1] == ' ') |
{ |
dst[i] = 0; //Stringende |
break; // nach zwei Leerzeichen ist der Name zuende |
} |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_discover( void ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t devcount = 0; |
uint16_t Timeout = 10000; |
uint8_t progress = 0; |
uint8_t keyesc = false; |
#define BT_SEARCHTIME 7500 // = 75 Sekunden BT-Geraete suchen (60 werden vom BTM222 min. dafuer veranschlagt + 15 Sekunden Sicherheit) |
#define PROGRESSUPDATE 100 // = 1 Sekunde - jede Sekunde die Progressbar updaten bei BT-Inquiry |
//PKT_Progress_Init( max, yoffs, width,height) |
PKT_Progress_Init( 4 + ((BT_SEARCHTIME/PROGRESSUPDATE)+1), 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
//------------------------ |
//------------------------ |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 2 |
send_cmd( BT_TEST, NULL); |
fifo_clear( &in_fifo ); |
if( !send_cmd( BT_FIND_DEVICES, NULL) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 3 |
//------------------------ |
//------------------------ |
do |
{ |
uart_receive(); |
Timeout--; |
_delay_ms(1); |
//write_ndigit_number_u( 0,7,fifo_getcount( &in_fifo) ,5,0,0); |
if( fifo_is_full(&in_fifo) ) |
break; |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
keyesc = true; |
break; |
} |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
fifo_clear( &in_fifo ); |
//------------------------ |
//------------------------ |
if( Timeout!=0 && !keyesc ) |
{ |
// 25.06.2014 OG: umgestellt auf zeitliches Timeout anstatt einer Zaehlschleife |
timer3 = BT_SEARCHTIME; // 75 Sekunden BT Device suchen |
while( timer3 && (!fifo_search( &in_fifo, PSTR("Inquiry End")))) |
{ |
if( (timer3 / PROGRESSUPDATE) != progress ) // Progressbar: next step |
{ |
progress = (timer3 / PROGRESSUPDATE); |
PKT_Progress_Next(); |
} |
uart_receive(); |
_delay_ms(2); // 25.06.2014 OG: diese Verzoegerung muss aktuell sein - sonst werden scheinbar keine Geraete gefunden :-( |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
//keyesc = true; |
break; |
} |
} |
} |
PKT_Progress_Next(); // Progressbar: Extra 4 |
//set_beep( 300, 0xffff, BeepNormal ); _delay_ms(3000); // Test um das Ende der Progressbar zu ueberpruefen |
//------------------------ |
//------------------------ |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
// write_ndigit_number_u( 0,6,devcount ,5,0,0); |
// set_beep( 300, 0xffff, BeepNormal ); |
// _delay_ms(1000); |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
if( strstr_P( buffer, PSTR("Inquiry End"))) |
{ |
break; |
} |
copy_DevName( &buffer[3], device_list[devcount].DevName); |
copy_mac( &buffer[3], device_list[devcount].mac); |
devcount++; |
if( devcount >= NUTS_LIST ) |
{ |
break; |
} |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
return devcount; |
} |
//-------------------------------------------------------------- |
uint8_t bt_showsettings( void ) |
{ |
char buffer[512]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t Timeout = 100; |
bt_rxerror = 0; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart1_flush(); |
PKT_Progress_Init( 4 , 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("1 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("1 E A off") ); |
PKT_Progress_Next(); // Progressbar: Extra 2 |
EchoAnswerOn = true; |
if( !send_cmd( BT_SHOW_SETTINGS, NULL) ) |
ScrollBox_Push_P( MNORMAL, PSTR("Command Error") ); |
PKT_Progress_Next(); // Progressbar: Extra 3 |
do |
{ |
uart_receive(); |
// write_ndigit_number_u( 0,6,fifo_getcount( &in_fifo) ,5,0,0); |
Timeout--; |
_delay_ms(2); |
if( fifo_is_full(&in_fifo) ) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("Fifo is full") ); |
break; |
} |
} while(Timeout != 0 ); |
// } while( (Timeout != 0) && (!fifo_search(&in_fifo, PSTR("+++"))) ); |
PKT_Progress_Next(); // Progressbar: Extra 4 |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
ScrollBox_Push( MNORMAL, buffer ); |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("2 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("2 E A off") ); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
/* |
uint16_t bt_discover_OLD( char result[8][12] ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t pos = 0; |
uint16_t Timeout = 10000; |
uint16_t posC = 0; |
uint8_t Result=false; |
PKT_Progress_Init( 25, 0, 0,0); // 25 Progress Steps |
if (!bt_set_mode(BLUETOOTH_MASTER)) Result = false; |
PKT_Progress_Next(); // Progressbar |
send_cmd(BT_TEST, NULL); |
if( !send_cmd(BT_FIND_DEVICES, NULL)) Result = false; |
PKT_Progress_Next(); // Progressbar |
do |
{ |
uart_receive(); |
Timeout--; |
posC++; |
_delay_ms(1); |
write_ndigit_number_u(0,5,fifo_getcount(&in_fifo),5,0,0); |
if (posC ==1000) |
{ |
PKT_Progress_Next(); // Progressbar |
posC = 0; |
} |
if (fifo_is_full(&in_fifo)) break; |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
if (Timeout==0) |
{ |
Result = false; |
} |
else |
{ |
for (uint16_t i = 0; i < 40000; i++) |
{ |
if( (i % 2000) == 0 ) |
PKT_Progress_Next(); // Progressbar |
uart_receive(); |
write_ndigit_number_u( 0,5,fifo_getcount(&in_fifo),5,0,0); |
_delay_ms(1); |
} |
} |
while( !fifo_is_empty(&in_fifo) ) |
{ |
// Get next line |
while (!fifo_cmp_pgm(&in_fifo, PSTR("\r\n"))) |
{ |
fifo_read(&in_fifo, bufferhead); |
bufferhead++; |
} |
// terminate string |
*bufferhead = 0; |
//reset bufferhead |
bufferhead = buffer; |
if (strlen(buffer) == 0) |
continue; //the empty line before end of inquiry |
if( strstr_P(buffer, PSTR("Inquiry End"))) |
//if (searchend) |
{ |
fifo_clear(&in_fifo); |
return true; |
} |
copy_DevName(&buffer[3],device_list[pos].DevName); |
copy_mac(&buffer[3], device_list[pos].mac); |
pos++; |
} // end: while( !fifo_is_empty(&in_fifo) ) |
return Result; |
} |
*/ |
//-------------------------------------------------------------- |
// ermittelt die MAC vom BTM2222 |
//-------------------------------------------------------------- |
uint8_t bt_getID( void ) |
{ |
//char buffer[255]; //oversized, but who cares? |
char buffer[80]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint8_t i; |
while_timeout( !fifo_strstr_pgm(&in_fifo, PSTR("r\n")), 2000) |
uart_receive(); |
while( !fifo_is_empty(&in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) |
continue; // the empty line before end of inquiry |
copy_localID( &buffer[0], &localID[0] ); |
for( i = 0; i < 13; i++) |
{ |
//lcd_putc (i, 6, localID[i],0); |
Config.bt_Mac[i] = localID[i]; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("OK")) ) |
{ |
fifo_clear( &in_fifo ); |
return true; |
} |
else |
{ |
return false; |
} |
} // end: if( !fifo_is_empty(&in_fifo) ) |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_downlink_init(void) |
{ |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
_delay_ms(100); |
// debug_pgm(PSTR("bt_downlink_init")); |
uart_receive(); |
fifo_clear(&in_fifo); |
// send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init Start")); |
#endif |
if( Config.BTIsSlave == true ) // nur Init wenn BT ist Slave |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:try to set Master")); |
#endif |
//comm_mode = BT_NOECHO; |
// |
//if (!send_cmd (BT_SET_ECHO,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Echo!")); |
// #endif |
//} |
// |
//comm_mode = BT_NOANSWER; |
//if (!send_cmd(BT_SET_ANSWER,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Answer!")); |
// #endif |
// |
//} |
comm_mode = BT_CMD; |
//send_cmd(BT_TEST, NULL); |
bt_set_EchoAnswer(true); |
if( !bt_set_mode(BLUETOOTH_MASTER) ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Couldn't set master!")); |
#endif |
return; |
} |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:master is set ")); |
#endif |
//WriteBTMasterFlag(); // Master merken |
comm_mode = BT_CMD; |
} |
else |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Master was set")); |
#endif |
comm_mode = BT_CMD; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_searchDevice( void ) //Bluetoothgeräte suchen |
{ |
bt_devicecount = bt_discover(); |
} |
// |
////-------------------------------------------------------------- |
//uint16_t bt_receiveNMEA2(void) |
//{ |
// |
// char received; |
// static uint8_t line_flag = 1; |
// static char* ptr_write = bt_rx_buffer; |
// uint16_t timeout_ms=2000; |
// |
//// while_timeout(true, timeout_ms) { |
//while(1){ |
// |
// uart_receive(); |
// if (fifo_is_empty(&in_fifo)) |
// continue; |
// fifo_read(&in_fifo, &received); |
// USART_putc(received); |
//// _delay_ms(1); |
// } |
//// |
//return true; |
//} |
#endif |
#endif |
//#endif |
#endif // end: #ifdef USE_BLUETOOTH |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/bluetooth.h |
---|
0,0 → 1,182 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY bluetooth.h |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover() Parameter |
//# |
//# 16.06.2014 OG |
//# - add: BTM222_Initalize() |
//# - del: bt_init() |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() |
//############################################################################ |
#ifndef _BLUETOOTH_H_ |
#define _BLUETOOTH_H_ |
#include <avr/io.h> |
//#include <common.h> |
#include "fifo.h" |
#define SQUIRREL |
#define NUTS_LIST 16 |
#define EXTENSIONS_LIST 16 |
#define RXD_BUFFER_SIZE 150 |
//void InitBT(void); |
extern char bt_rx_buffer[RXD_BUFFER_SIZE]; |
extern volatile uint8_t bt_rx_len; |
extern volatile uint8_t bt_rx_ready; |
//extern char data_decode[RXD_BUFFER_SIZE]; |
extern uint8_t bt_rxerror; |
extern uint8_t bt_frameerror; |
extern uint8_t bt_overrun; |
extern uint8_t bt_bufferoverflow; |
extern fifo_t in_fifo; |
typedef struct _device_info device_info; |
// device info struct, holds mac , class and extensions + values of a device |
struct _device_info |
{ |
char DevName[20]; |
char mac[14]; |
// uint8_t class; |
// uint8_t extension_types[EXTENSIONS_LIST]; |
// uint16_t values_cache[EXTENSIONS_LIST]; |
}; |
extern uint8_t bt_devicecount; |
extern device_info device_list[NUTS_LIST]; |
extern char localID[15]; |
#define valid(num) (num < NUTS_LIST && (device_list[num].mac[0] != 0 || device_list[num].mac[1] != 0 || device_list[num].mac[2] != 0 || device_list[num].mac[3] != 0 || device_list[num].mac[4] != 0 || device_list[num].mac[5] != 0 || device_list[num].mac[6] != 0 || device_list[num].mac[7] != 0 || device_list[num].mac[8] != 0 || device_list[num].mac[9] != 0 || device_list[num].mac[10] != 0 || device_list[num].mac[11] != 0)) |
extern void bt_start(void); // EchoAnswervariable setzen |
void bt_set_EchoAnswer (uint8_t onoff); |
extern uint8_t bt_getID (void); |
//extern static communication_mode_t update_comm_mode(uint16_t timeout_ms); |
// Bluetooth mode ENUM |
typedef enum |
{ |
BLUETOOTH_MASTER, // < Master Mode (to create outgoinng connections). |
BLUETOOTH_SLAVE // < Slave Mode (to wait for incoming connections). |
} bt_mode_t; |
// set baudrate bluetooth |
// @return true = ok |
// false = error |
//extern uint16_t bt_setbaud (uint8_t baudrate); |
uint16_t bt_setbaud(uint8_t baudrate); |
// init bluetooth driver |
// @return always true |
// |
//extern uint16_t bt_init (void (*upate_percentage) (uint16_t)); |
//extern uint16_t bt_init (void); |
// Set the Bluetooth mode |
// @param mode bt_mode_t Bluetooth Mode ENUM (BLUETOOTH_MASTER or BLUETOOTH_SLAVE) |
// @return true if mode change was succesful, false if not |
// |
extern uint8_t bt_set_mode (const bt_mode_t mode); |
// recieve data over bluetooth |
// @param data pointer to memory for data storage |
// @param length value of length after call holds the actual recived data length |
// @param timeout_ms timeout in ms after the recive function aborts and returns with false |
// @return false if recived length > length parameter or it timeouted, true otherwise |
// |
extern uint16_t bt_receive (void * data, uint8_t length, uint16_t timeout_ms); |
// send data over bluetooth |
// @param data pointer to the data to send |
// @param length length of the data to be send |
// @return true if sendingn was successful, false otherwise |
// |
extern uint16_t bt_send (void * data, const uint8_t length); |
// squirrelt only functions |
#ifdef SQUIRREL |
// open bluetoot connection (only one at a time possible) |
// @param address connection is opened to this device mac address |
// @return true if connection was established, false otherwise |
// |
extern uint16_t bt_connect (const char *address); |
// closes bluetooth connection |
// @return false if failed, true otherwise |
// |
extern uint16_t bt_disconnect (void); |
// discover bluetooth devices |
// @param result in a 2 dimensional array first index are devicecs (max 8) second is mac address string |
// @param update_callback to inform of progress (in % from 0 to 100) |
// @return true if successful, false if error occured |
// |
//extern uint16_t bt_discover(void); |
//extern uint16_t bt_discover (char result[8][12], void (*update_callback)(const uint16_t progress)); |
extern void bt_downlink_init(void); // Auf Master stellen für Devicesuche und GPS Empfang |
extern void bt_searchDevice(void); //Bluetoothgeräte suchen |
#endif // SQUIRREL |
void uart_receive(void); |
void bt_fixname(void); |
void BTM222_Initalize( void ); |
uint8_t bt_showsettings( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/error.c |
---|
0,0 → 1,218 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
** |
* Error handling functions |
*/ |
//############################################################################ |
//# HISTORY error.c |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//########################################################################## |
#include <stdbool.h> |
#include <avr/pgmspace.h> |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
//-------------------------------------------------------------- |
inline void _send_msg(const char *msg) |
{ |
for (uint8_t i=0; i<255 && msg[i]!='\0'; i++) |
{ |
errordriver_write_c(msg[i]); |
} |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void send_pgm(const char *msg) |
{ |
uint8_t myByte; |
myByte = pgm_read_byte(msg); |
for(int i = 1; myByte != '\0'; i++) |
{ |
errordriver_write_c(myByte); |
myByte = pgm_read_byte(msg+i); |
} |
} |
//-------------------------------------------------------------- |
void error_init(void) |
{ |
error_driver_Init(); |
} |
//-------------------------------------------------------------- |
void error_putc(const char c) |
{ |
errordriver_write_c(c); |
} |
//-------------------------------------------------------------- |
void assert (bool condition, const char *msg) |
{ |
if (!condition) |
{ |
send_pgm(PSTR("ASS:")); |
_send_msg(msg); |
} |
} |
//-------------------------------------------------------------- |
void info (const char *msg) |
{ |
send_pgm(PSTR("INF:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void warn (const char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void debug (const char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void Error (const char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
_send_msg(msg); |
} |
#endif |
#ifdef DEBUG |
//-------------------------------------------------------------- |
void assert_pgm(bool condition, const prog_char *msg) |
{ |
if (condition) { |
send_pgm(PSTR("ASS:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
} |
//-------------------------------------------------------------- |
void info_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("INF:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void warn_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void error_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void debug_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
////-------------------------------------------------------------- |
//void print_hex(uint8_t num) |
//{ |
// if (num<10) |
// error_putc(num+48); |
// else |
// { |
// switch (num) |
// { |
// case 10: |
// error_putc('A'); break; |
// case 11: |
// error_putc('B'); break; |
// case 12: |
// error_putc('C'); break; |
// case 13: |
// error_putc('D'); break; |
// case 14: |
// error_putc('E'); break; |
// case 15: |
// error_putc('F'); break; |
// default: |
// error_putc('#'); break; |
// } |
// } |
//} |
// |
// |
////-------------------------------------------------------------- |
//void byte_to_hex(uint8_t byte) |
//{ |
// uint8_t b2 = (byte & 0x0F); |
// uint8_t b1 = ((byte & 0xF0)>>4); |
// print_hex(b1); |
// print_hex(b2); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/error.h |
---|
0,0 → 1,79 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
* |
* Error handling functions. |
*/ |
#ifndef __ERROR__ |
#define __ERROR__ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include "../main.h" |
#ifdef DEBUG |
void error_init(void); |
void error_putc(const char c); |
void assert (bool condition, const char *msg); |
void info (const char *msg); |
void warn(const char *msg); |
void debug(const char *msg); |
void Error(const char *msg); |
void assert_pgm(bool condition, const prog_char *msg); |
void info_pgm (const prog_char *msg); |
void warn_pgm(const prog_char *msg); |
void debug_pgm(const prog_char *msg); |
void error_pgm(const prog_char *msg); |
void byte_to_hex(uint8_t byte); |
#else |
#define error_init() {} |
#define error_putc(c) {} |
#define assert(cond, msg) {} |
#define info(msg) {} |
#define warn(msg) {} |
#define debug(msg) {} |
#define error(msg) {} |
#define assert_pgm(cond, msg) {} |
#define info_pgm(msg) {} |
#define warn_pgm(msg) {} |
#define debug_pgm(msg) {} |
#define error_pgm(msg) {} |
#define byte_to_hex(byte) {} |
#endif |
#endif //__ERROR__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/error_driver.c |
---|
0,0 → 1,27 |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
//#include "cpu.h" |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
void errordriver_write_c(char c) |
{ |
USART_putc(c); |
} |
void error_driver_Init(void) |
{ |
// USART_Init(UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/error_driver.h |
---|
0,0 → 1,22 |
/* |
* Functions to write error message to FTDI or USART |
*/ |
#ifndef __ERROR_DRIVER__ |
#define __ERROR_DRIVER__ |
#include <avr/io.h> |
#include "../main.h" |
#ifdef DEBUG |
extern void errordriver_write_c(char c); |
extern void error_driver_Init(void); |
#else |
#define errordriver_write_c(c) {} |
#define error_driver_init() {} |
#endif |
#endif //__ERROR_DRIVER__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/fifo.c |
---|
0,0 → 1,197 |
/** |
* a simple Fifo |
* @file fifo.c |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//############################################################################ |
#include <string.h> |
#include "fifo.h" |
//#if defined HWVERSION1_3W || defined HWVERSION3_9 || defined HWVERSION1_2W |
//-------------------------------------------------------------- |
void fifo_init (fifo_t * fifo, uint8_t * buffer, uint16_t size) |
{ |
fifo->size = size; |
fifo->buffer = buffer; |
fifo->head = 0; |
fifo->count = 0; |
} |
//-------------------------------------------------------------- |
uint16_t fifo_getcount (const fifo_t * fifo) |
{ |
return fifo->count; |
} |
//-------------------------------------------------------------- |
bool fifo_is_empty (const fifo_t * fifo) |
{ |
return (fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_is_full (const fifo_t * fifo) |
{ |
return (fifo->size - fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_read (fifo_t * fifo, char *data) |
{ |
if (fifo_is_empty (fifo)) |
return false; |
uint8_t *head = fifo->buffer + fifo->head; |
*data = (char) * head; |
fifo->head = ( (fifo->head + 1) % fifo->size); |
fifo->count--; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_write (fifo_t * fifo, const char data) |
{ |
if (fifo_is_full (fifo)) |
return false; |
uint8_t *end = fifo->buffer + ( (fifo->head + fifo->count) % fifo->size); |
*end = (uint8_t) data; |
fifo->count++; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_clear (fifo_t * fifo) |
{ |
fifo->count = 0; |
fifo->head = 0; |
return true; |
} |
//-------------------------------------------------------------- |
static bool fifo_cmp_pgm_at (fifo_t * fifo, const char * pgm, const uint16_t index) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= (i + index)) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i + index) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// We found the string, lets move the pointer |
fifo->head = ( (fifo->head + i + index) % fifo->size); |
fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_search (fifo_t * fifo, const char * pgm) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= i) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// // We found the string, lets move the pointer |
// fifo->head = ( (fifo->head + i + index) % fifo->size); |
// |
// fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t * fifo, const char * pgm) |
{ |
return fifo_cmp_pgm_at (fifo, pgm, 0); |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t * fifo, const char * pgm) |
{ |
for (uint16_t i = 0; i < fifo->count; i++) |
{ |
if (fifo_cmp_pgm_at (fifo, pgm, i)) |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Idee: buffer nach str durchsuchen ohne die fifo Zeiger zu aendern |
// ungetestet |
//-------------------------------------------------------------- |
bool fifo_isstr( fifo_t * fifo, const char * str) |
{ |
/* |
void *memmem(const void *s1, |
size_t len1, |
const void *s2, |
size_t len2); |
The memmem() function finds the start of the first occurrence of the substring s2 of length len2 in the memory area s1 of length len1. |
*/ |
return memmem( fifo->buffer + fifo->head, fifo->count, str, strlen(str) ) != NULL; |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth/fifo.h |
---|
0,0 → 1,107 |
/** |
* a simple Fifo |
* @file fifo.h |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//############################################################################ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#ifndef _FIFO_H_ |
#define _FIFO_H_ |
/** |
*fifo data structure all vital fifo information |
*/ |
typedef struct |
{ |
uint16_t count; /**< current number of elements */ |
uint16_t head; /**< position of the head element */ |
uint16_t size; /**< size equals max number of entrys*/ |
uint8_t* buffer; /**< pointer to memory area where the fifo is to be saved */ |
} fifo_t; |
uint16_t fifo_getcount (const fifo_t * fifo); |
/** \brief initialize of a fifo |
* sets all the information in your given fifo structure |
* @param fifo pointer to an allocated fifo_t structure |
* @param buffer pointer to an a allocated memory space for the fifo of size = sizeof(uint8_t) * size |
* @param size max number of entrys the fifo will hold |
*/ |
void fifo_init (fifo_t *fifo, uint8_t *buffer, uint16_t size); |
/** \brief checks if fifo is empty |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if empty otherwise false |
*/ |
bool fifo_is_empty (const fifo_t *fifo); |
/** \brief checks if fifo is full |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if full otherwise false |
*/ |
bool fifo_is_full (const fifo_t *fifo); |
/** \brief clears the fifo |
* resets your fifo structure to 0 elements |
* @param fifo pointer to your initialized fifo_t structure |
* @return always true (never fails) |
*/ |
bool fifo_clear (fifo_t *fifo); |
/** \brief reads head of fifo |
* reads the first element and removes it |
* @param fifo pointer to your initialized fifo_t structure |
* @return false if fifo is empty false otherwise |
*/ |
bool fifo_read (fifo_t *fifo, char *data); |
/** \brief inserts a char into the fifo |
* adds a char to the end of the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param data the char data to be inserted |
* @return false if fifo is full true otherwise |
*/ |
bool fifo_write (fifo_t *fifo, const char data); |
/** \brief compares first elements with prog_char string |
* if pgm equals the first elements of the fifo these elements are removed |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char string for comparison |
* @return true if pgm is equal to the first entrys in the fifo, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t* fifo, const char* pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_search (fifo_t * fifo, const char * pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t *fifo, const char *pgm); |
bool fifo_isstr( fifo_t * fifo, const char * str); |
#endif /* _FIFO_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/bluetooth |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/connect.c |
---|
0,0 → 1,436 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.c |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: wait_and_restore() redundanten Code einsparen |
//# - add: Port_BLE2Wi() (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 28.02.2014 OG |
//# - chg: alle Anzeigescreens von 'PKT Connect' auf Connect_Screen() umgestellt |
//# - add: Connect_Screen() |
//# |
//# 12.02.2014 OG |
//# - chg: Port_WiFly2Wi() Auskommentiert: "unused variable 'i'" |
//# |
//# 15.07.2013 Cebra |
//# - chg: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC, Texte geändert |
//# |
//# 02.07.2013 Cebra |
//# - add: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# - add: Port_WiFly2Wi, Wlan-Wi232 Verbindung |
//# |
//# 24.6.2013 Cebra |
//# - fix: Fehler beim UART1 bei der Rückschaltung von USB2FC,USB2WI,BT2FC,BT2WI |
//# |
//# 19.06.2013 OG |
//# - chg: Screen-Layout von Port_BT2Wi(), Port_USB2Wi() |
//############################################################################ |
#include "cpu.h" |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "lcd/lcd.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lipo/lipo.h" |
#include "main.h" |
#include "bluetooth/bluetooth.h" |
#include "wifly/PKT_WiFlyHQ.h" |
#include "uart/uart1.h" |
#include "utils/xutils.h" |
//#if defined HWVERSION3_9 |
//-------------------------------------------------------------- |
void Change_Output(uint8_t UartMode) // Schaltet die Rx/Tx Richtungen |
{ |
// hiermit werden die 74HTC125 (IC5) Gatter geschaltet |
clr_USB2FC(); // PC2 aus IC5 A/D |
clr_USB2Wi(); // PB0 aus IC5 B/C |
clr_Uart02FC(); // PC6 aus IC4 C/D |
clr_Uart02Wi(); // PC5 aus IC4 A/B |
switch (UartMode) |
{ |
case USB2FC: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2FC(); |
break; |
case Uart02Wi: |
set_Uart02Wi(); |
break; |
case Uart02FC: |
set_Uart02FC(); |
break; |
case USB2Wi: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2Wi(); |
break; |
} |
} |
//-------------------------------------------------------------- |
// gibt eine uebergebene Baud-Rate als uint32_t zurueck |
//-------------------------------------------------------------- |
uint32_t Baud_to_uint32( uint8_t baud ) |
{ |
switch( baud ) |
{ |
case Baud_2400: return( 2400 ); |
case Baud_4800: return( 4800 ); |
case Baud_9600: return( 9600 ); |
case Baud_19200: return( 19200 ); |
case Baud_38400: return( 38400 ); |
case Baud_57600: return( 57600 ); |
case Baud_115200: return( 115200 ); |
} |
return( 0 ); |
} |
//-------------------------------------------------------------- |
// Connect_Screen( |
// |
// Parameter: |
// title : PROGMEM |
// message : RAM oder PROGMEM je nach msg_progmem |
// msg_progmem: true/false |
// true = message im PROGMEM |
// false = message im RAM |
//-------------------------------------------------------------- |
void Connect_Screen( const char *title, const char *message, uint8_t msg_progmem ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, title, MINVERS); // Titel |
show_Lipo(); |
lcdx_printp_center( 2, title , MNORMAL, 0,-3); // |
lcdx_printp_center( 3, strGet(STR_ACTIVE), MNORMAL, 0,1); // "AKTIV!" |
if( (!msg_progmem && strlen(message)>0) || (msg_progmem && strlen_P(message)>0) ) |
{ |
if( msg_progmem ) |
lcdx_printp_center( 5, message, MNORMAL, 0,1); |
else |
lcdx_print_center( 5, (unsigned char *) message, MNORMAL, 0,1); |
lcd_rect_round( 0, 5*7+2, 127, 8+6, 1, R2); // Rahmen |
} |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// _port_wait_and_restore() |
// |
// Wartet bis der Benutzer beendet und restauriert Einstellungen |
// |
// gemeinsamer Code um Speicherplatz zu sparen |
//-------------------------------------------------------------- |
void wait_and_restore( void ) |
{ |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
} |
uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); |
SetBaudUart1( Config.PKT_Baudrate ); |
if( Config.U02SV2 == 1 ) |
Change_Output( Uart02FC ); |
else |
Change_Output( Uart02Wi ); |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Function: BT2FC() |
// Purpose: Connect BT direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2FC(void) |
{ |
/* |
// mit Anzeige von 'Baud' |
// verbraucht einige Bytes mehr... |
char *message; |
uint32_t baud; |
switch( Config.PKT_Baudrate ) |
{ |
case Baud_2400: baud = 2400; break; |
case Baud_4800: baud = 4800; break; |
case Baud_9600: baud = 9600; break; |
case Baud_19200: baud = 19200; break; |
case Baud_38400: baud = 38400; break; |
case Baud_57600: baud = 57600; break; |
case Baud_115200: baud = 115200; break; |
} |
message = buffered_sprintf_P( PSTR("%lu Baud"), baud); |
Connect_Screen( PSTR("BT -> SV2 Kabel"), message, false ); |
*/ |
// ohne Anzeige von 'Baud' |
Connect_Screen( PSTR("BT -> SV2 Kabel"), "", false ); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
_delay_ms(1000); |
//_delay_ms(2000); |
if( !Config.BTIsSlave==true ) |
bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2FC); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BT2Wi() |
// Purpose: Connect BT direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2Wi( void ) |
{ |
char *message = "RE-ID: 0000"; |
strcpy( &message[7], Config.RE_ID); |
Connect_Screen( PSTR("BT Extender"), message, false ); |
set_Modul_On( Bluetooth ); // set_BTOn(); |
_delay_ms(1500); |
if( !Config.BTIsSlave==true ) bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: WiFly2Wi() |
// Purpose: Connect WiFly direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_WiFly2Wi( void ) |
{ |
Connect_Screen( PSTR("WLAN Extender"), PSTR("169.254.1.1:2000"), true ); |
set_Modul_On( Wlan ); |
Change_Output( USB2Wi ); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BLE2Wi() |
// Purpose: Connect BLE direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_BLE2Wi( void ) |
{ |
Connect_Screen( PSTR("BLE Extender"), "", false ); |
set_Modul_On(Wlan); // verwendet wie Wlan/WiFly SV2 |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: FC2CFG_BT() |
// Purpose: Connect FC (Tx1 Pin3, Rx1 Pin4) direct to BT |
// Returns: |
//-------------------------------------------------------------- |
void Port_FC2CFG_BT(void) |
{ |
lcd_cls (); |
lcd_printp_at (0, 0, PSTR("BTM-222 Konfigurieren"), 2); |
lcd_printp_at (0, 1, PSTR("FC > MK-USB > BTM-222"), 2); |
lcd_printp_at (0, 3, PSTR("MK-USB an PC anschl. "), 0); |
lcd_printp_at (0, 4, PSTR("Zwischen MK-USB und "), 0); |
lcd_printp_at (0, 5, PSTR("PKT ein gekreuztes "), 0); |
lcd_printp_at (0, 6, PSTR("Kabel anschliessen. "), 0); |
lcd_printp_at(12, 7, strGet(ESC), 0); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
Change_Output(USB2FC); |
while(!get_key_press (1 << KEY_ESC)); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
set_Modul_On(USB); |
// set_BTOff(); |
return; |
} |
//-------------------------------------------------------------- |
// Function: USB2FC() |
// Purpose: Connect USB direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2FC(void) |
{ |
Connect_Screen( PSTR("USB -> SV2 Kabel"), PSTR("MK-USB"), true ); |
Change_Output(USB2FC); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} while(!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
uart1_init (UART_BAUD_SELECT(57600,F_CPU)); |
SetBaudUart1(Config.PKT_Baudrate); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
return; |
*/ |
} |
//-------------------------------------------------------------- |
// Function: USB2Wi() |
// Purpose: Connect USB direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2Wi(void) |
{ |
Connect_Screen( PSTR("USB -> Wi.232"), "", false ); |
Change_Output(USB2Wi); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} |
while( !get_key_press(1 << KEY_ESC) ); |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
get_key_press(KEY_ALL); |
return; |
*/ |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/connect.h |
---|
0,0 → 1,68 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.h |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: Port_BLE2Wi() |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _CONNECT_H |
#define _CONNECT_H |
void Change_Output(uint8_t UartMode); |
void Port_BT2Wi(void); |
void Port_BT2FC(void); |
void Port_WiFly2Wi( void ); |
void Port_FC2CFG_BT(void); |
void Port_USB2FC(void); |
void Port_USB2Wi(void); |
void Port_BLE2Wi( void ); |
uint32_t Baud_to_uint32( uint8_t baud ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/cpu.h |
---|
0,0 → 1,41 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _CPU_H |
#define _CPU_H |
// Quarz Frequenz in Hz |
#define F_CPU 20000000UL |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/eeprom/eeprom.c |
---|
0,0 → 1,827 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.c |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# Initialisierung der neuen Parameter hinzugefügt |
//# |
//# 17.06.2014 OG |
//# - chg: ReadParameter() - Updatecode auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: Delete_EEPROM(() Unterstuetzung Config.PKTOffTimeout (ehemals LCD_DisplayMode) |
//# |
//# 30.05.2014 OG |
//# - chg: Delete_EEPROM() Config.Lipo_UOffset auf 10000 gesetzt (vorher 8500) |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# - fix: ReadParameter() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# ab EEprom-Version 138 |
//# - fix: Delete_EEPROM() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# - fix: ReadParameter() - Kommentare zu Version 136 und 137 korrigiert |
//# bzgl. der PKT-Versionsangabe und Datum ergaenzt |
//# |
//# 06.04.2014 OG |
//# - chg: ReadParameter(): umgestellt auf lcdx_printp_center() |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: ReadParameter(): upgrade Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# - add: ReadParameter(): upgrade Config.UseBLE, Config.MKParam_Favs |
//# - add: Delete_EEPROM(): init von Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# |
//# 01.04.2014 OG |
//# - add: Delete_EEPROM(): init von Config.UseBLE |
//# - add: Delete_EEPROM(): init von Config.MKParam_Favs |
//# |
//# 27.03.2014 OG |
//# - chg: ReadParameter() Niederländisch wird auf Englisch umgeschaltet |
//# -> Vorbereitung zum entfernen der Niederländischen Sprache |
//# |
//# 11.02.2014 Cebra |
//# - add: Delete EEProm: Config.OSD_ShowCellU = false; |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU in ReadParameter (EEpromversion 0x87) |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 07.07.2013 OG |
//# - chg: Strings fuer WLan gekuerzt |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 0x85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 0x84 |
//# |
//# 03.07.2013 OG |
//# - chg: ReadParameter() - PKT-Upgrade restrukturiert bzgl. inkrementelle |
//# Updates vorheriger Versionen |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 02.07.2013 OG |
//# - chg: frei geworden: Config.OSD_HomeMKView (nicht mehr benoetigt) |
//# |
//# 26.06.2013 OG |
//# - chg: frei geworden: Config.PKT_StartInfo (nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - fix: ReadParameter(): ergaenzt um bt_fixname() um ggf. ungueltige |
//# Zeichen im Bluetooth-Namen zu eliminieren |
//# |
//# 23.06.2013 OG |
//# - chg: Delete_EEPROM(): Default von Config.Lipo_UOffset von 6000 auf 8500 gesetzt |
//# - chg: Delete_EEPROM(): Default von Config.bt_name auf "PKT" gekuerzt |
//# - chg: Delete_EEPROM(): Config.bt_name -> nicht mehr mit Leerzeichen auffuellen! |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 13.06.2013 OG |
//# - fix: Config.PKT_Accutyp hinzugefuegt in Delete_EEPROM() |
//# - chg: Default GPS-LastPosition auf 0/0 gesetzt in Delete_EEPROM() |
//# - chg: Default Config.PKT_StartInfo auf false gesetzt in Delete_EEPROM() |
//# - chg: Code Layout |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung auf Version 81, Versionsanpassung angepasst |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup EEPROM Parameter |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <avr/interrupt.h> |
#include <avr/eeprom.h> |
#include <stdbool.h> |
#include <avr/wdt.h> |
#include "../lcd/lcd.h" |
#include "../main.h" |
#include "../timer/timer.h" |
#include "eeprom.h" |
#include "../wi232/Wi232.h" |
#include "../setup/setup.h" |
#include "../bluetooth/bluetooth.h" |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../osd/osd.h" |
#include "../uart/uart1.h" |
#include "../messages.h" |
#include "../osd/osddata.h" |
#include "../mksettings/paramset.h" |
//------------------------------------------------------------------------------------------ |
ST EEMEM EEStruct; |
ST Config; |
//############################################################# |
// ANMERKUNG OG: |
// |
// es ware besser wenn die gesamte Config-struct zum PC |
// gesendet wird anstatt aufbreitete Textlines. Das spart |
// Speicher im PKT (der ist limitiert). |
// |
// Die Aufbereitung der Daten (Umrechnungen) kann dann auf |
// der PC Seite erfolgen. |
// |
// Nachteil dabei: wenn sich die Config-struct des PKT |
// ändert muss auch das PKT-Tool auf dem PC angepasst werden. |
// |
//CB: deshalb ist das auch jetzt auskommentiert |
// |
//############################################################# |
//char printbuff[100]; |
//void print_data_int8( const char *text, int8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
//// |
//// |
//void print_data_uint8( const char *text, uint8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_uint16( const char *text, uint16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_int16( const char *text, int16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_char( const char *text, char *variable, uint8_t length) |
//{ |
// uart1_puts_p(text); |
// uart1_puts(variable); |
// uart1_puts("\r\n"); |
//} |
// |
// |
//void print_time( const char *text, PKTdatetime_t variable) |
//{ |
// uart1_puts_p(text); |
// sprintf(printbuff, "%d", variable.day); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.month); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.year); |
// uart1_puts(printbuff); |
//// uart1_puts(" "); |
//// sprintf(printbuff, "%d", variable.seconds); |
//// uart1_puts(printbuff); |
// uart1_puts("\r\n"); |
//} |
// |
// |
// |
//void print_OSD_Statistic(void) |
//{ |
// print_time (PSTR("begin_StatTime:"),Config.OSD_Statistic.begin_StatTime); |
// print_time (PSTR("end_StatTime:"),Config.OSD_Statistic.end_StatTime); |
// print_data_uint16 (PSTR("total_FlyTime:"),&Config.OSD_Statistic.total_FlyTime); |
// print_data_uint16 (PSTR("last_FlyTime:"),&Config.OSD_Statistic.last_FlyTime); |
// print_data_uint16 (PSTR("count_Errorcode:"),&Config.OSD_Statistic.count_Errorcode); |
// print_data_int16 (PSTR("max_Altimeter:"),&Config.OSD_Statistic.max_Altimeter); |
// print_data_uint16 (PSTR("max_GroundSpeed:"),&Config.OSD_Statistic.max_GroundSpeed); |
// print_data_uint16 (PSTR("max_Distance:"),&Config.OSD_Statistic.max_Distance); |
// print_data_uint16 (PSTR("max_Current:"),&Config.OSD_Statistic.max_Current); |
// print_data_uint16 (PSTR("max_Capacity:"),&Config.OSD_Statistic.max_Capacity); |
// print_data_int16 (PSTR("max_Variometer:"),&Config.OSD_Statistic.max_Variometer); |
// print_data_int8 (PSTR("max_AngleNick:"),&Config.OSD_Statistic.max_AngleNick); |
// print_data_int8 (PSTR("max_AngleRoll:"),&Config.OSD_Statistic.max_AngleRoll); |
// print_data_uint8 (PSTR("RC Quality:"),&Config.OSD_Statistic.max_RCQuality); |
// print_data_int16 (PSTR("max_TopSpeed:"),&Config.OSD_Statistic.max_TopSpeed); |
// print_data_int8 (PSTR("min_AngleNick:"),&Config.OSD_Statistic.min_AngleNick); |
// print_data_int8 (PSTR("min_AngleRoll:"),&Config.OSD_Statistic.min_AngleRoll); |
// print_data_uint8 (PSTR("min_RCQuality:"),&Config.OSD_Statistic.min_RCQuality); |
// print_data_uint8 (PSTR("min_UBat:"),&Config.OSD_Statistic.min_UBat); |
// print_data_uint8 (PSTR("LiPoCells:"),&Config.OSD_Statistic.LiPoCells); |
// print_data_uint8 (PSTR("BL_Count:"),&Config.OSD_Statistic.BL_Count); |
//} |
//void SendConfigData(void) |
//{ |
// print_data_uint8 (strGet(START_VERSIONCHECK),&Config.Version ); |
// print_data_uint8 (strGet(LOWBAT),&Config.MK_LowBat ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayTimeout ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayLanguage ); |
// print_data_uint8 (strGet(WITXRX),&Config.WiTXRXChannel); |
// print_data_uint8 (strGet(WINETWG),&Config.WiNetworkGroup); |
// print_data_uint8 (strGet(WINETWM),&Config.WiNetworkMode); |
// print_data_uint8 (strGet(WITIMEOUT),&Config.WiTXTO); |
// print_data_uint8 (strGet(WIUART),&Config.WiUartMTU); |
// print_data_uint8 (strGet(DISPLAY8),&Config.OSD_ShowMKSetting); |
// print_data_uint8 (strGet(DISPLAY7),&Config.LCD_DisplayMode); |
// print_data_uint8 (strGet(DISPLAY6),&Config.LCD_Kontrast); |
// print_data_uint8 (strGet(DISPLAY5),&Config.LCD_Helligkeit); |
// print_data_uint8 (PSTR("USB-Betrieb:"),&Config.USBBT); |
// print_data_uint8 (PSTR("Uart-FC/Wi:"),&Config.U02SV2); |
// print_data_uint8 (PSTR("PKT-Debug:"),&Config.Debug); |
// print_data_uint8 (PSTR("Wi232 eingebaut:"),&Config.UseWi); |
// print_data_uint8 (PSTR("BTM222 eingebaut:"),&Config.UseBT); |
// print_data_uint8 (PSTR("Wi232 ok:"),&Config.WiIsSet); |
// print_data_uint8 (PSTR("BTM222 ok:"),&Config.BTIsSet); |
// print_data_uint8 (PSTR("BTM222 Slave:"),&Config.BTIsSlave); |
// |
// print_data_char (PSTR("BTM222 Pin:"),&Config.bt_pin[0],bt_pin_length ); |
// print_data_char (PSTR("BTM222 Name:"),&Config.bt_name[0],bt_name_length); |
// print_data_char (PSTR("BTM222 REID:"),&Config.RE_ID[0],RE_ID_length ); |
// print_data_char (PSTR("BTM222 MAC:"),&Config.bt_Mac[0],bt_mac_length); |
// print_data_char (PSTR("GPS DevName:"),&Config.gps_UsedDevName[0],20); |
// print_data_char (PSTR("GPS MAC:"),&Config.gps_UsedMac[0],14); |
// |
// print_data_int32 (PSTR("LastLongitude:"),&Config.LastLongitude); |
//// print_data_int32(PSTR("LastLatitude:"),&Config.LastLatitude); |
// |
// print_data_uint8 (PSTR("PKT_IdleBeep:"),&Config.PKT_IdleBeep); |
// print_data_uint8 (strGet(DISPLAY2),&Config.PKT_StartInfo); |
// print_data_uint16 (strGet(LIPO3),&Config.Lipo_UOffset); |
// print_data_uint8 (strGet(LIPO2),&Config.PKT_Accutyp); |
// print_data_uint8 (strGet(DISPLAY9),&Config.OSD_RCErrorbeep); |
// print_data_uint8 (strGet(OSD_Invert_Out),&Config.OSD_InvertOut); |
// print_data_uint8 (strGet(OSD_LED_Form),&Config.OSD_LEDform); |
// print_data_uint8 (strGet(OSD_Send_OSD),&Config.OSD_SendOSD); |
// print_data_uint8 (strGet(FALLSPEED),&Config.OSD_Fallspeed); |
// print_data_uint8 (strGet(OSD_VARIOBEEP),&Config.OSD_VarioBeep); |
// print_data_uint8 (strGet(OSD_HOMEMKVIEW),&Config.OSD_HomeMKView); |
// print_data_uint16 (strGet(OSD_MAHWARNING),&Config.OSD_mAh_Warning); |
// print_data_uint8 (strGet(OSD_SCREENMODE),&Config.OSD_ScreenMode); |
// print_data_uint8 (strGet(OSD_LIPOBAR),&Config.OSD_LipoBar); |
// print_data_uint8 (strGet(PKT_BAUDRATE),&Config.PKT_Baudrate); |
// print_data_uint16 (strGet(FOLLOWME_1),&Config.FM_Refresh); |
// print_data_uint16 (strGet(FOLLOWME_2),&Config.FM_Speed); |
// print_data_uint16 (strGet(FOLLOWME_3),&Config.FM_Radius); |
// print_data_uint8 (strGet(HWSOUND),&Config.HWSound); |
// print_data_uint8 (strGet(HWBEEPER),&Config.HWBeeper); |
// print_data_uint8 (PSTR("sIdxSteps:"),&Config.sIdxSteps); |
// print_data_uint8 (strGet(SV_TEST3),&Config.servo_frame); |
// print_data_uint8 (strGet(SV_SINGLESTEP),&Config.single_step); |
// print_data_uint8 (strGet(SV_COUNTTEST),&Config.repeat); |
// print_data_uint8 (strGet(SV_PAUSEEND),&Config.pause); |
// print_data_uint8 (strGet(SV_PAUSEINC),&Config.pause_step); |
//// print_data_int8 (,&Config.tracking); |
//// print_data_int8 (,&Config.track_tx); |
// print_data_uint16 (PSTR("Stick1_min:"),&Config.stick_min[0]); |
// print_data_uint16 (PSTR("Stick2_min:"),&Config.stick_min[1]); |
// print_data_uint16 (PSTR("Stick3_min:"),&Config.stick_min[2]); |
// print_data_uint16 (PSTR("Stick4_min:"),&Config.stick_min[3]); |
// print_data_uint16 (PSTR("Stick5_min:"),&Config.stick_min[4]); |
// print_data_uint16 (PSTR("Stick1_max:"),&Config.stick_max[0]); |
// print_data_uint16 (PSTR("Stick2_max:"),&Config.stick_max[1]); |
// print_data_uint16 (PSTR("Stick3_max:"),&Config.stick_max[2]); |
// print_data_uint16 (PSTR("Stick4_max:"),&Config.stick_max[3]); |
// print_data_uint16 (PSTR("Stick5_max:"),&Config.stick_max[4]); |
// print_data_uint8 (PSTR("Stick1_typ:"),&Config.stick_typ[0]); |
// print_data_uint8 (PSTR("Stick2_typ:"),&Config.stick_typ[1]); |
// print_data_uint8 (PSTR("Stick3_typ:"),&Config.stick_typ[2]); |
// print_data_uint8 (PSTR("Stick4_typ:"),&Config.stick_typ[3]); |
// print_data_uint8 (PSTR("Stick5_typ:"),&Config.stick_typ[4]); |
// print_data_uint8 (PSTR("Stick1_neutral:"),&Config.stick_neutral[0]); |
// print_data_uint8 (PSTR("Stick2_neutral"),&Config.stick_neutral[1]); |
// print_data_uint8 (PSTR("Stick3_neutral:"),&Config.stick_neutral[2]); |
// print_data_uint8 (PSTR("Stick4_neutral:"),&Config.stick_neutral[3]); |
// print_data_uint8 (PSTR("Stick5_neutral:"),&Config.stick_neutral[4]); |
// print_data_uint8 (strGet(LIPO_MESSUNG),&Config.Lipomessung); |
// print_OSD_Statistic(); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWiInitFlag(void) |
{ |
Config.WiIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTInitFlag(void) |
{ |
Config.BTIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWLInitFlag(void) |
{ |
Config.WLIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTSlaveFlag(void) |
{ |
Config.BTIsSlave = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTMasterFlag(void) |
{ |
Config.BTIsSlave = false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteLastPosition( uint32_t ELongitude, uint32_t ELatitude) |
{ |
Config.LastLongitude = ELongitude; |
Config.LastLatitude = ELatitude; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ReadParameter( void ) |
{ |
eeprom_read_block( (void*)&Config, (const void*)&EEStruct, sizeof(ST) ); |
// DEBUG !! |
//Config.Version = 0x82; |
//WriteParameter(); |
//return; |
//------------------- |
// ggf. ungueltige Zeichen aus dem |
// Bluetooth-Namen eliminieren |
//------------------- |
#ifdef USE_BLUETOOTH |
bt_fixname(); |
#endif |
//------------------- |
// 27.03.2014 OG |
// Als Vorbereitung die Niederländische Sprache |
// zu entfernen wird hier ggf. Niederländisch |
// auf Englisch umgeschaltet |
//------------------- |
if( Config.DisplayLanguage == 2 ) // 2 = ehemals Niederländisch |
Config.DisplayLanguage = 1; // -> wird zu 1 = Englisch |
//------------------- |
// 0. EEprom-Version nicht geaendert |
//------------------- |
if( Config.Version == EEpromVersion ) // nichts zu tun... |
{ |
return; // !!! EXIT !!! |
} |
//------------------- |
// 1. PKT-Upgrade NICHT Update faehig |
// ODER Downgrade |
// |
// 0x77 = PKT Version 3.6.6a |
//------------------- |
if( (Config.Version < 0x77) || (Config.Version > EEpromVersion) ) |
{ |
Delete_EEPROM(); |
return; // !!! EXIT !!! |
} |
//------------------- |
// 2. PKT-Upgrade |
//------------------- |
//++++++++++++++++++++++++++++ |
// bei Eeprom-Versionwechsel |
// IMMER loeschen |
//++++++++++++++++++++++++++++ |
#ifdef USE_WAYPOINTS |
PointList_Clear(); // Init Waypoints |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
//++++++++++++++++++++++++++++ |
// inkrementelle Updates |
//++++++++++++++++++++++++++++ |
//----------------------------- |
//--- ab PKT v3.6.9bX6 |
//----------------------------- |
if( Config.Version < 0x81 ) |
{ |
Config.timezone = 1; |
Config.summertime = 1; |
} |
//----------------------------- |
//--- ab PKT v3.7.0b |
//----------------------------- |
if( Config.Version < 0x82 ) |
{ |
Config.OSD_ShowMKSetting = true; |
} |
//----------------------------- |
//--- ab PKT v3.7.0e |
//----------------------------- |
if( Config.Version < 0x83 ) |
{ |
Config.UseWL = false; |
Config.WLIsSet = false; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
} |
//----------------------------- |
//--- ab PKT v3.7.0eX1 |
//----------------------------- |
if( Config.Version < 0x84 ) |
{ |
Config.OSD_UseScreen = 0xffffffff; // alle verfuegbaren OSD-Screens eingeschaltet |
} |
//----------------------------- |
//--- ab PKT v3.7.0f |
//----------------------------- |
if( Config.Version < 0x85 ) |
{ |
Config.Wlan_DHCP = 0; |
Config.Wlan_WPA = 0; |
Config.Wlan_Adhoc = true; |
Config.Wlan_Channel = 1; |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
} |
//----------------------------- |
//--- ab PKT v3.7.3cX5 |
//----------------------------- |
if( Config.Version < 0x87 ) |
{ |
Config.OSD_ShowCellU = false; // |
} |
//----------------------------- |
//--- 01.04.2014 OG |
//--- ab PKT v3.7.4aX7 |
//----------------------------- |
if( Config.Version < 136 ) |
{ |
Config.UseBLE = false; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
memset( Config.MKParam_Favs, param_EOF, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers |
} |
//----------------------------- |
//--- 06.04.2014 OG |
//--- ab PKT v3.7.4aX8 |
//----------------------------- |
if( Config.Version < 137 ) |
{ |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
} |
//----------------------------- |
//--- 07.05.2014 OG |
//--- ab PKT v3.80bX3 |
//----------------------------- |
if( Config.Version < 138 ) |
{ |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers (Vorbelegung jetzt 0) |
} |
//----------------------------- |
//--- 17.06.2014 OG |
//--- ab PKT v3.80cX5 |
//----------------------------- |
if( Config.Version < 139 ) |
{ |
Config.PKTOffTimeout = 0; // autom. PKT ausschalten nach n Minuten (0=immer an) |
} |
//------------------- |
// 3. Update Message |
//------------------- |
Config.Version = EEpromVersion; // Update EEPROM version number |
sei(); |
lcd_cls(); |
lcdx_printp_center( 2, PSTR("EEProm updated to"), MNORMAL, 0,0); |
lcdx_printp_center( 3, PSTR("new Version") , MNORMAL, 0,0); |
lcdx_printp_center( 5, PSTR("check settings!") , MNORMAL, 0,0); |
lcd_printp_at (18, 7, PSTR("OK") , MNORMAL); // Keyline |
while( !get_key_press(1 << KEY_ENTER) ); |
WriteParameter(); |
cli(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteParameter( void ) |
{ |
copy_line(7); |
lcd_printp_at( 0, 7, PSTR(" Write EEPROM "), MNORMAL); |
eeprom_update_block( (const void*)&Config, (void*)&EEStruct, sizeof(ST) ); |
paste_line(7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delete_EEPROM( void ) |
{ |
uint8_t i; |
// EEPROM auf Default setzen |
lcd_cls(); |
lcd_printp_at( 0, 0, PSTR(" EEPROM Parameter "), MINVERS); |
lcd_printp_at( 0, 1, PSTR("werden auf") , MNORMAL); |
lcd_printp_at( 0, 2, PSTR("Standardwerte gesetzt"), MNORMAL); |
Config.MK_LowBat = 137; // 13,7V |
Config.DisplayTimeout = 0; // Display immer an (autom. LCD ausschalten nach n Minuten) |
Config.DisplayLanguage = 5; // default undefined |
Config.WiTXRXChannel = 1; // Kanal 1 MK Standard |
Config.WiNetworkGroup = 66; // Gruppe 66 MK Standard |
Config.WiNetworkMode = NetMode_Normal; // MK Standard |
Config.WiTXTO = TWaitTime16; // MK Standard |
Config.WiUartMTU = UartMTU64; // MK Standard |
Config.OSD_ShowMKSetting= true; // Anzeige MK-Setting beim OSD Start |
Config.PKTOffTimeout = 0; // PKT immer an (autom. PKT ausschalten nach n Minuten) |
Config.LCD_Kontrast = 20; // Kontrast normal |
Config.LCD_Helligkeit = 100; // Helligkeit in % |
Config.USBBT = 0; // USB Betrieb |
Config.U02SV2 = 0; // SV2 (Kabel) Standard |
Config.Debug = 0; // kein Debug |
Config.UseWi = false; // Wi.232 eingebaut? |
Config.UseBT = false; // BT-222 eingebaut? |
Config.WiIsSet = false; // Flag für die Initialisierung Wi232 |
Config.BTIsSet = false; // Flag für die Initialisierung Bluetooth |
Config.BTIsSlave = true; // Slave Flag setzen |
Config.PKT_IdleBeep = 0; // kein Piepsen bei Inaktivität |
Config.PKT_StartInfo = false; // * FREI * (ehemals: PKT Startinfo anzeigen) |
Config.OSD_RCErrorbeep = true; // OSD Receiveerrorbeep |
Config.OSD_InvertOut = false; // LED Anzeige invertiren |
Config.OSD_LEDform = 1; // Form der Anzeige ( + oder schwarz) |
Config.OSD_SendOSD = false; // OSD Daten an SV2 |
Config.OSD_Fallspeed = 40; // maximale Sinkrate |
Config.OSD_VarioBeep = 1; // Vario Beep ein |
Config.OSD_HomeMKView = true; // * FREI * (ehemals: Home Circle from MK View) |
Config.OSD_mAh_Warning = 10000; // mAh Warnschwelle |
Config.OSD_ScreenMode = 0; // Variante des OSD Screen |
Config.OSD_LipoBar = 0; // Bargraphanzeige für MK Lipo |
Config.PKT_Baudrate = Baud_57600; // Baudrate für BT und Wi232 |
Config.PKT_Accutyp = true; // verwendeter Akkutyp (true=Lipo, false=LiON) |
Config.Lipo_UOffset = 10000; // Offset für PKT-Lipomessung |
Config.FM_Azimuth = 0; // FollowMe Azimuth |
Config.FM_Distance = 0; // FollowMe Distance |
Config.FM_Speed = 30; // FollowMe Speed in m/s *0.1 |
Config.FM_Radius = 5; // Waypoint Tolerance Radius in meter |
Config.HWSound = 0; // Hardware Sounderweiterung an PD7 |
Config.HWBeeper = 1; // Hardware Beeper an PC7 |
Config.Volume = 0; // Lautstaerke |
Config.LastLongitude = 0x00000000; |
Config.LastLatitude = 0x00000000; |
strcpy_P(Config.bt_pin , PSTR("0000")); |
strcpy_P(Config.bt_name, PSTR("PKT")); // Bluetooth-Name max. 10 Zeichen! - NICHT mit Leerzeichen auffüllen (bt_name_length) |
strcpy_P(Config.bt_Mac , PSTR("0000-00-000000")); |
strcpy_P(Config.RE_ID , PSTR("0000")); |
for( i = 0; i < 20; i++) |
{ |
Config.gps_UsedDevName[i] = 0; // benutztes GPS Device Name |
} |
for( i = 0; i < 14; i++) |
{ |
Config.gps_UsedMac[i] = '0'; // benutztes GPS Device Mac Adresse |
} |
Config.gps_UseGPS = false; // ist GPS aktiv? |
Config.gps_UsedGPSMouse = GPS_Bluetoothmouse1; |
//Config.WiIsSet = false; // 15.07.2013 CB doppelt drin |
//Config.BTIsSet = false; |
Config.Version = EEpromVersion; |
Config.sIdxSteps = STEPS_255; |
Config.servo[0].rev = SERVO_REV; |
Config.servo[0].min = SERVO_I0_RIGHT; |
Config.servo[0].max = SERVO_I0_LEFT; |
Config.servo[0].mid = SERVO_I0_MIDDLE; |
Config.servo[1].rev = SERVO_REV; |
Config.servo[1].min = SERVO_I0_RIGHT; |
Config.servo[1].max = SERVO_I0_LEFT; |
Config.servo[1].mid = SERVO_I0_MIDDLE; |
Config.servo_frame = SERVO_PERIODE; |
Config.single_step = SINGLE_STEP; // nur bei Test-Servo |
Config.repeat = REPEAT; // nur bei Test-Servo |
Config.pause = PAUSE; // nur bei Test-Servo |
Config.pause_step = PAUSE_STEP; // nur bei Test-Servo |
Config.tracking = TRACKING_MIN; |
Config.track_hyst = TRACKING_HYSTERESE; |
Config.track_tx = 0; |
for( i=0; i<5; i++) |
{ |
Config.stick_min[i] = 30+i; // Joystick |
Config.stick_max[i] = 270+i; // Joystick |
Config.stick_typ[i] = 0; // Joystick |
Config.stick_dir[i] = 0; // Joystick |
Config.stick_neutral[i] = 0; // Joystick |
} |
Config.Lipomessung = true; |
Config.timezone = 1; |
Config.summertime = 1; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
Config.WLIsSet = false; // Wlan |
Config.UseWL = false; |
Config.OSD_UseScreen = 0xffffffff; // alle OSD-Screens eingeschaltet |
Config.Wlan_DHCP = 0; // kein DHCP |
Config.Wlan_WPA = 0; // 0 = adhoc 1= WPA2 |
Config.Wlan_Adhoc = true; // Adhoc |
Config.Wlan_Channel = 1; // Wlan Channel |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
Config.OSD_ShowCellU = false; |
Config.UseBLE = false; |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Favoriten MK-Parameter (Anzahl: MAX_MKPARAM_FAVORITES = 10) |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
//------------------------------------------------- |
//------------------------------------------------- |
#ifdef USE_WAYPOINTS |
PointList_Clear(); |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
WriteParameter(); |
//lcd_printp_at (0, 4, PSTR("Waypoints loeschen"), 0); |
//EEWayPointList_Clear(); |
lcd_printp_at( 0, 6, PSTR("!!Check Parameter!! "), MNORMAL); |
lcd_printp_at(18, 7, PSTR("OK") , MNORMAL); |
sei(); |
set_beep( 200, 0x0080, BeepNormal); |
while( !get_key_press (1 << KEY_ENTER) ); |
//#if defined HWVERSION3_9 |
clr_V_On(); |
//#else |
// |
// wdt_enable( WDTO_250MS ); |
// while (1) |
// {;} |
//#endif |
} |
//-------------------------------------------------------------- |
// |
//void EEWayPointList_Clear(void) // löschen der Waypointliste im EEProm |
//{ |
// uint8_t i; |
// PKTWayPoint.Waypoint.Position.Latitude = 0; |
// PKTWayPoint.Waypoint.Position.Longitude = 0; |
// PKTWayPoint.Waypoint.Position.Altitude = 0; |
// PKTWayPoint.Waypoint.Heading = 361; |
// for(i = 0; i < MAX_WPLIST_LEN; i++) |
// { |
// PKTWayPointDirectory.WPList.WPDirectory[i] = 0; |
// } |
// for(i = 0; i < NumberOfWaypoints; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWayPointList[i].WPIndex, i); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Position.Status, INVALID); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Latitude, (void*)&EEWayPointList[i].Waypoint.Position.Latitude, sizeof(EEWayPointList[i].Waypoint.Position.Latitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Longitude, (void*)&EEWayPointList[i].Waypoint.Position.Longitude, sizeof(EEWayPointList[i].Waypoint.Position.Longitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Altitude, (void*)&EEWayPointList[i].Waypoint.Position.Altitude, sizeof(EEWayPointList[i].Waypoint.Position.Altitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Heading, (void*)&EEWayPointList[i].Waypoint.Heading, sizeof(EEWayPointList[i].Waypoint.Heading)); |
// |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.ToleranceRadius, 0); // in meters, if the MK is within that range around the target, then the next target is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.HoldTime, 0); // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Type, POINT_TYPE_INVALID); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Event_Flag, 0); // future implementation |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.AltitudeRate, 0); // no change of setpoint |
// } |
// for(i = 0; i < NumberOfWPLists; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListnumber, i); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListAktiv, false); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.POI_CAM_NICK_CTR, 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.UsePOI, 0); |
// eeprom_write_block ((const void*)&PKTWayPointDirectory.WPList.WPDirectory, (void*)&EEWPDirectory[i].WPList.WPDirectory, sizeof(EEWPDirectory[i].WPList.WPDirectory)); |
// |
// } |
// lcd_printp (PSTR("\r\n"), 0); |
//} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/eeprom/eeprom.h |
---|
0,0 → 1,333 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.h |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# |
//# 25.06.2014 OG |
//# - chg: Kommentare der eeprom Struct aktualisert bzgl. FollowMe und Tracking |
//# |
//# 17.06.2014 OG |
//# - chg: EEpromversion erhoeht auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: LCD_DisplayMode (Obsolete) geaendert zu PKTOffTimeout |
//# |
//# 31.05.2014 OG |
//# - Recherche welche Config-Werte ueberahupt noch im PKT-Code verwendet werden |
//# und ggf. "OBSOLETE" Anmerkung in der Config-Struct |
//# - Config-Struct wurde um etliche Kommentare ergaenzt bzgl. Verwendungszweck |
//# |
//# 30.05.2014 OG |
//# - chg: Kommentare geaendert |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# |
//# 06.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# |
//# 01.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 136 |
//# - chg: ACHTUNG! EEpromversion umgestellt von HEX auf DEZIMAL: 0x87 => 135 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# - add: MAX_MKPARAM_FAVORITES |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU; EEpromversion erhoeht auf 0x87 |
//# |
//# 28.08.2013 Cebra |
//# - chg: LastLatitude war uint32_t, auf int32_t korrigiert |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 84 |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung, Listen/Statistik ganz ans Ende gesetzt |
//# |
//# 31.05.2013 OG |
//# - chg: Eeprom Version auf 81 wegen Erweiterungen der Statistik-Daten |
//# - chg: Code-Layout |
//# |
//# 12.04.2013 CB |
//# - chg: Kommentarergänzung U02SV2 |
//# |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <stdbool.h> |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../waypoints/waypoints.h" |
#include "../osd/osd.h" |
//[General] |
//FileVersion = 2 |
//NumberOfWaypoints = 15 |
//UsePOI = 0 |
//POI_CAM_NICK_CTRL = 0 |
//[POI] |
//Altitude = 1 |
//Latitude = 46.7140763 |
//Longitude = 19.2507334 |
//[Waypoint1] |
//Latitude = 46.7145686 |
//Longitude = 19.2515702 |
//Radius = 10 |
//Altitude = 15 |
//ClimbRate = 0 |
//DelayTime = 4 |
//WP_Event_Channel_Value = 96 |
//Heading = 180 |
#define EEpromVersion 139 // wird nach jeder Parametererweiterung hochgezählt |
// Anmerkung 01.04.2014 OG: umgestellt auf DEZIMALE schreibweise (wiso war das vorher Hex?) |
#define NumberOfWaypoints 55 // Anzahl der Waypoints in der EEPromliste |
#define NumberOfWPLists 5 // Anzahl WP Listen im PKT |
#define MAX_LIST_LEN 31 // Länge Waypointlist |
#define MAX_MKPARAM_FAVORITES 10 // Anzahl der MK-Parameter Favoriten (nicht aendern!) |
#define bt_pin_length 4 |
#define RE_ID_length 4 // Länge der RE-ID |
#define bt_name_length 10 |
#define bt_mac_length 14 |
#define GPS_Bluetoothmouse1 0 // NMEA BT-Mouse |
#define GPS_Mikrokopter 1 // GPS Daten vom MK für Tracking |
#define wlan_password_length 10 // Länge Wlan-Passwort |
#define wlan_ssid_length 10 // Länge Wlan-SSID |
#define wlan_ip_length 15 |
#define wlan_netmask_length 15 |
#define wlan_gateway_length 15 |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
#define MAX_WPLIST_LEN 31 |
//typedef struct { |
// uint8_t rev; |
// uint16_t min; |
// uint16_t max; |
// uint16_t mid; |
//} servo_t; |
typedef struct |
{ |
uint8_t WPIndex; // Index in der EEpromliste |
Point_t Waypoint; // Waypoint |
} WayPoints; |
typedef struct |
{ |
uint8_t WPListnumber; // Nummer der WP Liste im PKT |
uint8_t WPListAktiv; // Liste aktiv |
uint8_t WPDirectory[31]; // Enthält die Indexe der Waypoints im EEPROM |
uint8_t UsePOI; |
uint8_t POI_CAM_NICK_CTR; |
} WPListHeader; |
typedef struct |
{ |
WPListHeader WPList; // Waypointliste im PKT |
} WPListDirectory; |
typedef struct SStructure |
{ |
uint8_t Version; // PKT-Eeprom Version! |
uint8_t MK_LowBat; // MK-LowBat Warnung u.a. fuer osd.c |
uint8_t DisplayTimeout; // autom. LCD ausschalten nach n Minuten |
uint8_t DisplayLanguage; // eingestellte Sprache |
uint8_t WiTXRXChannel; // Wi.232: |
uint8_t WiNetworkGroup; // Wi.232: |
uint8_t WiNetworkMode; // Wi.232: |
uint8_t WiTXTO; // Wi.232: |
uint8_t WiUartMTU; // Wi.232: |
uint8_t OSD_ShowMKSetting; // Anzeige MK-Setting beim OSD Start |
uint8_t PKTOffTimeout; // autom. PKT ausschalten nach n Minuten |
uint8_t LCD_Kontrast; // LCD-Kontrast |
uint8_t LCD_Helligkeit; // LCD-Helligkeit - Aktuell nicht mehr verfuegbar weil auskommentiert! (mit HW 3.9x geht das sowiso nicht mehr!) |
uint8_t USBBT; // ## OBSOLETE ## 31.05.2014 OG: wird nicht verwendet! (wofuer war da in der Vergangenheit?) |
uint8_t U02SV2; // 0=Wi232-Verbindung zum MK, 1 = Kabelverbindung zum MK |
uint8_t Debug; // ??? OBSOLETE ??? 31.05.2014 OG: wird eigentlich nicht mehr verwendet (war zum debuggen vom PKT) |
uint8_t UseWi; // Wi232 wird genutzt |
uint8_t UseBT; // BT wird genutzt |
uint8_t WiIsSet; // Wi232 ist initialisiert |
uint8_t BTIsSet; // BT ist initialisiert |
uint8_t BTIsSlave; // Slave Flag |
char bt_pin[bt_pin_length + 1]; // BT Pinnummer |
char bt_name[bt_name_length + 1]; // BT Name |
char RE_ID[RE_ID_length + 1]; // RE-ID |
char bt_Mac[bt_mac_length + 1]; // MAC-Adresse BTM222 |
char gps_UsedDevName[20]; // benutztes GPS Device Name |
char gps_UsedMac[14]; // benutztes GPS Device Mac Adresse |
uint8_t gps_UseGPS; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() - "ist GPS aktiv?" |
uint8_t gps_UsedGPSMouse; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() und tracking.c/PKT_tracking() - "GPS Maustyp" |
int32_t LastLongitude; // Letzte Position |
int32_t LastLatitude; // Letzte Position |
uint8_t PKT_IdleBeep; // ## OBSOLETE ## 31.05.2014 OG: wurde das jemals verwendet? |
uint8_t PKT_StartInfo; // ## OBSOLETE ## 26.06.2013 OG: ehemals "PKT Startinfo anzeigen" |
uint16_t Lipo_UOffset; // Offset für die Lipospannugsmessung |
uint8_t PKT_Accutyp; // verwendeter Akkutyp |
uint8_t OSD_RCErrorbeep; // Empfangsausffallwarnung im OSD Screen |
uint8_t OSD_InvertOut; // Out1/2 invertiert anzeigen |
uint8_t OSD_LEDform; // Form der Anzeige ( + oder schwarz) |
uint8_t OSD_SendOSD; // OSD Daten an SV2 senden |
uint8_t OSD_Fallspeed; // maximale Sinkrate |
uint8_t OSD_VarioBeep; // Vario Beep im OSD Screen |
uint8_t OSD_HomeMKView; // ## OBSOLETE ## 02.07.2013 OG: ehemals "Home Circle from MK-View" |
uint16_t OSD_mAh_Warning; // mAh Warnschwelle |
uint8_t OSD_ScreenMode; // Variante des OSD-Screen |
uint8_t OSD_LipoBar; // Bargraphanzeige für MK Lipo |
uint8_t PKT_Baudrate; // Baudrate für BT und Wi232 |
int16_t FM_Azimuth; // Azimuth für FollowMe 4.8.2015 CB |
uint16_t FM_Speed; // FollowMe Speed in m/s *0.1 |
uint16_t FM_Radius; // Waypoint Tolerance Radius in meter |
uint8_t HWSound; // Hardware Sounderweiterung an PD7 |
uint8_t HWBeeper; // Hardware Beeper an PC7 |
uint8_t Volume; // Lautstärke |
servo_t servo[2]; // Tracking: |
uint8_t sIdxSteps; // Tracking: |
uint16_t FM_Distance; // Distance für FollowMe 4.8.2015 CB |
uint8_t servo_frame; // Tracking: |
uint8_t single_step; // Tracking: |
uint8_t repeat; // Tracking: |
uint8_t pause; // Tracking: |
uint8_t pause_step; // Tracking: |
uint8_t tracking; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_hyst; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_tx; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint16_t stick_min[5]; // Joystick: Stickparameter |
uint16_t stick_max[5]; // Joystick: |
uint8_t stick_typ[5]; // Joystick: |
uint8_t stick_dir[5]; // Joystick: |
uint8_t stick_neutral[5]; // Joystick: Stick neutralisierend: ja/nein |
uint8_t Lipomessung; // wenn Lipomessung deaktiviert(Lötbrücke öffnen), kann der Kanal als Stick verwendet werden |
int8_t timezone; // Einstellbereich -12 .. 0 .. 12 Defaultwert: 1 (entspricht unserer Zeitzone) |
uint8_t summertime; // Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) Defaultwert: 1 (entspricht unserer Sommerzeit in D) |
uint8_t UseWL; // WLAN (WiFly) wird an SV2 genutzt (SV2 Patch erforderlich) |
char Wlan_SSID[wlan_ssid_length+1]; // Wlan SSID |
char Wlan_Password[wlan_password_length+1]; // Wlan Passwort |
uint8_t WLIsSet; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Wlan ist initialisiert" |
uint32_t OSD_UseScreen; // welche OSD-Screens nutzen (Bit-codiert) |
uint8_t Wlan_DHCP; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-4" |
uint8_t Wlan_WPA; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-8" |
uint8_t Wlan_Adhoc; // ## OBSOLETE ## 31.05.2014 OG: ehemals "false = AP , true = Adhoc" |
uint8_t Wlan_Channel; // 0-13 |
char Wlan_IP[wlan_ip_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "IP-Adresse" (16 Bytes!) |
char Wlan_Netmask[wlan_netmask_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "SubnetMask" (16 Bytes!) |
char Wlan_Gateway[wlan_gateway_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Defaultgateway" (16 Bytes!) |
uint8_t OSD_ShowCellU; // OSD Azeige: Anzeige UBat als Einzelzellenspannung (ja/nein) |
uint8_t UseBLE; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
char Wlan_HomeSSID[wlan_ssid_length+1]; // Wlan SSID (Home-WLAN) |
char Wlan_HomePassword[wlan_password_length+1]; // Wlan Passwort (Home-WLAN) |
//!!!neue Parameter ab hier anfügen!!!!!! |
// ab hier werden die Parameter bei EEPROMversionsänderungen gelöscht |
Point_t PointList[MAX_LIST_LEN]; // ab EEPROM Version 78 |
osd_statistic_t OSD_Statistic; // ab EEPROM Version 79 & 81 OSD Statistiken |
pkt_gpspos_t GPS_User[MAX_GPS_USER]; // ab EEPROM Version 79 speichert Benutzer GPS-Positionen |
mkerror_t MKErr_Log[MAX_MKERR_LOG]; // ab EEPROM Version 79 speichert auftretende ErrorCode's vom MK |
} ST; |
extern ST Config; |
void ReadParameter (void); |
void WriteParameter (void); |
void ReadLastPosition(void); |
void WriteLastPosition(uint32_t ELongitude,uint32_t ELatitude); |
void WriteWiInitFlag(void); |
void WriteBTInitFlag(void); |
void WriteWLInitFlag(void); |
void WriteBTSlaveFlag(void); |
void WriteBTMasterFlag(void); |
void Delete_EEPROM(void); |
void SendConfigData(void); |
//void EEWayPointList_Clear(void); // l�schen der Waypointliste im EEProm |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.c |
---|
0,0 → 1,756 |
/* |
* FollowMe.c |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY followme.c |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - FollowMeStep2 erweitert mit Kreisberechnung und test auf PKT |
//# - PKT-Pos von lat lon auf latitude und longitude umbenannt |
//# |
//# 20.09.2015 Starter |
//# FollowMeStep2 erweitert auf aktuelle GPS-Daten und followme_calculate_offset(...) |
//# |
//# 03.08.2015 Cebra |
//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe |
//# |
//# 20.07.2015 CB |
//# - chg: FollowMe Datensatz an NC.211 angepasst |
//# |
//# 27.06.2014 OG |
//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX |
//# |
//# 26.06.2014 OG |
//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h) |
//# |
//# 24.06.2014 OG |
//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c) |
//# - del: Variable CheckGPS |
//# |
//# 21.06.2014 OG |
//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der |
//# Entfernung zwischen Kopter und Target |
//# - chg: MK-Timeoutout Erkennung verbessert/angepasst |
//# |
//# 19.06.2014 OG |
//# - erster Prototyp der Follow Me zum Kopter sendet |
//# - etliche Aenderungen im Codeaufbau |
//# |
//# 01.06.2014 OG |
//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# Hauptmenue erledigt wird |
//# |
//# 13.05.2014 OG |
//# - chg: FollowMe() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb |
//# auskommentiert weil man das a) vorallem nicht benoetigt |
//# und b) die load_setting() so nicht mehr existiert |
//# (der Codebereich kann meines erachtens geloescht werden) |
//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es |
//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird |
//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt) |
//# und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio) |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 05.05.2013 Cebra |
//# - chg: #ifdef USE_FOLLOWME |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdio.h> |
#include "../main.h" |
#ifdef USE_FOLLOWME |
#include "../followme/followme.h" |
#include "../osd/osd.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../gps/gps.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
//####################################################################################################################### |
//-------------------- |
// Timings |
//-------------------- |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
#define COSD_WASFLYING 4 |
// global definitions and global vars |
NaviData_t *naviData; |
unsigned char Element; |
uint16_t heading_home; |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
positionOffset followMeOffset; |
// Flags |
//uint8_t COSD_FLAGS2 = 0; |
// |
//GPS_Pos_t last5pos[7]; |
uint8_t FM_error = 0; |
//--------------------- |
// Waypoint Types |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
//--------------------- |
// Status |
// GPS_Pos_t |
// aus MK source |
// |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define INVALID 0x00 |
#define NEWDATA 0x01 |
#define PROCESSED 0x02 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void FollowMe( void ) |
{ |
//uint8_t status; |
GPS_Pos_t currpos; |
uint8_t tmp_dat; |
uint8_t redraw; |
uint8_t drawmode; |
uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
uint32_t send_followme_counter; |
int8_t ok; |
int8_t xoffs; |
int8_t yoffs; |
Point_t FollowMe; |
uint8_t mktimeout = false; |
GPS_PosDev_t targetdev; |
//--------------------- |
// 1. Daten GPS-Maus |
//--------------------- |
ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
if( ok <= 0 ) |
{ |
return; // Fehler bzgl. BT GPS-Maus -> exit |
} |
//--------------------- |
// 2. Follow Me |
//--------------------- |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
lcd_cls (); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
timer_mk_timeout = MK_TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
MKLiPoCells_Init(); |
redraw = true; |
NMEA.Counter = 0; |
NMEA_GPGGA_counter_old = 0; |
send_followme_counter = 0; |
while( (receiveNMEA) ) |
{ |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
if( redraw ) |
{ |
#ifdef USE_FOLLOWME_STEP2 |
printFollowMeStatic(); |
#else |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance) |
lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:" |
draw_icon_mk( 1, 18); |
draw_icon_target_round( 1, 50); |
redraw = false; |
#endif |
} |
//----------------------------------------- |
// neue MK Daten vorhanden |
//----------------------------------------- |
if( rxd_buffer_locked ) // neue MK Daten |
{ |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
if( mktimeout ) redraw = true; |
mktimeout = false; |
FM_error = 0; // noch benoetigt? |
currpos.Latitude = naviData->CurrentPosition.Latitude; |
currpos.Longitude = naviData->CurrentPosition.Longitude; |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln |
//################# |
//# MK |
//################# |
//----------------- |
// Oben: MK-Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: MK-Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 3; |
yoffs = -1; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// MK: Sat Anzahl |
//----------------- |
yoffs = -27; |
if( naviData->SatsInUse > 5 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs); |
draw_icon_satmini( 116+3, 42+yoffs); |
//----------------- |
// MK: GPS |
//----------------- |
writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude |
writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude |
// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude); |
// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude); |
//################# |
//# DISTANCE TO TARGET |
//################# |
//----------------- |
// Target: Distance to Target |
//----------------- |
xoffs = 7; |
yoffs = 4; |
//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition); |
lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) ); |
//################# |
//# TARGET (GPS Maus) |
//################# |
yoffs = 8; |
//----------------- |
// Send Counter |
//----------------- |
//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
//----------------- |
// Target: Fix |
//----------------- |
if( NMEA.SatFix == 1 || NMEA.SatFix == 2 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix |
//----------------- |
// Target: Sat Anzahl |
//----------------- |
if( NMEA.SatsInUse > 5 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs); |
draw_icon_satmini( 116+4, 40+yoffs); |
//----------------- |
// Target: Lat / Long |
//----------------- |
writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
rxd_buffer_locked = FALSE; |
timer_mk_timeout = MK_TIMEOUT; |
} |
//----------------------------------------- |
// if( !NMEA_receiveBT() ) |
// { |
// receiveNMEA = false; |
// } |
// else |
// { |
// NMEA_GetNewData(); |
// } |
// Sourcecode aus NaviCtrl/V2.10e/uart1.c |
// FOLLOW_ME |
// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
// { |
// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
// { |
// TransmitAlsoToFC = 1; |
// // update FollowMe content |
// FollowMe.Position.Longitude = GPSData.Position.Longitude; |
// FollowMe.Position.Latitude = GPSData.Position.Latitude; |
// FollowMe.Position.Status = NEWDATA; |
// FollowMe.Position.Altitude = 1; |
// // 0 -> no Orientation |
// // 1-360 -> CompassCourse Setpoint |
// // -1 -> points to WP1 -> itself |
// FollowMe.Heading = -1; |
// FollowMe.ToleranceRadius = 1; |
// FollowMe.HoldTime = 60; |
// FollowMe.Event_Flag = 1; |
// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list |
// FollowMe.Type = POINT_TYPE_WP; |
// FollowMe.WP_EventChannelValue = 100; // set servo value |
// FollowMe.AltitudeRate = 0; // do not change height |
// FollowMe.Speed = 0; // rate to change the Position (0 = max) |
// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
// FollowMe.Name[0] = 'F'; // Name of that point (ASCII) |
// FollowMe.Name[1] = 'O'; // Name of that point (ASCII) |
// FollowMe.Name[2] = 'L'; // Name of that point (ASCII) |
// FollowMe.Name[3] = 'L'; // Name of that point (ASCII) |
// FollowMe.reserve[0] = 0; // reserve |
// FollowMe.reserve[1] = 0; // reserve |
// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe)); |
// } |
// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
// } |
// |
#ifdef USE_FOLLOWME_STEP2 |
sendFollowMeData(&FollowMe, &send_followme_counter, &NMEA_GPGGA_counter_old); |
#else |
//----------------------------------------- |
// neue NMEA Daten? |
//----------------------------------------- |
if(NMEA_isdataready() && receiveNMEA) |
{ |
if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
{ |
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
{ |
//Config.FM_Refresh |
FollowMe.Position.Status = NEWDATA; |
FollowMe.Position.Longitude = NMEA.Longitude; |
FollowMe.Position.Latitude = NMEA.Latitude; |
FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
// FollowMe.Event_Flag = 0; // no event |
FollowMe.Event_Flag = 1; // 20.7.2015 CB |
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
FollowMe.Name[1] = 'O'; |
FollowMe.Name[2] = 'L'; |
FollowMe.Name[3] = 'L'; |
// FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
send_followme_counter++; |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
} |
NMEA_GPGGA_counter_old = NMEA.Counter; |
} |
} |
#endif |
//----------------------------------------- |
// TASTEN |
//----------------------------------------- |
if( get_key_press(1 << KEY_ESC) ) |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
break; |
} |
//----------------------------------------- |
//----------------------------------------- |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
//OSDScreenRefresh = OSD_SCREEN_REDRAW; |
// OSD_MK_Connect( MK_CONNECT ); |
} |
} // end: while( (receiveNMEA) ); |
OSD_active = false; |
GPSMouse_Disconnect(); |
} |
// Funktion sendFollowMeData angepasst auf Zeiger |
// ausgelagert als Funktion, da auch von FollowMeStep2 verwendet wird |
// TODO: testen mit Simulator und Kopter :-) |
void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old) |
{ |
Point_t FollowMe = *tFollowMe; |
if(NMEA_isdataready() && receiveNMEA) |
{ |
if( NMEA.Counter > *tNMEA_GPGGA_counter_old ) |
{ |
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
{ |
FollowMe.Position.Status = NEWDATA; |
FollowMe.Position.Longitude = NMEA.Longitude; |
FollowMe.Position.Latitude = NMEA.Latitude; |
FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
FollowMe.Event_Flag = 1; // 20.7.2015 CB |
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
FollowMe.Name[1] = 'O'; |
FollowMe.Name[2] = 'L'; |
FollowMe.Name[3] = 'L'; |
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
*tsend_followme_counter += 1; |
} |
*tNMEA_GPGGA_counter_old = NMEA.Counter; |
} |
} |
} |
void printFollowMeStatic(void) |
{ |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte |
lcd_line( 6*6-3, 0, 6*6-3, 11, 1); // Linie Vertikal links |
lcd_line( 15*6+5, 0, 15*6+5, 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance) |
lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:" |
draw_icon_mk( 1, 18); |
draw_icon_target_round( 1, 50); |
} |
// FollowMeStep2: |
#define ONLINE |
#define DEBUG |
#ifdef USE_FOLLOWME_STEP2 |
void Debug_GPS (void) |
{ |
uint8_t redraw = true; |
nmeaPOS NMEApos; |
nmeaPOS NMEATarget; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
#ifdef ONLINE |
int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen -> Quick an Dirty ;-) |
if( retcode <= 0 ) |
{ |
return; |
} |
#endif |
#ifdef DEBUG |
// NMEApos.lat = 520000000; |
// NMEApos.lon = 0; |
// Config.FM_Azimuth = 90; |
// Config.FM_Distance = 10000; |
#endif |
while( true ) |
{ |
NMEApos.latitude = NMEA.Latitude; |
NMEApos.longitude = NMEA.Longitude; |
if( redraw ) |
{ |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMeStep2") ); |
lcdx_printf_center_P( 1, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
lcdx_printf_center_P( 3, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
redraw = false; |
} |
writex_gpspos( 1, 2, NMEApos.latitude, MNORMAL, 0,0 ); // GPS-Maus: Latitude |
writex_gpspos(10, 2, NMEApos.longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset); |
#ifdef DEBUG |
writex_gpspos( 1, 6, (int32_t)Config.FM_Azimuth*100, MNORMAL, 0, 0 ); |
writex_gpspos( 10, 6, (int32_t)Config.FM_Distance*100, MNORMAL, 0, 0 ); |
writex_gpspos( 1, 7, (int32_t)followMeOffset.latitude*100, MNORMAL, 0, 0 ); |
writex_gpspos( 10, 7, (int32_t)followMeOffset.longitude*100, MNORMAL, 0, 0 ); |
#endif |
followme_add_offset(&NMEApos, &NMEATarget, &followMeOffset); |
writex_gpspos( 1, 4, (int32_t)NMEATarget.latitude, MNORMAL, 0, 0 ); // Ziel Latitude |
writex_gpspos(10, 4, (int32_t)NMEATarget.longitude, MNORMAL, 0, 0 ); // Ziel Longitude |
// Tasten |
if( get_key_press(1 << KEY_ESC) ) |
{ |
#ifdef ONLINE |
GPSMouse_Disconnect(); |
#endif |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
Config.FM_Azimuth -= 10; |
redraw = true; |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
Config.FM_Azimuth += 10; |
redraw = true; |
} |
} |
} |
#endif // FOLLOW_ME_STEP2 |
#endif // #ifdef USE_FOLLOWME |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.h |
---|
0,0 → 1,20 |
/* |
* FollowMe.h |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
#ifndef FOLLOWME_H_ |
#define FOLLOWME_H_ |
#include "../mk-data-structs.h" |
#include <inttypes.h> |
void FollowMe (void); |
void Debug_GPS (void); |
void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old); |
void printFollowMeStatic(void); |
#endif /* FOLLOWME_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.c |
---|
0,0 → 1,195 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.c |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT |
//# - add my_abs(...) |
//# |
//# 20.09.2015 Starter |
//# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
//# followme_calculate_offset(...) |
//# |
//# 03.08.2015 cebra |
//# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
//# int nmea_move_horz( |
//# const nmeaPOS *start_pos, /**< Start position in radians */ |
//# nmeaPOS *end_pos, /**< Result position in radians */ |
//# double azimuth, /**< Azimuth (degree) [0, 359] */ |
//# double distance) /**< Distance (km) */ |
//# |
//# 27.06.2014 OG - NEU |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
#include "../mk-data-structs.h" |
#include "../gps/mymath.h" |
#include "gps.h" |
#define NMEA_PI (3.141592653589793) /**< PI value */ |
#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ |
#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ |
#define R (6371) |
#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ |
#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ |
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ |
#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
// Definitonen für FollowMeStep2 |
#define LONG_DIV 10000000 |
#define LAT_DIV LONG_DIV |
#define FOLLOWME_M2DEG 111111 |
#define FOLLOWME_ROUND_100 100 |
# define NMEA_POSIX(x) x |
/** |
* \fn nmea_degree2radian |
* \brief Convert degree to radian |
*/ |
double nmea_degree2radian(double val) |
{ return (val * NMEA_PI180); } |
//------------------------------------------------------------------------------------------ |
nmeaPOS NMEApos; |
nmeaPOS NMEATarget; |
/** |
* \brief Horizontal move of point position |
*/ |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
) |
{ |
nmeaPOS p1 = *start_pos; |
int RetVal = 1; |
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
azimuth = nmea_degree2radian(azimuth); |
end_pos->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth)); |
end_pos->longitude = p1.longitude + atan2( sin(azimuth) * sin(distance) * cos(p1.latitude), cos(distance) - sin(p1.latitude) * sin(end_pos->latitude)); |
if(NMEA_POSIX(isnan)(end_pos->latitude) || NMEA_POSIX(isnan)(end_pos->longitude)) |
{ |
end_pos->latitude = 0; end_pos->longitude = 0; |
RetVal = 0; |
} |
return RetVal; |
} |
// Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück |
// |
// Benutzt die c_cos_8192 der FC |
// TODO: move to followme.c |
// TODO: *8192 optimieren |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *pFollowMeOffset /**< Position Offset in Millimeters */ |
) |
{ |
nmeaPOS pktPos = *pPktPos; |
positionOffset followMeOffset = * pFollowMeOffset; |
target_pos->latitude = pktPos.latitude + ( ( followMeOffset.latitude * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000; |
target_pos->longitude = pktPos.longitude + ( ( followMeOffset.longitude * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.latitude / LONG_DIV ) ) ) ); |
return 1; |
} |
// schlanke abs-Methode |
// TODO: move to mymath.h |
int16_t my_abs(int16_t input) |
{ |
if(input < 0) |
return -input; |
else |
return input; |
} |
// Rechnet einen Offset aus Radius und Winkel nach Lat/Long |
// Benutzt die c_cos_8192 und c_sin_8192 der FC |
// TODO: move to followme.c |
uint8_t followme_calculate_offset( |
int32_t radius, // in mm |
int16_t angle, // in Grad ° |
positionOffset *followMeOffset |
) |
{ |
angle %= 360; // map angle to 0° - 360° |
followMeOffset->latitude = ( radius * c_cos_8192( angle ) ) / 8192; |
followMeOffset->longitude = ( radius * c_sin_8192( angle ) ) / 8192; |
return 1; |
} |
//############################################################################################### |
//-------------------------------------------------------------- |
GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
GPS_PosDev_t PosDev; |
lon1 = pos1.Longitude; |
lat1 = pos1.Latitude; |
lon2 = pos2.Longitude; |
lat2 = pos2.Latitude; |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = (1113 * (lat1 - lat2) / 10000); |
PosDev.Bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ); // |
//PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10; // *10 um von dm auf cm zu kommen |
return PosDev; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.h |
---|
0,0 → 1,81 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.h |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - PKT-Pos von lat lon auf latitude und longitude umbenannt |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPS_H_ |
#define GPS_H_ |
/** |
* Position data in fractional degrees or radians |
*/ |
typedef struct _nmeaPOS |
{ |
int32_t latitude; // Latitude in 1e-7 Grad |
int32_t longitude; // Longitude in 1e-7 Grad |
} nmeaPOS; |
typedef struct _positionOffset |
{ |
int32_t latitude; // latitude offset in mm |
int32_t longitude; // longitude offset in mm |
}positionOffset; |
GPS_PosDev_t gps_Deviation( |
GPS_Pos_t pos1, |
GPS_Pos_t pos2 |
); |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
int nmea_move_horz1( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *followMeOffset /**< Position Offset in Millimeters */ |
); |
uint8_t followme_calculate_offset( |
int32_t radius, |
int16_t angle, |
positionOffset *followMeOffset |
); |
int16_t my_abs(int16_t input); |
#endif // #define GPS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gpsmouse.c |
---|
0,0 → 1,532 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.c |
//# |
//# 30.07.2015 CB |
//# - add: Anzeige von CRC Fehlern im NMEA Datensatz in GPSMouse_ShowData |
//# |
//# 28.07.2015 CB |
//# - fix: Überprüfung von führenden 00 in der MAC-Adresse der BT Maus abgeschaltet |
//# |
//# 28.06.2014 OG |
//# - fix: GPSMouse_ShowData() - wenn BT-Datenverlust dann immer GPSMouse_Disconnect() |
//# auch wenn waitsatfix Modus aktiv ist |
//# - fix: GPSMouse_ShowData() - Rueckgabe von 0 wenn Verbindung bereits vorhanden war |
//# und dabei aber ein Verbindungsverlust zur BT GPS-Maus auftrat |
//# |
//# 27.06.2014 OG |
//# - chg: GPSMouse_Disconnect() - Anzeige von BT-Datenverlust optimiert |
//# - chg: GPSMouse_ShowData() - nach GPSMouse_Connect ein clear_key_all() eingefuegt |
//# |
//# 24.06.2014 OG |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 23.06.2014 OG |
//# - chg: GPSMouse_ShowData() - neue Parameter und Return-Codes fuer |
//# hoehere Flexibilitaet |
//# |
//# 22.06.2014 OG - NEU |
//# ehemals BT_ShowGPSData() in setup.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../setup/setup.h" |
#include "../eeprom/eeprom.h" |
#include "../lipo/lipo.h" |
#include "../timer/timer.h" |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../uart/uart1.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../messages.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
static const char STR_FIX0[] PROGMEM = " no"; |
static const char STR_FIX1[] PROGMEM = " GPS"; |
static const char STR_FIX2[] PROGMEM = "DGPS"; |
static const char STR_FIX6[] PROGMEM = "esti"; |
static const char STR_FIX[] PROGMEM = " ---"; |
//-------------------------------------------------------------- |
// retcode = GPSMouse_Connect() |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: verbunden |
//-------------------------------------------------------------- |
int8_t GPSMouse_Connect( void ) |
{ |
uint8_t status; |
if( !BT_CheckBTM222() ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return -2; // -2: kein BTM222 eingebaut |
} |
// deaktiviert, MAC Adressen von Handys haben keine 00 in der MAC-Adresse |
// if( (Config.gps_UsedMac[5] == '0') || (Config.gps_UsedMac[6] == '0') ) |
// { |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// PKT_Message_P( strGet(STR_NOGPSMOUSE), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "keine GPS-Maus!" |
// return -1; // -1: keine GPS Maus konfiguriert |
// } |
//-------------------- |
// verbinde BT GPS-Maus |
//-------------------- |
PKT_Title( Config.gps_UsedDevName, true, true); // PKT_Title( text, lShowLipo, clearscreen ) |
lcdx_printp_center( 2, strGet(STR_GPSMOUSECONNECT), MNORMAL, 0,1); // "suche GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
if( Config.BTIsSlave ) |
{ |
bt_downlink_init(); |
} |
status = bt_connect( Config.gps_UsedMac ); |
uart1_flush(); |
fifo_clear(&in_fifo); |
UART1_receiveNMEA = true; // Uartflag setzen zur Erkennung der NMEA Datensätze |
PKT_Gauge_End(); // Gauge: Ende |
if( !status ) // keine Verbindung zum BT-Device |
{ |
set_BTOff(); |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_GPSMOUSECONNECTION), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "GPS-Maus Verbindung" |
return 0; // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
} |
return 1; // 1 = ok, Verbindung steht |
} |
//-------------------------------------------------------------- |
// GPSMouse_Disconnect() |
//-------------------------------------------------------------- |
void GPSMouse_Disconnect( void ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
//--------------------- |
// GPS beenden |
//--------------------- |
UART1_receiveNMEA = false; |
if( !receiveNMEA ) // ggf. anders implementieren, z.B. via Parameter; aber erstmal geht's auch so |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 7, MINVERSX, 0 ); |
lcdx_printp_center( 7, strGet(STR_BT_LOSTDATA), MINVERSX, 0,0); // "BT Datenverlust" |
set_beep( 600, 0xffff, BeepNormal); |
} |
lcdx_printp_center( 2, strGet(STR_GPSMOUSEDISCONNECT), MNORMAL, 0,1); // "trenne GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
receiveNMEA = false; // Uartflag zurücksetzen, es kommen keine NMEA Datensätze mehr |
if (!bt_disconnect()); |
{ |
// lcdx_printp_center( 2, PSTR("Error on disconnect"), MNORMAL, 0,1); // Fehler beim Disconncet aufgetreten |
// _delay_ms(2000); // 09.08.2015 cebra, muss noch untersucht werden, ist aber jetzt kein Problem |
} |
set_BTOff(); |
PKT_Gauge_End(); // Gauge: Ende |
} |
//-------------------------------------------------------------- |
// retcode = GPSMouse_ShowData( mode, waitsatfix ) |
// |
// zeigt GPS-Daten der aktuellen BT-Maus an |
// |
// PARAMETER: |
// show: |
// was soll in der 2. Zeile angezeigt werden |
// GPSMOUSE_SHOW_TIME || GPSMOUSE_SHOW_WAITSATFIX |
// |
// waitsatifix: |
// 0 = Verbindungsaufbau; Daten anzeigen und beenden der |
// BT-Verbindung (fuer setup.c) |
// n = Zeit (via timer) die nach einem erfolgreichen Satfix gewartet wird bis |
// GPSMouse_ShowData() beendet wird - die Verbindung zur GPS-Maus bleibt erhalten |
// und kann weiterverwendet werden! (z.B. in followme.c) |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
// 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// |
// zusammengefasst: |
// >0 : Ok |
// <=0: Fehler |
//-------------------------------------------------------------- |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ) |
{ |
uint8_t redraw = true; |
uint8_t state_posfix = 0; |
const char *pStr; |
int8_t yoffs; |
int8_t retcode; |
//-------------------- |
// BT GPS-Maus verbinden |
//-------------------- |
retcode = GPSMouse_Connect(); |
if( retcode <= 0 ) |
{ |
return retcode; |
} |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
clear_key_all(); |
//--------------------- |
// zeige GPS-Daten |
//--------------------- |
receiveNMEA = true; |
do |
{ |
//------------------------- |
// Screen redraw |
//------------------------- |
if( redraw ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
if( show == GPSMOUSE_SHOW_TIME ) // Zeile 2: GPS-Zeit |
{ |
lcdx_printp_at( 6, 1, PSTR("["), MNORMAL, -1,2); // Mitte: GPS Time |
lcdx_printp_at(15, 1, PSTR("]"), MNORMAL, -2,2); // .. |
lcd_line( 0, 13, 34, 13, 1); // .. |
lcd_line(92, 13, 127, 13, 1); // .. |
} |
if( show == GPSMOUSE_SHOW_WAITSATFIX ) // Zeile 2: "warte auf Satfix..." |
{ |
yoffs = 1; |
lcdx_cls_row( 1, MINVERS, yoffs); |
lcdx_printp_center( 1, strGet(STR_WAITSATFIX), MINVERS, 0,yoffs); // "warte auf Satfix..." |
} |
// yoffs = 1; |
// lcdx_cls_row( 1, MINVERS, yoffs); |
// lcdx_printp_center( 1,strGet(STR_WAITNMEA), MINVERS, 0,yoffs); // "warte auf NMEA..." |
lcdx_printp_at( 0, 2, PSTR("Fix:") , MNORMAL, 0,3); // Links; Fix |
lcdx_printp_at( 0, 3, PSTR("Sat:") , MNORMAL, 0,3); // Links; Sat |
lcdx_printp_at( 0, 4, PSTR("Alt:") , MNORMAL, 0,3); // Links; Alt |
lcdx_printp_at(11, 2, PSTR("RCnt:"), MNORMAL, 0,3); // Rechts: RCnt - Zaehler empfangener GPGGA Datenpakete |
lcdx_printp_at(11, 3, PSTR("CErr:"), MNORMAL, 0,3); // Rechts: RErr - Zaehler fehlerhafte NMEA Pakete |
lcdx_printp_at(11, 4, PSTR("HDOP:"), MNORMAL, 0,3); // Rechts: HDOP |
lcdx_cls_row( 5, MINVERS, 4); // Hintergrund invers |
lcdx_printp_at( 1, 5, strGet(START_LASTPOS1), MINVERS, 0,4); // "Breitengr. Längengr." |
lcd_rect( 0, (4*8)+11, 127, (2*8)+4, 1); // Rahmen |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT-Update geht hier nicht... |
// wegen der Hardware-Verbindung |
//------------------------- |
//if( PKT_CtrlHook() ) |
//{ |
// redraw = true; |
//} |
//------------------------- |
//------------------------- |
if(NMEA_isdataready()) // NMEA Daten vorhanden? |
{ |
if( show == GPSMOUSE_SHOW_TIME ) |
{ |
lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
} |
//-------------- |
// Sat-Fix |
//-------------- |
//writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
switch( NMEA.SatFix ) |
{ |
case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
default: pStr = 0; // Sat-Fix Code unbekannt! |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA.SatFix); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
break; |
} |
if( pStr ) |
{ |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
} |
if( NMEA.SatFix==1 || NMEA.SatFix==2 ) // Sat-Fix OK => einen Haken anzeigen |
lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Sat-Anzahl |
//-------------- |
writex_ndigit_number_u( 6, 3, NMEA.SatsInUse, 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
if( NMEA.SatsInUse >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Altitude |
//-------------- |
lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA.Altitude); // Links: Altitude |
//-------------- |
// empfangene GPGGA Datenpakete |
//-------------- |
lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA.Counter ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
//-------------- |
// NMEA CRC Fehler |
//-------------- |
lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl CRC Fehler im NMEA GPGGA-Pakete |
// lcdx_printf_at( 15, 3, mode, 0, 3, "%3.5ld", (uint32_t)NMEA_getBearing()); |
// writex_ndigit_number_u_10th(15, 3, NMEA_getBearing(), 4,0, MNORMAL, 0,3); |
//-------------- |
// HDOP |
//-------------- |
if( NMEA.HDOP == 0 ) // Rechts:: HDOP |
lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
else |
writex_ndigit_number_u_10th(16, 4, NMEA.HDOP, 4,0, MNORMAL, 0,3); |
//-------------- |
// Lat / Lon |
//-------------- |
writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); // unten rechts: Longitude |
//-------------- |
//-------------- |
if( waitsatfix > 0 ) |
{ |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==0) ) |
{ |
state_posfix = 1; |
timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
} |
if( (NMEA.SatFix!=1 && NMEA.SatFix!=2) && (state_posfix==1) && (timer!=0) ) |
{ |
state_posfix = 0; |
} |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==1) && (timer==0) ) |
{ |
clear_key_all(); |
return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
} |
} |
} |
if (timer_nmea_timeout == 0) receiveNMEA = false; |
} while( !get_key_press(1 << KEY_ESC) && receiveNMEA ); |
//--------------------- |
// GPS beenden |
//--------------------- |
if( (waitsatfix == 0) || !receiveNMEA ) |
{ |
GPSMouse_Disconnect(); |
} |
clear_key_all(); |
return (receiveNMEA ? 1 : 0); // 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
} |
// save, Anzeige der Daten im Format der neuen NMEA Routinen, nach Umbau auf NMEA Structur nicht mehr notwendig |
// { |
// |
// |
// if( show == GPSMOUSE_SHOW_TIME ) |
// { |
//// lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
// |
// writex_ndigit_number_u( 7, 1, NMEA_getHour(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 9, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 10, 1, NMEA_getMinute(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 12, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 13, 1, NMEA_getSecond(), 2,0, MNORMAL, -2,2); |
// |
// } |
// |
// //-------------- |
// // Sat-Fix |
// //-------------- |
// //writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
// switch( NMEA_getGPSfix() ) |
// { |
// case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
// case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
// case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
// case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
// default: pStr = 0; // Sat-Fix Code unbekannt! |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA_getGPSfix()); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
// break; |
// } |
// |
// if( pStr ) |
// { |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
// } |
// |
// if( NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2 ) // Sat-Fix OK => einen Haken anzeigen |
// lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Sat-Anzahl |
// //-------------- |
// writex_ndigit_number_u( 6, 3, NMEA_getSatellites(), 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
// if( NMEA_getSatellites() >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
// lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Altitude |
// //-------------- |
// lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA_getAltitude()); // Links: Altitude |
// |
// |
// //-------------- |
// // empfangene GPGGA Datenpakete |
// //-------------- |
// lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getNMEAcounter() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
// |
// //-------------- |
// // NMEA CRC Fehler |
// //-------------- |
// lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
//// writex_ndigit_number_u(1, 5, UART1_GPGGA , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(5, 5, bt_overrun , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(10, 5, bt_bufferoverflow , 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // HDOP |
// //-------------- |
// if(NMEA_getHDOP() == 0 ) // Rechts:: HDOP |
// lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
// else |
// writex_ndigit_number_u_10th(16, 4, NMEA_getHDOP(), 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // Lat / Lon |
// //-------------- |
// writex_gpspos( 1, 6, NMEA_getLatitude() , MNORMAL, 0,6 ); // unten links: Latitude |
// writex_gpspos(12, 6, NMEA_getLongitude(), MNORMAL, 0,6 ); // unten rechts: Longitude |
// |
// |
// //-------------- |
// //-------------- |
// if( waitsatfix > 0 ) |
// { |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==0) ) |
// { |
// state_posfix = 1; |
// timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
// } |
// |
// if( (NMEA_getGPSfix()!=1 && NMEA_getGPSfix()!=2) && (state_posfix==1) && (timer!=0) ) |
// { |
// state_posfix = 0; |
// } |
// |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==1) && (timer==0) ) |
// { |
// clear_key_all(); |
// return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// } |
// } |
// |
// } |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gpsmouse.h |
---|
0,0 → 1,46 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.h |
//# |
//# 24.06.2014 OG |
//# - chg: Parameteraenderung bei GPSMouse_ShowData() |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 22.06.2014 OG |
//# - add: GPSMouse_ShowData() |
//# - add: #define GPSMOUSE_SHOWDATA, GPSMOUSE_WAITSATFIX |
//# |
//# 22.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPSMOUSE_H_ |
#define GPSMOUSE_H_ |
//---------------------------------- |
// GPSMouse_ShowData() - show |
//---------------------------------- |
#define GPSMOUSE_SHOW_TIME 1 |
#define GPSMOUSE_SHOW_WAITSATFIX 2 |
//---------------------------------- |
// minimal akzeptierte Sat-Zahl fuer |
// Steuerungsfunktionen des MK |
//---------------------------------- |
#define GPSMOUSE_MINSAT 6 |
//---------------------------------- |
// Export |
//---------------------------------- |
int8_t GPSMouse_Connect( void ); |
void GPSMouse_Disconnect( void ); |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ); |
#endif // #define GPSMOUSE_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.c |
---|
0,0 → 1,220 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY mymath.c |
//# |
//# 24.04.2013 OG |
//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM' |
//# wegen Compile-Error in AtmelStudio 6 |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <math.h> |
#include <avr/pgmspace.h> |
#include "mymath.h" |
// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29 |
unsigned int sqrt32(uint32_t q) |
{ uint16_t r, mask; |
uint16_t i = 8*sizeof (r) -1; |
r = mask = 1 << i; |
for (; i > 0; i--) { |
mask >>= 1; |
if (q < (uint32_t) r*r) |
r -= mask; |
else |
r += mask; |
} |
if (q < (uint32_t) r*r) |
r -= 1; |
return r; |
} |
// aus Orginal MK source code |
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const int16_t sinlookup[91] = { 0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
8161, 8172, 8181, 8187, 8191, 8192}; |
int16_t c_cos_8192(int16_t angle) |
{ |
return (c_sin_8192(90 - angle)); |
} |
int16_t c_sin_8192(int16_t angle) |
{ |
int8_t m, n; |
int16_t sinus; |
// avoid negative angles |
if (angle < 0) |
{ |
m = -1; |
angle = -angle; |
} |
else m = +1; |
// fold angle to interval 0 to 359 |
angle %= 360; |
// check quadrant |
if (angle <= 90) n = 1; // first quadrant |
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
else {angle = 360 - angle; n = -1;} //fourth quadrant |
// get lookup value |
sinus = sinlookup[angle]; |
// calculate sinus value |
return (sinus * m * n); |
} |
// ---------------------------------------------------------------------------------------------- |
const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, |
75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, |
61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, |
44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, |
18, 14, 10, 0}; |
int16_t c_arccos2(int32_t a, int32_t b) |
{ |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
} |
/* ---------------------------------------------------------------------------------------------- |
atan2.S |
Author: Reiner Patommel |
atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder. |
CORDIC can be used to calculate trigonometric, hyperbolic and linear |
functions and is until today implemented in most pocket calculators. |
The algorithm makes only use of simple integer arithmetic. |
The CORDIC equations in vectoring mode for trigonometric functions are: |
Xi+1 = Xi - Si * (Yi * 1 / 2^i) |
Yi+1 = Yi + Si * (Xi * 1 / 2^i) |
Zi+1 = Zi - Si * atan(1 / 2^i) |
where Si = +1 if Yi < 0 else Si = -1 |
The rotation angles are limited to -PI/2 and PI/2 i.e. |
-90 degrees ... +90 degrees |
For the calculation of atan2(x,y) we need a small pre-calculated table of |
angles or radians with the values Tn = atan(1/2^i). |
We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and |
accumulate the rotated angles or radians in Z. The direction of the rotation |
will be positive or negative depending on the sign of Y after the previous |
rotation and the rotation angle will decrease from step to step. (The first |
step is 45 degrees, the last step is 0.002036 degrees for n = 15). |
After n rotations the variables will have the following values: |
Yn = ideally 0 |
Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector) |
Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians) |
c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to |
approx. 1.64676 for n = 15. |
In order to represent X, Y and Z as integers we introduce a scaling factor Q |
which is chosen as follows: |
1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and |
set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0. |
2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q |
3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2 |
i.e. X and Z are always > 0 and therefore can be unsigned. |
(We do the quadrant correction afterwards based on the initial signs of x and y) |
4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140. |
i.e. |
Xnmax = 65536 --> unsigned int |
Ynmax = +/- 28140 --> signed int |
Znmax = PI/2 * 28140 = 44202 --> unsigned int |
The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad. |
Below is atan2 and atan in C: */ |
#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x]) |
const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; |
int16_t my_atan2(int32_t y, int32_t x) |
{ unsigned char i; |
uint32_t xQ; |
int32_t yQ; |
uint32_t angle = 0; |
uint32_t tmp; |
double x1, y1; |
x1 = abs(x); |
y1 = abs(y); |
if (y1 == 0) { |
if (x < 0) |
return (180); |
else |
return 0; |
} |
if (x1 < y1) { |
x1 /= y1; |
y1 = 1; |
} |
else { |
y1 /= x1; |
x1 = 1; |
} |
xQ = SCALED(x1); |
yQ = SCALED(y1); |
for (i = 0; i < T-1; i++) { |
tmp = xQ >> i; |
if (yQ < 0) { |
xQ -= yQ >> i; |
yQ += tmp; |
angle -= getAtanVal(i); |
} |
else { |
xQ += yQ >> i; |
yQ -= tmp; |
angle += getAtanVal(i); |
} |
} |
angle = RAD_TO_DEG * angle/Q; |
if (x <= 0) |
angle = 180 - angle; |
if (y <= 0) |
return(-angle); |
return(angle); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.h |
---|
0,0 → 1,16 |
#ifndef _MYMATH_H_ |
#define _MYMATH_H_ |
#include <avr/io.h> |
#define T 16 |
#define Q 28140 |
#define SCALED(X) ((int32_t)((X) * Q)) |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
uint16_t sqrt32(uint32_t qzahl); |
int16_t c_cos_8192(int16_t angle); |
int16_t c_sin_8192(int16_t angle); |
int16_t my_atan2(int32_t y, int32_t x); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Copy of gpsmouse.h |
+ |
+_old_source |
+ |
+Copy of gpsmouse.c |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/Font8x8.c |
---|
0,0 → 1,273 |
/* |
* font8x8.c |
* LCD-OSD |
* |
* Created by Peter Mack on 26.12.09. |
* Copyright 2009 SCS GmbH & Co. KG. All rights reserved. |
* |
*/ |
#include <avr/pgmspace.h> |
const uint8_t Font8x8[256][8]PROGMEM= |
{ |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x00 |
{0x7E,0x81,0x95,0xB1,0xB1,0x95,0x81,0x7E}, // 0x01 |
{0x7E,0xFF,0xEB,0xCF,0xCF,0xEB,0xFF,0x7E}, // 0x02 |
{0x0E,0x1F,0x3F,0x7E,0x3F,0x1F,0x0E,0x00}, // 0x03 |
{0x08,0x1C,0x3E,0x7F,0x3E,0x1C,0x08,0x00}, // 0x04 |
{0x38,0x3A,0x9F,0xFF,0x9F,0x3A,0x38,0x00}, // 0x05 |
{0x10,0x38,0xBC,0xFF,0xBC,0x38,0x10,0x00}, // 0x06 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x07 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x08 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x09 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0A |
{0x70,0xF8,0x88,0x88,0xFD,0x7F,0x07,0x0F}, // 0x0B |
{0x00,0x4E,0x5F,0xF1,0xF1,0x5F,0x4E,0x00}, // 0x0C |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0D |
{0xC0,0xFF,0x7F,0x05,0x05,0x65,0x7F,0x3F}, // 0x0E |
{0x99,0x5A,0x3C,0xE7,0xE7,0x3C,0x5A,0x99}, // 0x0F |
{0x7F,0x3E,0x3E,0x1C,0x1C,0x08,0x08,0x00}, // 0x10 |
{0x08,0x08,0x1C,0x1C,0x3E,0x3E,0x7F,0x00}, // 0x11 |
{0x00,0x24,0x66,0xFF,0xFF,0x66,0x24,0x00}, // 0x12 |
{0x00,0x5F,0x5F,0x00,0x00,0x5F,0x5F,0x00}, // 0x13 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0x14 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0x15 |
{0x00,0x70,0x70,0x70,0x70,0x70,0x70,0x00}, // 0x16 |
{0x80,0x94,0xB6,0xFF,0xFF,0xB6,0x94,0x80}, // 0x17 |
{0x00,0x04,0x06,0x7F,0x7F,0x06,0x04,0x00}, // 0x18 |
{0x00,0x10,0x30,0x7F,0x7F,0x30,0x10,0x00}, // 0x19 |
{0x08,0x08,0x08,0x2A,0x3E,0x1C,0x08,0x00}, // 0x1A |
{0x08,0x1C,0x3E,0x2A,0x08,0x08,0x08,0x00}, // 0x1B |
{0x3C,0x3C,0x20,0x20,0x20,0x20,0x20,0x00}, // 0x1C |
{0x08,0x1C,0x3E,0x08,0x08,0x3E,0x1C,0x08}, // 0x1D |
{0x30,0x38,0x3C,0x3E,0x3E,0x3C,0x38,0x30}, // 0x1E |
{0x06,0x0E,0x1E,0x3E,0x3E,0x1E,0x0E,0x06}, // 0x1F |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x20 |
{0x00,0x06,0x5F,0x5F,0x06,0x00,0x00,0x00}, // 0x21 |
{0x00,0x07,0x07,0x00,0x07,0x07,0x00,0x00}, // 0x22 |
{0x14,0x7F,0x7F,0x14,0x7F,0x7F,0x14,0x00}, // 0x23 |
{0x24,0x2E,0x6B,0x6B,0x3A,0x12,0x00,0x00}, // 0x24 |
{0x46,0x66,0x30,0x18,0x0C,0x66,0x62,0x00}, // 0x25 |
{0x30,0x7A,0x4F,0x5D,0x37,0x7A,0x48,0x00}, // 0x26 |
{0x04,0x07,0x03,0x00,0x00,0x00,0x00,0x00}, // 0x27 |
{0x00,0x1C,0x3E,0x63,0x41,0x00,0x00,0x00}, // 0x28 |
{0x00,0x41,0x63,0x3E,0x1C,0x00,0x00,0x00}, // 0x29 |
{0x08,0x2A,0x3E,0x1C,0x1C,0x3E,0x2A,0x08}, // 0x2A |
{0x08,0x08,0x3E,0x3E,0x08,0x08,0x00,0x00}, // 0x2B |
{0x00,0xA0,0xE0,0x60,0x00,0x00,0x00,0x00}, // 0x2C |
{0x08,0x08,0x08,0x08,0x08,0x08,0x00,0x00}, // 0x2D |
{0x00,0x00,0x60,0x60,0x00,0x00,0x00,0x00}, // 0x2E |
{0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00}, // 0x2F |
{0x3E,0x7F,0x59,0x4D,0x7F,0x3E,0x00,0x00}, // 0x30 |
{0x42,0x42,0x7F,0x7F,0x40,0x40,0x00,0x00}, // 0x31 |
{0x62,0x73,0x59,0x49,0x6F,0x66,0x00,0x00}, // 0x32 |
{0x22,0x63,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x33 |
{0x18,0x1C,0x16,0x13,0x7F,0x7F,0x10,0x00}, // 0x34 |
{0x27,0x67,0x45,0x45,0x7D,0x39,0x00,0x00}, // 0x35 |
{0x3C,0x7E,0x4B,0x49,0x79,0x30,0x00,0x00}, // 0x36 |
{0x03,0x63,0x71,0x19,0x0F,0x07,0x00,0x00}, // 0x37 |
{0x36,0x7F,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x38 |
{0x06,0x4F,0x49,0x69,0x3F,0x1E,0x00,0x00}, // 0x39 |
{0x00,0x00,0x6C,0x6C,0x00,0x00,0x00,0x00}, // 0x3A |
{0x00,0xA0,0xEC,0x6C,0x00,0x00,0x00,0x00}, // 0x3B |
{0x08,0x1C,0x36,0x63,0x41,0x00,0x00,0x00}, // 0x3C |
{0x14,0x14,0x14,0x14,0x14,0x14,0x00,0x00}, // 0x3D |
{0x00,0x41,0x63,0x36,0x1C,0x08,0x00,0x00}, // 0x3E |
{0x02,0x03,0x51,0x59,0x0F,0x06,0x00,0x00}, // 0x3F |
{0x3E,0x7F,0x41,0x5D,0x5D,0x1F,0x1E,0x00}, // 0x40 |
{0x7C,0x7E,0x13,0x13,0x7E,0x7C,0x00,0x00}, // 0x41 |
{0x41,0x7F,0x7F,0x49,0x49,0x7F,0x36,0x00}, // 0x42 |
{0x1C,0x3E,0x63,0x41,0x41,0x63,0x22,0x00}, // 0x43 |
{0x41,0x7F,0x7F,0x41,0x63,0x7F,0x1C,0x00}, // 0x44 |
{0x41,0x7F,0x7F,0x49,0x5D,0x41,0x63,0x00}, // 0x45 |
{0x41,0x7F,0x7F,0x49,0x1D,0x01,0x03,0x00}, // 0x46 |
{0x1C,0x3E,0x63,0x41,0x51,0x73,0x72,0x00}, // 0x47 |
{0x7F,0x7F,0x08,0x08,0x7F,0x7F,0x00,0x00}, // 0x48 |
{0x00,0x41,0x7F,0x7F,0x41,0x00,0x00,0x00}, // 0x49 |
{0x30,0x70,0x40,0x41,0x7F,0x3F,0x01,0x00}, // 0x4A |
{0x41,0x7F,0x7F,0x08,0x1C,0x77,0x63,0x00}, // 0x4B |
{0x41,0x7F,0x7F,0x41,0x40,0x60,0x70,0x00}, // 0x4C |
{0x7F,0x7F,0x06,0x0C,0x06,0x7F,0x7F,0x00}, // 0x4D |
{0x7F,0x7F,0x06,0x0C,0x18,0x7F,0x7F,0x00}, // 0x4E |
{0x1C,0x3E,0x63,0x41,0x63,0x3E,0x1C,0x00}, // 0x4F |
{0x41,0x7F,0x7F,0x49,0x09,0x0F,0x06,0x00}, // 0x50 |
{0x1E,0x3F,0x21,0x71,0x7F,0x5E,0x00,0x00}, // 0x51 |
{0x41,0x7F,0x7F,0x19,0x39,0x6F,0x46,0x00}, // 0x52 |
{0x26,0x67,0x4D,0x59,0x7B,0x32,0x00,0x00}, // 0x53 |
{0x03,0x41,0x7F,0x7F,0x41,0x03,0x00,0x00}, // 0x54 |
{0x7F,0x7F,0x40,0x40,0x7F,0x7F,0x00,0x00}, // 0x55 |
{0x1F,0x3F,0x60,0x60,0x3F,0x1F,0x00,0x00}, // 0x56 |
{0x7F,0x7F,0x30,0x18,0x30,0x7F,0x7F,0x00}, // 0x57 |
{0x63,0x77,0x1C,0x08,0x1C,0x77,0x63,0x00}, // 0x58 |
{0x07,0x4F,0x78,0x78,0x4F,0x07,0x00,0x00}, // 0x59 |
{0x67,0x73,0x59,0x4D,0x47,0x63,0x71,0x00}, // 0x5A |
{0x00,0x7F,0x7F,0x41,0x41,0x00,0x00,0x00}, // 0x5B |
{0x01,0x03,0x06,0x0C,0x18,0x30,0x60,0x00}, // 0x5C |
{0x00,0x41,0x41,0x7F,0x7F,0x00,0x00,0x00}, // 0x5D |
{0x08,0x0C,0x06,0x03,0x06,0x0C,0x08,0x00}, // 0x5E |
{0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80}, // 0x5F |
{0x00,0x00,0x03,0x07,0x04,0x00,0x00,0x00}, // 0x60 |
{0x20,0x74,0x54,0x54,0x3C,0x78,0x40,0x00}, // 0x61 |
{0x41,0x3F,0x7F,0x44,0x44,0x7C,0x38,0x00}, // 0x62 |
{0x38,0x7C,0x44,0x44,0x6C,0x28,0x00,0x00}, // 0x63 |
{0x30,0x78,0x48,0x49,0x3F,0x7F,0x40,0x00}, // 0x64 |
{0x38,0x7C,0x54,0x54,0x5C,0x18,0x00,0x00}, // 0x65 |
{0x48,0x7E,0x7F,0x49,0x03,0x02,0x00,0x00}, // 0x66 |
{0x98,0xBC,0xA4,0xA4,0xF8,0x7C,0x04,0x00}, // 0x67 |
{0x41,0x7F,0x7F,0x08,0x04,0x7C,0x78,0x00}, // 0x68 |
{0x00,0x44,0x7D,0x7D,0x40,0x00,0x00,0x00}, // 0x69 |
{0x40,0xC4,0x84,0xFD,0x7D,0x00,0x00,0x00}, // 0x6A |
{0x41,0x7F,0x7F,0x10,0x38,0x6C,0x44,0x00}, // 0x6B |
{0x00,0x41,0x7F,0x7F,0x40,0x00,0x00,0x00}, // 0x6C |
{0x7C,0x7C,0x0C,0x18,0x0C,0x7C,0x78,0x00}, // 0x6D |
{0x7C,0x7C,0x04,0x04,0x7C,0x78,0x00,0x00}, // 0x6E |
{0x38,0x7C,0x44,0x44,0x7C,0x38,0x00,0x00}, // 0x6F |
{0x84,0xFC,0xF8,0xA4,0x24,0x3C,0x18,0x00}, // 0x70 |
{0x18,0x3C,0x24,0xA4,0xF8,0xFC,0x84,0x00}, // 0x71 |
{0x44,0x7C,0x78,0x44,0x1C,0x18,0x00,0x00}, // 0x72 |
{0x48,0x5C,0x54,0x54,0x74,0x24,0x00,0x00}, // 0x73 |
{0x00,0x04,0x3E,0x7F,0x44,0x24,0x00,0x00}, // 0x74 |
{0x3C,0x7C,0x40,0x40,0x3C,0x7C,0x40,0x00}, // 0x75 |
{0x1C,0x3C,0x60,0x60,0x3C,0x1C,0x00,0x00}, // 0x76 |
{0x3C,0x7C,0x60,0x30,0x60,0x7C,0x3C,0x00}, // 0x77 |
{0x44,0x6C,0x38,0x10,0x38,0x6C,0x44,0x00}, // 0x78 |
{0x9C,0xBC,0xA0,0xA0,0xFC,0x7C,0x00,0x00}, // 0x79 |
{0x4C,0x64,0x74,0x5C,0x4C,0x64,0x00,0x00}, // 0x7A |
{0x08,0x08,0x3E,0x77,0x41,0x41,0x00,0x00}, // 0x7B |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0x7C |
{0x41,0x41,0x77,0x3E,0x08,0x08,0x00,0x00}, // 0x7D |
{0x02,0x03,0x01,0x03,0x02,0x03,0x01,0x00}, // 0x7E |
{0x78,0x7C,0x46,0x43,0x46,0x7C,0x78,0x00}, // 0x7F |
{0x1E,0xBF,0xE1,0x61,0x33,0x12,0x00,0x00}, // 0x80 |
{0x3A,0x7A,0x40,0x40,0x7A,0x7A,0x40,0x00}, // 0x81 |
{0x38,0x7C,0x56,0x57,0x5D,0x18,0x00,0x00}, // 0x82 |
{0x02,0x23,0x75,0x55,0x55,0x7D,0x7B,0x42}, // 0x83 |
{0x21,0x75,0x54,0x54,0x7D,0x79,0x40,0x00}, // 0x84 |
{0x20,0x75,0x57,0x56,0x7C,0x78,0x40,0x00}, // 0x85 |
{0x00,0x22,0x77,0x55,0x55,0x7F,0x7A,0x40}, // 0x86 |
{0x1C,0xBE,0xE2,0x62,0x36,0x14,0x00,0x00}, // 0x87 |
{0x02,0x3B,0x7D,0x55,0x55,0x5D,0x1B,0x02}, // 0x88 |
{0x39,0x7D,0x54,0x54,0x5D,0x19,0x00,0x00}, // 0x89 |
{0x38,0x7D,0x57,0x56,0x5C,0x18,0x00,0x00}, // 0x8A |
{0x01,0x45,0x7C,0x7C,0x41,0x01,0x00,0x00}, // 0x8B |
{0x02,0x03,0x45,0x7D,0x7D,0x43,0x02,0x00}, // 0x8C |
{0x00,0x45,0x7F,0x7E,0x40,0x00,0x00,0x00}, // 0x8D |
{0x79,0x7D,0x26,0x26,0x7D,0x79,0x00,0x00}, // 0x8E |
{0x70,0x7A,0x2D,0x2D,0x7A,0x70,0x00,0x00}, // 0x8F |
{0x44,0x7C,0x7E,0x57,0x55,0x44,0x00,0x00}, // 0x90 |
{0x20,0x74,0x54,0x54,0x7C,0x7C,0x54,0x54}, // 0x91 |
{0x7C,0x7E,0x0B,0x09,0x7F,0x7F,0x49,0x00}, // 0x92 |
{0x32,0x7B,0x49,0x49,0x7B,0x32,0x00,0x00}, // 0x93 |
{0x32,0x7A,0x48,0x48,0x7A,0x32,0x00,0x00}, // 0x94 |
{0x30,0x79,0x4B,0x4A,0x78,0x30,0x00,0x00}, // 0x95 |
{0x3A,0x7B,0x41,0x41,0x7B,0x7A,0x40,0x00}, // 0x96 |
{0x38,0x79,0x43,0x42,0x78,0x78,0x40,0x00}, // 0x97 |
{0xBA,0xBA,0xA0,0xA0,0xFA,0x7A,0x00,0x00}, // 0x98 |
{0x39,0x7D,0x44,0x44,0x44,0x7D,0x39,0x00}, // 0x99 |
{0x3D,0x7D,0x40,0x40,0x7D,0x3D,0x00,0x00}, // 0x9A |
{0x38,0x7C,0x64,0x54,0x4C,0x7C,0x38,0x00}, // 0x9B |
{0x68,0x7E,0x7F,0x49,0x43,0x66,0x20,0x00}, // 0x9C |
{0x5C,0x3E,0x73,0x49,0x67,0x3E,0x1D,0x00}, // 0x9D |
{0x44,0x6C,0x38,0x38,0x6C,0x44,0x00,0x00}, // 0x9E |
{0x40,0xC8,0x88,0xFE,0x7F,0x09,0x0B,0x02}, // 0x9F |
{0x20,0x74,0x56,0x57,0x7D,0x78,0x40,0x00}, // 0xA0 |
{0x00,0x44,0x7E,0x7F,0x41,0x00,0x00,0x00}, // 0xA1 |
{0x30,0x78,0x48,0x4A,0x7B,0x31,0x00,0x00}, // 0xA2 |
{0x38,0x78,0x40,0x42,0x7B,0x79,0x40,0x00}, // 0xA3 |
{0x7A,0x7B,0x09,0x0B,0x7A,0x73,0x01,0x00}, // 0xA4 |
{0x7A,0x7B,0x19,0x33,0x7A,0x7B,0x01,0x00}, // 0xA5 |
{0x00,0x26,0x2F,0x29,0x2F,0x2F,0x28,0x00}, // 0xA6 |
{0x00,0x26,0x2F,0x29,0x29,0x2F,0x26,0x00}, // 0xA7 |
{0x30,0x78,0x4D,0x45,0x60,0x20,0x00,0x00}, // 0xA8 |
{0x1C,0x22,0x7D,0x4B,0x5B,0x65,0x22,0x1C}, // 0xA9 |
{0x08,0x08,0x08,0x08,0x38,0x38,0x00,0x00}, // 0xAA |
{0x61,0x3F,0x1F,0xCC,0xEE,0xAB,0xB9,0x90}, // 0xAB |
{0x61,0x3F,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xAC |
{0x00,0x00,0x60,0xFA,0xFA,0x60,0x00,0x00}, // 0xAD |
{0x08,0x1C,0x36,0x22,0x08,0x1C,0x36,0x22}, // 0xAE |
{0x22,0x36,0x1C,0x08,0x22,0x36,0x1C,0x08}, // 0xAF |
{0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00}, // 0xB0 |
{0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55}, // 0xB1 |
{0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF}, // 0xB2 |
{0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00}, // 0xB3 |
{0x10,0x10,0x10,0xFF,0xFF,0x00,0x00,0x00}, // 0xB4 |
{0x70,0x78,0x2C,0x2E,0x7B,0x71,0x00,0x00}, // 0xB5 |
{0x72,0x79,0x2D,0x2D,0x79,0x72,0x00,0x00}, // 0xB6 |
{0x71,0x7B,0x2E,0x2C,0x78,0x70,0x00,0x00}, // 0xB7 |
{0x1C,0x22,0x5D,0x55,0x55,0x41,0x22,0x1C}, // 0xB8 |
{0x14,0x14,0xF7,0xF7,0x00,0xFF,0xFF,0x00}, // 0xB9 |
{0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0x00}, // 0xBA |
{0x14,0x14,0xF4,0xF4,0x04,0xFC,0xFC,0x00}, // 0xBB |
{0x14,0x14,0x17,0x17,0x10,0x1F,0x1F,0x00}, // 0xBC |
{0x18,0x3C,0x24,0xE7,0xE7,0x24,0x24,0x00}, // 0xBD |
{0x2B,0x2F,0xFC,0xFC,0x2F,0x2B,0x00,0x00}, // 0xBE |
{0x10,0x10,0x10,0xF0,0xF0,0x00,0x00,0x00}, // 0xBF |
{0x00,0x00,0x00,0x1F,0x1F,0x10,0x10,0x10}, // 0xC0 |
{0x10,0x10,0x10,0x1F,0x1F,0x10,0x10,0x10}, // 0xC1 |
{0x10,0x10,0x10,0xF0,0xF0,0x10,0x10,0x10}, // 0xC2 |
{0x00,0x00,0x00,0xFF,0xFF,0x10,0x10,0x10}, // 0xC3 |
{0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10}, // 0xC4 |
{0x10,0x10,0x10,0xFF,0xFF,0x10,0x10,0x10}, // 0xC5 |
{0x22,0x77,0x55,0x57,0x7E,0x7B,0x41,0x00}, // 0xC6 |
{0x72,0x7B,0x2D,0x2F,0x7A,0x73,0x01,0x00}, // 0xC7 |
{0x00,0x00,0x1F,0x1F,0x10,0x17,0x17,0x14}, // 0xC8 |
{0x00,0x00,0xFC,0xFC,0x04,0xF4,0xF4,0x14}, // 0xC9 |
{0x14,0x14,0x17,0x17,0x10,0x17,0x17,0x14}, // 0xCA |
{0x14,0x14,0xF4,0xF4,0x04,0xF4,0xF4,0x14}, // 0xCB |
{0x00,0x00,0xFF,0xFF,0x00,0xF7,0xF7,0x14}, // 0xCC |
{0x14,0x14,0x14,0x14,0x14,0x14,0x14,0x14}, // 0xCD |
{0x14,0x14,0xF7,0xF7,0x00,0xF7,0xF7,0x14}, // 0xCE |
{0x66,0x3C,0x3C,0x24,0x3C,0x3C,0x66,0x00}, // 0xCF |
{0x05,0x27,0x72,0x57,0x7D,0x38,0x00,0x00}, // 0xD0 |
{0x49,0x7F,0x7F,0x49,0x63,0x7F,0x1C,0x00}, // 0xD1 |
{0x46,0x7D,0x7D,0x55,0x55,0x46,0x00,0x00}, // 0xD2 |
{0x45,0x7D,0x7C,0x54,0x55,0x45,0x00,0x00}, // 0xD3 |
{0x44,0x7D,0x7F,0x56,0x54,0x44,0x00,0x00}, // 0xD4 |
{0x0A,0x0E,0x08,0x00,0x00,0x00,0x00,0x00}, // 0xD5 |
{0x00,0x44,0x7E,0x7F,0x45,0x00,0x00,0x00}, // 0xD6 |
{0x02,0x45,0x7D,0x7D,0x45,0x02,0x00,0x00}, // 0xD7 |
{0x01,0x45,0x7C,0x7C,0x45,0x01,0x00,0x00}, // 0xD8 |
{0x10,0x10,0x10,0x1F,0x1F,0x00,0x00,0x00}, // 0xD9 |
{0x00,0x00,0x00,0xF0,0xF0,0x10,0x10,0x10}, // 0xDA |
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, // 0xDB |
{0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0}, // 0xDC |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0xDD |
{0x00,0x45,0x7F,0x7E,0x44,0x00,0x00,0x00}, // 0xDE |
{0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}, // 0xDF |
{0x38,0x7C,0x46,0x47,0x45,0x7C,0x38,0x00}, // 0xE0 |
{0xFC,0xFE,0x2A,0x2A,0x3E,0x14,0x00,0x00}, // 0xE1 |
{0x3A,0x7D,0x45,0x45,0x45,0x7D,0x3A,0x00}, // 0xE2 |
{0x38,0x7C,0x45,0x47,0x46,0x7C,0x38,0x00}, // 0xE3 |
{0x32,0x7B,0x49,0x4B,0x7A,0x33,0x01,0x00}, // 0xE4 |
{0x3A,0x7F,0x45,0x47,0x46,0x7F,0x39,0x00}, // 0xE5 |
{0x80,0xFE,0x7E,0x20,0x20,0x3E,0x1E,0x00}, // 0xE6 |
{0x42,0x7E,0x7E,0x54,0x1C,0x08,0x00,0x00}, // 0xE7 |
{0x41,0x7F,0x7F,0x55,0x14,0x1C,0x08,0x00}, // 0xE8 |
{0x3C,0x7C,0x42,0x43,0x7D,0x3C,0x00,0x00}, // 0xE9 |
{0x3A,0x79,0x41,0x41,0x79,0x3A,0x00,0x00}, // 0xEA |
{0x3C,0x7D,0x43,0x42,0x7C,0x3C,0x00,0x00}, // 0xEB |
{0xB8,0xB8,0xA2,0xA3,0xF9,0x78,0x00,0x00}, // 0xEC |
{0x0C,0x5C,0x72,0x73,0x5D,0x0C,0x00,0x00}, // 0xED |
{0x02,0x02,0x02,0x02,0x02,0x02,0x00,0x00}, // 0xEE |
{0x00,0x00,0x02,0x03,0x01,0x00,0x00,0x00}, // 0xEF |
{0x10,0x10,0x10,0x10,0x10,0x10,0x00,0x00}, // 0xF0 |
{0x44,0x44,0x5F,0x5F,0x44,0x44,0x00,0x00}, // 0xF1 |
{0x28,0x28,0x28,0x28,0x28,0x28,0x00,0x00}, // 0xF2 |
{0x71,0x35,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xF3 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0xF4 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0xF5 |
{0x08,0x08,0x6B,0x6B,0x08,0x08,0x00,0x00}, // 0xF6 |
{0x00,0x80,0xC0,0x40,0x00,0x00,0x00,0x00}, // 0xF7 |
{0x00,0x06,0x0F,0x09,0x0F,0x06,0x00,0x00}, // 0xF8 |
{0x02,0x02,0x00,0x00,0x02,0x02,0x00,0x00}, // 0xF9 |
{0x00,0x00,0x00,0x10,0x10,0x00,0x00,0x00}, // 0xFA |
{0x00,0x12,0x13,0x1F,0x1F,0x10,0x10,0x00}, // 0xFB |
{0x00,0x11,0x15,0x15,0x1F,0x1F,0x0A,0x00}, // 0xFC |
{0x00,0x19,0x1D,0x15,0x17,0x12,0x00,0x00}, // 0xFD |
{0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00}, // 0xFE |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00} // 0xFF |
}; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/Font8x8.h |
---|
0,0 → 1,31 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X8_H |
#define _FONT8X8_H |
#include <avr/pgmspace.h> |
extern const uint8_t Font8x8[256][8]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/font8x6.c |
---|
0,0 → 1,232 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY font8x6.c |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK (ehemals 'Antenne' Ascii 31) |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: ASC 11 (0x0B) SYMBOL_SMALLDEGREE |
//# - add: ASC 16 (0x10) SYMBOL_RCQUALITY |
//############################################################################ |
#include <avr/pgmspace.h> |
// one byte is a column |
// bit 7 is the bottom |
// |
// 123456 |
// L 1 | XXX | |
// O 2 |X X | |
// W 4 |X X | |
// 8 | XXX | |
// H 1 |X X | |
// I 2 |X X | |
// G 4 | XXX | |
// H 8 | | |
// |
// 0x36,0x49,0x49,0x49,0x36,0x00 |
// |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | X| |
// 8 | X | |
// H 1 | X | |
// I 2 |X X | |
// G 4 | X | |
// H 8 | | |
// 0x20,0x40,0x20,0x10,0x08,0x04 |
// 123456 |
// L 1 | | |
// O 2 | X| |
// W 4 | X | |
// 8 | X | |
// H 1 |X X | |
// I 2 | X | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x10,0x08,0x04,0x02 |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | XX| |
// 8 | XX | |
// H 1 |X XX | |
// I 2 | XX | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x30,0x18,0x0c,0x04 |
//---------------------------------------------------------------- |
const uint8_t font8x6[128][6] PROGMEM = |
{ |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 0 00 (not useable) |
{ 0x78,0x15,0x14,0x15,0x78,0x00 }, // ASCII - 1 01 'Ä' |
{ 0x20,0x55,0x54,0x55,0x78,0x00 }, // ASCII - 2 02 'ä' |
{ 0x38,0x45,0x44,0x45,0x38,0x00 }, // ASCII - 3 03 'Ö' |
{ 0x30,0x49,0x48,0x49,0x30,0x00 }, // ASCII - 4 04 'ö' |
{ 0x3c,0x41,0x40,0x41,0x3c,0x00 }, // ASCII - 5 05 'Ü' |
{ 0x38,0x41,0x40,0x21,0x78,0x00 }, // ASCII - 6 06 'ü' |
{ 0x7e,0x15,0x15,0x15,0x0a,0x00 }, // ASCII - 7 07 'ß' |
{ 0x22,0x17,0x0F,0x17,0x22,0x00 }, // ASCII - 8 08 SAT Symbol |
{ 0x00,0x84,0x82,0xFF,0x82,0x84 }, // ASCII - 9 09 Altitude Symbol |
{ 0x1c,0x14,0x1c,0x00,0x00,0x00 }, // ASCII - 10 0A (not useable) SYMBOL_AVG |
{ 0x00,0x07,0x05,0x07,0x00,0x00 }, // ASCII - 11 0B small degree SYMBOL_SMALLDEGREE |
{ 0x10,0x38,0x54,0x10,0x10,0x1e }, // ASCII - 12 0C Enter Symbol |
{ 0x18,0x0c,0x18,0x00,0x00,0x00 }, // ASCII - 13 0D (not useable) SYMBOL_MAX |
{ 0x10,0x10,0x10,0x10,0x10,0x10 }, // ASCII - 14 0E hor. line |
{ 0x10,0x10,0x10,0x7c,0x10,0x10 }, // ASCII - 15 0F hor. line with tick mark |
{ 0x08,0x10,0x08,0x00,0x00,0x00 }, // ASCII - 16 10 rc quality SYMBOL_MIN |
{ 0x08,0x14,0x00,0x00,0x14,0x08 }, // ASCII - 17 11 <> Change |
{ 0x10,0x08,0x04,0x04,0x08,0x10 }, // ASCII - 18 12 /\ Up |
{ 0x08,0x10,0x20,0x20,0x10,0x08 }, // ASCII - 19 13 \/ Down |
{ 0x00,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 20 14 < Left |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 21 15 > Right |
{ 0x04,0x02,0x7f,0x02,0x04,0x00 }, // ASCII - 22 16 /|\ Arrow up |
{ 0x10,0x20,0x7f,0x20,0x10,0x00 }, // ASCII - 23 17 \|/ Arrow down |
{ 0x10,0x38,0x54,0x10,0x10,0x10 }, // ASCII - 24 18 <- Arrow left |
{ 0x10,0x10,0x10,0x54,0x38,0x10 }, // ASCII - 25 19 -> Arrow right |
{ 0x10,0x18,0x1c,0x1c,0x18,0x10 }, // ASCII - 26 1A ^ Triangle up |
{ 0x08,0x18,0x38,0x38,0x18,0x08 }, // ASCII - 27 1B v Triangle down |
{ 0x00,0x08,0x1c,0x3e,0x7f,0x00 }, // ASCII - 28 1C < Triangle left |
{ 0x00,0x7f,0x3e,0x1c,0x08,0x00 }, // ASCII - 29 1D > Triangle right |
{ 0x06,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 30 1E '°' big degree SYMBOL_BIGDEGREE |
// { 0x06,0x49,0x7d,0x49,0x06,0x00 }, // ASCII - 31 1F Antenne (ALT) |
{ 0x10,0x20,0x10,0x08,0x04,0x02 }, // ASCII - 31 1F SYMBOL_CHECK |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 32 20 ' ' |
{ 0x00,0x00,0x2f,0x00,0x00,0x00 }, // ASCII - 33 21 '!' |
{ 0x00,0x07,0x00,0x07,0x00,0x00 }, // ASCII - 34 22 '"' |
{ 0x14,0x7f,0x14,0x7f,0x14,0x00 }, // ASCII - 35 23 '#' |
{ 0x24,0x2a,0x6b,0x2a,0x12,0x00 }, // ASCII - 36 24 '$' |
{ 0x23,0x13,0x08,0x64,0x62,0x00 }, // ASCII - 37 25 '%' |
{ 0x36,0x49,0x55,0x22,0x50,0x00 }, // ASCII - 38 26 '&' |
{ 0x00,0x05,0x03,0x00,0x00,0x00 }, // ASCII - 39 27 ''' |
{ 0x00,0x1c,0x22,0x41,0x00,0x00 }, // ASCII - 40 28 '(' |
{ 0x00,0x41,0x22,0x1c,0x00,0x00 }, // ASCII - 41 29 ')' |
{ 0x14,0x08,0x3e,0x08,0x14,0x00 }, // ASCII - 42 2a '*' |
{ 0x08,0x08,0x3e,0x08,0x08,0x00 }, // ASCII - 43 2b '+' |
{ 0x00,0x50,0x30,0x00,0x00,0x00 }, // ASCII - 44 2c ',' |
{ 0x08,0x08,0x08,0x08,0x08,0x00 }, // ASCII - 45 2d '-' |
{ 0x00,0x60,0x60,0x00,0x00,0x00 }, // ASCII - 46 2e '.' |
{ 0x20,0x10,0x08,0x04,0x02,0x00 }, // ASCII - 47 2f '/' |
{ 0x3e,0x51,0x49,0x45,0x3e,0x00 }, // ASCII - 48 30 '0' |
{ 0x00,0x42,0x7f,0x40,0x00,0x00 }, // ASCII - 49 31 '1' |
{ 0x42,0x61,0x51,0x49,0x46,0x00 }, // ASCII - 50 32 '2' |
{ 0x21,0x41,0x45,0x4b,0x31,0x00 }, // ASCII - 51 33 '3' |
{ 0x18,0x14,0x12,0x7f,0x10,0x00 }, // ASCII - 52 34 '4' |
{ 0x27,0x45,0x45,0x45,0x39,0x00 }, // ASCII - 53 35 '5' |
{ 0x3c,0x4a,0x49,0x49,0x30,0x00 }, // ASCII - 54 36 '6' |
{ 0x03,0x01,0x71,0x09,0x07,0x00 }, // ASCII - 55 37 '7' |
{ 0x36,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 56 38 '8' |
{ 0x06,0x49,0x49,0x29,0x1e,0x00 }, // ASCII - 57 39 '9' |
{ 0x00,0x36,0x36,0x00,0x00,0x00 }, // ASCII - 58 3a ':' |
{ 0x00,0x56,0x36,0x00,0x00,0x00 }, // ASCII - 59 3b ';' |
{ 0x08,0x14,0x22,0x41,0x00,0x00 }, // ASCII - 60 3c '<' |
{ 0x14,0x14,0x14,0x14,0x14,0x00 }, // ASCII - 61 3d '=' |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 62 3e '>' |
{ 0x02,0x01,0x51,0x09,0x06,0x00 }, // ASCII - 63 3f '?' |
{ 0x32,0x49,0x79,0x41,0x3e,0x00 }, // ASCII - 64 40 '@' |
{ 0x7e,0x11,0x11,0x11,0x7e,0x00 }, // ASCII - 65 41 'A' |
{ 0x7f,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 66 42 'B' |
{ 0x3e,0x41,0x41,0x41,0x22,0x00 }, // ASCII - 67 43 'C' |
{ 0x7f,0x41,0x41,0x22,0x1c,0x00 }, // ASCII - 68 44 'D' |
{ 0x7f,0x49,0x49,0x49,0x41,0x00 }, // ASCII - 69 45 'E' |
{ 0x7f,0x09,0x09,0x09,0x01,0x00 }, // ASCII - 70 46 'F' |
{ 0x3e,0x41,0x49,0x49,0x7a,0x00 }, // ASCII - 71 47 'G' |
{ 0x7f,0x08,0x08,0x08,0x7f,0x00 }, // ASCII - 72 48 'H' |
{ 0x00,0x41,0x7f,0x41,0x00,0x00 }, // ASCII - 73 49 'I' |
{ 0x20,0x40,0x41,0x3f,0x01,0x00 }, // ASCII - 74 4a 'J' |
{ 0x7f,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 75 4b 'K' |
{ 0x7f,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 76 4c 'L' |
{ 0x7f,0x02,0x0c,0x02,0x7f,0x00 }, // ASCII - 77 4d 'M' |
{ 0x7f,0x04,0x08,0x10,0x7f,0x00 }, // ASCII - 78 4e 'N' |
{ 0x3e,0x41,0x41,0x41,0x3e,0x00 }, // ASCII - 79 4f 'O' |
{ 0x7f,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 80 50 'P' |
{ 0x3e,0x41,0x51,0x21,0x5e,0x00 }, // ASCII - 81 51 'Q' |
{ 0x7f,0x09,0x19,0x29,0x46,0x00 }, // ASCII - 82 52 'R' |
{ 0x46,0x49,0x49,0x49,0x31,0x00 }, // ASCII - 83 53 'S' |
{ 0x01,0x01,0x7f,0x01,0x01,0x00 }, // ASCII - 84 54 'T' |
{ 0x3f,0x40,0x40,0x40,0x3f,0x00 }, // ASCII - 85 55 'U' |
{ 0x1f,0x20,0x40,0x20,0x1f,0x00 }, // ASCII - 86 56 'V' |
{ 0x3f,0x40,0x38,0x40,0x3f,0x00 }, // ASCII - 87 57 'W' |
{ 0x63,0x14,0x08,0x14,0x63,0x00 }, // ASCII - 88 58 'X' |
{ 0x07,0x08,0x70,0x08,0x07,0x00 }, // ASCII - 89 59 'Y' |
{ 0x61,0x51,0x49,0x45,0x43,0x00 }, // ASCII - 90 5a 'Z' |
{ 0x7f,0x41,0x41,0x00,0x00,0x00 }, // ASCII - 91 5b '[' |
{ 0x02,0x04,0x08,0x10,0x20,0x00 }, // ASCII - 92 5c '\' |
{ 0x00,0x41,0x41,0x7f,0x00,0x00 }, // ASCII - 93 5d ']' |
{ 0x04,0x02,0x01,0x02,0x04,0x00 }, // ASCII - 94 5e '^' |
{ 0x40,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 95 5f '_' |
{ 0x00,0x01,0x02,0x04,0x00,0x00 }, // ASCII - 96 60 '`' |
{ 0x20,0x54,0x54,0x54,0x78,0x00 }, // ASCII - 97 61 'a' |
{ 0x7f,0x48,0x44,0x44,0x38,0x00 }, // ASCII - 98 62 'b' |
{ 0x38,0x44,0x44,0x44,0x20,0x00 }, // ASCII - 99 63 'c' |
{ 0x38,0x44,0x44,0x48,0x7f,0x00 }, // ASCII - 100 64 'd' |
{ 0x38,0x54,0x54,0x54,0x18,0x00 }, // ASCII - 101 65 'e' |
{ 0x08,0x7e,0x09,0x01,0x02,0x00 }, // ASCII - 102 66 'f' |
{ 0x0c,0x52,0x52,0x52,0x3e,0x00 }, // ASCII - 103 67 'g' |
{ 0x7f,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 104 68 'h' |
{ 0x00,0x44,0x7d,0x40,0x00,0x00 }, // ASCII - 105 69 'i' |
{ 0x20,0x40,0x44,0x3d,0x00,0x00 }, // ASCII - 106 6a 'j' |
{ 0x7f,0x10,0x28,0x44,0x00,0x00 }, // ASCII - 107 6b 'k' |
{ 0x00,0x41,0x7f,0x40,0x00,0x00 }, // ASCII - 108 6c 'l' |
{ 0x7c,0x04,0x18,0x04,0x78,0x00 }, // ASCII - 109 6d 'm' |
{ 0x7c,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 110 6e 'n' |
{ 0x38,0x44,0x44,0x44,0x38,0x00 }, // ASCII - 111 6f 'o' |
{ 0x7c,0x14,0x14,0x14,0x08,0x00 }, // ASCII - 112 70 'p' |
{ 0x08,0x14,0x14,0x18,0x7c,0x00 }, // ASCII - 113 71 'q' |
{ 0x7c,0x08,0x04,0x04,0x08,0x00 }, // ASCII - 114 72 'r' |
{ 0x48,0x54,0x54,0x54,0x20,0x00 }, // ASCII - 115 73 's' |
{ 0x04,0x3f,0x44,0x40,0x20,0x00 }, // ASCII - 116 74 't' |
{ 0x3c,0x40,0x40,0x20,0x7c,0x00 }, // ASCII - 117 75 'u' |
{ 0x1c,0x20,0x40,0x20,0x1c,0x00 }, // ASCII - 118 76 'v' |
{ 0x3c,0x40,0x38,0x40,0x3c,0x00 }, // ASCII - 119 77 'w' |
{ 0x44,0x28,0x10,0x28,0x44,0x00 }, // ASCII - 120 78 'x' |
{ 0x0c,0x50,0x50,0x50,0x3c,0x00 }, // ASCII - 121 79 'y' |
{ 0x44,0x64,0x54,0x4c,0x44,0x00 }, // ASCII - 122 7a 'z' |
{ 0x00,0x08,0x36,0x41,0x00,0x00 }, // ASCII - 123 7b '{' |
{ 0x00,0x00,0x7f,0x00,0x00,0x00 }, // ASCII - 124 7c '|' |
{ 0x00,0x41,0x36,0x08,0x00,0x00 }, // ASCII - 125 7d '}' |
{ 0x08,0x08,0x2a,0x1c,0x08,0x00 }, // ASCII - 126 7e -> |
{ 0x08,0x1c,0x2a,0x08,0x08,0x00 }, // ASCII - 127 7f <- |
}; |
/* |
{ 0x02,0x0a,0x2a,0x0a,0x02,0x00 }, // ASCII - 16 10 rc quality SYMBOL_RCQUALITY (wieder entfernt weil der Platz gebraucht wurde) |
*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/font8x6.h |
---|
0,0 → 1,30 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X6_H |
#define _FONT8X6_H |
#include <avr/pgmspace.h> |
//extern prog_uint8_t font8x6[128][6]; |
extern const uint8_t font8x6[128][6]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/lcd.c |
---|
0,0 → 1,1990 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.c |
//# |
//# 10.11.2014 cebra |
//# - fix: lcd_init() weiter Helligkeitseinstellungen korrigiert (OCR2A) |
//# |
//# 28.06.2014 OG |
//# - chg: lcdx_cls_rowwidth() kompatibel zu mode MINVERSX und MNORMALX |
//# - chg: lcdx_cls_row() auf lcdx_cls_rowwidth() umgestellt |
//# - fix: lcdx_printp() umgestellt auf strlen_P() statt strlen() |
//# |
//# 27.06.2014 OG |
//# - add: die lcd-print Funktionen wurden um MINVERSX und MNORMALX erweitert |
//# um einen zusaetzlichen Rand links und oben als Linie zu zeichnen |
//# (wird z.B. in followme.c verwendet) |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# - add: include <avr/interrupt.h> |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 26.05.2014 OG |
//# - chg: LCD_Init() - LCD-Modus nur noch normal (kein Invers mehr) |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 10.04.2014 OG |
//# - add: include: lipo.h |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# - chg: PRINTF_BUFFER_SIZE von 80 auf 40 |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: lcd_frect_round() |
//# - chg: lcd_rect_round() um R2 ergaenzt |
//# |
//# 12.02.2014 OG |
//# - add: lcd_rect_round() |
//# |
//# 04.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Aufbau der Formatmaske |
//# |
//# 03.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Berechnung Formatstring korrigiert |
//# - fix: writex_ndigit_number_u_100th() Parameter 'mode' fehlte |
//# |
//# 16.06.2013 OG |
//# - chg: LCD_ORIENTATION festgesetzt auf 0 (Normal) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - chg: writex_datetime_time(), writex_datetime_date() Lokalzeitberechnung |
//# via UTCdatetime2local() |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer GPS-Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 29.04.2013 OG |
//# - chg: lcd_cls() geaendert auf memset |
//# |
//# 28.04.2013 OG |
//# - chg: high-level Funktionen wie writex_ndigit... auf xprintf umgebaut |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# siehe dazu: Doku in utils/xstring.h fuer xprintf |
//# |
//# 27.03.2013 OG |
//# - add: writex_datetime_time() |
//# - add: writex_datetime_date() |
//# - add: Show_PKTError_NoRAM() |
//# |
//# 22.03.2013 OG |
//# - add: writex_gpspos() |
//# |
//# 11.03.2013 OG |
//# - fix: writex_time() Ausgabe Doppelpunkt ":" ergaenzt um mode,xoffs,yoffs |
//# |
//# 07.03.2013 OG |
//# - del: Kompatibilitaetsfunktion lcd_printpj_at() entfernt |
//# |
//# 06.03.2013 OG |
//# - lcdx_putc() wurde erweitert mit Unterstuetzung des 8x8 Font (alte Jeti |
//# Funktionen) und Pixel-Verschiebung (xoffs,yoffs) |
//# - etliche Char basierte Funktionen wurden erweitert um Parameter xoffs,yoffs |
//# um die Ausgabe um +/- Pixel zu verschieben. Fuer Pixelgenaue Positionierung |
//# von OSD-Screen Elementen. |
//# Die neuen Funktionen mit Pixel-Verschiebung beginnen mit lcdx_, writex_ ... |
//# - add: Kompatibilitaet gewaehrleistet durch Mapper-Funktionen |
//# - add: defines fuer Char-Drawmode's (MNORMAL, MINVERS, MBIG, MBIGINVERS) |
//# - del: Jeti-Funktionen (teilweise ersetzt durch Kompatibilitaetsfunktionen) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <stdarg.h> |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "lcd.h" |
#include "../main.h" |
#include "../HAL_HW3_9.h" |
#include "../utils/xutils.h" // xprintf, UTCdatetime2local |
#include "../lipo/lipo.h" // show_Lipo() |
#include "font8x6.h" |
#ifdef USE_FONT_BIG |
#include "Font8x8.h" |
#endif |
#define DISP_W 128 |
#define DISP_H 64 |
#define DISP_BUFFER ((DISP_H * DISP_W) / 8) |
#define LINE_BUFFER (((DISP_H/8) * DISP_W) / 8) |
#define Jeti 1 // Jeti Routinen |
volatile uint8_t display_buffer[DISP_BUFFER]; // Display-Puffer, weil nicht zurückgelesen werden kann |
volatile uint8_t line_buffer[LINE_BUFFER]; // Zeilen-Puffer, weil nicht zurückgelesen werden kann |
volatile uint16_t display_buffer_pointer; // Pointer auf das aktuell übertragene Byte |
volatile uint8_t display_buffer_counter; // Hilfszähler zur Selektierung der Page |
volatile uint8_t display_page_counter; // aktuelle Page-Nummer |
volatile uint8_t display_mode; // Modus für State-Machine |
volatile uint8_t LCD_ORIENTATION = 0; // 0=Normal / 1=reversed |
// DOG: 128 x 64 with 6x8 Font => 21 x 8 |
// MAX7456: 30 x 16 |
uint8_t lcd_xpos; |
uint8_t lcd_ypos; |
char s[7]; |
#define PRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer lcdx_printf_at() / lcdx_printf_at_P() |
char printf_buffer[PRINTF_BUFFER_SIZE]; |
char format_buffer[20]; |
//----------------------------------------------------------- |
void send_byte( uint8_t data ) |
{ |
clr_cs (); |
SPDR = data; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
// * Writes one command byte |
// * cmd - the command byte |
// |
void lcd_command( uint8_t cmd ) |
{ |
// LCD_SELECT(); |
// LCD_CMD(); |
// spi_write(cmd); |
// LCD_UNSELECT(); |
clr_cs (); |
SPDR = cmd; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_set_contrast( uint8_t value ) |
{ |
cli(); |
clr_A0(); |
send_byte( 0x81 ); |
send_byte( value ); // Daten zum LCD senden |
set_A0(); |
sei(); |
} |
//----------------------------------------------------------- |
void lcd_cls( void ) |
{ |
uint16_t i, j; |
memset( (void *)display_buffer, 0, 1024); |
// for (i = 0; i < DISP_BUFFER; i++) |
// display_buffer[i] = 0x00; |
for (i = 0; i < 8; i++) |
{ |
clr_A0 (); |
send_byte (0xB0 + i); //1011xxxx |
send_byte (0x10); //00010000 |
// send_byte(0x04); //00000100 gedreht plus 4 Byte |
// send_byte(0x00); //00000000 |
send_byte (LCD_ORIENTATION); //00000000 |
set_A0 (); |
for (j = 0; j < 128; j++) |
send_byte (0x00); |
} |
lcd_xpos = 0; |
lcd_ypos = 0; |
} |
//----------------------------------------------------------- |
void set_adress (uint16_t adress, uint8_t data) |
{ |
uint8_t page; |
uint8_t column; |
page = adress >> 7; |
clr_A0 (); |
send_byte (0xB0 + page); |
column = (adress & 0x7F) + LCD_ORIENTATION; |
send_byte (0x10 + (column >> 4)); |
send_byte (column & 0x0F); |
set_A0 (); |
send_byte (data); |
} |
//----------------------------------------------------------- |
void scroll (void) |
{ |
uint16_t adress; |
for (adress = 0; adress < 896; adress++) |
{ |
display_buffer[adress] = display_buffer[adress + 128]; |
set_adress (adress, display_buffer[adress]); |
} |
for (adress = 896; adress < 1024; adress++) |
{ |
display_buffer[adress] = 0; |
set_adress (adress, 0); |
} |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
// + Plot (set one Pixel) |
//----------------------------------------------------------- |
// mode: |
// 0=Clear, 1=Set, 2=XOR |
void lcd_plot (uint8_t xpos, uint8_t ypos, uint8_t mode) |
{ |
uint16_t adress; |
uint8_t mask; |
if ((xpos < DISP_W) && (ypos < DISP_H)) |
{ |
adress = (ypos / 8) * DISP_W + xpos; // adress = 0/8 * 128 + 0 = 0 |
mask = 1 << (ypos & 0x07); // mask = 1<<0 = 1 |
adress &= DISP_BUFFER - 1; |
switch (mode) |
{ |
case 0: |
display_buffer[adress] &= ~mask; |
break; |
case 1: |
display_buffer[adress] |= mask; |
break; |
case 2: |
display_buffer[adress] ^= mask; |
break; |
} |
set_adress (adress, display_buffer[adress]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t ch; |
uint16_t adress; |
uint8_t x1,y1; |
uint8_t x0,y0; |
uint8_t xw; |
uint8_t mask; |
uint8_t bit; |
uint8_t *font; |
//------------------------ |
// char translate |
//------------------------ |
switch (c) |
{ // ISO 8859-1 |
case 0xc4: c = 0x01; break; // Ä |
case 0xe4: c = 0x02; break; // ä |
case 0xd6: c = 0x03; break; // Ö |
case 0xf6: c = 0x04; break; // ö |
case 0xdc: c = 0x05; break; // Ü |
case 0xfc: c = 0x06; break; // ü |
case 0xdf: c = 0x1e; break; // ß c = 0x07; ° (used by Jeti) |
} |
c &= 0x7f; |
//------------------------ |
// Font Parameter setzen |
//------------------------ |
#ifdef USE_FONT_BIG |
if( mode <=2 ) // normaler font (8x6) |
{ |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
} |
else // grosser font (8x8) |
{ |
font = (uint8_t *) &Font8x8[0][0]; |
xw = 8; |
} |
#else |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
#endif |
//------------------------ |
//------------------------ |
x0 = (x*xw) + xoffs; |
y0 = (y*8) + yoffs; |
if( yoffs == 0) |
{ |
//---------------------------------------------------------- |
// orginaler Character Algo |
// |
// funktioniert auch mit x Verschiebung aber nicht |
// mit y Verschiebung. |
// |
// Da 8 Bit aufeinmal gesetzt werden ist dieser Algo |
// bzgl. Geschwindigkeit effektiver als der Y-Algo. |
//---------------------------------------------------------- |
if( mode==MINVERS || mode==MBIGINVERS ) |
lcd_frect( (x*xw)+xoffs, (y*8), xw-1, 7, 1); // invertierte Darstellung |
adress = y * 128 + x * xw + xoffs; |
adress &= 0x3FF; |
for( x1 = 0; x1 < xw; x1++) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
if( mode==MINVERS || mode==MBIGINVERS ) |
display_buffer[adress + x1] ^= ch; |
else |
display_buffer[adress + x1] = ch; |
set_adress (adress + x1, display_buffer[adress + x1]); |
} |
} |
else |
{ |
//---------------------------------------------------------- |
// Y-Algo |
// neuer Character Algo (nur wenn Pixel y-Verschiebung) |
//---------------------------------------------------------- |
for( x1 = 0; x1 < xw; x1++ ) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
mask = 1; |
for( y1 = 0; y1 < 8; y1++ ) |
{ |
bit = (ch & mask); |
if( bit ) |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 0 : 1) ); |
else |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 1 : 0) ); |
mask = (mask << 1); |
} |
} |
} |
} |
//----------------------------------------------------------- |
//--- Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode ) |
{ |
lcdx_putc( x, y, c, mode, 0,0 ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w) |
{ |
uint8_t lcd_width; |
uint8_t lcd_zpos; |
uint8_t i; |
uint8_t max = 21; |
lcd_width = w; |
lcd_xpos = x; |
lcd_ypos = y; |
if ((lcd_xpos + lcd_width) > max) |
lcd_width = max - lcd_xpos; |
lcd_zpos = lcd_xpos + lcd_width; |
for (i = lcd_xpos; i < lcd_zpos; i++) |
lcd_putc (i, lcd_ypos, 0x20, 0); |
} |
//----------------------------------------------------------- |
void wait_1ms (void) |
{ |
_delay_ms (1); |
} |
//----------------------------------------------------------- |
void wait_ms (uint16_t time) |
{ |
uint16_t i; |
for (i = 0; i < time; i++) |
wait_1ms (); |
} |
//----------------------------------------------------------- |
void LCD_Init( uint8_t LCD_Mode ) // LCD_Mode 0= Default Mode 1= EEPROM-Parameter) |
{ |
lcd_xpos = 0; |
lcd_ypos = 0; |
// DDRB = 0xFF; |
// SPI max. speed |
// the DOGM128 lcd controller can work at 20 MHz |
SPCR = (1 << SPE) | (1 << MSTR) | (1 << CPHA) | (1 << CPOL); |
SPSR = (1 << SPI2X); |
set_cs(); |
clr_reset(); |
wait_ms(10); |
set_reset(); |
clr_cs(); |
clr_A0(); |
send_byte( 0x40 ); //Display start line = 0 |
//------------------------------------------------------------ |
// 10.11.2014 cebra |
// Umschaltung der Displayansicht wird nicht mehr benötigt |
// if (LCD_Mode == 1) |
// { |
// if (LCD_ORIENTATION == 0) |
// { |
// send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
// else |
// { |
// send_byte( 0xA0 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC8 ); // C0 normal C8 reverse(original) |
// } |
// } |
// else |
// { |
send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
/* |
//------------------------------------------------------------ |
// 26.05.2014 OG: einstellen von Normal/Invers durch den User |
// nicht mehr unterstuetzt, da der Modus Invers beim PKT |
// auf verschiedenen Display's nicht gut aussieht. |
// Ab jetzt nur noch LC-Modus Normal. |
//------------------------------------------------------------ |
if (LCD_Mode == 1) |
{ |
if (Config.LCD_DisplayMode == 0) |
send_byte (0xA6); //Display normal, not mirrored |
else |
send_byte (0xA7); //Display reverse, not mirrored |
} |
else |
send_byte (0xA6); |
*/ |
send_byte( 0xA6 ); //Display normal, not mirrored |
send_byte( 0xA2 ); //Set bias 1/9 (Duty 1/65) |
send_byte( 0x2F ); //Booster, regulator and follower on |
send_byte( 0xF8 ); //Set internal booster to 4x |
send_byte( 0x00 ); //Set internal booster to 4x |
send_byte( 0x27 ); //resistor ratio set |
if( LCD_Mode == 1 ) |
{ |
send_byte( 0x81 ); //Electronic volume register set |
send_byte( Config.LCD_Kontrast ); //Electronic volume register set |
} |
else |
{ |
send_byte( 0x81 ); |
send_byte( 0x16 ); |
} |
send_byte( 0xAC ); //Cursor |
send_byte( 0x00 ); //No Cursor |
send_byte( 0xAF ); //No indicator |
if( Config.HWSound == 0 ) |
{ |
if( LCD_Mode == 1 ) |
{ |
//------------------------------------------------------------ |
//10.11.2014 cebra, |
//Helligkeitseinstellungen werden nicht mehr genutzt |
// Helligkeit setzen |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
//OCR2A = 255; |
} |
else |
{ |
// OCR2A = 255; |
} |
} |
lcd_cls(); |
} |
//----------------------------------------------------------- |
// sicher eine Zeile für die Statusanzeige |
void copy_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
line_buffer[i] = display_buffer[adress+i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
// holt gesicherte Zeile wieder zurück |
void paste_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
display_buffer[adress+i] =line_buffer[i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
while (*s) |
{ |
lcdx_putc(x, y, *s++, mode, xoffs,yoffs); |
x++; |
} |
}/* lcd_puts */ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode ) |
{ |
lcdx_puts_at( x, y, s, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void new_line (void) |
{ |
lcd_ypos++; |
if (lcd_ypos > 7) |
{ |
scroll (); |
lcd_ypos = 7; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns (const char *text, uint8_t mode) |
{ |
while (pgm_read_byte(text)) |
{ |
switch (pgm_read_byte(text)) |
{ |
case 0x0D: |
lcd_xpos = 0; |
break; |
case 0x0A: |
new_line(); |
break; |
default: |
lcd_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode); |
lcd_xpos++; |
if (lcd_xpos > 21) |
{ |
lcd_xpos = 0; |
// new_line (); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_printpns (text, mode); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// erweitert bei mode MINVERSX und MNORMALX links und oben den |
// Text um jeweils einen Pixel -> der Invers-Modus sieht besser aus |
//-------------------------------------------------------------- |
uint8_t _lcdx_print_modeextend( uint8_t progmem, uint8_t x, uint8_t y, uint8_t textlen, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t draw = 0; |
if( (mode == MNORMALX) || (mode == MINVERSX) ) |
{ |
if( mode == MNORMALX ) mode = MNORMAL; |
else mode = MINVERS; |
if( mode == MINVERS ) |
draw = 1; |
if( (y*8)+yoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs, (y*8)+yoffs-1, (x*6)+xoffs+(textlen*6)-1, (y*8)+yoffs-1, draw); // horizontale Linie ueber dem Text |
if( (x*6)+xoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs-1, (y*8)+yoffs-1, (x*6)+xoffs-1, (y*8)+yoffs+7, draw); // vertikale Linie links neben dem Text |
} |
return mode; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void _lcdx_print_outchar( unsigned char c, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc( lcd_xpos, lcd_ypos, c, mode, xoffs,yoffs ); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_modeextend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
_lcdx_print_outchar( *text, mode, xoffs,yoffs); |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
char c; |
mode = _lcdx_print_modeextend( true, lcd_xpos, lcd_ypos, strlen_P( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( (c = pgm_read_byte(text)) ) |
{ |
_lcdx_print_outchar( c, mode, xoffs,yoffs); |
text++; |
} |
} |
/* |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
switch( *text ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, *text, mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( true, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( pgm_read_byte(text) ) |
{ |
switch( pgm_read_byte(text) ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
*/ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp (const char *text, uint8_t mode) |
{ |
lcdx_printp ( text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_printp (text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcdx_printp_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//--- lcd_print: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print (uint8_t *text, uint8_t mode ) |
{ |
lcdx_print (text, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_print (text, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen((const char *)text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_print_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen_P(text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_printp_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center_P( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//--- lcd_print_at: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode ) |
{ |
lcdx_print_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
void print_display (uint8_t *text) |
{ |
while (*text) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, *text, 0); |
lcd_xpos++; |
if (lcd_xpos >= 20) |
{ |
lcd_xpos = 0; |
new_line (); |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
print_display (text); |
} |
//----------------------------------------------------------- |
// + Line (draws a line from x1,y1 to x2,y2 |
// + Based on Bresenham line-Algorithm |
// + found in the internet, modified by thkais 2007 |
//----------------------------------------------------------- |
void lcd_line( unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode) |
{ |
int x, y, count, xs, ys, xm, ym; |
x = (int) x1; |
y = (int) y1; |
xs = (int) x2 - (int) x1; |
ys = (int) y2 - (int) y1; |
if (xs < 0) |
xm = -1; |
else |
if (xs > 0) |
xm = 1; |
else |
xm = 0; |
if (ys < 0) |
ym = -1; |
else |
if (ys > 0) |
ym = 1; |
else |
ym = 0; |
if (xs < 0) |
xs = -xs; |
if (ys < 0) |
ys = -ys; |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
if (xs > ys) // Flat Line <45 degrees |
{ |
count = -(xs / 2); |
while (x != x2) |
{ |
count = count + ys; |
x = x + xm; |
if (count > 0) |
{ |
y = y + ym; |
count = count - xs; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
else // Line >=45 degrees |
{ |
count =- (ys / 2); |
while (y != y2) |
{ |
count = count + xs; |
y = y + ym; |
if (count > 0) |
{ |
x = x + xm; |
count = count - ys; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
} |
//----------------------------------------------------------- |
// + Filled rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_frect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
uint16_t i; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
for (i = y1; i <= y2; i++) |
{ |
lcd_line (x1, i, x2, i, mode); |
} |
} |
//----------------------------------------------------------- |
// ausgefuelltes Rechteck mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_frect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_rect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
lcd_line (x1, y1, x2, y1, mode); |
lcd_line (x2, y1, x2, y2, mode); |
lcd_line (x2, y2, x1, y2, mode); |
lcd_line (x1, y2, x1, y1, mode); |
} |
//----------------------------------------------------------- |
// Rechteck mit mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_rect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+1 , y1+1 , mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1+1 , mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+1 , y1+widthy-1, mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
lcd_plot( x1+widthx-1, y1+widthy-1, mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of a circle |
// + Based on Bresenham-algorithm found in wikipedia |
// + modified by thkais (2007) |
//----------------------------------------------------------- |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_plot (x0, y0 + radius, mode); |
lcd_plot (x0, y0 - radius, mode); |
lcd_plot (x0 + radius, y0, mode); |
lcd_plot (x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y --; |
ddF_y += 2; |
f += ddF_y; |
} |
x ++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
lcd_plot (x0 + y, y0 + x, mode); |
lcd_plot (x0 - y, y0 + x, mode); |
lcd_plot (x0 + y, y0 - x, mode); |
lcd_plot (x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// + filled Circle |
// + modified circle-algorithm thkais (2007) |
//----------------------------------------------------------- |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius,uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_line (x0, y0 + radius, x0, y0 - radius, mode); |
lcd_line (x0 + radius, y0, x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y--; |
ddF_y += 2; |
f += ddF_y; |
} |
x++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_line (x0 + x, y0 + y, x0 - x, y0 + y, mode); |
lcd_line (x0 + x, y0 - y, x0 - x, y0 - y, mode); |
lcd_line (x0 + y, y0 + x, x0 - y, y0 + x, mode); |
lcd_line (x0 + y, y0 - x, x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) r); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) r); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) ry); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) rx); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode) |
{ |
const int16_t rx2 = rx * rx; |
const int16_t ry2 = ry * ry; |
int16_t F = round (ry2 - rx2 * ry + 0.25 * rx2); |
int16_t ddF_x = 0; |
int16_t ddF_y = 2 * rx2 * ry; |
int16_t x = 0; |
int16_t y = ry; |
lcd_plot (x0, y0 + ry, mode); |
lcd_plot (x0, y0 - ry, mode); |
lcd_plot (x0 + rx, y0, mode); |
lcd_plot (x0 - rx, y0, mode); |
// while ( 2*ry2*x < 2*rx2*y ) { we can use ddF_x and ddF_y |
while (ddF_x < ddF_y) |
{ |
if(F >= 0) |
{ |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F -= ddF_y; |
} |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x + ry2; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
F = round (ry2 * (x + 0.5) * (x + 0.5) + rx2 * (y - 1) * (y - 1) - rx2 * ry2); |
while(y > 0) |
{ |
if(F <= 0) |
{ |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x; |
} |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F += rx2 - ddF_y; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
lcd_ellipse (x0, y0, radius + 3, radius, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
lcd_ellipse_line(x, y, r + 3, r, deg, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_view_font (uint8_t page) |
{ |
int x; |
int y; |
lcd_cls (); |
lcd_printp (PSTR(" 0123456789ABCDEF\r\n"), 0); |
lcd_printpns_at (0, 7, PSTR(" \x1a \x1b Exit"), 0); |
lcd_ypos = 2; |
for (y = page * 4 ; y < (page * 4 + 4); y++) |
{ |
if (y < 10) |
{ |
lcd_putc (0, lcd_ypos, '0' + y, 0); |
} |
else |
{ |
lcd_putc (0, lcd_ypos, 'A' + y - 10, 0); |
} |
lcd_xpos = 2; |
for (x = 0; x < 16; x++) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, y * 16 + x, 0); |
lcd_xpos++; |
} |
lcd_ypos++; |
} |
} |
//----------------------------------------------------------- |
uint8_t hdigit (uint8_t d) |
{ |
if (d < 10) |
{ |
return '0' + d; |
} |
else |
{ |
return 'A' + d - 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos, lcd_ypos, hdigit (h & 0x0f), mode); |
} |
//----------------------------------------------------------- |
void lcd_print_hex (uint8_t h, uint8_t mode) |
{ |
// lcd_xpos = x; |
// lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h & 0x0f), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, ' ', mode); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//----------------------------------------------------------- |
// lcd_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
void lcd_write_number_u (uint8_t number) |
{ |
uint8_t num = 100; |
uint8_t started = 0; |
while (num > 0) |
{ |
uint8_t b = number / num; |
if (b > 0 || started || num == 1) |
{ |
lcd_putc (lcd_xpos++, lcd_ypos, '0' + b, 0); |
started = 1; |
} |
number -= b * num; |
num /= 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_write_number_u (number); |
} |
//----------------------------------------------------------- |
// numtype: 'd' oder 'u' |
// |
// Ergebnis: ein String in format_buffer |
// -> "%4d" "%4.2d" "%03.1d" "%4u" usw... |
//----------------------------------------------------------- |
void make_number_format( char numtype, int16_t length, int16_t decimals, uint8_t pad ) |
{ |
register char *p; |
register char *psrc; |
p = format_buffer; |
*p = '%'; p++; // start '%' |
if(pad) { *p = '0'; p++; } // pad '0' |
itoa( length, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // vorkomma |
if( decimals > 0 ) |
{ |
*p = '.'; p++; // punkt '.' |
itoa( decimals, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // nachkomma |
} |
*p = numtype; p++; // 'd' oder 'u' |
*p = 0; |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s (x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u_10th( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-2, 2, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad) |
{ |
writex_ndigit_number_u_100th( x, y, number, length, pad, MNORMAL, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s_10th ( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// write <seconds> as human readable time at <x>/<y> to MAX7456 display mem |
// |
void writex_time( uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
uint16_t min = seconds / 60; |
seconds -= min * 60; |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u", min, seconds); |
/* |
writex_ndigit_number_u (x, y, min, 2, 0,mode, xoffs,yoffs); |
lcdx_putc (x + 2, y, ':', mode, xoffs,yoffs); |
writex_ndigit_number_u (x + 3, y, seconds, 2, 1,mode, xoffs,yoffs); |
*/ |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_time( uint8_t x, uint8_t y, uint16_t seconds) |
{ |
writex_time( x, y, seconds, 0, 0,0); |
} |
//-------------------------------------------------------------- |
// writex_datetime_time() |
// |
// Anzeigeformat: 'hh:mm:ss' |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u:%02u", (uint8_t)(dtlocal.seconds/3600), (uint8_t)((dtlocal.seconds/60)%60), (uint8_t)(dtlocal.seconds%60)); |
} |
//-------------------------------------------------------------- |
// writex_datetime_date() |
// |
// Anzeigeformat: 'dd.mm.yyyy' (keine Unterstuetzung von anderen Formaten aus aller Welt) |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
if( datetime.year > 0 ) |
{ |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u.%02u.%04u", dtlocal.day, dtlocal.month, dtlocal.year); |
} |
else |
{ |
// keine UTCZeit -> keine Datumsanzeige |
lcdx_printp_at( x, y, PSTR(" . . "), mode, xoffs,yoffs); |
} |
} |
//----------------------------------------------------------- |
// writes a single gps-pos |
//----------------------------------------------------------- |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%3.5ld", GPSpos/100); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Show_PKTError_NoRAM(void) |
{ |
lcd_cls(); |
lcd_rect( 8, 8, 127-16, 6*8, 1); |
// 123456789012345678901 |
lcd_printp_at( 2,2 , PSTR("** OUT OF RAM **"), MINVERS); |
lcd_printp_at( 2,4 , PSTR("this function is"), MNORMAL); |
lcd_printp_at( 2,5 , PSTR("not available!"), MNORMAL); |
while ( !( (get_key_press (1 << KEY_ENTER)) || (get_key_press (1 << KEY_ESC)) || (get_key_press (1 << KEY_PLUS)) || (get_key_press (1 << KEY_MINUS)) ) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ShowTitle_P( const char *title, uint8_t clearscreen ) |
{ |
if( clearscreen ) |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( strlen_P(title) < 17 ) |
show_Lipo(); |
lcd_printp_at ( 1, 0, title , MINVERS); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void Popup_Draw( uint8_t heigthC ) |
{ |
uint8_t y, h; |
h = heigthC*8; |
y = 63-h; |
//lcd_frect( 0, (hC*8)-6, 127, 63-(hC*8)+6, 1); // Box white |
lcd_frect( 0, y-8, 127, 3, 0); // Box clear |
lcd_frect( 0, y-4, 127, h+4, 1); // Box fill white |
lcd_plot ( 0, y-4, 0); // Ecke links oben 1 |
lcd_plot ( 127, y-4, 0); // Ecke rechts oben 1 |
lcd_line ( 1, y-4, 0, y-3, 0); // Ecke links oben 2 |
lcd_line ( 127-1, y-4, 127, y-3, 0); // Ecke rechts oben 2 |
} |
//----------------------------------------------------------- |
// lcdx_cls_rowwidth( y, width, mode, xoffs,yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
// |
// width: in Zeichen |
//----------------------------------------------------------- |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
int8_t xadd = 0; |
int8_t yadd = 0; |
if( mode == MNORMALX || mode == MINVERSX ) |
{ |
if( xoffs > 0 ) xadd = 1; |
if( (y*8)+yoffs > 0 ) yadd = 1; |
if( mode == MNORMALX ) |
mode = MNORMAL; |
else |
mode = MINVERS; |
} |
lcd_frect( xoffs-xadd, (y*8)+yoffs-yadd, (width*6)+xadd, 7+yadd, (mode == MNORMAL ? 0 : 1) ); // Zeile loeschen |
} |
//----------------------------------------------------------- |
// lcdx_cls_row( y, mode, yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
//----------------------------------------------------------- |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_rowwidth( y, 21, mode, 0, yoffs ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_setpos( uint8_t x, uint8_t y ) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_print_char( uint8_t c, uint8_t mode ) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, c, mode, 0,0); |
lcd_xpos++; |
if( lcd_xpos > 20 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//---------------------------------------------------- |
// gibt einen Linefeed aus (CR+LF) |
//---------------------------------------------------- |
void lcd_print_LF( void ) |
{ |
lcd_print_char( 0x0D, MNORMAL ); |
lcd_print_char( 0x0A, MNORMAL ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd/lcd.h |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* Thanks to Oliver Schwaneberg for adding several functions to this library!* |
* * |
* Author: Jan Michel (jan at mueschelsoft dot de) * |
* License: GNU General Public License, version 3 * |
* Version: v0.93 September 2010 * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.h |
//# |
//# 27.06.2014 OG |
//# - add: define MINVERSX, MNORMALX |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: R# define's fuer lcd_rect_round() |
//# |
//# 12.02.2014 OG |
//# - add: lcd_frect_round() |
//# - add: lcd_rect_round() |
//# |
//# 03.02.2014 OG |
//# - fix: bei writex_ndigit_number_u_100th() fehlte der Parameter 'mode' |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: define SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 28.04.2013 OG |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# - del: write_gps_pos() |
//# |
//# 22.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 11.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 07.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 06.03.2013 OG |
//# - add: extended Funktionen lcdx_... / writex... |
//############################################################################ |
#ifndef _LCD_H |
#define _LCD_H |
#include <stdarg.h> // Notwendig! (OG) |
#include "../timer/timer.h" |
#define MNORMAL 0 // Zeichendarstellung: Normal |
#define MINVERS 2 // Zeichendarstellung: Invers |
#define MBIG 3 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Normal |
#define MBIGINVERS 4 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Invers |
#define MNORMALX 5 // Zeichendarstellung: Normal - oben und Links wird eine 1 Pixel Linie gezogen |
#define MINVERSX 6 // Zeichendarstellung: Invers - oben und Links wird eine 1 Pixel Linie gezogen |
// fuer lcd_rect_round() |
#define R0 0 // Radius 0 |
#define R1 1 // Radius 1 |
#define R2 2 // Radius 2 |
#define SYMBOL_AVG 10 |
#define SYMBOL_MAX 13 |
#define SYMBOL_MIN 16 |
#define SYMBOL_SMALLDEGREE 11 |
#define SYMBOL_BIGDEGREE 30 |
#define SYMBOL_CHECK 31 |
//------------------------------------------------------------------------------------ |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
//------------------------------------------------------------------------------------ |
// X-tended |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcdx_print (uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp (const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_puts_at(uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_time (uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs); |
//void writex_gpspos( uint8_t x, uint8_t y, uint32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void Show_PKTError_NoRAM(void); |
void ShowTitle_P( const char *title, uint8_t clearscreen ); |
//------------------------------------------------------------------------------------ |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ); |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcd_setpos( uint8_t x, uint8_t y ); |
void lcd_print_char( uint8_t c, uint8_t mode ); |
void lcd_print_LF( void ); |
void Popup_Draw( uint8_t heigthC ); |
/* |
//----------------------------------------------------------------------------- |
// Command Codes |
//----------------------------------------------------------------------------- |
//1: Display on/off |
#define LCD_DISPLAY_ON 0xAF //switch display on |
#define LCD_DISPLAY_OFF 0xAE //switch display off |
//2: display start line set (lower 6 bits select first line on lcd from 64 lines in memory) |
#define LCD_START_LINE 0x40 |
//3: Page address set (lower 4 bits select one of 8 pages) |
#define LCD_PAGE_ADDRESS 0xB0 |
//4: column address (lower 4 bits are upper / lower nibble of column address) |
#define LCD_COL_ADDRESS_MSB 0x10 |
#define LCD_COL_ADDRESS_LSB 0x00 //second part of column address |
//8: select orientation (black side of the display should be further away from viewer) |
#define LCD_BOTTOMVIEW 0xA1 //6 o'clock view |
#define LCD_TOPVIEW 0xA0 //12 o'clock view |
//9: select normal (white background, black pixels) or reverse (black background, white pixels) mode |
#define LCD_DISPLAY_POSITIVE 0xA6 //not inverted mode |
#define LCD_DISPLAY_INVERTED 0xA7 //inverted display |
//10: show memory content or switch all pixels on |
#define LCD_SHOW_NORMAL 0xA4 //show dram content |
#define LCD_SHOW_ALL_POINTS 0xA5 //show all points |
//11: lcd bias set |
#define LCD_BIAS_1_9 0xA2 |
#define LCD_BIAS_1_7 0xA3 |
//14: Reset Controller |
#define LCD_RESET_CMD 0xE2 |
//15: output mode select (turns display upside-down) |
#define LCD_SCAN_DIR_NORMAL 0xC0 //normal scan direction |
#define LCD_SCAN_DIR_REVERSE 0xC8 //reversed scan direction |
//16: power control set (lower 3 bits select operating mode) |
//Bit 0: Voltage follower on/off - Bit 1: Voltage regulator on/off - Bit 2: Booster circuit on/off |
#define LCD_POWER_CONTROL 0x28 //base command |
#define LCD_POWER_LOW_POWER 0x2F |
#define LCD_POWER_WIDE_RANGE 0x2F |
#define LCD_POWER_LOW_VOLTAGE 0x2B |
//17: voltage regulator resistor ratio set (lower 3 bits select ratio) |
//selects lcd voltage - 000 is low (~ -2V), 111 is high (~ - 10V), also depending on volume mode. Datasheet suggests 011 |
#define LCD_VOLTAGE 0x20 |
//18: Volume mode set (2-byte command, lower 6 bits in second word select value, datasheet suggests 0x1F) |
#define LCD_VOLUME_MODE_1 0x81 |
#define LCD_VOLUME_MODE_2 0x00 |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
//19: static indicator (2-byte command), first on/off, then blinking mode |
#define LCD_INDICATOR_ON 0xAD //static indicator on |
#define LCD_INDICATOR_OFF 0xAC //static indicator off |
#define LCD_INDICATOR_MODE_OFF 0x00 |
#define LCD_INDICATOR_MODE_1HZ 0x01 |
#define LCD_INDICATOR_MODE_2HZ 0x10 |
#define LCD_INDICATOR_MODE_ON 0x11 |
//20: booster ratio set (2-byte command) |
#define LCD_BOOSTER_SET 0xF8 //set booster ratio |
#define LCD_BOOSTER_234 0x00 //2x-4x |
#define LCD_BOOSTER_5 0x01 //5x |
#define LCD_BOOSTER_6 0x03 //6x |
//#endif |
//22: NOP command |
#define LCD_NOP 0xE3 |
//#if DISPLAY_TYPE == 102 |
////25: advanced program control |
//#define LCD_ADV_PROG_CTRL 0xFA |
//#define LCD_ADV_PROG_CTRL2 0x10 |
//#endif |
//----------------------------------------------------------------------------- |
// Makros to execute commands |
//----------------------------------------------------------------------------- |
#define LCD_SWITCH_ON() lcd_command(LCD_DISPLAY_ON) |
#define LCD_SWITCH_OFF() lcd_command(LCD_DISPLAY_OFF) |
#define LCD_SET_FIRST_LINE(i) lcd_command(LCD_START_LINE | ((i) & 0x3F)) |
#define LCD_SET_PAGE_ADDR(i) lcd_command(LCD_PAGE_ADDRESS | ((i) & 0x0F)) |
#define LCD_SET_COLUMN_ADDR(i) lcd_command(LCD_COL_ADDRESS_MSB | ((i>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((i) & 0x0F)) |
#define LCD_GOTO_ADDRESS(page,col); lcd_command(LCD_PAGE_ADDRESS | ((page) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_MSB | ((col>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((col) & 0x0F)); |
#define LCD_SET_BOTTOM_VIEW() lcd_command(LCD_BOTTOMVIEW) |
#define LCD_SET_TOP_VIEW() lcd_command(LCD_TOPVIEW) |
#define LCD_SET_MODE_POSITIVE() lcd_command(LCD_DISPLAY_POSITIVE) |
#define LCD_SET_MODE_INVERTED() lcd_command(LCD_DISPLAY_INVERTED) |
#define LCD_SHOW_ALL_PIXELS_ON() lcd_command(LCD_SHOW_ALL_POINTS) |
#define LCD_SHOW_ALL_PIXELS_OFF() lcd_command(LCD_SHOW_NORMAL) |
#define LCD_SET_BIAS_RATIO_1_7() lcd_command(LCD_BIAS_1_7) |
#define LCD_SET_BIAS_RATIO_1_9() lcd_command(LCD_BIAS_1_9) |
#define LCD_SEND_RESET() lcd_command(LCD_RESET_CMD) |
#define LCD_ORIENTATION_NORMAL() lcd_command(LCD_SCAN_DIR_NORMAL) |
#define LCD_ORIENTATION_UPSIDEDOWN() lcd_command(LCD_SCAN_DIR_REVERSE) |
#define LCD_SET_POWER_CONTROL(i) lcd_command(LCD_POWER_CONTROL | ((i) & 0x07)) |
#define LCD_SET_LOW_POWER() lcd_command(LCD_POWER_LOW_POWER) |
#define LCD_SET_WIDE_RANGE() lcd_command(LCD_POWER_WIDE_RANGE) |
#define LCD_SET_LOW_VOLTAGE() lcd_command(LCD_POWER_LOW_VOLTAGE) |
#define LCD_SET_BIAS_VOLTAGE(i) lcd_command(LCD_VOLTAGE | ((i) & 0x07)) |
#define LCD_SET_VOLUME_MODE(i) lcd_command(LCD_VOLUME_MODE_1); \ |
lcd_command(LCD_VOLUME_MODE_2 | ((i) & 0x3F)) |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
#define LCD_SET_INDICATOR_OFF() lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_INDICATOR_MODE_OFF) |
#define LCD_SET_INDICATOR_STATIC() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_ON) |
#define LCD_SET_INDICATOR_1HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_1HZ) |
#define LCD_SET_INDICATOR_2HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_2HZ) |
#define LCD_SET_INDICATOR(i,j) lcd_command(LCD_INDICATOR_OFF | ((i) & 1)); \ |
lcd_command(((j) & 2)) |
#define LCD_SLEEP_MODE lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_DISPLAY_OFF); \ |
lcd_command(LCD_SHOW_ALL_POINTS) |
//#endif |
//#if DISPLAY_TYPE == 102 |
//#define LCD_TEMPCOMP_HIGH 0x80 |
//#define LCD_COLWRAP 0x02 |
//#define LCD_PAGEWRAP 0x01 |
//#define LCD_SET_ADV_PROG_CTRL(i) lcd_command(LCD_ADV_PROG_CTRL); |
// lcd_command(LCD_ADV_PROG_CTRL2 & i) |
//#endif |
*/ |
extern volatile uint8_t LCD_ORIENTATION; |
//#define LCD_LINES 8 |
//#define LCD_COLS 21 |
extern uint8_t lcd_xpos; |
extern uint8_t lcd_ypos; |
void lcd_set_contrast( uint8_t value ); |
void lcd_command(uint8_t cmd); |
void send_byte (uint8_t data); |
void LCD_Init (uint8_t LCD_Mode); |
void new_line (void); |
void lcd_puts_at(uint8_t x, uint8_t y,const char *s, uint8_t mode ); |
void lcd_putc (uint8_t x, uint8_t y, uint8_t c, uint8_t mode); |
void send_byte (uint8_t data); |
void lcd_print (uint8_t *text, uint8_t mode); |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode); |
void lcd_printp (const char *text, uint8_t mode); |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_printpns (const char *text, uint8_t mode); |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_cls (void); |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w); |
void print_display (uint8_t *text); |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text); |
void copy_line (uint8_t y); |
void paste_line (uint8_t y); |
void lcd_plot (uint8_t x, uint8_t y, uint8_t mode); |
void lcd_line (unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode); |
void lcd_rect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_frect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode); |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode); |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_view_font (uint8_t page); |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode); |
void lcd_write_number_u (uint8_t number); |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number); |
void lcd_print_hex (uint8_t h, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad,uint8_t mode); |
/** |
* Write only some digits of a signed <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /100th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_100th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad); |
/** |
* Write only some digits of a signed <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* write <seconds> as human readable time at <x>/<y> |
*/ |
void write_time (uint8_t x, uint8_t y, uint16_t seconds); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lcd |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lipo/lipo.c |
---|
0,0 → 1,178 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lipo.c |
//# |
//# 15.06.2014 OG |
//# - fix; show_Lipo() - wenn die Spannung unter 3.20 Volt ging, wurde wieder |
//# ein voller Akkubalken angezeigt aufgrund einer Subtraktion - fixed |
//# |
//# 21.02.2014 OG |
//# - chg: show_Lipo() neu geschrieben - etwas smarter, ein paar Bytes kleiner |
//# |
//# 20.02.2014 OG |
//# - chg: Codeformattierungen |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "lipo.h" |
#include "../eeprom/eeprom.h" |
// Global variables |
double accumulator = 0; //!< Accumulated 10-bit samples |
double Vin = 0; //!< 16-bit float number result |
short temp = 0; //!< Temporary variable |
short samples = 0; //!< Number of conversions |
uint16_t volt_avg = 0; |
//-------------------------------------------------------------- |
//! ADC interrupt routine |
//-------------------------------------------------------------- |
ISR (ADC_vect) |
{ |
accumulator += ADCW; |
samples++; |
if( samples>4095 ) |
{ |
oversampled(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ADC_Init (void) |
{ |
DDRA &= ~(1<<DDA1); // MartinR |
PORTA &= ~(1<<PORTA1); // MartinR |
ADMUX = (0<<REFS1) | (1<<REFS0); // externe 5V Referenzspannung nutzen |
ADMUX = (ADMUX & ~(0x1F)) | (1 & 0x1F); // ADC1 verwenden |
ADCSRA = (1<<ADEN)|(1<<ADIE)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // Prescaler 128, Freilaufend, Interrupte frei |
} |
//-------------------------------------------------------------- |
// Error compensation, Scaling 16-bit result, Rounding up, |
// Calculate 16-bit result, Resets variables |
// |
// Quelle AVR121: Enhancing ADC resolution by versampling |
//-------------------------------------------------------------- |
void oversampled(void) |
{ |
cli(); |
accumulator += Config.Lipo_UOffset; //5150 Offset error compensation |
// accumulator *= 0.9993; // Gain error compensation |
accumulator *= 0.9600; //0.9800 Gain error compensation |
temp=(int)accumulator%64; |
accumulator/=64; // Scaling the answer |
if(temp>=32) |
{ |
accumulator += 1; // Round up |
} |
// Vin = (accumulator/65536)*4.910; // Calculating 16-bit result |
Vin =accumulator/7.5; |
volt_avg = Vin; |
// write_ndigit_number_u(0, 3, Vin, 5, 0,0); |
// write_ndigit_number_u(0, 4, volt_avg, 5, 0,0); |
samples = 0; |
accumulator = 0; |
sei(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void show_Lipo( void ) |
{ |
int16_t w = 0; |
//-------------------------------------------- |
// Batterie Symbol zeichnen |
//-------------------------------------------- |
lcd_rect(104, 0, 23, 7, 1); // Rahmen aussen |
lcd_rect(105, 1, 21, 5, 0); // Rahmen innen |
lcd_frect(102, 0, 1, 7, 0); // ganz links etwas loeschen |
lcd_rect(103,2,1,3,1); // Batterie "Kappe" |
//-------------------------------------------- |
// Batterie Balken berechnen / zeichnen |
//-------------------------------------------- |
// |
// Config.PKT_Accutyp |
// true = LiPO Akku mit max. 4.2 Volt (420) |
// false = LiON Akku mit max. 4.1 Volt (410) |
if( volt_avg > 320 ) |
{ |
w = (volt_avg-320)/(Config.PKT_Accutyp ? 4.8 : 4.5); // die Faktoren 4.8 (kann bis 5.0) und 4.5 fuer LiPo / LiON wurden per Test ermittelt |
//lcdx_printf_at_P( 12, 1, MNORMAL, 0,0, PSTR("v=%3d"), w ); // Debug Anzeige um Faktoren zu ermitteln |
} |
if(w<0) w = 0; |
if(w>20) w = 20; // nicht mehr als 20 Pixel Breite |
if( w>0 ) |
lcd_frect( 106+(20-w), 2, w-1, 3, 1); // Batterie Balken: fill |
if( w<20 ) |
lcd_frect( 106, 2, 19-w, 3, 0); // Batterie Balken: clear |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/lipo/lipo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _LIPO_H |
#define _LIPO_H |
short samples; //!< Number of conversions |
double Vin; |
double accumulator; |
uint16_t volt_avg; |
void ADC_Init (void); |
void oversampled(void); |
void show_Lipo(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/main.c |
---|
0,0 → 1,202 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mk/mksettings.h" geaendert auf "mksettings/mksettings.h" |
//# |
//# 29.03.2014 OG |
//# - chg: MK_Show_LastGPS_Position() ersetzt durch OSDDATA_ShowLastGPSPosition() |
//# |
//# 28.03.2014 OG |
//# - chg: mk_last_position() ersetzt durch MK_Show_LastGPS_Position() (mkbase.c) |
//# - del: mk_last_position() |
//# |
//# 21.02.2014 OG |
//# - chg: mk_show_lastposition() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 19.02.2014 OG |
//# - chg: main() hinzugefuegt: MKSettings_TMP_Init0() |
//# - add: #include "mk/mksettings.h" |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 11.02.2014 OG |
//# - chg: main() Aufruf von searchMK() bzgl. Parametern angepasst |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# |
//# 02.02.2014 OG |
//# - chg: Screen-Layout von der Anzeige der letzten MK-Position und |
//# Ausgliederung davon in mk_show_lastposition() |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# - chg: verschoben: MK_load_setting() und searchMK() nach mk/mkbase.c |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: searchMK(): kuerzerer Fehlerbeep bei falscher FC-Revision und |
//# automatisches beenden des Fehlerscreens nach 4 Sekunden |
//# |
//# 24.01.2014 OG |
//# - fix: searchMK(): wenn MK_load_setting() keine Daten liefert wird keine |
//# falsche RC-Settings-Revisionsnummer mehr angezeigt sondern "ERR!" |
//# - chg: searchMK(): Bestaetigung Fehleranzeige bei falscher FC-Revision |
//# geaendert von KEY_ESC nach KEY_ENTER - Grund: das PKT behandelt |
//# dieses Problem nun flexibler und z.B. OSD (und anderes) geht noch |
//# - chg: searchMK(): beschleunigte Erkennung inkompatibler FC-Settings-Revisionen |
//# - chg: timings in MK_load_setting() |
//# |
//# 07.1.2016 Cebra |
//# - chg: setzen globale Variable für Abfrage WrongFCVersion etwas verändert |
//# |
//# 26.06.2013 OG |
//# - del: searchMK() Layout und Code-Struktur |
//# - fix: searchMK() laden der MK-Settings |
//# - add: MK_load_setting() - wird ggf. spaeter zu einer bessern Stelle verschoben |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 20.05.2013 OG |
//# - chg: searchMK() Code fuer KEY_ESC geaendert |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Aufruf von main_menu() geaendert zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 16.05.2013 Cebra |
//# - chg: Startinfo wird nur noch komplett angezeigt wenn im Setup auf JA |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 07.05.2013 Cebra |
//# - chg: wenn NC vorhanden wird die Version der NC in NCversion gespeichert |
//# sonst die FC-Version in FCversion |
//# |
//# 27.04.2013 OG |
//# - chg: GPS-Positionsausgabe umgestellt auf writex_gpspos() |
//# - chg: einige clear-lines auf lcd_frect() umgestellt um Speicher zu sparen |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
//************************************************************************************ |
// Watchdog integrieren und abschalten, wird für Bootloader benötigt |
// !!muss hier stehen bleiben!! |
//-------------------------------------------------------------- |
void wdt_init(void) __attribute__((naked)) __attribute__((section(".init1"))); |
//-------------------------------------------------------------- |
void wdt_init(void) |
{ |
MCUSR = 0; |
wdt_disable(); |
return; |
} |
//************************************************************************************ |
// erst ab hier weitere #includes |
#include "main.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "mk-data-structs.h" |
#include "menu.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "wi232/Wi232.h" |
#include "motortest/twimaster.h" |
#include "messages.h" |
#include "utils/scrollbox.h" |
#include "pkt/pkt.h" |
#include "lipo/lipo.h" |
#include "mk/mkbase.h" |
#include "mksettings/mksettings.h" |
#include "osd/osddata.h" |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
volatile uint8_t mode = 0; |
uint8_t hardware = 0; |
uint8_t current_hardware = 0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int main(void) |
{ |
InitHWPorts(); // Hardwareanhängige Ports konfigurieren |
// dafür wird je nach Hardware die HAL_HWxxx verwendet |
// Define dazu in der main.h |
hardware = NO; |
current_hardware = 0; |
MKSettings_TMP_Init0(); // TMP-Setting initialisieren |
if( Config.LastLongitude>0x00000000 && Config.LastLatitude>0x00000000 ) |
{ |
OSDDATA_ShowLastGPSPosition(); // letzte MK GPS-Position zeigen |
} |
searchMK(true); // MK suchen (true = MK_Info() anzeigen) |
while(true) |
{ |
Menu_Main(); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/main.h |
---|
0,0 → 1,444 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.h |
//# |
//# 20.09.2015 Starter |
//# - FollowMeStep2 wird jetzt nurnoch von USE_FOLLOWME_STEP2 aktiviert |
//# |
//# 19.09.2015 Cebra |
//# - add: Ordner 10DOF hinzugefügt für die Bearbeitung des GY-87 Sensorboard |
//# Kompass, Gyro, ACC, Luftdruck |
//# |
//# 03.08.2015 Cebra |
//# - add: Define für GPS Debug hinzugefügt für GPS Berechnung FollowMe |
//# - add: #define USE_WAYPOINTS , Code sparen |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85e wegen einigen Änderungen in GPS-Maus Bereich, CRC Erkennung und Weitergabe |
//# |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85d wegen Änderungen in gpsmouse.c |
//# |
//# |
//# 16.07.2015 Cebra |
//# - chg: neue Version 3.85a wegen FC2.11a |
//# |
//# 09.04.2015 Cebra |
//# - chg: neue Version 3.84a wegen FC2.09j |
//# |
//# 05.04.2015 Cebra (PKT383b) |
//# - chg: Änderungen wegen Fehlfunktion mit NC 2.09h |
//# SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) |
//# ergänzt um 2. Parameter |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - chg: Anpassung an FC209d |
//# |
//# 27.01.2015 Cebra (PKT382a) |
//# - chg: Anpassung an FC209a |
//# |
//# 26.09.2014 Cebra (PKT381b) |
//# |
//# |
//# 26.09.2014 Cebra (PKT381a) |
//# |
//# 01.07.2014 OG (PKT380e) |
//# |
//# 28.06.2014 OG (PKT380dX4) |
//# |
//# 27.06.2014 OG (PKT380dX3) |
//# - chg: USE_FOLLOWME eingeschaltet |
//# - chg: USE_TRACKING abgeschaltet |
//# |
//# 22.06.2014 OG (PKT380dX3) |
//# |
//# 20.06.2014 OG (PKT380dX2) |
//# |
//# 18.06.2014 OG (PKT380dX1) |
//# |
//# 18.06.2014 OG (PKT380d) |
//# |
//# 05.06.2014 OG (PKT380cX5) |
//# |
//# 02.06.2014 OG (PKT380cX4) |
//# Update auf Windows Atmel AVR Studio 6.2.1153 (Release) |
//# (von ehemals Atmel AVR Studio 6.2 Beta) |
//# |
//# 27.05.2014 OG (PKT380cX3) |
//# |
//# 20.05.2014 OG (PKT380cX2) |
//# |
//# 19.05.2014 OG (PKT380cX1) |
//# - add: USE_OSD_SCREEN_WAYPOINTS (keine Freigabe! In Entwicklung!) |
//# |
//# 18.05.2014 OG (PKT380cX1) |
//# |
//# 15.05.2014 OG (PKT380c) |
//# |
//# 15.05.2014 OG (PKT380bX7) |
//# - chg: USE_OSD_SCREEN_3DLAGE eingeschaltet (war ungewollt abgeschaltet) |
//# |
//# 14.05.2014 OG (PKT380bX7) |
//# |
//# 13.05.2014 OG (PKT380bX6) |
//# - add: USE_MAINMENU_SEPARATOR (aus menu.h hierhin verschoben) |
//# - del: MKINFO_AUTO_REFRESH |
//# |
//# 12.05.2014 OG (PKT380bX5b) |
//# - chg: keine Code-Changes - dafuer Compiler jetzt Atmel Studio 6.2 Beta |
//# (statt bisher Atmel Studio 5.1) |
//# |
//# 07.05.2014 OG (PKT380bX4) |
//# |
//# 07.05.2014 OG (PKT380bX3) |
//# - fix: EEpromversion erhoeht auf 138 (siehe eeprom.c) |
//# |
//# 02.05.2014 OG (PKT380bX2) |
//# |
//# 28.04.2014 OG |
//# - chg: USE_OSD_SCREEN_OLD abgeschaltet |
//# |
//# 20.04.2014 OG (PKT380bX1) |
//# |
//# 18.04.2014 OG (PKT380b) |
//# |
//# 17.04.2014 OG (PKT380aX1) |
//# - chg: PKT Versionsanzeige geaendert auf "3.80a" statt wie frueher "3.8.0a" |
//# |
//# 16.04.2014 OG (PKT380a) |
//# |
//# 14.04.2014 OG (PKT374aX8) |
//# |
//# 02.04.2014 OG |
//# - chg: define fuer Baud_2400 von 0 auf 7 geaendert |
//# (0/false wird benoetigt um zu erkennen ob keine Baud-Rate erkannt wurde) |
//# |
//# 01.04.2014 OG |
//# - add: USE_SV2MODULE_BLE (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 31.03.2014 OG (PKT374aX7) |
//# |
//# 30.03.2014 OG (PKT374aX6) |
//# Sprachunterstuetzung fuer Hollaendisch vollstaendig entfernt |
//# |
//# 30.03.2014 OG (PKT374aX5) |
//# Source-Schnipp PKT374aX5 |
//# |
//# 01.03.2014 OG (PKT374aX5) |
//# |
//# 26.02.2014 OG (PKT374aX4) |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 22.02.2014 OG (PKT374aX4) |
//# |
//# 17.02.2014 OG (PKT374aX3) |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 14.02.2014 OG (PKT374aX2) |
//# - add: MKINFO_AUTO_REFRESH |
//# - del: veraltete PKT defines HWVERSION... entfernt |
//# |
//# 12.02.2014 OG (PKT374aX2) |
//# - del: defines zu MKVERSIONnnn entfernt |
//# |
//# 10.02.2014 OG (PKT374aX1) |
//# - add: ... |
//# |
//# 05.02.2014 OG (PKT374aX1) |
//# - add: DEBUG_NEW_PARAMETERS - nur fuer Entwicklung! |
//# |
//# 30.01.2014 OG (PKT373cX4) |
//# - add: USE_BLUETOOTH |
//# |
//# 29.01.2014 OG (3.7.3cX3) |
//# - del: extern uint8_t searchMK(void) ist jetzt in mk/mkbase.c |
//# |
//# 24.01.2014 OG (3.7.3cX1) |
//# - add: DEBUG_FC_COMMUNICATION (Auswirkung in usart.c) |
//# |
//# 06.01.2014 CB |
//# - add: Abschaltung MK Parameter aendern bei nicht passender FC Software |
//# |
//# 02.12.2013 CB |
//# - add: Anpassung Parameter an FC 2.01f |
//# |
//# 21.10.2013 CB |
//# - add: Anpassung Parameter an FC 2.01a |
//# |
//# 16.07.2013 CB |
//# - chg: neuen Versionsnummer wegen Wiflypatch |
//# |
//# 07.07.2013 OG |
//# - add: ABO_TIMEOUT und von 3 sec auf 2 sec gesetzt (war vorher in timer.h) |
//# - del: define USE_OSD_SCREEN_ELECTRIC_N |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# - add: DEBUG_SV2_EXTENSION um Debug-Ausgaben fuer den SV2-Patch an-/auszuschalten |
//# |
//# 05.07.2013 CB |
//# - add: USE_WLAN |
//# |
//# 30.06.2013 OG |
//# - add: USE_OSD_PKTHOOK |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - add: USE_PKTTOOLS_SV2 - zeigt SV2-Verbindungsmenuepunkte in PKT-Tools an |
//# bzw. nicht an. Am PKT-SV2 liegen +5 Volt für die FC/NC an. |
//# Anmerkung OG: bei meinen Kompilaten wird das erstmal ausgeschaltet |
//# sein bis 100% klar ist ob das keine Probleme macht |
//# wenn der Kopter mit FC/NC nicht bereits anderweitig |
//# (Netzteil/Lipo) mit Strom versorgt wird. |
//# |
//# 22.06.2013 Cebra |
//# - chg: Fehler bei der Lipooffset Einstellung beseitigt, neue Version 3.7.0c |
//# |
//# 20.06.2013 Cebra |
//# - chg: falsche Versionsnummer korrigiert, neu jetzt 3.7.0b |
//# |
//# 13.06.2013 OG |
//# - del: USE_PCFASTCONNECT (nicht mehr benoetigt) |
//# |
//# 12.06.2013 Cebra |
//# - chg: Versionswechsel auf 3.7.0a, Zielrelease für FC-Software 0.91 (bzw.1.00??) |
//# |
//# 10.06.2013 Cebra |
//# - add: #define MKVERSION091f für FC-Version 0.91f |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a für FC-Version 0.91a |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Kommentare zum Module-Support |
//# |
//# 19.05.2013 OG |
//# - add: USE_MODULEINFO |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: USE_MKGPSINFO |
//# |
//# 15.05.2013 OG |
//# - add: define USE_PKT_STARTINFO |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 13.05.2013 Cebra |
//# - add: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//# 05.05.2013 Cebra |
//# - chg: #define USE_FOLLOWME |
//# |
//# 03.05.2013 OG |
//# - del: USE_XPRINTF_LONG - spart keinen Platz mehr und ist nun dauerhaft notwendig |
//# |
//# 28.04.2013 OG |
//# - add: define USE_PCFASTCONNECT |
//# - add: define USE_FONT_BIG |
//# - add: define USE_XPRINTF_LONG |
//# - chg: Anordnung der Feature- & Debug-defines um einfacher via Copy&Paste |
//# die Einstellungen notieren bzw. posten zu koennen |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: USE_OSD_... defines |
//############################################################################ |
#ifndef _MAIN_H |
#define _MAIN_H |
// Softwareversion des PKT |
#define PKTSWVersion "3.85_f_fm2" // PKT Version |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
#define USE_MAINMENU_SEPARATOR // Menuetrennlinien im PKT-Hauptmenues anzeigen |
//######################################################################### |
//# MODULE SUPPORT |
//# ein-/ausgeschalten von Modulen des PKT um ggf. Speicherplatz zu sparen |
//# |
//# Hinweis: |
//# wenn neue USE_* hinzugefuegt werden bitte die in PKT_Info()(pkt.c) |
//# ergaenzen fuer eine aktualisierte Modul-Info |
//######################################################################### |
//--------------------------------------------- |
//- Module: Hauptfunktionen und allgemeine |
//--------------------------------------------- |
#define USE_OSDDATA // zeigt die OSD-Statistikdaten an (ca. 3.5 KByte) |
#define USE_MKSETTINGS // lesen, aktivieren und bearbeiten der MK-Settings 1-5 (ca. ?? KByte) |
#define USE_MKDEBUGDATA // Anzeige MK-Debug-Data (ca. 1.7 KByte) |
#define USE_MKDISPLAY // Anzeige MK-Display (ca. 1 KByte) |
#define USE_MKGPSINFO // Anzeige MK-GPS-Daten (ca. 2 KByte) |
//#define USE_SOUND // PKT Sounderweiterung, benoetigt entsprechende Hardware (ca. 2.5 Kbyte) |
#define USE_MODULEINFO // Anzeige installierter Module in PKT_Info() (ca. 1.8 KByte) |
#define USE_BLUETOOTH // Bluetooth Unterstuetzung des PKT (ca. 13 KByte) |
// Achtung! Das schaltet USE_TRACKING, USE_FOLLOWME aus! |
//#define USE_WAYPOINTS // Waypoint mit dem PKT |
//Entwicklung: |
//#define USE_ACCCALIBRATION // schaltet ACC Calibration im Menü ein/aus |
//#define USE_KOMPASS // Tilt kompensierter Kompass für FollowMe Funktionen |
//--------------------------------------------- |
//- Module: OSD-Screens |
//--------------------------------------------- |
//#define USE_OSD_SCREEN_OLD // OSD Screens OSD0, OSD1, OSD2 (ca. 6.6 KByte) |
#define USE_OSD_SCREEN_NAVIGATION // OSD-Screen: Navigation |
#define USE_OSD_SCREEN_WAYPOINTS // OSD-Screen: Waypoints |
#define USE_OSD_SCREEN_ELECTRIC // OSD-Screen: Electric |
#define USE_OSD_SCREEN_MKSTATUS // OSD-Screen: MK-Status |
#define USE_OSD_SCREEN_USERGPS // OSD-Screen: UserGPS |
#define USE_OSD_SCREEN_3DLAGE // OSD-Screen: 3D-Lage (ca. 900 Bytes) |
#define USE_OSD_SCREEN_STATISTIC // OSD-Screen: Statistic |
//--------------------------------------------- |
//- Module fuer spezielle Benutzergruppen |
//--------------------------------------------- |
#define USE_FOLLOWME // FollowMe Funktionen (ca. 3 Kbyte) |
#define USE_FOLLOWME_STEP2 // FollowMe Funktionen Abstand und Winkel einstellbar |
//#define USE_JOYSTICK // Joystick Support, benoetigt spezielle Hardware (ca. 4.1 KByte) |
#define USE_WLAN // WLAN WiFly Modul an SV2 (ca. 6 KByte)) |
#define USE_SV2MODULE_BLE // Bluetooth 4 Low Energy Modul - externes Modul an SV2 (RedBearLab BLE Mini) (ca. 200 Bytes) |
//--------------------------------------------- |
//- Module mit geringer Relevanz |
//--------------------------------------------- |
//#define USE_I2CMOTORTEST // I2C Funktionen für Motortest (ca. 3.8 KByte) |
//#define USE_FONT_BIG // grosser 8x8 Font (ca. 2.2 KByte, bei Verwendung von USE_OSD_SCREEN_OLD ca. 2.4 KByte) |
//--------------------------------------------- |
//- Module unfertig bzw. noch in der Entwicklung |
//--------------------------------------------- |
//#define USE_TRACKING // Antennentracking, benoetigt spezielle Hardware (ca. 6 KByte) |
//--------------------------------------------- |
//- zusaetzliche Optionen |
//--------------------------------------------- |
//#define USE_OSD_PKTHOOK |
#define USE_PKTTOOLS_SV2 // im Menue von PKT-Tools erscheinen Punkte zur Verbindung zum MK via |
// 10-Pol-Kabel am PKT-SV2. Der PKT-SV2-Anschluss liefert zur FC/NC +5 Volt! |
//######################################################################### |
//# Debug Module & Einstellungen |
//# Nur fuer die Entwicklung - fuer Veroeffentlichung alles abschalten! |
//######################################################################### |
//#define IgnoreFCVersion // keine FC-Revisions Pruefung |
//#define DEBUG_SV2_EXTENSION // ... |
//#define USE_OSD_DEMO // zeigt Demo-Daten in den OSD-Screens (sofern vom Screen unterstuetzt) (fuer Fotos) (nicht fuer die Oeffentlichkeit) |
//#define USE_OSD_SCREEN_DEBUG // zusaetzliche Debug-Screen's aktivieren (ca. 1 KByte) (nicht fuer die Oeffentlichkeit) |
//#define DEBUG // ?? Funktion unbekannt! |
//#define DEBUG_FC_COMMUNICATION // in usart.c Debugausgaben auf dem PKT bzgl. Datenempfang vom Kopter |
//#define DEBUG_PARAMSET // nur fuer Entwicklung! Fuer Release ABSCHALTEN! |
//#define DEBUG_GPS // Entwicklung der GPS Berechnung für FollowMe (wird im Moment nicth genutzt) |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
//#define ABO_TIMEOUT 300 // 3 sec |
#define ABO_TIMEOUT 200 // 2 sec |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ ggf. abhaengige USE-defines deaktivieren |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef USE_BLUETOOTH |
#undef USE_TRACKING |
#undef USE_FOLLOWME |
#endif |
// Fusebits fü½r Hardware 1.2 D7 DC FC |
// Fusebits für Hardware 1.3 D7 DC FC |
// Fusebits für Hardware 3.x D7 DC FC |
// avrdude -pm1284p -cavr109 -P/dev/ttyUSB1 -b115200 -V -Uflash:w:Dateiname.hex:a |
// hier die entsprechende Hardwareversion der Leiterplatte einstellen |
#define HWVERSION3_9 // Hardware Cebra Oktober 2011 ATmega1284P |
#ifdef HWVERSION3_9 |
#include "HAL_HW3_9.h" |
#endif |
#define NO 0 |
#define NC 1 |
#define FC 2 |
#define MK3MAG 3 |
#define MKGPS 4 |
#define Wi232 5 |
// CB #define ENABLE_PWM |
// Baud Rate |
#define Baud_9600 1 |
#define Baud_19200 2 |
#define Baud_38400 3 |
#define Baud_57600 4 |
#define Baud_115200 5 |
#define Baud_4800 6 |
#define Baud_2400 7 |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
extern volatile uint8_t mode; |
extern uint8_t hardware; |
extern uint8_t current_hardware; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/menu.c |
---|
0,0 → 1,844 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.c |
//# |
//# 03.08.2015 Cebra |
//# - add: Menü _DefMenu_Main_NO erweitert für GPS Test , mit #define schaltbar |
//# |
//# 09.04.2015 Cebra |
//# - add: Vorbereitung für ACC Kalibrartion aus dem Menü |
//# #ifdef USE_ACCCALIBRATION |
//# |
//# 27.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Tracking etwas nach oben verschoben |
//# |
//# 26.06.2014 OG |
//# - chg: Menu_PKTSpezial() - ergaenzt um Setup_FollowMe() |
//# |
//# 18.06.2014 OG |
//# - chg: Menu_PKTSpezial() umorganisiert |
//# |
//# 14.06.2014 OG |
//# - chg: _DefMenu_Main_NO() - "PKT Spezial" wird nicht mehr angezeigt wenn |
//# kein Kopter gefunden wurde; aktuell sind dort nur Funktionen |
//# enthalten die einen Kopter benoetigen - wenn sich das mal irgendwann |
//# aendert muss man das anpassen |
//# |
//# 01.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Aufruf von Tracking und Followme ergaenzt |
//# um Abfrage MKVersion.isNC |
//# - chg: _ConfigMenu_Main() umgestellt auf MKVersion.isNC/isFC |
//# |
//# 14.05.2014 OG |
//# - chg: Aufruf von gps() geaendert zu MK_Gps_Info() |
//# - chg: include "gps/gps.h" geaendert zu "mk/mkgpsinfo.h" |
//# |
//# 13.05.2014 OG |
//# - del: USE_MAINMENU_SEPARATOR (verschoben nach main.h) |
//# - chg: Menu_Main() - del unused variable 'event' |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_PKTSpezial() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 08.04.2014 OG |
//# - chg: die Menuetrennlinien werden jetzt via define USE_MAINMENU_SEPARATOR |
//# angezeigt (das define ist hier im Code) und sind erstmal wieder |
//# abgeschaltet |
//# |
//# 08.04.2014 OG |
//# - add: _DefMenu_Main_NC() - Menuetrennlinien hinzugefuegt |
//# - add: _DefMenu_Main_FC(), _DefMenu_Main_NO() - Menuetrennlinien hinzugefuegt |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# - add: PKT-Connect(): "BLE Extender" (ext. Bluetooth 4 Low Energy Modul an SV2) |
//# - fix: Menu_PKTConnect(): "WLAN Extender" nur sichtar wenn Config.UseWL gesetzt ist |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 24.03.2014 OG |
//# - chg: "MK Info" vor "MK Settings" verschoben |
//# |
//# 28.02.2014 OG |
//# - chg: ID_MKSETTINGS in den Menues fuer NC/FC weiter nach oben geschoben |
//# |
//# 27.02.2014 OG |
//# - chg: Menuetexte fuer Menu_PKTConnect() |
//# |
//# 26.02.2014 OG |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 18.02.2014 OG |
//# - chg: Aufruf von MK_Parameters() ist jetzt MK_Settings() und geht auf |
//# den Source mk/mksettings.c |
//# |
//# 17.02.2014 OG |
//# - chg: aus "MK Parameters" ist "MK Settings" geworden! Entsprechende |
//# defines zu ID's, USE und Sprachen haben sich geaendert |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# - chg: auf mk/mkparameters.c/h umgestellt -> MK_Parameters() |
//# - chg: auf 'MKVersion.paramsetOK' umgestellt |
//# |
//# 05.02.2014 OG |
//# - add: DEBUG_NEW_PARAMETERS in Hauptmenues fuer Entwicklung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - chg: Umstrukturierung vom Hauptmenue |
//# chg: "PKT Tools" -> "PKT Connect" (nur noch Connect-Funktionen drin) |
//# add: "PKT Spezial" -> MK-Motortest und Spezial-Funktionen |
//# wie Joystick, Tracking, FollowMe |
//# - chg: call: display_debug() -> MK_DebugData() |
//# - chg: call: display_data() -> MK_Display() |
//# - chg: #include "debug.h" -> "mk/mkdebugdata.h" |
//# - chg: #include "display.h" -> #include "mk/mkdisplay.h" |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: MK-Display und MK-DebugData auch bei falscher FC-Revision wieder |
//# aktiviert |
//# |
//# 07.01.2014 Cebra |
//# - chg: Abfrage WrongFCVersion etwas verändert |
//# |
//# 06.01.2014 Cebra |
//# - add: Alle Menüpunkte die FC versionabhängig werden bei falscher FC Version ausgeblendet |
//# |
//# 04.10.2013 Cebra |
//# - add: Motortest ohne FC = I2C-Anschluss PKT, mit FC/NC Motortest über FC |
//# |
//# 26.06.2013 Cebra |
//# - add: Menüpunkt Wlan Rangeextender |
//# |
//# 24.06 2013 OG |
//# - chg: Menu_PKTTools(): Menuepunkte fuer PKT-SV2-Verbindungen werden |
//# durch define USE_PKTTOOLS_SV2 (main.h) an-/ausgeschaltet |
//# - chg: Menu_PKTTools(): Menuepunkte werden je nach aktiviertem Wi232 und |
//# Bluetooth ein-/ausgeblendet |
//# - chg: Menuetexte fuer ID_USB2SV2 und ID_BT2SV2 geaendert |
//# - chg: defines fuer Menuetexte von PKT-Tools geaendert |
//# - del: verschiedene Exec_* Funktionen |
//# |
//# 24.06 2013 Cebra |
//# - add: Menuepunkt MKUSB in PKT-Tools |
//# |
//# 13.06.2013 OG |
//# - chg: Fastconnect auf Menu_PKTTools() umgelegt |
//# - chg: Menueeintraege von Menu_PKTTools() modifiziert und reduziert |
//# - chg: in allen Hauptmenues ist PKT-Setup und PKT-Info drin |
//# |
//# 11.06.2013 OG |
//# - chg: Code zu Menu_OSDData() ausgegliedert nach osddata.c |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 21.05.2013 OG |
//# - chg: Menu_Main() umgestellt auf MENUCTRL_EVENT |
//# - fix: Menu_Main() nach Menu-Redraw searchMK() |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# - del: include utils/menuold.h |
//# |
//# 20.05.2013 OG |
//# - chg: Layout Menutitel |
//# |
//# 19.05.2013 OG |
//# - chg: Menuepunkte Tracking und FollowMe werden nur angezeigt wenn BTM222 |
//# installiert ist (Config.UseBT == true) |
//# - add: Menu_PKTTools() (ehemals in tools.c) |
//# - del: PC_Fast_Connect() verschoben nach pkt.c |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.c verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - chg: PKT_Info() erweitert um optionale Module-Info (via USE_MODULEINFO) |
//# und Credits chronoligisch umgedreht |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# - chg: redundante Menue-Strings sind untereinander verpointert um Platz zu |
//# sparen |
//# - chg: Umstellung auf neue menuctrl.c und damit starke Strukturaenderung |
//# Hinweis: autom. PKT-Update via Hauptmenue geht noch nicht - wird |
//# wieder eingebaut (wenn m�glich direkt in menuctrl.c dann geht |
//# das in allen Menues) |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//# |
//# 16.05.2013 OG |
//# - add: define USE_MKGPSINFO fuer gps() |
//# |
//# 05.05.2013 Cebra |
//# - chg: Fehler im Menü bei nicht verfügbaren Funktionen |
//# |
//# 02.05.2013 OG |
//# - chg: Menuetext: osddata_menuitems: "MK Fehlerliste" -> "MK Fehler" |
//# - chg: Menuetext: "OSD Anzeige" wieder zurueck zu "OSD" |
//# |
//# 28.04.2013 OG |
//# - chg: Show_Version() auf ScrollBox umgestellt |
//# - chg: Menuetext "Debug Data" zu "MK Debug Data" (kam frueher zu Verwirrungen bei mir) |
//# - chg: main_menu() bzgl. Menu_OSDDATA() |
//# - add: Menu_OSDDATA() (aus ehemaligem osd/osd_tools.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: OSD-Tools im Menue integriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 14.04.2013 OG |
//# - WIP: Anzeige "OSD-Daten" in param_menuitems_nc, param_menuitems_no |
//# |
//# 04.04.2013 Cebra |
//# - chg:Texttuning |
//# not_possible, für Menüpunkte nicht nicht wählbar sind |
//# |
//# 03.04.2013 OG |
//# - fix: Anzeigefehler wenn (hardware == NC) und nicht gesetzt define 'USE_MKDEBUGDATA' (bzw. 'analognames') |
//# - chg: Layout von Anzeige wenn USE_MKDEBUGDATA nicht verfuegbar (jetzt Invers) |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# |
//# 30.03.2013 CB |
//# - add: automatischer Start der PKT-Updatefunktion im Mainmenüe für das Updatetool |
//# |
//# 28.03.2013 CB |
//# - chg: Hinweis wenn Debug Data nicht möglich ist |
//# |
//# 10.03.2013 Cebra |
//# - add: menu_select, gemeinsame Routine für alle Setupmenüs |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> |
#include "main.h" |
#include "lcd/lcd.h" |
#include "menu.h" |
#include "mk/mkdisplay.h" |
#include "mk/mkdebugdata.h" |
#include "timer/timer.h" |
#include "osd/osd.h" |
#include "osd/osddata.h" |
#include "motortest/motortest.h" |
#include "mk/mkgpsinfo.h" |
#include "eeprom/eeprom.h" |
#include "setup/setup.h" |
#include "uart/uart1.h" |
#include "mk-data-structs.h" |
#include "wi232/Wi232.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "messages.h" |
#include "bluetooth/bluetooth.h" |
#include "followme/followme.h" |
#include "tracking/ng_servo.h" |
#include "tracking/tracking.h" |
#include "stick/stick.h" |
#include "utils/menuctrl.h" |
#include "pkt/pkt.h" |
#include "mk/mkbase.h" |
#include "mksettings/paramset.h" |
#include "mksettings/mksettings.h" |
//############################################################################ |
char titlemain[] = "PKT v "; // buffer fuer PKT-Versionsinfo |
//123456789012345678901 |
// nur fuer Entwicklung... |
static const char DEBUG_PARAMSET_Text[] PROGMEM = "DEBUG PARAMSET"; |
//############################################################################ |
//----------------------- |
// main_menu |
//----------------------- |
#define ID_OSD 1 |
#define ID_OSDDATA 2 |
#define ID_MKDISPLAY 3 |
#define ID_MKSETTINGS 4 |
#define ID_MKDEBUGDATA 5 |
#define ID_MKGPSINFO 6 |
#define ID_SEARCHMK 7 |
#define ID_PKTCONNECT 8 |
#define ID_PKTSETUP 9 |
#define ID_PKTINFO 10 |
#define ID_PKTSPEZIAL 11 |
#define ID_MKINFO 12 |
#define ID_MKACCCAL 13 |
#define ID_DEBUG_GPS 14 |
static const char SEARCHMK_de[] PROGMEM = "Suche Mikrokopter"; |
static const char SEARCHMK_en[] PROGMEM = "search Kopter"; |
static const char OSD_de[] PROGMEM = "OSD"; |
#define OSD_en OSD_de |
static const char OSDDATA_de[] PROGMEM = "OSD Daten"; |
static const char OSDDATA_en[] PROGMEM = "OSD Data"; |
static const char MKINFO_de[] PROGMEM = "MK Info"; |
#define MKINFO_en MKINFO_de |
static const char MKDISPLAY_de[] PROGMEM = "MK Display"; |
#define MKDISPLAY_en MKDISPLAY_de |
static const char MKACCCAL_de[] PROGMEM = "MK ACC Kalibr."; |
static const char MKACCCAL_en[] PROGMEM = "MK ACC Calibr."; |
static const char MKSETTINGS_de[] PROGMEM = "MK Settings"; |
#define MKSETTINGS_en MKSETTINGS_de |
static const char MKDEBUGDATA_de[] PROGMEM = "MK Daten"; |
static const char MKDEBUGDATA_en[] PROGMEM = "MK Data"; |
static const char MKGPSINFO_de[] PROGMEM = "MK GPS Info"; |
#define MKGPSINFO_en MKGPSINFO_de |
static const char PKTCONNECT_de[] PROGMEM = "PKT Connect"; |
#define PKTCONNECT_en PKTCONNECT_de |
static const char PKTSPEZIAL_de[] PROGMEM = "PKT Spezial"; |
static const char PKTSPEZIAL_en[] PROGMEM = "PKT Special"; |
static const char PKTSETUP_de[] PROGMEM = "PKT Setup"; |
#define PKTSETUP_en PKTSETUP_de |
static const char PKTINFO_de[] PROGMEM = "PKT Info"; |
#define PKTINFO_en PKTINFO_de |
#ifdef USE_FOLLOWME_STEP2 |
static const char PKTGPS_de[] PROGMEM = "PKT GPS Debug"; |
#define PKTGPS_en PKTGPS_de |
#endif |
//############################################################################ |
//----------------------------- |
// Menu_PKTSpezial() |
//----------------------------- |
#define ID_BLTESTER 60 |
#define ID_TRACKING 61 |
#define ID_JOYSTICK 62 |
#define ID_FOLLOWME 63 |
#define ID_FOLLOWMESETUP 64 |
static const char BLTESTER_de[] PROGMEM = "MK Motortest"; |
#define BLTESTER_en BLTESTER_de |
static const char TRACKING_de[] PROGMEM = "Tracking"; |
#define TRACKING_en TRACKING_de |
static const char JOYSTICK_de[] PROGMEM = "Joystick"; |
#define JOYSTICK_en JOYSTICK_de |
static const char FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define FOLLOWME_en FOLLOWME_de |
static const char FOLLOWMESETUP_de[] PROGMEM = "Follow Me Setup"; |
#define FOLLOWMESETUP_en FOLLOWMESETUP_de |
//############################################################################ |
//----------------------------- |
// Menu_PKTConnect() |
//----------------------------- |
#define ID_BT2WI232 40 |
#define ID_USB2WI232 41 |
#define ID_USB2SV2 42 |
#define ID_BT2SV2 43 |
#define ID_WIFLY2WI232 44 |
#define ID_BLE2WI232 45 |
static const char BT2WI232_de[] PROGMEM = "BT Extender"; |
#define BT2WI232_en BT2WI232_de |
static const char BLE2WI232_de[] PROGMEM = "BLE Extender"; |
#define BLE2WI232_en BLE2WI232_de |
static const char WIFLY2WI232_de[] PROGMEM = "WLAN Extender"; |
#define WIFLY2WI232_en WIFLY2WI232_de |
static const char USB2WI232_de[] PROGMEM = "USB -> Wi232"; |
#define USB2WI232_en USB2WI232_de |
#ifdef USE_PKTTOOLS_SV2 |
static const char USB2SV2_de[] PROGMEM = "USB -> SV2"; |
#define USB2SV2_en USB2SV2_de |
#endif |
#ifdef USE_PKTTOOLS_SV2 |
static const char BT2SV2_de[] PROGMEM = "BT -> SV2"; |
#define BT2SV2_en BT2SV2_de |
#endif |
//############################################################################################# |
//# Hilfsfunktionen & Verschiedenes |
//############################################################################################# |
//-------------------------------------------------------------- |
// wird von Menu_PKTTools() verwendet |
//-------------------------------------------------------------- |
void Exec_BLTESTER(void) |
{ |
if(hardware == NO) motor_test( I2C_Mode ); |
if(hardware == NC) motor_test( FC_Mode ); |
if(hardware == FC) motor_test( FC_Mode ); |
} |
//############################################################################################# |
//# Menu: Spezial |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer 'PKTSpezial' |
//-------------------------------------------------------------- |
void Menu_PKTSpezial( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Spezial" |
//MenuCtrl_SetTitle_P( PSTR("PKT Spezial") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems: PKTSpezial |
//--------------- |
#ifdef USE_FOLLOWME |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_FOLLOWME , MENU_ITEM, &FollowMe , FOLLOWME_de , FOLLOWME_en ); |
MenuCtrl_PushML2_P( ID_FOLLOWMESETUP, MENU_ITEM, &Setup_FollowMe, FOLLOWMESETUP_de, FOLLOWMESETUP_en ); |
} |
#endif |
#ifdef USE_TRACKING |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_TRACKING , MENU_ITEM, &PKT_tracking , TRACKING_de , TRACKING_en ); |
} |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( ID_JOYSTICK , MENU_ITEM, &joystick , JOYSTICK_de , JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_BLTESTER , MENU_ITEM, &Exec_BLTESTER , BLTESTER_de , BLTESTER_en ); |
// {"Servo Tester ","servo test ","Servo Tester "}, |
// if((val+offset) == 2 ) servo_test(); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: PKT-Connect |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer; PKT-Connect |
// |
// verbindet das PKT mit anderen Devices wie Tablet usw. |
//-------------------------------------------------------------- |
void Menu_PKTConnect( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle_P( PSTR("PKT Connect") ); // kann nicht auf MenuCtrl_SetTitleFromParentItem() umgestellt |
// werden da PKT-Connect auch durch einen Hot-Key aufgerufen |
// werden kann |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_BLUETOOTH |
if( Config.UseBT && Config.UseWi ) |
MenuCtrl_PushML2_P( ID_BT2WI232 , MENU_ITEM, &Port_BT2Wi , BT2WI232_de , BT2WI232_en ); |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch && Config.UseBLE ) |
MenuCtrl_PushML2_P( ID_BLE2WI232 , MENU_ITEM, &Port_BLE2Wi , BLE2WI232_de , BLE2WI232_en ); |
#endif |
if( PCB_SV2RxTxPatch && Config.UseWL ) |
MenuCtrl_PushML2_P( ID_WIFLY2WI232 , MENU_ITEM, &Port_WiFly2Wi , WIFLY2WI232_de , WIFLY2WI232_en ); |
if( Config.UseWi ) |
MenuCtrl_PushML2_P( ID_USB2WI232 , MENU_ITEM, &Port_USB2Wi , USB2WI232_de , USB2WI232_en ); |
#ifdef USE_PKTTOOLS_SV2 // am PKT-SV2 liegen +5 Volt an... |
MenuCtrl_PushML2_P( ID_USB2SV2 , MENU_ITEM, &Port_USB2FC , USB2SV2_de , USB2SV2_en ); |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
MenuCtrl_PushML2_P( ID_BT2SV2 , MENU_ITEM, &Port_BT2FC , BT2SV2_de , BT2SV2_en ); |
#endif |
#endif // end: #ifdef USE_PKTTOOLS_SV2 |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: Menu_Main |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NC( void ) |
{ |
MenuCtrl_PushML2_P( ID_OSD , MENU_ITEM, &osd , OSD_de , OSD_en ); |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MKGPSINFO |
MenuCtrl_PushML2_P( ID_MKGPSINFO , MENU_ITEM, &MK_Gps_Info , MKGPSINFO_de , MKGPSINFO_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_FC( void ) |
{ |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NO( void ) |
{ |
#ifdef USE_FOLLOWME |
MenuCtrl_PushML2_P( ID_DEBUG_GPS , MENU_ITEM, &Debug_GPS , PKTGPS_de , PKTGPS_en ); |
#endif |
MenuCtrl_PushML2_P( ID_SEARCHMK , MENU_ITEM, NOFUNC , SEARCHMK_de , SEARCHMK_en ); |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
//MenuCtrl_PushML2_P( ID_PKTSPEZIAL, MENU_SUB , &Menu_PKTSpezial, PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT, MENU_SUB , &Menu_PKTConnect, PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM, &PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _ConfigMenu_Main( void ) |
{ |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( titlemain ); |
//MenuCtrl_SetShowBatt( true ); |
//----------- |
// Tasten |
//----------- |
MenuCtrl_SetKey( KEY_ESC , strGet(OFF), &PKT_SwitchOff ); |
MenuCtrl_SetKey( KEY_ENTER_LONG , NOTEXT, &Menu_PKTConnect ); |
if ( MKVersion.isNC ) _DefMenu_Main_NC(); |
else if( MKVersion.isFC ) _DefMenu_Main_FC(); |
else _DefMenu_Main_NO(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Main( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT = Config.UseBT; // merkt sich Bluetooth Einstellung falls durch Setup geaendert |
uart1_flush(); |
ADC_Init(); // ADC für Lipomessung // MartinR hierher verschoben |
get_key_press(KEY_ALL); |
strncpy( &titlemain[5], PKTSWVersion, 12); // baue Titel mit PKT-Versionsnummer zusammen |
MenuCtrl_ShowLevel( true ); // zeige Menuelevel in der Titelzeile aller Menues |
_ConfigMenu_Main(); // initialisiert das Menu je nach Hardware (NO,FC,NC) verschieden |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); // wartet nicht auf Event, springt direkt zurueck (wegen evtl. BT-Aenderung) |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------- |
// ID_SEARCHMK |
//-------------- |
if( itemid == ID_SEARCHMK ) // neuer Hardware Modus? |
{ |
if( searchMK(true) ) // true = zeige MK_Info() an |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_MKINFO |
//-------------- |
if( itemid == ID_MKINFO ) |
{ |
if( MK_Info( 0, true) ) // neuer Hardware Modus? (in MK_Info() kann man eine Neusuche des MK starten) (true = mit Settings-Refresh) |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_PKTSETUP |
//-------------- |
if( itemid == ID_PKTSETUP ) |
{ |
Setup_MAIN(); |
} |
//-------------- |
// Bluetooth geaendert? |
//-------------- |
if( UseBT != Config.UseBT ) // falls sich im PKT-Setup die Einstellung bzgl. installiertem Bluetooth Modul |
{ // geaendert hat werden ggf. andere Menuepunkte angezeigt (Tracking, FollowMe) |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
UseBT = Config.UseBT; |
} |
} |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/menu.h |
---|
0,0 → 1,57 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.h |
//# |
//# 19.05.2013 OG |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.h verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//############################################################################ |
#ifndef MENU_H |
#define MENU_H |
void Menu_Main(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/messages.c |
---|
0,0 → 1,829 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.c |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 28.06.2014 OG |
//# - chg: Text von ERROR_NODATA statt "Datenverlust!" jetzt "MK Datenverlust!" |
//# - chg: Text von OSD_ERROR statt "FEHLER: Datenverlust!" jetzt "MK Datenverlust!" |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 11.05.2014 OG |
//# - chg: STR_SAVE_de - jetzt "Speichern" statt "speichern" |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# - chg: OSD_ERROR_de,OSD_ERROR_en Space am Ende entfernt / Ausrufungszeichen hinzugefuegt |
//# |
//# 09.04.2014 OG |
//# - chg: WL1 von "WiFly vorh.:" zu "WiFly Modus:" |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 02.04.2014 OG |
//# - chg: Text WL1_de gekuerzt |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: Text zu WL1 geaendert von "Wlan eingebaut" zu "WiFly vorhanden" |
//# -> das ist ein Aufsteckmodul und wird nicht eingebaut |
//# - chg: strGet() angepasst auf 2 Sprachen (Deutsch, Englisch) |
//# - del: Hollaendisch vollstaendig geloescht |
//# |
//# 29.03.2014 OG |
//# - chg: Test geaendert von START_LASTPOS2 zu "löschen Nein" |
//# - chg: einige Texte bzgl. OSD-Data gekuerzt wegen Umstellung auf lcdx_printp_center() |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - chg: Texte von BOOT1, BOOT2, TESTSTRING |
//# - add: STR_VON, STR_NACH, STR_PKT |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: STARTMSG2 |
//# - add: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - chg: START_LASTPOS |
//# - chg: START_LASTPOS1 |
//# - chg: START_LASTPOS3 |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - del: etliche ":" aus Bezeichnungen geloescht (bei verschiedenen Einstellungen) |
//# -> woher kamen die ueberhaupt? In der Historie sehe ich dazu nichts! |
//# - add: MSG_WARNUNG, UPDATE3 |
//# - chg: UPDATE1, UPDATE2 (PKT Update) |
//# - chg: SHUTDOWN (Space's entfernt) |
//# - chg: START_VERSIONCHECK3: angepasst auf neue flexibilitaet des PKT wenn |
//# FC-Revision nicht unterstuetzt wird |
//# - chg: OK_de von "Ok" nach "OK" |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: Texte geaendert von START_VERSIONCHECK... |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting (fuer Setup: ein-/ausschalten der MK-Settingsanzeige) |
//# - del: DISPLAY8 (ehemals LCD-Orientierung, nicht mehr benoetigt) |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 OG |
//# - chg: kleine Textaenderung an STARTMSG2 |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 CB |
//# - add: Texte für OSD Statistik erweitert |
//# |
//# 25.05.2013 OG |
//# - chg: redundante Strings durch define ersetzt um Speicherplatz zu sparen |
//# |
//# 05.05.2013 Cebra |
//# - add: Erweiterung PKT Zeitsetup |
//# |
//# 25.04.2013 Cebra |
//# - chg: LIPO entfernt, Text doppelt |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "eeprom/eeprom.h" |
#include "messages.h" |
typedef enum |
{ |
GERMAN, |
ENGLISH, |
NETHERLAND, |
LAST_LANGUAGE, |
} LANGUAGE; |
static const char KEYLINE1_de[] PROGMEM = " \x1a \x1b Ende OK"; |
static const char KEYLINE1_en[] PROGMEM = " \x1a \x1b end OK"; |
static const char KEYLINE2_de[] PROGMEM = " \x18 \x19 Ende OK"; |
static const char KEYLINE2_en[] PROGMEM = " \x18 \x19 end OK"; |
static const char KEYLINE3_de[] PROGMEM = " \x18 \x19 Ende "; |
static const char KEYLINE3_en[] PROGMEM = " \x18 \x19 end "; |
static const char KEYLINE4_de[] PROGMEM = "Ende OK"; |
static const char KEYLINE4_en[] PROGMEM = "end OK"; |
static const char KEYLINE5_de[] PROGMEM = " \x18 \x19 Abbr. OK"; |
static const char KEYLINE5_en[] PROGMEM = " \x18 \x19 cancel OK"; |
static const char KEYCANCEL_de[] PROGMEM = " Abbr."; |
static const char KEYCANCEL_en[] PROGMEM = "cancel"; |
static const char BOOT1_de[] PROGMEM = "Taste 1 Sekunde"; |
static const char BOOT1_en[] PROGMEM = "press button"; |
static const char BOOT2_de[] PROGMEM = "festhalten!"; |
static const char BOOT2_en[] PROGMEM = "for 1 second!"; |
static const char START_LASTPOS_de[] PROGMEM = "Letzte Position"; |
static const char START_LASTPOS_en[] PROGMEM = "last position"; |
static const char START_LASTPOS1_de[] PROGMEM = "Breitengr. Längengr."; |
static const char START_LASTPOS1_en[] PROGMEM = "latitude longitude"; |
static const char START_LASTPOS2_de[] PROGMEM = "löschen Nein"; |
static const char START_LASTPOS2_en[] PROGMEM = "delete No"; |
static const char START_LASTPOS3_de[] PROGMEM = "Google Eingabe:"; |
static const char START_LASTPOS3_en[] PROGMEM = "Google Input:"; |
static const char START_SEARCHFC_de[] PROGMEM = "suche Mikrokopter..."; |
static const char START_SEARCHFC_en[] PROGMEM = "searching Kopter..."; |
static const char ENDE_de[] PROGMEM = "Ende"; |
static const char ENDE_en[] PROGMEM = "end"; |
static const char OK_de[] PROGMEM = "OK"; |
static const char OK_en[] PROGMEM = "ok"; |
static const char FEHLER_de[] PROGMEM = "Fehler"; |
static const char FEHLER_en[] PROGMEM = "error"; |
static const char AKTIV_de[] PROGMEM = "aktiv"; |
static const char AKTIV_en[] PROGMEM = "activ"; |
static const char STR_SAVE_de[] PROGMEM = "Speichern"; |
static const char STR_SAVE_en[] PROGMEM = "save"; |
static const char STR_SAVING_de[] PROGMEM = "wird gespeichert..."; |
static const char STR_SAVING_en[] PROGMEM = "saving..."; |
static const char STR_DISCARD_de[] PROGMEM = "Verwerfen"; |
static const char STR_DISCARD_en[] PROGMEM = "discard"; |
static const char STR_COPY_de[] PROGMEM = "Kopieren"; |
static const char STR_COPY_en[] PROGMEM = "copy"; |
static const char STR_SEARCH_de[] PROGMEM = "suche"; |
static const char STR_SEARCH_en[] PROGMEM = "search"; |
static const char STR_FOUND_de[] PROGMEM = "gefunden"; |
static const char STR_FOUND_en[] PROGMEM = "found"; |
static const char STR_SET_de[] PROGMEM = "setze"; |
static const char STR_SET_en[] PROGMEM = "set"; |
static const char STR_WITH_de[] PROGMEM = "mit"; |
static const char STR_WITH_en[] PROGMEM = "with"; |
static const char STR_WITHOUT_de[] PROGMEM = "ohne"; |
static const char STR_WITHOUT_en[] PROGMEM = "without"; |
static const char STR_SWITCHMOTOROFF_de[] PROGMEM = "Motoren ausschalten!"; |
static const char STR_SWITCHMOTOROFF_en[] PROGMEM = "switch motors off!"; |
static const char ON_de[] PROGMEM = "Ein "; |
static const char ON_en[] PROGMEM = "On "; |
static const char OFF_de[] PROGMEM = "Aus "; |
static const char OFF_en[] PROGMEM = "off "; |
static const char ESC_de[] PROGMEM = "ESC"; |
#define ESC_en ESC_de |
static const char SHUTDOWN_de[] PROGMEM = "PKT ausschalten?"; |
static const char SHUTDOWN_en[] PROGMEM = "shutdown PKT?"; |
static const char YESNO_de[] PROGMEM = "Ja Nein"; |
static const char YESNO_en[] PROGMEM = "yes no"; |
static const char NOYES_de[] PROGMEM = "Nein Ja"; |
static const char NOYES_en[] PROGMEM = "no yes"; |
static const char DELETEDATA_de[] PROGMEM = "Daten löschen?"; |
static const char DELETEDATA_en[] PROGMEM = "delete data?"; |
static const char UPDATE1_de[] PROGMEM = "Verbinde PKT mit PC!"; |
static const char UPDATE1_en[] PROGMEM = "Connect PKT to PC"; |
static const char UPDATE2_de[] PROGMEM = "Drücke 'Start' für"; |
static const char UPDATE2_en[] PROGMEM = "Press 'Start' for"; |
static const char UPDATE3_de[] PROGMEM = "min. 2 Sekunden"; |
static const char UPDATE3_en[] PROGMEM = "min. 2 seconds"; |
static const char ENDSTART_de[] PROGMEM = "Ende Start"; |
static const char ENDSTART_en[] PROGMEM = "End Start"; |
static const char CONNECT13_de[] PROGMEM = "Modul eingebaut"; |
static const char CONNECT13_en[] PROGMEM = "Modul built in"; |
static const char KABEL_de[] PROGMEM = "Kabel"; |
static const char KABEL_en[] PROGMEM = "cable"; |
static const char SLAVE_de[] PROGMEM = "Slave"; |
#define SLAVE_en SLAVE_de |
static const char NORMAL_de[] PROGMEM = "Normal"; |
#define NORMAL_en NORMAL_de |
static const char REVERSE_de[] PROGMEM = "Reverse"; |
static const char REVERSE_en[] PROGMEM = "inverse"; |
static const char ENDOK_de[] PROGMEM = "Ende OK"; |
static const char ENDOK_en[] PROGMEM = "End OK"; |
static const char EEPROM1_de[] PROGMEM = "PKT-EEProm"; |
static const char EEPROM1_en[] PROGMEM = "Realy delete"; |
static const char EEPROM2_de[] PROGMEM = "wirklich löschen?"; |
static const char EEPROM2_en[] PROGMEM = "PKT-EEProm?"; |
static const char DEUTSCH_de[] PROGMEM = "deutsch"; |
static const char DEUTSCH_en[] PROGMEM = "german"; |
static const char ENGLISCH_de[] PROGMEM = "englisch"; |
static const char ENGLISCH_en[] PROGMEM = "english"; |
static const char YES_de[] PROGMEM = "Ja "; |
static const char YES_en[] PROGMEM = "yes "; |
static const char NOO_de[] PROGMEM = "Nein"; |
static const char NOO_en[] PROGMEM = "no "; |
static const char OSD_3D_V_de[] PROGMEM = "V"; |
static const char OSD_3D_V_en[] PROGMEM = "F"; |
static const char OSD_3D_H_de[] PROGMEM = "H"; |
static const char OSD_3D_H_en[] PROGMEM = "B"; |
static const char OSD_3D_L_de[] PROGMEM = "L"; |
#define OSD_3D_L_en OSD_3D_L_de |
static const char OSD_3D_R_de[] PROGMEM = "R"; |
#define OSD_3D_R_en OSD_3D_R_de |
static const char OSD_3D_NICK_de[] PROGMEM = "Ni"; |
#define OSD_3D_NICK_en OSD_3D_NICK_de |
static const char OSD_3D_ROLL_de[] PROGMEM = "Ro"; |
#define OSD_3D_ROLL_en OSD_3D_ROLL_de |
static const char OSD_ERROR_de[] PROGMEM = "MK Datenverlust!"; |
static const char OSD_ERROR_en[] PROGMEM = "MK Data lost!"; |
static const char PARA_AKTIVI_de[] PROGMEM = "Aktivieren"; |
static const char PARA_AKTIVI_en[] PROGMEM = "activate"; |
static const char PARA_COPY_de[] PROGMEM = "Kopiere Setting"; |
static const char PARA_COPY_en[] PROGMEM = "copy settings"; |
static const char PARA_COPYQ_de[] PROGMEM = "Wirklich kopieren?"; |
static const char PARA_COPYQ_en[] PROGMEM = "really copy?"; |
static const char GPS2_de[] PROGMEM = "gewähltes GPS Gerät "; |
static const char GPS2_en[] PROGMEM = "selected GPS device "; |
static const char STATS_ITEM_0_de[] PROGMEM = "max Höhe :"; |
static const char STATS_ITEM_0_en[] PROGMEM = "max altitude:"; |
static const char STATS_ITEM_1_de[] PROGMEM = "max Geschw. :"; |
static const char STATS_ITEM_1_en[] PROGMEM = "max speed :"; |
static const char STATS_ITEM_2_de[] PROGMEM = "max Entfern.:"; |
static const char STATS_ITEM_2_en[] PROGMEM = "max distance:"; |
static const char STATS_ITEM_3_de[] PROGMEM = "min Spannung:"; |
static const char STATS_ITEM_3_en[] PROGMEM = "min voltage :"; |
static const char STATS_ITEM_4_de[] PROGMEM = "max Zeit :"; |
static const char STATS_ITEM_4_en[] PROGMEM = "max time :"; |
static const char STATS_ITEM_5_de[] PROGMEM = "max Strom :"; |
static const char STATS_ITEM_5_en[] PROGMEM = "max current :"; |
static const char STATS_ITEM_6_de[] PROGMEM = "Ent.Kapazit.:"; |
static const char STATS_ITEM_6_en[] PROGMEM = "UsedCapacity:"; |
static const char POTI_de[] PROGMEM = "Poti "; |
static const char POTI_en[] PROGMEM = "poti "; |
static const char TASTER_de[] PROGMEM = "Taster"; |
static const char TASTER_en[] PROGMEM = "switch"; |
static const char STAT_OSDBL_de[] PROGMEM = "OSD- und BL-Daten"; |
static const char STAT_OSDBL_en[] PROGMEM = "OSD- and BL-Data"; |
static const char STAT_ERROR_de[] PROGMEM = "MK Fehlerliste"; |
static const char STAT_ERROR_en[] PROGMEM = "MK errorlist"; |
static const char STAT_GPS_de[] PROGMEM = "User GPS-Daten"; |
static const char STAT_GPS_en[] PROGMEM = "User GPS-data"; |
static const char STAT_POS_de[] PROGMEM = "letzte Position"; |
static const char STAT_POS_en[] PROGMEM = "last position"; |
static const char STAT_ALL_de[] PROGMEM = "ALLE Daten"; |
static const char STAT_ALL_en[] PROGMEM = "ALL data"; |
static const char DELETE_de[] PROGMEM = "löschen?"; |
static const char DELETE_en[] PROGMEM = "delete?"; |
static const char STR_OSDSCREEN_GENERAL_de[] PROGMEM = "General"; |
#define STR_OSDSCREEN_GENERAL_en STR_OSDSCREEN_GENERAL_de |
static const char STR_OSDSCREEN_NAVIGATION_de[] PROGMEM = "Navigation"; |
#define STR_OSDSCREEN_NAVIGATION_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_WAYPOINTS_de[] PROGMEM = "Waypoints"; |
#define STR_OSDSCREEN_WAYPOINTS_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_ELECTRIC_de[] PROGMEM = "Electric"; |
#define STR_OSDSCREEN_ELECTRIC_en STR_OSDSCREEN_ELECTRIC_de |
static const char STR_OSDSCREEN_MKSTATUS_de[] PROGMEM = "MK-Status"; |
#define STR_OSDSCREEN_MKSTATUS_en STR_OSDSCREEN_MKSTATUS_de |
static const char STR_OSDSCREEN_USERGPS_de[] PROGMEM = "User GPS"; |
#define STR_OSDSCREEN_USERGPS_en STR_OSDSCREEN_USERGPS_de |
static const char STR_OSDSCREEN_3DLAGE_de[] PROGMEM = "3D Lage"; |
#define STR_OSDSCREEN_3DLAGE_en STR_OSDSCREEN_3DLAGE_de |
static const char STR_OSDSCREEN_STATISTIC_de[] PROGMEM = "Statistics"; |
#define STR_OSDSCREEN_STATISTIC_en STR_OSDSCREEN_STATISTIC_de |
static const char STR_OSDSCREEN_OSD0_de[] PROGMEM = "OSD0"; |
#define STR_OSDSCREEN_OSD0_en STR_OSDSCREEN_OSD0_de |
static const char STR_OSDSCREEN_OSD1_de[] PROGMEM = "OSD1"; |
#define STR_OSDSCREEN_OSD1_en STR_OSDSCREEN_OSD1_de |
static const char STR_OSDSCREEN_OSD2_de[] PROGMEM = "OSD2"; |
#define STR_OSDSCREEN_OSD2_en STR_OSDSCREEN_OSD2_de |
static const char NOMKFOUND_de[] PROGMEM = "keinen MK gefunden!"; |
static const char NOMKFOUND_en[] PROGMEM = "no MK found!"; |
static const char STR_ERROR_de[] PROGMEM = "FEHLER"; |
static const char STR_ERROR_en[] PROGMEM = "ERROR"; |
static const char ERROR_NODATA_de[] PROGMEM = "MK Datenverlust!"; |
static const char ERROR_NODATA_en[] PROGMEM = "MK Data lost!"; |
static const char MSG_LOADSETTINGS_de[] PROGMEM = "lade Settings..."; |
static const char MSG_LOADSETTINGS_en[] PROGMEM = "loading settings..."; |
static const char MSG_ACTIVATESETTING_de[] PROGMEM = "aktiviere Setting..."; |
static const char MSG_ACTIVATESETTING_en[] PROGMEM = "activate setting..."; |
static const char EDIT_SETTING_de[] PROGMEM = "Bearbeiten"; |
static const char EDIT_SETTING_en[] PROGMEM = "edit"; |
static const char STR_VON_de[] PROGMEM = "von"; |
static const char STR_VON_en[] PROGMEM = "from"; |
static const char STR_NACH_de[] PROGMEM = "nach"; |
static const char STR_NACH_en[] PROGMEM = "to"; |
static const char STR_PKT_de[] PROGMEM = "PKT"; |
#define STR_PKT_en STR_PKT_de |
static const char STR_ACTIVE_de[] PROGMEM = "AKTIV!"; |
static const char STR_ACTIVE_en[] PROGMEM = "ACTIVE!"; |
static const char MODULE_EXIST_de[] PROGMEM = "Modul vorhanden"; |
static const char MODULE_EXIST_en[] PROGMEM = "Module exist"; |
static const char STR_INITIALIZE_de[] PROGMEM = "Initialisieren"; |
static const char STR_INITIALIZE_en[] PROGMEM = "Initialize"; |
static const char STR_LONGPRESS_de[] PROGMEM = "langer Tastendruck:"; |
static const char STR_LONGPRESS_en[] PROGMEM = "long press:"; |
static const char STR_FAV_ADD_de[] PROGMEM = "Fav hinzugefügt!"; |
static const char STR_FAV_ADD_en[] PROGMEM = "fav added!"; |
static const char STR_FAV_FULL_de[] PROGMEM = "* Fav ist voll *"; |
static const char STR_FAV_FULL_en[] PROGMEM = "* fav is full *"; |
static const char STR_FAV_EXIST_de[] PROGMEM = "* Fav vorhanden *"; |
static const char STR_FAV_EXIST_en[] PROGMEM = "* fav exists*"; |
static const char STR_FAV_NOTEXIST_de[] PROGMEM = "kein Fav vorhanden!"; |
static const char STR_FAV_NOTEXIST_en[] PROGMEM = "no fav exist!"; |
static const char STR_MENUCTRL_DELITEM_de[] PROGMEM = "Eintrag entfernt!"; |
static const char STR_MENUCTRL_DELITEM_en[] PROGMEM = "item deleted!"; |
static const char STR_MENUCTRL_DELASK_de[] PROGMEM = "Eintrag entfernen?"; |
static const char STR_MENUCTRL_DELASK_en[] PROGMEM = "delete item?"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_de[] PROGMEM = "nicht möglich!"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_en[] PROGMEM = "not possible"; |
static const char STR_HELP_PKTOFFTIME1_de[] PROGMEM = "Minuten (0=Aus)"; |
static const char STR_HELP_PKTOFFTIME1_en[] PROGMEM = "minutes (0=off)"; |
static const char STR_HELP_LOWBAT1_de[] PROGMEM = "<4.20 Einzelzelle "; |
static const char STR_HELP_LOWBAT1_en[] PROGMEM = "<4.20 single cell "; |
static const char STR_HELP_LOWBAT2_de[] PROGMEM = ">4.20 Gesamtspannu."; |
static const char STR_HELP_LOWBAT2_en[] PROGMEM = ">4.20 total voltage"; |
static const char STR_HELP_LIPOOFFSET1_de[] PROGMEM = "verstellen, bis die"; |
static const char STR_HELP_LIPOOFFSET1_en[] PROGMEM = "adjust offset until"; |
static const char STR_HELP_LIPOOFFSET2_de[] PROGMEM = "Spannung passt!"; |
static const char STR_HELP_LIPOOFFSET2_en[] PROGMEM = "voltage fits!"; |
static const char STR_DATA_de[] PROGMEM = "Daten"; |
static const char STR_DATA_en[] PROGMEM = "data"; |
static const char STR_READING_de[] PROGMEM = "lese"; |
static const char STR_READING_en[] PROGMEM = "reading"; |
static const char STR_LABELS_de[] PROGMEM = "Labels"; |
static const char STR_LABELS_en[] PROGMEM = "labels"; |
static const char STR_EXTSV2MODULE_de[] PROGMEM = "externes SV2-Modul"; |
static const char STR_EXTSV2MODULE_en[] PROGMEM = "external SV2 module"; |
static const char STR_ATTENTION_de[] PROGMEM = "* ACHTUNG *"; |
static const char STR_ATTENTION_en[] PROGMEM = "* ATTENTION *"; |
static const char STR_SWITCHOFFMK_de[] PROGMEM = "Kopter ausschalten!"; |
static const char STR_SWITCHOFFMK_en[] PROGMEM = "switch off kopter!"; |
static const char STR_WI232CONFIG_de[] PROGMEM = "Wi.232 Konfig."; |
static const char STR_WI232CONFIG_en[] PROGMEM = "Wi.232 Config"; |
static const char STR_USBCONNECTED_de[] PROGMEM = "mit USB verbunden"; |
static const char STR_USBCONNECTED_en[] PROGMEM = "connected with USB"; |
static const char STR_SEEMKWIKI_de[] PROGMEM = "siehe MK-Wiki:"; |
static const char STR_SEEMKWIKI_en[] PROGMEM = "see MK-Wiki:"; |
static const char STR_PKT_SWITCHOFF_NOW_de[] PROGMEM = "jetzt ausschalten?"; |
static const char STR_PKT_SWITCHOFF_NOW_en[] PROGMEM = "switch off now?"; |
static const char STR_PKT_RESTART_NOW_de[] PROGMEM = "PKT neu starten!"; |
static const char STR_PKT_RESTART_NOW_en[] PROGMEM = "restart PKT!"; |
static const char STR_WI232_ACTIVATE_de[] PROGMEM = "aktiviere Wi.232!"; |
static const char STR_WI232_ACTIVATE_en[] PROGMEM = "activate Wi.232!"; |
static const char STR_WAITSATFIX_de[] PROGMEM = "* warte auf Satfix *"; |
static const char STR_WAITSATFIX_en[] PROGMEM = "* waiting satfix *"; |
static const char STR_GPSMOUSECONNECT_de[] PROGMEM = "verbinde GPS-Maus"; |
static const char STR_GPSMOUSECONNECT_en[] PROGMEM = "search gps-mouse"; |
static const char STR_GPSMOUSEDISCONNECT_de[] PROGMEM = "trenne GPS-Maus"; |
static const char STR_GPSMOUSEDISCONNECT_en[] PROGMEM = "disjoin gps-mouse"; |
static const char STR_NOBTM222_de[] PROGMEM = "kein BTM222 vorh."; |
static const char STR_NOBTM222_en[] PROGMEM = "no BTM222 installed"; |
static const char STR_NOGPSMOUSE_de[] PROGMEM = "keine GPS-Maus!"; |
static const char STR_NOGPSMOUSE_en[] PROGMEM = "no gps-mouse!"; |
static const char STR_GPSMOUSECONNECTION_de[] PROGMEM = "GPS-Maus Verbindung"; |
static const char STR_GPSMOUSECONNECTION_en[] PROGMEM = "gps-mouse connection"; |
static const char STR_SEARCH_BT_ASK_de[] PROGMEM = "BT Geräte suchen?"; |
static const char STR_SEARCH_BT_ASK_en[] PROGMEM = "search BT devices?"; |
static const char STR_SEARCH_BT_de[] PROGMEM = "suche BT Geräte"; |
static const char STR_SEARCH_BT_en[] PROGMEM = "search BT devices"; |
static const char STR_NO_BT_FOUND_de[] PROGMEM = "kein Gerät gefunden"; |
static const char STR_NO_BT_FOUND_en[] PROGMEM = "no device found"; |
static const char STR_BT_DEVICES_de[] PROGMEM = "BT Geräte"; |
static const char STR_BT_DEVICES_en[] PROGMEM = "BT devices"; |
static const char STR_BT_SEARCHTIME_de[] PROGMEM = "(ca. 2 Minuten)"; |
static const char STR_BT_SEARCHTIME_en[] PROGMEM = "(ca. 2 minutes)"; |
static const char STR_BT_LOSTDATA_de[] PROGMEM = "BT Datenverlust"; |
static const char STR_BT_LOSTDATA_en[] PROGMEM = "BT data loss"; |
static const char STR_METERS_de[] PROGMEM = "Meter"; |
static const char STR_METERS_en[] PROGMEM = "meters"; |
static const char STR_WAITNMEA_de[] PROGMEM = "Warte auf GPS Daten"; |
static const char STR_WAITNMEA_en[] PROGMEM = "waiting for GPS Data"; |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
static const char LAST_STR_de[] PROGMEM = "Lastring"; |
#define LAST_STR_en LAST_STR_de |
//****************************************************************** |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * const strings[] PROGMEM = |
{ |
KEYLINE1_de, KEYLINE1_en, |
KEYLINE2_de, KEYLINE2_en, |
KEYLINE3_de, KEYLINE3_en, |
KEYLINE4_de, KEYLINE4_en, |
KEYLINE5_de, KEYLINE5_en, |
KEYCANCEL_de, KEYCANCEL_en, |
BOOT1_de, BOOT1_en, |
BOOT2_de, BOOT2_en, |
START_LASTPOS_de, START_LASTPOS_en, |
START_LASTPOS1_de, START_LASTPOS1_en, |
START_LASTPOS2_de, START_LASTPOS2_en, |
START_LASTPOS3_de, START_LASTPOS3_en, |
START_SEARCHFC_de, START_SEARCHFC_en, |
ENDE_de, ENDE_en, |
OK_de, OK_en, |
FEHLER_de, FEHLER_en, |
AKTIV_de, AKTIV_en, |
STR_SAVE_de, STR_SAVE_en, |
STR_SAVING_de, STR_SAVING_en, |
STR_DISCARD_de, STR_DISCARD_en, |
STR_COPY_de, STR_COPY_en, |
STR_SEARCH_de, STR_SEARCH_en, |
STR_FOUND_de, STR_FOUND_en, |
STR_SET_de, STR_SET_en, |
STR_WITH_de, STR_WITH_en, |
STR_WITHOUT_de, STR_WITHOUT_en, |
STR_SWITCHMOTOROFF_de, STR_SWITCHMOTOROFF_en, |
ON_de, ON_en, |
OFF_de, OFF_en, |
ESC_de, ESC_en, |
SHUTDOWN_de, SHUTDOWN_en, |
YESNO_de, YESNO_en, |
NOYES_de, NOYES_en, |
DELETEDATA_de, DELETEDATA_en, |
UPDATE1_de, UPDATE1_en, |
UPDATE2_de, UPDATE2_en, |
UPDATE3_de, UPDATE3_en, |
ENDSTART_de, ENDSTART_en, |
CONNECT13_de, CONNECT13_en, |
KABEL_de, KABEL_en, |
SLAVE_de, SLAVE_en, |
NORMAL_de, NORMAL_en, |
REVERSE_de, REVERSE_en, |
ENDOK_de, ENDOK_en, |
EEPROM1_de, EEPROM1_en, |
EEPROM2_de, EEPROM2_en, |
DEUTSCH_de, DEUTSCH_en, |
ENGLISCH_de, ENGLISCH_en, |
YES_de, YES_en, |
NOO_de, NOO_en, |
OSD_3D_V_de, OSD_3D_V_en, |
OSD_3D_H_de, OSD_3D_H_en, |
OSD_3D_L_de, OSD_3D_L_en, |
OSD_3D_R_de, OSD_3D_R_en, |
OSD_3D_NICK_de, OSD_3D_NICK_en, |
OSD_3D_ROLL_de, OSD_3D_ROLL_en, |
OSD_ERROR_de, OSD_ERROR_en, |
PARA_AKTIVI_de, PARA_AKTIVI_en, |
PARA_COPY_de, PARA_COPY_en, |
PARA_COPYQ_de, PARA_COPYQ_en, |
GPS2_de, GPS2_en, |
STATS_ITEM_0_de, STATS_ITEM_0_en, |
STATS_ITEM_1_de, STATS_ITEM_1_en, |
STATS_ITEM_2_de, STATS_ITEM_2_en, |
STATS_ITEM_3_de, STATS_ITEM_3_en, |
STATS_ITEM_4_de, STATS_ITEM_4_en, |
STATS_ITEM_5_de, STATS_ITEM_5_en, |
STATS_ITEM_6_de, STATS_ITEM_6_en, |
POTI_de, POTI_en, |
TASTER_de, TASTER_en, |
STAT_OSDBL_de, STAT_OSDBL_en, |
STAT_ERROR_de, STAT_ERROR_en, |
STAT_GPS_de, STAT_GPS_en, |
STAT_POS_de, STAT_POS_en, |
STAT_ALL_de, STAT_ALL_en, |
DELETE_de, DELETE_en, |
STR_OSDSCREEN_GENERAL_de, STR_OSDSCREEN_GENERAL_en, |
STR_OSDSCREEN_NAVIGATION_de, STR_OSDSCREEN_NAVIGATION_en, |
STR_OSDSCREEN_WAYPOINTS_de, STR_OSDSCREEN_WAYPOINTS_en, |
STR_OSDSCREEN_ELECTRIC_de, STR_OSDSCREEN_ELECTRIC_en, |
STR_OSDSCREEN_MKSTATUS_de, STR_OSDSCREEN_MKSTATUS_en, |
STR_OSDSCREEN_USERGPS_de, STR_OSDSCREEN_USERGPS_en, |
STR_OSDSCREEN_3DLAGE_de, STR_OSDSCREEN_3DLAGE_en, |
STR_OSDSCREEN_STATISTIC_de, STR_OSDSCREEN_STATISTIC_en, |
STR_OSDSCREEN_OSD0_de, STR_OSDSCREEN_OSD0_en, |
STR_OSDSCREEN_OSD1_de, STR_OSDSCREEN_OSD1_en, |
STR_OSDSCREEN_OSD2_de, STR_OSDSCREEN_OSD2_en, |
NOMKFOUND_de, NOMKFOUND_en, |
STR_ERROR_de, STR_ERROR_en, |
ERROR_NODATA_de, ERROR_NODATA_en, |
MSG_LOADSETTINGS_de, MSG_LOADSETTINGS_en, |
MSG_ACTIVATESETTING_de, MSG_ACTIVATESETTING_en, |
EDIT_SETTING_de, EDIT_SETTING_en, |
STR_VON_de, STR_VON_en, |
STR_NACH_de, STR_NACH_en, |
STR_PKT_de, STR_PKT_en, |
STR_ACTIVE_de, STR_ACTIVE_en, |
MODULE_EXIST_de, MODULE_EXIST_en, |
STR_INITIALIZE_de, STR_INITIALIZE_en, |
STR_LONGPRESS_de, STR_LONGPRESS_en, |
STR_FAV_ADD_de, STR_FAV_ADD_en, |
STR_FAV_FULL_de, STR_FAV_FULL_en, |
STR_FAV_EXIST_de, STR_FAV_EXIST_en, |
STR_FAV_NOTEXIST_de, STR_FAV_NOTEXIST_en, |
STR_MENUCTRL_DELITEM_de, STR_MENUCTRL_DELITEM_en, |
STR_MENUCTRL_DELASK_de, STR_MENUCTRL_DELASK_en, |
STR_MENUCTRL_NOTPOSSIBLE_de, STR_MENUCTRL_NOTPOSSIBLE_en, |
STR_HELP_PKTOFFTIME1_de, STR_HELP_PKTOFFTIME1_en, |
STR_HELP_LOWBAT1_de, STR_HELP_LOWBAT1_en, |
STR_HELP_LOWBAT2_de, STR_HELP_LOWBAT2_en, |
STR_HELP_LIPOOFFSET1_de, STR_HELP_LIPOOFFSET1_en, |
STR_HELP_LIPOOFFSET2_de, STR_HELP_LIPOOFFSET2_en, |
STR_DATA_de, STR_DATA_en, |
STR_READING_de, STR_READING_en, |
STR_LABELS_de, STR_LABELS_en, |
STR_EXTSV2MODULE_de, STR_EXTSV2MODULE_en, |
STR_ATTENTION_de, STR_ATTENTION_en, |
STR_SWITCHOFFMK_de, STR_SWITCHOFFMK_en, |
STR_WI232CONFIG_de, STR_WI232CONFIG_en, |
STR_USBCONNECTED_de, STR_USBCONNECTED_en, |
STR_SEEMKWIKI_de, STR_SEEMKWIKI_en, |
STR_PKT_SWITCHOFF_NOW_de, STR_PKT_SWITCHOFF_NOW_en, |
STR_PKT_RESTART_NOW_de, STR_PKT_RESTART_NOW_en, |
STR_WI232_ACTIVATE_de, STR_WI232_ACTIVATE_en, |
STR_WAITSATFIX_de, STR_WAITSATFIX_en, |
STR_GPSMOUSECONNECT_de, STR_GPSMOUSECONNECT_en, |
STR_GPSMOUSEDISCONNECT_de, STR_GPSMOUSEDISCONNECT_en, |
STR_NOBTM222_de, STR_NOBTM222_en, |
STR_NOGPSMOUSE_de, STR_NOGPSMOUSE_en, |
STR_GPSMOUSECONNECTION_de, STR_GPSMOUSECONNECTION_en, |
STR_SEARCH_BT_ASK_de, STR_SEARCH_BT_ASK_en, |
STR_SEARCH_BT_de, STR_SEARCH_BT_en, |
STR_NO_BT_FOUND_de, STR_NO_BT_FOUND_de, |
STR_BT_DEVICES_de, STR_BT_DEVICES_en, |
STR_BT_SEARCHTIME_de, STR_BT_SEARCHTIME_en, |
STR_BT_LOSTDATA_de, STR_BT_LOSTDATA_en, |
STR_METERS_de, STR_METERS_en, |
STR_WAITNMEA_de, STR_WAITNMEA_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
LAST_STR_de, LAST_STR_en, |
}; // end: strings |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache den entsprechenden String |
// aus der obigen Stringtabelle zureuck |
//-------------------------------------------------------------- |
const char * strGet( int str_no) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings[(str_no*2)+1] ); // Englisch (=1) |
return (const char*) pgm_read_word( &strings[str_no*2] ); // Deutsch (=0) |
} |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache Parameter str_de oder |
// Parameter str_en zurueck |
// |
// wird z.B. in Verbindung mit de/en-Menuetexten verwendet |
//-------------------------------------------------------------- |
const char * strGetLanguage( char const *str_de, char const *str_en) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return str_en; // Englisch (=1) |
return str_de; // Deutsch (=0) |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/messages.h |
---|
0,0 → 1,205 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.h |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: NUM_LANG von 3 auf 2 reduziert (Hollaendisch vollstaendig geloescht) |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - add: STR_VON, STR_NACH |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: START_MSG2 |
//# - add:: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - add: MSG_WARNUNG, UPDATE3 |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting |
//# |
//# 13.06.2013 CB |
//# - add: ENUM Texte für OSD Statistik erweitert |
//# |
//############################################################################ |
#ifndef MESSAGES_H |
#define MESSAGES_H |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings, LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
KEYLINE1, KEYLINE2, KEYLINE3, KEYLINE4, KEYLINE5, KEYCANCEL, BOOT1, BOOT2, |
START_LASTPOS, START_LASTPOS1, START_LASTPOS2, START_LASTPOS3, |
START_SEARCHFC, ENDE, OK, FEHLER, AKTIV, STR_SAVE, STR_SAVING, STR_DISCARD, STR_COPY, STR_SEARCH, |
STR_FOUND, STR_SET, STR_WITH, STR_WITHOUT, STR_SWITCHMOTOROFF, |
ON, OFF, ESC, SHUTDOWN, YESNO, NOYES,DELETEDATA, UPDATE1, UPDATE2, UPDATE3, ENDSTART, CONNECT13, |
KABEL, SLAVE, NORMAL, REVERSE, ENDOK, EEPROM1, EEPROM2, DEUTSCH, ENGLISCH, |
YES, NOO, OSD_3D_V, OSD_3D_H, OSD_3D_L, |
OSD_3D_R, OSD_3D_NICK, OSD_3D_ROLL, OSD_ERROR, PARA_AKTIVI, PARA_COPY, PARA_COPYQ, |
GPS2, STATS_ITEM_0, STATS_ITEM_1, STATS_ITEM_2, STATS_ITEM_3, STATS_ITEM_4, STATS_ITEM_5, STATS_ITEM_6, |
POTI, TASTER, STAT_OSDBL, STAT_ERROR, STAT_GPS, STAT_POS, STAT_ALL, DELETE, |
STR_OSDSCREEN_GENERAL, STR_OSDSCREEN_NAVIGATION, STR_OSDSCREEN_WAYPOINTS, STR_OSDSCREEN_ELECTRIC, STR_OSDSCREEN_MKSTATUS, STR_OSDSCREEN_USERGPS, STR_OSDSCREEN_3DLAGE, |
STR_OSDSCREEN_STATISTIC, STR_OSDSCREEN_OSD0, STR_OSDSCREEN_OSD1, STR_OSDSCREEN_OSD2, NOMKFOUND, |
STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING, EDIT_SETTING, |
STR_VON, STR_NACH, STR_PKT, STR_ACTIVE, MODULE_EXIST, STR_INITIALIZE, STR_LONGPRESS, |
STR_FAV_ADD, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST, STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE, |
STR_HELP_PKTOFFTIME1, STR_HELP_LOWBAT1, STR_HELP_LOWBAT2, STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2, |
STR_DATA, STR_READING, STR_LABELS, STR_EXTSV2MODULE, STR_ATTENTION, STR_SWITCHOFFMK, STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI, |
STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE, |
STR_WAITSATFIX, STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT, STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION, STR_SEARCH_BT_ASK, |
STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES, STR_BT_SEARCHTIME, STR_BT_LOSTDATA, STR_METERS,STR_WAITNMEA, |
LAST_STR |
} STR; |
#define NUM_LANG 2 // German, English |
const char * strGet( int str_no); |
const char * strGetLanguage( char const *str_de, char const *str_en); |
void Test_Language (void); // bleibt für Tests |
#endif /* _MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkbase.c |
---|
0,0 → 1,825 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.c |
//# |
//# 21.06.2014 OG |
//# - chg: searchMK() schaltet am Ende wieder auf die NC um sofern diese |
//# vorhanden ist |
//# |
//# 19.05.2014 OG |
//# - chg: MKVersion_Setting_print() umgestellt auf strGet(STR_ERROR) fuer |
//# Multi-Sprachenunterstuetzung |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 13.05.2014 OG |
//# - chg: MK_Info() - del: MKINFO_AUTO_REFRESH (nicht mehr unterstuetzt) |
//# - chg: MK_Setting_write() - del: unused variable 'lsetting' |
//# |
//# 17.04.2014 OG |
//# - chg: MK_Setting_load - Ansprechverhalten verbessert wenn Windows |
//# Mikrokoptertool aktiv ist |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 24.03.2014 OG |
//# - chg: searchMK(): doppeltes MK_SwitchToNC() um evtl. vorhandene NC |
//# zuverlaessiger zu erkennen |
//# - fix: searchMK(): RetVal initialisiert |
//# - chg: Default-Anzeigezeit von MK_Info() in searchMK() auf 5 Sek. reduziert |
//# |
//# 21.02.2014 OG |
//# - fix: MKVersion_Setting_print() Anzeige der Settings-Nummer gekuerzt |
//# |
//# 20.02.2014 OG |
//#- chg: searchMK() auf aktuelle Version von PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: MKVersion_Setting_print() um ein paar Bytes zu sparen |
//# - chg: MK_Info() um ein paar Bytes zu sparen |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() (Anleitung bei der Funktion beachten!) |
//# - del: WrongFCVersion |
//# |
//# 12.02.2014 OG |
//# - chg: verschiedene Verbesserungen an MK_Info(), MK_load_settings(), searckMK() |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# |
//# 08.02.2014 OG |
//# - chg: searckMK() Anpassung an MKVersion |
//# - chg: MK_load_setting() Anpassung an MKVersion und Parameter geaendert |
//# - add: MKVersion_Init(), MKVersion_print_at() |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# - chg: kleine Aenderung in der Anzeige "für FC..." in searchMK() |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c: MK_load_setting(), searchMK() |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include <avr/wdt.h> |
#include <avr/eeprom.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "mkbase.h" |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
MKVersion_t MKVersion; |
uint8_t cmd; |
//############################################################################################# |
//# 1. MKVersion |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKVersion_Init() |
// |
// initialisiert MKVersion auf 0 |
//-------------------------------------------------------------- |
void MKVersion_Init( void ) |
{ |
memset( &MKVersion, 0, sizeof(MKVersion_t) ); |
} |
//-------------------------------------------------------------- |
// value = MKVersion_Cmp( FC/NCver, SWMajor,SWMinor,SWPatch ) |
// |
// Vergleicht eine uebergebene FC/NC-Version mit der vom MK |
// ermittelten Version |
// |
// Parameter: |
// |
// FC/NCver: MKVersion.FCVer oder MKVersion.NCVer |
// |
// SWMajor,SWMinor,SWPatch: siehe nachfolgendes... |
// |
//--- |
// Beispiel aus osd.c/OSD_MK_GetData(): |
// |
// v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version "0.30g" |
// |
// if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
// { ... |
// |
// Sowas geht auch: |
// |
// if( v && (v == GIVEN_VERSION) ) // nur wenn aktuelle NC-Version == "0.30g"... |
// |
// oder: |
// |
// if( v && (v < GIVEN_VERSION) ) // nur wenn aktuelle NC-Version kleiner als "0.30g"... |
// |
//--- |
// Warum "if( v && ..." ? |
// |
// Wenn das PKT keine FC/NC-Version ermitteln konnte, und somit kein |
// Vergleich moeglich ist, gibt MKVersion_Cmp() false (=0) zurueck! |
//-------------------------------------------------------------- |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ) |
{ |
SWPatch = SWPatch -'a'; |
if( ver.SWMajor > 0 || ver.SWMinor > 0 ) // NC/FC Version erkannt? |
{ |
if( (ver.SWMajor == SWMajor) && (ver.SWMinor == SWMinor) && (ver.SWPatch == SWPatch) ) |
return VERSION_EQUAL; // ==2 |
if( ver.SWMajor != SWMajor ) |
{ |
if( ver.SWMajor < SWMajor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWMinor != SWMinor ) |
{ |
if( ver.SWMinor < SWMinor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWPatch < SWPatch ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
return VERSION_NO; // ==0 |
} |
//-------------------------------------------------------------- |
// MKVersion_print_at() |
// |
// zeigt die aktuelle FC, NC Version auf dem LCD an |
// |
// Parameter: |
// what: NC, FC, |
//-------------------------------------------------------------- |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ) |
{ |
const char *mask = PSTR("%1u.%02u%c"); |
const char *nostr = PSTR("-----"); |
switch(what) |
{ |
case FC: if( MKVersion.FCVer.SWMajor > 0 || MKVersion.FCVer.SWMinor > 0 ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, mask, MKVersion.FCVer.SWMajor, MKVersion.FCVer.SWMinor, MKVersion.FCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
case NC: if( (MKVersion.NCVer.SWMajor != 0) || (MKVersion.NCVer.SWMinor != 0) ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, PSTR("%1u.%02u%c"), MKVersion.NCVer.SWMajor, MKVersion.NCVer.SWMinor, MKVersion.NCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
} |
} |
//-------------------------------------------------------------- |
// MKVersion_Setting_print( y, drawmode, xoffs,yoffs) |
// |
// Zeigt das aktuelle MK-Setting aus MKVersion zentriert in der |
// Zeile y auf dem PKT-Screen an |
//-------------------------------------------------------------- |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ) |
{ |
char buffer[18]; // String Buffer fuer eine Zeile |
if( MKVersion.mksetting == 0 ) |
xsnprintf_P( buffer, 18, PSTR("* %S *"), strGet(STR_ERROR) ); // FC Setting konnte NICHT gelesen werden |
else if( MKVersion.mksetting != 0 && MKVersion.paramsetOK ) // FC-Setting wurde gelesen... |
xsnprintf_P( buffer, 18, PSTR("%u %s"), MKVersion.mksetting, MKVersion.mksettingName ); // ... passende FC-Reversion -> Nummer und Name anzeigen |
else |
xsnprintf_P( buffer, 18, PSTR("%u"), MKVersion.mksetting ); // ... aber FALSCHE FC-Version -> nicht den Namen anzeigen (nur die Nummer) |
lcdx_print_center( y, (unsigned char *)buffer, drawmode, xoffs,yoffs); // Ausgabe Setting-Name (zentriert) |
} |
//############################################################################################# |
//# 2. Sonstiges - MK-Suche, Settings laden, MK-Info |
//############################################################################################# |
#define SWITCH_DELAY 25 // alt 50 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToNC( void ) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToFC( void ) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToMAG( void ) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToGPS( void ) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
// ret = MK_Setting_load( lsetting, &mk_param_struct, showerror) |
// |
// holt MK-Settings - es wird auf die FC umgeschaltet! |
//-------------------------------------------------------------- |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ) |
{ |
// timeout = 15 bis 20 ist meist ein guter Wert |
//MK_SwitchToFC(); |
mode = 'Q'; // Settings Request |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout ) |
{ |
MK_SwitchToFC(); |
SendOutData( 'q', ADDRESS_FC, 1, &lsetting, 1); // lsetting == 0xff -> aktuelles MK-Setting laden |
timer2 = 25; // timer ist ggf. belegt von MK_Info() -> versuchen wir es mit timer2.... |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
} |
mode = 0; |
// Initialisierungen |
paramsetInit(0); |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
paramsetInit( (unsigned char *) pRxData ); // fuellt u.a. auch MKVersion.paramsetRevision |
} |
return MKVersion.mksetting; // wird von paramsetInit() gesetzt/initialisert |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout) |
{ |
//uint8_t timeout = 50; |
uint8_t setting; |
if( !MKVersion.paramsetOK ) |
{ |
return 0; // Error |
} |
//MK_SwitchToFC(); // ? |
setting = 0; |
mode = 'S'; // Settings |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout && !setting) |
{ |
SendOutData( 's', ADDRESS_FC, 2, &wsetting, 1, (pRxData+1), paramsetSize() ); // pRxData+1 = die mk_datastruct beginnt bei +1; bei 0 ist die Settingsnummer |
timer2 = 70; |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
setting = *pRxData; |
if( !setting ) |
rxd_buffer_locked = FALSE; |
} |
} |
setting = MK_Setting_load( 0xff, 15); // damit pRxData & MKVersion initialisiert sind |
return setting; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_change( uint8_t setting ) |
{ |
uint8_t lsetting = 0; |
uint8_t wsetting = 0; |
lsetting = MK_Setting_load( setting, 20); |
if( lsetting == setting ) |
{ |
wsetting = MK_Setting_write( setting, 50); |
} |
/* |
//-------------------- |
// DEBUG... |
//-------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("load : %1u (req:%1u)"), lsetting, setting ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("write: %1u "), wsetting ); |
if( (setting != lsetting) || (setting != wsetting) ) |
set_beep( 1000, 0x0f0f, BeepNormal ); // Error |
timer2 = 500; while( timer2 > 0 ); |
*/ |
return wsetting; // 0 = Fehler; oder 1..5 = geschriebenes Setting |
} |
//-------------------------------------------------------------- |
// ok = searchMK( showMKInfo ) |
// |
// Parameter: |
// showMKInfo: true/false |
//-------------------------------------------------------------- |
uint8_t searchMK( uint8_t showMKInfo ) |
{ |
uint8_t timeout; |
//uint8_t setting; // aktuelles FC-Setting (1..5) |
uint8_t RetVal = false; |
uint8_t redraw = true; |
uint8_t searchbar_y; |
uint8_t searchbar_w = 0; |
MKVersion_Init(); |
//---------------- |
// Switch to NC |
//---------------- |
MK_SwitchToNC(); |
_delay_ms(50); // 24.03.2014 OG: manchmal wurde nicht auf eine vorhandene NC umgeschaltet |
MK_SwitchToNC(); // evtl. wird das durch doppeltes Senden verbessert |
//--------------------------------------- |
// MK-Suchscreen - Versionsabfrage NC |
//--------------------------------------- |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 50; |
//searchbar_y = 5*8-1; |
searchbar_y = 5*8+2; |
while( !rxd_buffer_locked ) |
{ |
//------------------------ |
// MK Suchscreen anzeigen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (mit Lipo-Anzeige) |
lcdx_printp_center( 2, strGet(STR_PKT), MNORMAL, 0,2); // "PKT" |
lcdx_printp_at( 0, 3, strGet(START_SEARCHFC), MNORMAL, 0,8); // "suche Mikrokopter..." |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
lcd_rect ( 0, searchbar_y , 126, 8, 1); // Rahmen fuer Bar-Anzeige |
searchbar_w = 2; |
redraw = false; |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
//------------------------ |
// MK Datenanfrage |
//------------------------ |
SendOutData( 'v', ADDRESS_ANY, 0); // Versions-Anfrage an MK senden |
timer = 16; // kurze Verögerung fuer Datenanfrage und Fortschrittsbalken |
while( timer > 0 ); |
//------------------------ |
// Fortschrittsbalken |
//------------------------ |
lcd_frect( 2, searchbar_y+2, searchbar_w, 4, 1); // Fortschrittsbalken zeichnen... |
searchbar_w += 2; |
if( searchbar_w >= 125 ) |
{ |
searchbar_w = 0; |
lcd_frect( 2, searchbar_y+2, 123, 4, 0); |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
hardware = NO; |
return false; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
} |
//-------------------------- |
// MK gefunden? |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
//--- |
// TODO OG: testen mit entsprechendem Hardware Setup des MK's! |
//--- |
if( (rxd_buffer[1] - 'a') == ADDRESS_FC ) |
{ |
hardware = FC; |
current_hardware = hardware; |
MKVersion.isFC = true; |
RetVal = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
if( (rxd_buffer[1] - 'a') == ADDRESS_NC ) |
{ |
hardware = NC; |
current_hardware = hardware; |
MKVersion.isNC = true; |
RetVal = true; |
memcpy( &MKVersion.NCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
//-------------------------- |
// jetzt: Version der FC abfragen |
//-------------------------- |
if( hardware == NC ) |
{ |
MK_SwitchToFC(); |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 40; |
while( !rxd_buffer_locked && timeout) |
{ |
SendOutData( 'v', ADDRESS_FC, 0); |
timer = 20; |
while( timer > 0 ); |
timeout--; |
} |
//-------------------------- |
// FC gefunden! |
// -> Version kopieren |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
MKVersion.isFC = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
} |
} // end: if( rxd_buffer_locked ) |
//--------------------------------------- |
// FC EEprom Version / Struktur pruefen |
//--------------------------------------- |
MK_Setting_load( 0xff, 15 ); // 0xff == aktuelles Parameterset holen; 15 == timeout; Ergebnisse in MKVersion |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( MKVersion.isNC ) |
{ |
MK_SwitchToNC(); // wieder auf NC umschalten als default |
} |
if( MKVersion.paramsetOK ) // FC Revision OK? |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep (FC-Revision ok) |
if( showMKInfo ) MK_Info( 500, true ); // Anzeige von MK_Info() max. 5 Sekunden (mit Settings-Refresh) |
} |
else // FC Revision nicht unterstuetzt... |
{ |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep (FC-Revision nicht unterstuetzt) |
if( showMKInfo ) MK_Info( 1600, true ); // Anzeige von MK_Info() max. 16 Sekunden (mit Settings-Refresh) |
} |
//timer = 50; |
//while (timer > 0); |
get_key_press(KEY_ALL); |
return RetVal; |
} // searchMK() |
//-------------------------------------------------------------- |
// chg = MK_Info( displaytimeout, refreshSettings ) |
// |
// Parameter: |
// displaytimeout : 0 = kein autom. Timeout |
// n = Anzeige verschwindet automatisch nach n Zeit |
// (bei z.B. n=600 nach 6 Sekunden) |
// |
// refreshSettings: true/false |
// true = in bestimmten Zeitintervallen (RELOAD_SETTING_TIME) |
// wird MK_Setting_load() durchgeführt um ggf. einen Kopterwechsel |
// automatisch zu erkennen. |
// Dazu muss MKINFO_AUTO_REFRESH in main.h eingeschaltet sein! |
// |
// Rueckgabe: |
// hardwarechange: true/false |
//-------------------------------------------------------------- |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ) |
{ |
uint8_t redraw; |
uint8_t old_isFC; |
uint8_t old_isNC; |
old_isFC = MKVersion.isFC; |
old_isNC = MKVersion.isNC; |
//displaytimeout = 0; |
if( displaytimeout ) |
timer = displaytimeout; |
#define RELOAD_SETTING_TIME 350 // alle 0.35 sec neue Settings vom MK laden |
timer_get_tidata = RELOAD_SETTING_TIME; // ich brauche weitere timer... und bediene mich jetzt mal bei einem osd-timer |
// (timer2 ist belegt von MK_Setting_load) |
//get_key_press(KEY_ALL); |
redraw = true; |
while( true ) |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw != 2 ) |
lcd_cls(); |
show_Lipo(); // LiPo direkt anzeigen damit es nicht flackert |
lcd_frect( 0, 0, 101, 8, 1); // Headline: Box fill Black mit Platz fuer PKT-LiPo rechts |
lcdx_printp_at( 1, 0, PSTR("MK Info"), MINVERS, 0,0); // Titel |
if( MKVersion.isFC || MKVersion.isNC ) // MK gefunden... |
{ |
lcd_frect_round( 0, 30, 127, 22, 1, R2); // Fill: Anzeigebereich der Settings in Schwarz |
//-------------------- |
// FC-Version |
//-------------------- |
lcdx_printp_at( 0, 2, PSTR("FC:") , MNORMAL, 4,-5); |
MKVersion_print_at( 4, 2, FC , MNORMAL, 2,-5); |
//-------------------- |
// FC-Revision |
//-------------------- |
lcdx_printp_at( 12, 2, PSTR("Rev:"), MNORMAL, 5,-5); |
lcdx_printf_at_P( 17, 2, MNORMAL, 2,-5, PSTR("%03u"), MKVersion.paramsetRevision ); |
if( !MKVersion.paramsetOK ) |
lcdx_printp_at( 11, 3, PSTR(" Rev-ERR!"), MINVERS, 5,-4); // Fehler FC-Revision (nicht vom PKT unterstuetzt) |
else |
lcd_frect( 11*6+5, 3*8-4, 9*6, 8, 0); // ggf. Fehleranzeige loeschen (nach refreshSettings) |
//-------------------- |
// NC-Version |
//-------------------- |
lcdx_printp_at( 0, 3, PSTR("NC:") , MNORMAL, 4,-3); |
MKVersion_print_at( 4, 3, NC , MNORMAL, 2,-3); |
//-------------------- |
// aktuelles Setting |
//-------------------- |
lcdx_printp_center( 4, PSTR("MK Setting"), MINVERS, 0,1); // "MK Setting" (zentriert) |
//lcdx_printp_at( 5, 4, PSTR("MK Setting"), MINVERS, 4,1); |
MKVersion_Setting_print( 5, MINVERS, 0,3); |
} |
else // else: if( MKVersion.isFC || MKVersion.isNC ) // KEINEN MK gefunden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(NOMKFOUND), false, 0, true, false ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
} // end: if( MKVersion.isFC || MKVersion.isNC ) |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
if( displaytimeout == 0 ) |
lcd_printp_at( 0, 7, PSTR("Refresh"), MNORMAL); // Keyline: Refresh |
redraw = false; |
} // end: if( redraw ) |
//------------------------------------------ |
// PKT-LiPo Anzeige |
//------------------------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_MINUS) && (displaytimeout == 0) ) |
{ |
searchMK( false ); // false = zeige nicht MK_Info() an |
redraw = true; |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
break; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
//------------------------------------------ |
// displaytimeout? |
//------------------------------------------ |
if( displaytimeout && !timer ) |
{ |
break; |
} |
} // end: while( true ) |
get_key_press(KEY_ALL); |
return( (old_isFC != MKVersion.isFC) || (old_isNC != MKVersion.isNC) ); |
} // MK_Info() |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkbase.h |
---|
0,0 → 1,143 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.h |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() |
//# - add: defines zu VERSION... fuer MKVersion_Cmp() |
//# - del: WrongFCVersion |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# - add: MKVersion_print_at |
//# |
//# 08.02.2014 OG |
//# - chg: MK_load_setting() Parameter geaendert |
//# - add: extern MKVersion_t MKVersion |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c |
//############################################################################ |
#ifndef _MKBASE_H |
#define _MKBASE_H |
//#include "../mksettings/paramset.h" |
#include "../mk-data-structs.h" |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char isFC; // true / false - FC vorhanden? -> wird gesetzt durch searchMK() |
unsigned char isNC; // true / false - NC vorhanden? -> wird gesetzt durch searchMK() |
Version_t FCVer; // -> wird gesetzt durch searchMK() |
Version_t NCVer; // -> wird gesetzt durch searchMK() |
unsigned char paramsetOK; // true wenn Revision in paramset.c vorhanden und initialisiert -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char paramsetRevision; // Revision FC-Parameterset -> wird gesetzt druch paramsetInit()/paramset.c |
uint8_t mksetting; // -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char mksettingName[13]; // -> wird gesetzt druch paramsetInit()/paramset.c |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
extern MKVersion_t MKVersion; |
//------------------------------------------------------ |
// defines fuer den Versionsvergleich von FC/NC |
// siehe: MKVersion_Cmp()... (mkbase.c) |
//------------------------------------------------------ |
#define GIVEN_VERSION 2 // das macht die Sache leserlicher fuer resultierenden Ergebnisse! => siehe Anmerkungen: MKVersion_Cmp() ! |
#define VERSION_NO 0 |
#define VERSION_LOWER 1 |
#define VERSION_EQUAL 2 |
#define VERSION_GREATER 3 |
void MKVersion_Init( void ); |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ); |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ); |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ); |
void MK_SwitchToNC( void ); |
void MK_SwitchToFC( void ); |
void MK_SwitchToMAG( void ); |
void MK_SwitchToGPS( void ); |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ); |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout); |
uint8_t MK_Setting_change( uint8_t setting ); |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ); |
uint8_t searchMK( uint8_t showMKInfo ); |
#endif // end: #ifndef _MKBASE_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkdebugdata.c |
---|
0,0 → 1,416 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.c |
//# |
//# 04.06.2014 OG |
//# komplett neues Layout |
//# - chg: MK_DebugData(), GetAnalogNames() umgestellt auf PKT_Message_Datenverlust() |
//# - chg: GetAnalogNames() Timeout reduziert |
//# - add: #include "../lipo/lipo.h" |
//# - add: #include "../pkt/pkt.h" |
//# - add: #include <stdbool.h> |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.c' jetzt 'mk/mkdebugdata.c' |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: Benutzer kann GetAnalogNames() via KEY_ESC abbrechen |
//# - chg: Screen-Layout GetAnalogNames() |
//# - chg: SwitchToNC/FC Reihenfolge |
//# |
//# 27.03.2013 OG |
//# - chg: auf malloc umgestellt |
//# - fix: diverse Anzeigefehler |
//# - chg: teilweise redunten Code entfernt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <string.h> |
#include "../main.h" |
#ifdef USE_MKDEBUGDATA |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../uart/usart.h" |
#include "mkdebugdata.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
//-------------------------------------------------------------- |
#define PAGELINES 5 // Anzahl der Zeilen pro Anzeigescreens |
#define XOFFS 4 |
#define YOFFS 12 |
#define TIMEOUT 200 // 2 sec |
#define ANALOGTIME 20 // 200 ms |
#define MKDD_ANALOGNAME_SIZE 2*32*17 // MALLOC: 32 names, 16 characters + 1 0x00 |
// WARNING: this work for NC & FC only |
// if current_hardware == MK3MAG or MKGPS the access is outside of the array... |
//uint8_t AnalogNames[2][32][16 + 1]; // 32 names, 16 characters + 1 0x00 |
uint8_t *AnalogNames; // MALLOC: MKDD_ANALOGNAME_SIZE - 32 names, 16 characters + 1 0x00 |
uint8_t AnalogNamesRead[2] = {0,0}; |
static const char strFC[] PROGMEM = "FC"; |
static const char strNC[] PROGMEM = "NC"; |
//-------------------------------------------------------------- |
// Speicher wieder freigeben |
//-------------------------------------------------------------- |
void MKDD_MemFree( void ) |
{ |
free( AnalogNames ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showTitle( void ) |
{ |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0, PSTR(" %2S %16S"), (current_hardware==FC ? strFC : strNC), strGet(STR_DATA) ); // Titelzeile |
show_Lipo(); |
} |
//-------------------------------------------------------------- |
// lOk = GetAnalogNames() |
// |
// Return: |
// true = OK |
// false = User-Abort (KEY_ESC) |
//-------------------------------------------------------------- |
uint8_t GetAnalogNames( void ) |
{ |
uint8_t i = AnalogNamesRead[current_hardware - 1]; |
uint8_t t = 0; |
uint8_t lOk = TRUE; |
int8_t yoffs = -2; // yoffs der Progress-Bar |
lcd_cls (); |
showTitle(); // Titel |
lcdx_printf_center_P( 3, MNORMAL, 0,0, PSTR("%S %S %S"), strGet(STR_READING), (current_hardware==FC ? strFC : strNC), strGet(STR_LABELS) ); |
lcd_rect_round( 13, 40+yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
mode = 'A'; // read Names |
_delay_ms(200); |
rxd_buffer_locked = FALSE; |
timer = ANALOGTIME; |
while( (i < 32) && lOk ) |
{ |
SendOutData ('a', ADDRESS_ANY, 1, &i, 1); |
while( !rxd_buffer_locked && timer && lOk) |
lOk = !get_key_press (1 << KEY_ESC); |
if( timer ) |
{ |
Decode64 (); |
if( i == *pRxData ) |
{ |
lcd_frect( 16, 42+yoffs, (i+1)*3, 2, 1); // Progress |
memcpy( (uint8_t *) (AnalogNames+((current_hardware-1)*32*17)+(i*17)), (uint8_t *) pRxData + 1, 16); |
*(AnalogNames+((current_hardware-1)*32*17)+(i*17)+16) = 0; |
i++; |
t = 0; |
} |
else |
{ |
_delay_ms(100); |
} |
timer = ANALOGTIME; |
rxd_buffer_locked = FALSE; |
} |
else if( lOk ) |
{ // timeout occured |
t++; |
timer = ANALOGTIME; |
if( t >= 15 ) // timeout? |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
break; |
} |
} |
else |
{ |
lOk = !get_key_press( 1 << KEY_ESC ); |
} |
} |
if(lOk) AnalogNamesRead[current_hardware - 1] = i; |
//_delay_ms(4000); // DEBUG |
return lOk; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showLabels( uint8_t page) |
{ |
uint8_t i = 0; |
for( i = 0; i < PAGELINES; i++ ) |
{ |
if( (i + (page * PAGELINES)) >= 32 ) |
{ |
//lcdx_cls_rowwidth( y, width, mode, xoffs, yoffs ) |
lcdx_cls_rowwidth( i, 20, MNORMAL, XOFFS,YOFFS ); |
} |
else |
{ |
lcdx_print_at( 0, i, AnalogNames+((current_hardware-1)*32*17)+(i + page * PAGELINES)*17, MNORMAL, XOFFS,YOFFS); |
} |
} |
} |
//-------------------------------------------------------------- |
// |
//-------------------------------------------------------------- |
void MK_DebugData(void) |
{ |
uint8_t tmp_dat; |
uint8_t i = 0; |
uint8_t page = 0; |
uint8_t pagemax = (32/PAGELINES); |
uint8_t redraw = 2; |
DebugData_t *DebugData; |
// alloc ram |
AnalogNames = malloc( MKDD_ANALOGNAME_SIZE ); |
if( !AnalogNames ) |
{ |
Show_PKTError_NoRAM(); |
return; |
} |
memset( AnalogNames, 0, MKDD_ANALOGNAME_SIZE ); // init: AnalogNames |
AnalogNamesRead[0] = 0; |
AnalogNamesRead[1] = 0; |
SwitchToFC(); |
timer = TIMEOUT; |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
if( !timer ) |
{ |
MKDD_MemFree(); |
return; |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
timer1 = 0; |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//------------------------------------------ |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw==2 ) lcd_cls(); |
showTitle(); // Titelzeile |
lcd_rect_round( 0, (1*8)+1, 127, (6*8)-4, 1, R2); // Rahmen fuer 5 Zeilen Display |
showLabels( page ); |
lcd_printp_at( 0, 7, strGet(KEYLINE3), 0); |
lcd_write_number_u_at (5, 7, page + 1); |
lcd_printp_at( (current_hardware==FC ? 3 : 19), 7, strFC, 0); |
lcd_printp_at( (current_hardware==FC ? 19 : 3), 7, strNC, 0); |
redraw = false; |
} |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
DebugData = (DebugData_t *) pRxData; |
if( !timer1 ) |
{ |
for( i = 0; i < PAGELINES && (i + (page * PAGELINES)) < 32; i++) |
{ |
writex_ndigit_number_s( 21-6, i, DebugData->Analog[i + page * PAGELINES], 5, 0, MNORMAL, XOFFS,YOFFS); |
} |
timer1 = 25; // Anzeigeverzoegerung damit es nicht flackert |
} |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} |
if( !abo_timer ) |
{ // renew abo every ... sec |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//------------------------------------------ |
// PKT LiPo anzeigen |
//------------------------------------------ |
show_Lipo(); |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = 2; |
timer = TIMEOUT; |
} |
//------------------------------------------ |
// Tasten |
//------------------------------------------ |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
page--; |
if( page > pagemax ) page = pagemax; |
redraw = true; |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
page++; |
if( page > pagemax ) page = 0; |
redraw = true; |
} |
if( (hardware == NC) && get_key_press (1 << KEY_ENTER) ) |
{ |
if( current_hardware == NC ) SwitchToFC(); |
else SwitchToNC(); |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
tmp_dat = 10; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//page = 0; |
redraw = true; |
} |
} |
while (!get_key_press(1 << KEY_ESC) && timer); // Taste: Ende oder Timeout |
clear_key_all(); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); // Request Debugdata abschalten |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
//SwitchToNC(); |
// free ram |
MKDD_MemFree(); |
} |
#endif // end: #ifdef USE_MKDEBUGDATA |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkdebugdata.h |
---|
0,0 → 1,51 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.h' jetzt 'mk/mkdebugdata.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _MKDEBUGDATA_H |
#define _MKDEBUGDATA_H |
extern uint8_t AnalogNamesRead[2]; |
void MK_DebugData(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkdisplay.c |
---|
0,0 → 1,267 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.c |
//# |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Display() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - chg: MK_Display() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 28.04.2014 OG |
//# - chg: MK_Display() - leichte Optimierung der Display-Ausgabe um ein paar |
//# Bytes Code zu sparen |
//# |
//# 09.04.2014 OG - vervollstaendigte Steuerung des MK-Display's |
//# - chg: die Tasten fuer 'Ende' und NC/FC-Umschaltung muessen jetzt lange |
//# gedruekckt werden (get_key_long) - keine Beschriftung mehr dafuer |
//# vorhanden da die Beschriftung jetzt hoch/runter zeigt! |
//# (aber 'Ende' ist da wo es immer im PKT ist) |
//# - add: Unterstuetzung der hoch/runter Tasten |
//# - chg: diverse Aenderungen bei der Kommunkation und Einbindung aktueller |
//# Funktionen von mkbase.c |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.c' jetzt 'mk/mkdisplay.c' |
//# |
//# 25.01.2014 OG |
//# - optisches Facelift |
//# - PKT_CtrlHook() eingeklinkt |
//# - Versionshistorie hinzugefuegt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk-data-structs.h" |
#include "mkbase.h" |
#include "mkdisplay.h" |
#define TIMEOUT 500 // 5 sec |
uint8_t ACC_Display=false; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Display(void) |
{ |
uint8_t cmd; |
uint8_t redraw = true; |
uint8_t exit = false; |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
// cmd = 0xfc; // Home = first page |
cmd = 0xff; // aktuelle Seite |
redraw = true; |
MK_SwitchToFC(); // add 31.3.2014 Cebra, definierter Zustand |
if (ACC_Display==true) |
{ |
cmd = 0x06; |
SendOutData( 'l', ADDRESS_ANY, 1, &cmd,1); |
cmd = 0xff; |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
// _delay_ms(250); |
} |
do |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
ShowTitle_P( ( current_hardware == NC ? PSTR("NC") : PSTR("FC") ), true ); |
lcd_printp_at( 4, 0, PSTR("Display"), MINVERS); |
lcd_rect_round( 0, 2*7-3, 127, 5*7+3+3, 1, R2); // Rahmen |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
redraw = false; |
} |
show_Lipo(); |
SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
cmd = 0xff; |
_delay_ms(250); |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
// (Anzeige von unten nach oben) |
//------------------------------------------ |
rxd_buffer[83] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &rxd_buffer[3+60], MNORMAL, 5,2); |
rxd_buffer[63] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &rxd_buffer[3+40], MNORMAL, 5,1); |
rxd_buffer[43] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &rxd_buffer[3+20], MNORMAL, 5,0); |
rxd_buffer[23] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &rxd_buffer[3+ 0], MNORMAL, 5,-1); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) |
{ |
cmd = 0xfe; // key: rechts (next page) |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) |
{ |
cmd = 0xfd; // key: links (previous page) |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
cmd = 0xfb; // key: runter |
} |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
cmd = 0xf7; // key: hoch |
} |
if( (hardware == NC) && get_key_long(1 << KEY_ENTER) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
if( current_hardware == NC ) |
{ |
MK_SwitchToFC(); |
redraw = true; |
} |
else |
{ |
MK_SwitchToNC(); |
MK_SwitchToNC(); |
redraw = true; |
} |
// cmd = 0xff; |
timer2 = 50; |
while( timer2 > 0 ); |
} |
if( get_key_long(1 << KEY_ESC) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
exit = true; |
} |
} |
while( (!exit) && timer ); |
mode = 0; |
rxd_buffer_locked = FALSE; |
//------------------------------------------ |
// Timeout? |
//------------------------------------------ |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkdisplay.h |
---|
0,0 → 1,53 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.h' jetzt 'mk/mkdisplay.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _DISPLAY_H |
#define _DISPLAY_H |
extern uint8_t ACC_Display; |
void MK_Display(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkgpsinfo.c |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.c |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Gps_Info() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 14.05.2014 OG |
//# - chg: Code-Redesign; neues Screen-Layout; Code-Reduzierung; |
//# Anpassungen an neue PKT-Funktionen |
//# - chg: umbenannt von 'gps.c' zu 'mkgpsinfo.c' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "../messages.h" |
//############################################################## |
//############################################################## |
//#define DEBUG_MKGPS // erweiterte Anzeigen zum debuggen |
#define GPSTIMEOUT 300 // 3 Sekunden |
#define UBX_BUFFER_SIZE 100 // 100 Byte Buffer fuer GPS-Daten |
uint8_t ck_a = 0; |
uint8_t ck_b = 0; |
union long_union |
{ |
uint32_t dword; |
uint8_t byte[4]; |
} longUnion; |
union int_union |
{ |
uint16_t dword; |
uint8_t byte[2]; |
} intUnion; |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint32_t join_4_bytes(uint8_t Buffer[]) |
{ |
longUnion.byte[0] = *Buffer; |
longUnion.byte[1] = *(Buffer+1); |
longUnion.byte[2] = *(Buffer+2); |
longUnion.byte[3] = *(Buffer+3); |
return (longUnion.dword); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void checksum(uint8_t data) |
{ |
ck_a += data; |
ck_b += ck_a; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Gps_Info( void ) |
{ |
uint8_t UBX_buffer[UBX_BUFFER_SIZE]; |
uint8_t UBX_payload_counter = 0; |
uint8_t UBX_class = 0; |
uint8_t UBX_id = 0; |
uint8_t UBX_ck_a = 0; |
uint8_t data = 0; |
uint8_t length = 0; |
uint8_t state = 0; |
uint8_t redraw = true; |
uint8_t refresh = false; |
uint8_t yoffs = 1; // Anzeige-Verschiebung: der obere Bereich |
uint8_t yoffs2 = 4; // Anzeige-Verschiebung: GPS-Koordinaten |
int16_t v_16; // tmp-Variable |
int32_t v_32; // tmp-Variable |
#ifdef DEBUG_MKGPS |
uint8_t maxlen = 0; |
#endif |
MK_SwitchToNC(); // Anmerkung OG: warum auch immer es besser ist erst auf die NC |
MK_SwitchToGPS(); // umzuschalten... es laeuft so scheinbar zuverlaessiger |
MK_SwitchToGPS(); |
timer_mk_timeout = GPSTIMEOUT; |
do |
{ |
//------------------------ |
// REDRAW |
//------------------------ |
if( redraw ) |
{ |
ShowTitle_P( PSTR("MK GPS Info"), true); |
lcdx_printp_at( 0,1, PSTR("Fix:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,2, PSTR("Sat:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,3, PSTR("Alt:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 9,1, PSTR("PDOP:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,2, PSTR("Accu:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,3, PSTR("Sped:"), MNORMAL, 3,yoffs ); |
lcd_frect( 0, (4*7)+4+yoffs2, 127, 7, 1); // GPS: Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8+yoffs2); // GPS: "Breitengr Längengr" |
lcd_rect( 0, (3*8)+7+yoffs2, 127, (2*8)+4, 1); // GPS: Rahmen |
lcdx_printp_at(12,7, strGet(ENDE), MNORMAL, 0,1); // KEYLINE |
redraw = false; |
} |
//------------------------ |
// REFRESH |
//------------------------ |
if( refresh ) |
{ |
if((UBX_class == 1) && (UBX_id == 6)) // GPS: SVINFO |
{ |
//-------------- |
// GPS Status |
//-------------- |
switch( UBX_buffer[10] ) |
{ |
case 4: |
case 3: lcdx_printp_at( 5, 1, PSTR("3D"), MNORMAL, 1,yoffs); break; |
case 2: lcdx_printp_at( 5, 1, PSTR("2D"), MNORMAL, 1,yoffs); break; |
default: lcdx_printp_at( 5, 1, PSTR("no"), MNORMAL, 1,yoffs); |
} |
// GPS ok => ein Haken wird angezeigt |
if( (UBX_buffer[11] & 1) == 1 ) lcdx_putc( 7, 1, SYMBOL_CHECK, MNORMAL, 3,yoffs ); |
else lcdx_putc( 7, 1, ' ', MNORMAL, 3,yoffs ); |
//--- |
// Anzeige von "D" - evtl. DGPS (Differential GPS)? |
// aktuell nicht mehr unterstuetzt da kein Platz auf dem Screen |
//--- |
//if( (UBX_buffer[11] & 3) == 3 ) lcd_printp_at (10,0, PSTR("D"), 0); |
//else lcd_printp_at (10,0, PSTR(" "), 0); |
//-------------- |
// Sat |
//-------------- |
lcdx_printf_at_P( 5, 2, MNORMAL, 0,yoffs, PSTR("%2u"), UBX_buffer[47] ); |
//-------------- |
// PDOP |
//-------------- |
v_16 = UBX_buffer[44]+UBX_buffer[45]*255; |
lcdx_printf_at_P( 15, 1, MNORMAL, 0,yoffs, PSTR("%2.2d"), v_16 ); |
//-------------- |
// Accuracy |
//-------------- |
v_32 = (int32_t)join_4_bytes(&UBX_buffer[24]); |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,yoffs, PSTR("%2.2ldm"), v_32 ); |
} |
if((UBX_class == 1) && (UBX_id == 18)) // GPS: VELNED |
{ |
//-------------- |
// Speed |
//-------------- |
v_16 = (int16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
lcdx_printf_at_P( 15, 3, MNORMAL, 0,yoffs, PSTR("%3dkmH"), v_16 ); |
//uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
//write_ndigit_number_u (11, 4, speed, 3, 0,0); |
//lcd_printp_at (15,4, PSTR("km/h"), 0); |
} |
if((UBX_class == 1) && (UBX_id == 2)) // GPS: POSLLH |
{ |
//-------------- |
// Altitude |
//-------------- |
v_16 = (int16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//v_16 = v_16 + 3000; |
lcdx_printf_at_P( 4, 3, MNORMAL, 1,yoffs, PSTR("%4d"), v_16 ); |
//uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//write_ndigit_number_u (11, 7, height, 4, 0,0); |
//lcd_printp_at(16,7, PSTR("m"), 0); |
//-------------- |
// Breitengrad - Lat (51.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[8]); |
writex_gpspos( 1, 4, v_32 , MNORMAL, 0,10+yoffs2); // Anzeige: Breitengrad |
//write_ndigit_number_u ( 0, 7, (uint16_t)(lat/10000000), 2, 0,0); |
//lcd_printp_at ( 2, 7, PSTR("."), 0); |
//write_ndigit_number_u ( 3, 7, (uint16_t)((lat/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u ( 7, 7, (uint16_t)((lat/10) % 100), 2, 1,0); |
//-------------- |
// Laengengrad - Long (7.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[4]); |
writex_gpspos( 12, 4, v_32, MNORMAL, -1,10+yoffs2); // Anzeige: Laengengrad |
//write_ndigit_number_u (10, 7, (uint16_t)(lon/10000000), 2, 0,0); |
//lcd_printp_at (12, 7, PSTR("."), 0); |
//write_ndigit_number_u (13, 7, (uint16_t)((lon/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u (17, 7, (uint16_t)((lon/10) % 100), 2, 1,0); |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
refresh = false; |
} // end: if( refresh ) |
//-------------------------- |
// PROCESS DATA |
//-------------------------- |
if( uart_getc_nb( &data ) ) |
{ |
switch( state ) |
{ |
case 0: if( data == 0xB5 ) // GPS: init 1 |
{ |
UBX_payload_counter = 0; |
UBX_id = 0; |
UBX_class = 0; |
ck_a = 0; |
ck_b = 0; |
state++; |
} |
break; |
case 1: if( data == 0x62 ) // GPS: init 2 |
state++; |
else |
state = 0; |
break; |
case 2: if( data != 1 ) // GPS: class |
state = 0; |
else |
{ |
checksum(data); |
UBX_class = data; |
state++; |
} |
break; |
case 3: if( (data != 48) // GPS: id |
&&(data != 6 ) |
&&(data != 18) |
&&(data != 2 )) |
{ |
state = 0; |
} |
else |
{ |
UBX_id = data; |
checksum(data); |
state++; |
} |
break; |
case 4: if( data >= UBX_BUFFER_SIZE ) // GPS: length lo |
state = 0; |
else |
{ |
checksum(data); |
length = data; |
state++; |
} |
#ifdef DEBUG_MKGPS |
// DEBUG - Anzeige der Paketgroesse |
if( data > maxlen ) |
{ |
// groesstes gemessenes Paket lag bei 181 |
// ob man das Paket braucht ist eine andere Frage |
maxlen = data; |
lcdx_printf_at_P( 1, 7, MNORMAL, 0,1, PSTR("%3u"), maxlen ); |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
} |
#endif |
break; |
case 5: if( data != 0 ) // GPS: length hi |
state = 0; |
else |
{ |
checksum(data); |
state++; |
} |
break; |
case 6: UBX_buffer[UBX_payload_counter] = data; // GPS: data |
checksum(data); |
UBX_payload_counter++; |
length--; |
if( length == 0 ) |
{ |
state++; |
}; |
break; |
case 7: state++; // GPS: check lo |
UBX_ck_a = data; |
break; |
case 8: state = 0; // GPS: check hi |
if((UBX_ck_a == ck_a) && (data == ck_b)) |
{ |
timer_mk_timeout = GPSTIMEOUT; |
refresh = true; |
} |
} // end: switch( state ) |
} // end: if( uart_getc_nb(&data) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
/* |
// funktioniert hier nicht - warum auch immer |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
*/ |
} while( !get_key_press(1 << KEY_ESC) && timer_mk_timeout ); |
//-------------------------- |
// in den Timeout gelaufen? |
//-------------------------- |
if( !timer_mk_timeout ) |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
SwitchToNC(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkgpsinfo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.h |
//# |
//# 14.05.2014 OG |
//# - chg: umbenannt von 'gps.h' zu 'mkgpsinfo.h' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef _GPS_H |
#define _GPS_H |
void MK_Gps_Info( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk-data-structs.h |
---|
0,0 → 1,501 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mk_data-stucts.h |
//# |
//# 16.07.2015 Cebra |
//# - chg: Receiver Selection |
//# RECEIVER_USER 8 -> RECEIVER_MLINK 8 |
//# - add: Receiver Selection |
//# RECEIVER_USER 9 |
//# |
//# 26.01.2015 Cebra |
//# - chg: #defines für ServoCompInvert an aktuelle Syntax angepasst |
//# SVNick -> SERVO_NICK_INV |
//# SVRoll -> SERVO_ROLL_INV |
//# SVRelMov -> SERVO_RELATIVE |
//# - add: #define CH_DIRECTION_1 0x08 für ComingHome Ausrichtung |
//# #define CH_DIRECTION_2 0x10 |
//# |
//# 19.06.2014 OG |
//# - chg: Aktualisiert: struct Point_t (aus Quelle: NaviCtrl V2.06f/waypoints.h - Revision 546) |
//# |
//# 12.02.2014 OG |
//# - del: die alten defines bzgl. MKVERSIONnnn und mk_param_struct_t entfernt |
//# |
//# 02.12.2013 Cebra |
//# - add: #define MKVERSION201f, Erweiterung für FC201f |
//# |
//# 21.10.2013 Cebra |
//# - add: #define MKVERSION201a, Erweiterung für FC201a |
//# |
//# 26.06.2013 OG |
//# - chg: "0.91f" -> "0.91f/l" |
//# |
//# 13.06.2013 OG |
//# - chg: "0.90j" -> "0.90h/j" |
//# - chg: Code-Layout |
//# |
//# 05.06.2013 Cebra |
//# - chg: Anpassungen an FC091c |
//# |
//# 03.06.2013 Cebra |
//# - chg: #define Fehler beseitigt |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a, Erweiterung für FC091a |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 11.05.2013 OG |
//# - add: DateTime_t |
//# |
//# 20.03.2013 OG |
//# - add: BLData_t (http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct) |
//############################################################################ |
#ifndef _MK_DATA_STRUCTS_H |
#define _MK_DATA_STRUCTS_H |
#include "main.h" |
//################################################################################## |
//################################################################################## |
#define u8 uint8_t |
#define s8 int8_t |
#define u16 uint16_t |
#define s16 int16_t |
#define u32 uint32_t |
#define s32 int32_t |
#define NUMBER_OF_DEBUG_DATAS 32 |
#define ANALOG_NAME_LENGTH 16 |
// Version of supported serial protocol |
#define MIN_VERSION 7 |
#define MAX_VERSION 10 |
// Setting index |
#define SETTING_1 1 |
#define SETTING_2 2 |
#define SETTING_3 3 |
#define SETTING_4 4 |
#define SETTING_5 5 |
#define SETTING_CURRENT 0xff |
// MikroKopter defines |
// taken from |
// FC Software eeprom.h |
// |
//GlobalConfig3 aus FC/eeprom.h |
#define CFG3_NO_SDCARD_NO_START 0x01 |
#define CFG3_DPH_MAX_RADIUS 0x02 |
#define CFG3_VARIO_FAILSAFE 0x04 |
#define CFG3_MOTOR_SWITCH_MODE 0x08 //FC0.88L 7.5.12 |
#define CFG3_NO_GPSFIX_NO_START 0x10 //FC0.88L 7.5.12 |
#define CFG3_USE_NC_FOR_OUT1 0x20 |
#define CFG3_SPEAK_ALL 0x40 |
#define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//GlobalConfig |
#define CFG_HOEHENREGELUNG 0x01 |
#define CFG_HOEHEN_SCHALTER 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_KOMPASS_AKTIV 0x08 |
#define CFG_KOMPASS_FIX 0x10 |
#define CFG_GPS_AKTIV 0x20 |
#define CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define CFG_DREHRATEN_BEGRENZER 0x80 |
//Bitconfig MAsk |
#define CFG_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
#define CFG_MOTOR_BLINK1 0x10 |
#define CFG_MOTOR_OFF_LED1 0x20 |
#define CFG_MOTOR_OFF_LED2 0x40 |
#define CFG_MOTOR_BLINK2 0x80 |
// ServoCompInvert, FC0.89 entfällt und geändert s.u. |
//#define SVNick 0x01 |
//#define SVRoll 0x02 |
//#define SVRelMov 0x04 |
//ab FC 209a (26.1.2015 cebra) |
// bitcoding for EE_Parameter.ServoCompInvert |
#define SERVO_NICK_INV 0x01 |
#define SERVO_ROLL_INV 0x02 |
#define SERVO_RELATIVE 0x04 // direct poti control or relative moving of the servo value |
#define CH_DIRECTION_1 0x08 |
#define CH_DIRECTION_2 0x10 |
//CH Orientation ServoBits 0x10 0x08 |
// --> no change 0 0 |
// --> front to starting point 0 1 |
// --> rear to to starting point 1 0 |
//-> start orientation 1 1 |
// ExtraConfig |
#define CFG2_HEIGHT_LIMIT 0x01 |
#define CFG2_VARIO_BEEP 0x02 |
#define CFG_SENSITIVE_RC 0x04 // 26.1.2015 entfällt ab FC 209a, cebra |
#define CFG_3_3V_REFERENCE 0x08 |
#define CFG_NO_RCOFF_BEEPING 0x10 |
#define CFG_GPS_AID 0x20 |
#define CFG_LEARNABLE_CAREFREE 0x40 |
#define CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
// bit mask for ParamSet.Config0 |
#define CFG0_AIRPRESS_SENSOR 0x01 |
#define CFG0_HEIGHT_SWITCH 0x02 |
#define CFG0_HEADING_HOLD 0x04 |
#define CFG0_COMPASS_ACTIVE 0x08 |
#define CFG0_COMPASS_FIX 0x10 |
#define CFG0_GPS_ACTIVE 0x20 |
#define CFG0_AXIS_COUPLING_ACTIVE 0x40 |
#define CFG0_ROTARY_RATE_LIMITER 0x80 |
// defines for the receiver selection |
#define RECEIVER_PPM 0 |
#define RECEIVER_SPEKTRUM 1 |
#define RECEIVER_SPEKTRUM_HI_RES 2 |
#define RECEIVER_SPEKTRUM_LOW_RES 3 |
#define RECEIVER_JETI 4 |
#define RECEIVER_ACT_DSL 5 |
#define RECEIVER_HOTT 6 |
#define RECEIVER_SBUS 7 |
#define RECEIVER_MLINK 8 //FC2.11 16.7.2015 CB |
#define RECEIVER_USER 9 //FC2.11 16.7.2015 CB |
#define RECEIVER_UNKNOWN 0xFF |
// MikroKopter Flags |
// taken from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.73d%2Ffc.h |
//alt 0.86 |
//#define FCFLAG_MOTOR_RUN 0x01 |
//#define FCFLAG_FLY 0x02 |
//#define FCFLAG_CALIBRATE 0x04 |
//#define FCFLAG_START 0x08 |
//#define FCFLAG_NOTLANDUNG 0x10 |
//#define FCFLAG_LOWBAT 0x20 |
//#define FCFLAG_SPI_RX_ERR 0x40 |
//#define FCFLAG_I2CERR 0x80 |
// FC_StatusFlags 0.88 |
#define FC_STATUS_MOTOR_RUN 0x01 |
#define FC_STATUS_FLY 0x02 |
#define FC_STATUS_CALIBRATE 0x04 |
#define FC_STATUS_START 0x08 |
#define FC_STATUS_EMERGENCY_LANDING 0x10 |
#define FC_STATUS_LOWBAT 0x20 |
#define FC_STATUS_VARIO_TRIM_UP 0x40 |
#define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
// FC_StatusFlags2 |
#define FC_STATUS2_CAREFREE 0x01 |
#define FC_STATUS2_ALTITUDE_CONTROL 0x02 |
#define FC_STATUS2_RC_FAILSAVE_ACTIVE 0x04 |
#define FC_STATUS2_OUT1_ACTIVE 0x08 |
#define FC_STATUS2_OUT2_ACTIVE 0x10 |
// NaviCtrl Flags |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
#define NC_FLAG_FREE 0x01 |
#define NC_FLAG_PH 0x02 |
#define NC_FLAG_CH 0x04 |
#define NC_FLAG_RANGE_LIMIT 0x08 |
#define NC_FLAG_NOSERIALLINK 0x10 |
#define NC_FLAG_TARGET_REACHED 0x20 |
#define NC_FLAG_MANUAL_CONTROL 0x40 |
#define NC_FLAG_GPS_OK 0x80 |
typedef struct |
{ |
unsigned char SWMajor; |
unsigned char SWMinor; |
unsigned char ProtoMajor; |
unsigned char ProtoMinor; |
unsigned char SWPatch; |
unsigned char HardwareError[5]; |
} __attribute__((packed)) Version_t; |
// FC Debug Struct |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.h |
typedef struct // 0.86 |
{ |
uint8_t Digital[2]; |
// NC: unsigned; FC: signed !!!! |
int16_t Analog[32]; // Debugvalues |
} __attribute__((packed)) DebugData_t; |
//****************************************************************** |
// uart1.h NC 0.87, zur Zeit hier nicht verwendet 28.01.2012 CB |
#define AMPEL_FC 0x01 |
#define AMPEL_BL 0x02 |
#define AMPEL_NC 0x04 |
#define AMPEL_COMPASS 0x08 |
typedef struct //0.87 |
{ |
u8 StatusGreen; |
u8 StatusRed; |
u16 Analog[32]; // Debugwerte |
} __attribute__((packed)) DebugOut_t; |
//****************************************************************** |
// NaviCtrl OSD Structs |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
typedef struct //NC uart1.h |
{ |
s16 AngleNick; // in 0.1 deg |
s16 AngleRoll; // in 0.1 deg |
s16 Heading; // in 0.1 deg |
u8 StickNick; |
u8 StickRoll; |
u8 StickYaw; |
u8 StickGas; |
u8 reserve[4]; |
} __attribute__((packed)) Data3D_t; |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
typedef struct |
{ |
u16 Distance; // distance to target in cm |
s16 Bearing; // course to target in deg |
} __attribute__((packed)) GPS_PosDev_t; |
//---------------------------- |
// aus NC waypoint.h |
// |
// AKTUALISIERT: 18.06.2014 OG |
// |
// Anmerkung 18.06.2014 OG |
// "Name" hinzugefuegt - das gibt es |
// seit dem 23.03.2012! |
//---------------------------- |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // Will be transferred to the FC and can be used as Poti value there |
u8 AltitudeRate; // rate to change the setpoint in steps of 0.1m/s |
u8 Speed; // rate to change the Position(0 = max) in steps of 0.1m/s |
u8 CamAngle; // Camera servo angle in degree (255 -> POI-Automatic) |
u8 Name[4]; // Name of that point (ASCII) |
u8 reserve[2]; // reserve |
} __attribute__((packed)) Point_t; |
//---------------------------- |
// aus NC waypoint.h |
// VERALTET! 18.06.2014 OG |
//---------------------------- |
/* |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // |
u8 AltitudeRate; // rate to change the setpoint |
u8 Speed; // rate to change the Position |
u8 CamAngle; // Camera servo angle |
u8 reserve[6]; // reserve |
} __attribute__((packed)) Point_t; |
*/ |
// NaviCtrl struct |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
#define NAVIDATA_VERSION 5 |
typedef struct |
{ |
u8 Version; // version of the data structure |
GPS_Pos_t CurrentPosition; // see ubx.h for details |
GPS_Pos_t TargetPosition; |
GPS_PosDev_t TargetPositionDeviation; |
GPS_Pos_t HomePosition; |
GPS_PosDev_t HomePositionDeviation; |
u8 WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
u8 WaypointNumber; // number of stored waypoints |
u8 SatsInUse; // number of satellites used for position solution |
s16 Altimeter; // hight according to air pressure |
s16 Variometer; // climb(+) and sink(-) rate |
u16 FlyingTime; // in seconds |
u8 UBat; // Battery Voltage in 0.1 Volts |
u16 GroundSpeed; // speed over ground in cm/s (2D) |
s16 Heading; // current flight direction in ° as angle to north |
s16 CompassHeading; // current compass value in |
s8 AngleNick; // current Nick angle in 1 |
s8 AngleRoll; // current Roll angle in 1 |
u8 RC_Quality; // RC_Quality |
u8 FCStatusFlags; // Flags from FC |
u8 NCFlags; // Flags from NC |
u8 Errorcode; // 0 --> okay |
u8 OperatingRadius; // current operation radius around the Home Position in m |
s16 TopSpeed; // velocity in vertical direction in cm/s |
u8 TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
u8 FCStatusFlags2; // StatusFlags2 (since version 5 added) |
s16 SetpointAltitude; // setpoint for altitude |
u8 Gas; // for future use |
u16 Current; // actual current in 0.1A steps |
u16 UsedCapacity; // used capacity in mAh |
} __attribute__((packed)) NaviData_t; |
typedef struct |
{ |
uint8_t Version; // the version of the BL (0 = old) |
uint8_t SetPoint; // written by attitude controller |
uint8_t SetPointLowerBits; // for higher Resolution of new BLs |
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present |
uint8_t ReadMode; // select data to read |
// the following bytes must be exactly in that order! |
uint8_t Current; // in 0.1 A steps, read back from BL |
uint8_t MaxPWM; // read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
int8_t Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in �C |
} __attribute__((packed)) MotorData_t; |
typedef struct |
{ |
uint8_t Revision; // must be BL_REVISION |
uint8_t SetMask; // settings mask |
uint8_t PwmScaling; // maximum value of control pwm, acts like a thrust limit |
uint8_t CurrentLimit; // current limit in A |
uint8_t TempLimit; // in �C |
uint8_t CurrentScaling; // scaling factor for current measurement |
uint8_t BitConfig; // see defines above |
uint8_t crc; // checksum |
} __attribute__((packed)) BLConfig_t; |
//############################################################################## |
//# Datenstruktur von ehemals mk_param_struct_t jetzt via mk/paramset.c ! |
//############################################################################## |
//------------------------------------- |
// 20.03.2013 OG: BLData_t |
// aus: http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct |
//------------------------------------- |
typedef struct |
{ |
unsigned char Index; // address of BL-Ctrl |
unsigned char Current; // in 0.1 A steps, read back from BL |
unsigned char Temperature; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
unsigned char MaxPWM; // EVENTUELL: read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
unsigned char Status; // EVENTUELL: 7 bit for I2C error counter, highest bit indicates if motor is present |
} BLData_t; |
//------------------------------------- |
// 11.05.2013 OG: DateTime_t |
// aus: NC v0.30g timer1.h |
//------------------------------------- |
typedef struct{ |
u16 Year; |
u8 Month; |
u8 Day; |
u8 Hour; |
u8 Min; |
u8 Sec; |
u16 mSec; |
u8 Valid; |
} DateTime_t; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/mkparameters.c |
---|
0,0 → 1,2692 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.c |
//# |
//# 16.07.2015 Cebra |
//# - add: Erweiterung SingleWpControlChannel; (FC2.11a) |
//# MenuKeyChannel; (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: Erweiterung paramEditItemTable und ID_MENU_KAMERA_Items[] um neue Parameter (FC2.09j) |
//# param_ServoNickFailsave, param_ServoRollFailsave, param_Servo3Failsave, param_Servo4Failsave, param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: Comming Home Ausrichtung hinzugefügt, |
//# neue Einstellungen ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: im Menü Höhe -> param_Hoehe_TiltCompensation, FC207d |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Parameters_MenuMain() eine Menue-Trennlinie hinter Favoriten eingefuegt |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_Favoriten() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# |
//# 10.05.2014 OG |
//# - del: editDisableDeclCalc() - wurde ersetzt durch eine Transform-Funktion |
//# in paramset.c und wird jetzt von editGeneric() bearbeitet |
//# |
//# 07.05.2014 OG |
//# - chg: Menu_Favoriten() - uebernimmt den gegebenen Menuetitel vom |
//# uebergeordneten Eintrag aus mkparameters_messages.h (also Multilanguage) |
//# - chg: fav_add() - erweitert um Menue-Separatoren (Trennlinien) aufzunehmen |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: Favoriten-Verwaltung implementiert (Aenderungen an verschiedenen Funktionen) |
//# |
//# 18.04.2014 OG |
//# - fix: im ID_MENU_NAVICTRL fehlten param_NaviStickThreshold ("GPS Stick-Schwelle") |
//# und param_NaviGpsMinSat ("Min. Sat") |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Menu_EditCategory() blendet ggf. doppelt aufeinanderfolgenden |
//# Menue-Separatoren aus |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktioionen del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - add: Unterstuetzung fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 27.03.2014 OG |
//# kompletter neuer Code fuer ein erstes Release |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 20.02.2014 OG |
//# - chg: MK_Parameters_Menu() meldet "nicht verfügbar" |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mkparameters_messages.h" |
//############################################################################################# |
//# Strukturen; Forward-Deklarationen |
//############################################################################################# |
#define DEBUG_PARAMEDIT // schaltet zusaetzliche Debug-Ausgaben ein |
#define EOF 255 // End Of File (bzw. end of Table...) |
#define SEPARATOR 254 // ID fuer einen Separator in einem Menue (Trennlinie) |
#define SCREEN_REFRESH 1 // neuzeichnen/refresh der Anzeige |
#define SCREEN_REDRAW 2 // neuzeichnen/refresh der Anzeige |
//-------------------------------------------- |
// editGenericCode_t |
// deklariert ein einzelnes Code-Zeichen fuer Edit-Generic |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char code; // z.B. '0', '1', 'v', 'P', 'C' ... (einzelnes Zeichen aus paramEditItem_t.format ) |
unsigned char min; |
unsigned char max; |
const char *shortText_de; // |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
//--------------------------------------------- |
//----- globale Modulvariablen |
//--------------------------------------------- |
editGenericCode_t genericCode; // Daten eines einzelnen Code-Zeichen (im RAM) |
paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_SHORT_VALUE_de[] PROGMEM = "%3u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
#define GENERIC_SHORT_VALUE_en GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_de GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_en GENERIC_SHORT_VALUE_de |
// |
//static const char GENERIC_SHORT_VALUE_ACCZ_de[] PROGMEM = "%4u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
//#define GENERIC_SHORT_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_de GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
static const char GENERIC_SHORT_NO_de[] PROGMEM = " N"; |
#define GENERIC_SHORT_NO_en GENERIC_SHORT_NO_de |
static const char GENERIC_LONG_NO_de[] PROGMEM = "Nein"; |
static const char GENERIC_LONG_NO_en[] PROGMEM = "No"; |
static const char GENERIC_SHORT_YES_de[] PROGMEM = " J"; |
static const char GENERIC_SHORT_YES_en[] PROGMEM = " Y"; |
static const char GENERIC_LONG_YES_de[] PROGMEM = "Ja"; |
static const char GENERIC_LONG_YES_en[] PROGMEM = "Yes"; |
static const char GENERIC_SHORT_POTI_de[] PROGMEM = " P%u"; |
#define GENERIC_SHORT_POTI_en GENERIC_SHORT_POTI_de |
static const char GENERIC_LONG_POTI_de[] PROGMEM = "Poti %u"; |
#define GENERIC_LONG_POTI_en GENERIC_LONG_POTI_de |
static const char GENERIC_SHORT_CHANNEL_de[] PROGMEM = "C%2u"; |
#define GENERIC_SHORT_CHANNEL_en GENERIC_SHORT_CHANNEL_de |
static const char GENERIC_LONG_CHANNEL_de[] PROGMEM = "Kanal %u"; |
static const char GENERIC_LONG_CHANNEL_en[] PROGMEM = "Channel %u"; |
static const char GENERIC_SHORT_SERCHANNEL_de[] PROGMEM = "S%2u"; |
#define GENERIC_SHORT_SERCHANNEL_en GENERIC_SHORT_SERCHANNEL_de |
static const char GENERIC_LONG_SERCHANNEL_de[] PROGMEM = "Ser. Kanal %u"; |
static const char GENERIC_LONG_SERCHANNEL_en[] PROGMEM = "Ser. Channel %u"; |
static const char GENERIC_SHORT_AUS_de[] PROGMEM = "Aus"; |
static const char GENERIC_SHORT_AUS_en[] PROGMEM = "Off"; |
#define GENERIC_LONG_AUS_de GENERIC_SHORT_AUS_de |
#define GENERIC_LONG_AUS_en GENERIC_SHORT_AUS_en |
static const char GENERIC_SHORT_INACTIV_de[] PROGMEM = "Ina"; |
#define GENERIC_SHORT_INACTIV_en GENERIC_SHORT_INACTIV_de |
static const char GENERIC_LONG_INACTIV_de[] PROGMEM = "Inaktiv"; |
#define GENERIC_LONG_INACTIV_en GENERIC_LONG_INACTIV_de |
static const char GENERIC_SHORT_WPEVENT_de[] PROGMEM = "WPE"; |
#define GENERIC_SHORT_WPEVENT_en GENERIC_SHORT_WPEVENT_de |
static const char GENERIC_LONG_WPEVENT_de[] PROGMEM = "WP-Event"; |
#define GENERIC_LONG_WPEVENT_en GENERIC_LONG_WPEVENT_de |
static const char GENERIC_SHORT_MINIMUM_de[] PROGMEM = "Min"; |
#define GENERIC_SHORT_MINIMUM_en GENERIC_SHORT_MINIMUM_de |
static const char GENERIC_LONG_MINIMUM_de[] PROGMEM = "Minimum"; |
#define GENERIC_LONG_MINIMUM_en GENERIC_LONG_MINIMUM_de |
static const char GENERIC_SHORT_MIDDLE_de[] PROGMEM = "Mid"; |
#define GENERIC_SHORT_MIDDLE_en GENERIC_SHORT_MIDDLE_de |
static const char GENERIC_LONG_MIDDLE_de[] PROGMEM = "Mitte"; |
static const char GENERIC_LONG_MIDDLE_en[] PROGMEM = "Middle"; |
static const char GENERIC_SHORT_MAXIMUM_de[] PROGMEM = "Max"; |
#define GENERIC_SHORT_MAXIMUM_en GENERIC_SHORT_MAXIMUM_de |
static const char GENERIC_LONG_MAXIMUM_de[] PROGMEM = "Maximum"; |
#define GENERIC_LONG_MAXIMUM_en GENERIC_LONG_MAXIMUM_de |
static const char GENERIC_SHORT_AN_de[] PROGMEM = " An"; |
static const char GENERIC_SHORT_AN_en[] PROGMEM = " On"; |
#define GENERIC_LONG_AN_de GENERIC_SHORT_AN_de |
#define GENERIC_LONG_AN_en GENERIC_SHORT_AN_en |
static const char GENERIC_SHORT_FREE_de[] PROGMEM = "Fre"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_FREE_en GENERIC_SHORT_FREE_de |
static const char GENERIC_LONG_FREE_de[] PROGMEM = "Free"; |
#define GENERIC_LONG_FREE_en GENERIC_LONG_FREE_de |
static const char GENERIC_SHORT_CH_de[] PROGMEM = " CH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_CH_en GENERIC_SHORT_CH_de |
static const char GENERIC_LONG_CH_de[] PROGMEM = "Coming Home (CH)"; |
#define GENERIC_LONG_CH_en GENERIC_LONG_CH_de |
static const char GENERIC_SHORT_PH_de[] PROGMEM = " PH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_PH_en GENERIC_SHORT_PH_de |
static const char GENERIC_LONG_PH_de[] PROGMEM = "Position Hold (PH)"; |
#define GENERIC_LONG_PH_en GENERIC_LONG_PH_de |
static const char GENERIC_SHORT_DISABLED_de[] PROGMEM = "Dis"; // z.b. "Auto Start / Land" |
#define GENERIC_SHORT_DISABLED_en GENERIC_SHORT_DISABLED_de |
static const char GENERIC_LONG_DISABLED_de[] PROGMEM = "Disabled"; |
#define GENERIC_LONG_DISABLED_en GENERIC_LONG_DISABLED_de |
static const char GENERIC_SHORT_OUT_de[] PROGMEM = "Ou%1u"; // z.B. Servo 3 und Servo 4 |
#define GENERIC_SHORT_OUT_en GENERIC_SHORT_OUT_de |
static const char GENERIC_LONG_OUT_de[] PROGMEM = "Out %1u"; |
#define GENERIC_LONG_OUT_en GENERIC_LONG_OUT_de |
static const char GENERIC_SHORT_CH_OR_NC_de[] PROGMEM = "kÄn"; |
static const char GENERIC_SHORT_CH_OR_NC_en[] PROGMEM = "nCh"; |
static const char GENERIC_LONG_CH_OR_NC_de[] PROGMEM = "Keine Änderung"; |
static const char GENERIC_LONG_CH_OR_NC_en[] PROGMEM = "no change"; |
static const char GENERIC_SHORT_CH_OR_FH_de[] PROGMEM = "FzH"; |
static const char GENERIC_SHORT_CH_OR_FH_en[] PROGMEM = "FtH"; |
static const char GENERIC_LONG_CH_OR_FH_de[] PROGMEM = "Front zu Home"; |
static const char GENERIC_LONG_CH_OR_FH_en[] PROGMEM = "front to home"; |
static const char GENERIC_SHORT_CH_OR_RH_de[] PROGMEM = "HzH"; |
static const char GENERIC_SHORT_CH_OR_RH_en[] PROGMEM = "RtH"; |
static const char GENERIC_LONG_CH_OR_RH_de[] PROGMEM = "Heck zu Home"; |
static const char GENERIC_LONG_CH_OR_RH_en[] PROGMEM = "rear to home"; |
static const char GENERIC_SHORT_CH_OR_SO_de[] PROGMEM = "wSt"; |
static const char GENERIC_SHORT_CH_OR_SO_en[] PROGMEM = "sOr"; |
static const char GENERIC_LONG_CH_OR_SO_de[] PROGMEM = "wie beim Start"; |
static const char GENERIC_LONG_CH_OR_SO_en[] PROGMEM = "same as start"; |
//------------------------------------------------ |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char code; // '0', '1', 'v', 'P', 'C' ... |
unsigned char min; |
unsigned char max; |
const char *shortText_de; |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
*************************************************/ |
// Anmerkung: kann bei Bedarf evtl. zu PROGMEM umschreiben |
//editGenericCode_t const editGenericCode[] PROGMEM = |
editGenericCode_t editGenericCode[] = |
{ |
{ 'v', 0, 0, GENERIC_SHORT_VALUE_de, GENERIC_SHORT_VALUE_en, GENERIC_LONG_VALUE_de, GENERIC_LONG_VALUE_en }, // nur Wert anzeigen - min/max wird uebergeordnet definiert |
// { 'z', 0, 0, GENERIC_SHORT_VALUE_ACCZ_de, GENERIC_SHORT_VALUE_ACCZ_en, GENERIC_LONG_VALUE_ACCZ_de, GENERIC_LONG_VALUE_ACCZ_en }, // nur Wert anzeigen x 4- min/max wird uebergeordnet definiert |
{ '0', 0, 0, GENERIC_SHORT_NO_de, GENERIC_SHORT_NO_en, GENERIC_LONG_NO_de, GENERIC_LONG_NO_en }, |
{ '1', 1, 1, GENERIC_SHORT_YES_de, GENERIC_SHORT_YES_en, GENERIC_LONG_YES_de, GENERIC_LONG_YES_en }, |
{ 'P', 255,248, GENERIC_SHORT_POTI_de, GENERIC_SHORT_POTI_en, GENERIC_LONG_POTI_de, GENERIC_LONG_POTI_en }, |
{ 'C', 1, 16, GENERIC_SHORT_CHANNEL_de, GENERIC_SHORT_CHANNEL_en, GENERIC_LONG_CHANNEL_de, GENERIC_LONG_CHANNEL_en }, |
{ 'S', 17, 28, GENERIC_SHORT_SERCHANNEL_de, GENERIC_SHORT_SERCHANNEL_en, GENERIC_LONG_SERCHANNEL_de, GENERIC_LONG_SERCHANNEL_en }, |
{ 'A', 0, 0, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, |
{ 'I', 0, 0, GENERIC_SHORT_INACTIV_de, GENERIC_SHORT_INACTIV_en, GENERIC_LONG_INACTIV_de, GENERIC_LONG_INACTIV_en }, |
{ 'W', 29, 29, GENERIC_SHORT_WPEVENT_de, GENERIC_SHORT_WPEVENT_en, GENERIC_LONG_WPEVENT_de, GENERIC_LONG_WPEVENT_en }, |
{ 'U', 30, 30, GENERIC_SHORT_MINIMUM_de, GENERIC_SHORT_MINIMUM_en, GENERIC_LONG_MINIMUM_de, GENERIC_LONG_MINIMUM_en }, |
{ 'M', 31, 31, GENERIC_SHORT_MIDDLE_de, GENERIC_SHORT_MIDDLE_en, GENERIC_LONG_MIDDLE_de, GENERIC_LONG_MIDDLE_en }, |
{ 'X', 32, 32, GENERIC_SHORT_MAXIMUM_de, GENERIC_SHORT_MAXIMUM_en, GENERIC_LONG_MAXIMUM_de, GENERIC_LONG_MAXIMUM_en }, |
{ 'O', 30, 30, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, // ein weiteres "Aus"; z.b. "Motors-Sicherheitsschalter" |
{ 'N', 31, 31, GENERIC_SHORT_AN_de, GENERIC_SHORT_AN_en, GENERIC_LONG_AN_de, GENERIC_LONG_AN_en }, // z.b. "Carefree" |
{ 'F', 30, 30, GENERIC_SHORT_FREE_de, GENERIC_SHORT_FREE_en, GENERIC_LONG_FREE_de, GENERIC_LONG_FREE_en }, // Free - z.b. "Carefree" |
{ 'H', 31, 31, GENERIC_SHORT_CH_de, GENERIC_SHORT_CH_en, GENERIC_LONG_CH_de, GENERIC_LONG_CH_en }, // Coming Home - z.b. "Carefree" |
{ 'Q', 32, 32, GENERIC_SHORT_PH_de, GENERIC_SHORT_PH_en, GENERIC_LONG_PH_de, GENERIC_LONG_PH_en }, // Positiosn Hold - z.b. "Carefree" |
{ 'D', 0, 0, GENERIC_SHORT_DISABLED_de, GENERIC_SHORT_DISABLED_en, GENERIC_LONG_DISABLED_de, GENERIC_LONG_DISABLED_en }, // Inaktiv - z.b. "Auto Start / Land" |
{ 'T', 247,246, GENERIC_SHORT_OUT_de, GENERIC_SHORT_OUT_en, GENERIC_LONG_OUT_de, GENERIC_LONG_OUT_en }, |
{ 'K', 0, 0, GENERIC_SHORT_CH_OR_NC_de, GENERIC_SHORT_CH_OR_NC_en, GENERIC_LONG_CH_OR_NC_de, GENERIC_LONG_CH_OR_NC_en }, // CommingHome Orientation, |
{ 'V', 1, 1, GENERIC_SHORT_CH_OR_FH_de, GENERIC_SHORT_CH_OR_FH_en, GENERIC_LONG_CH_OR_FH_de, GENERIC_LONG_CH_OR_FH_en }, // CommingHome Orientation, |
{ 'R', 2, 2, GENERIC_SHORT_CH_OR_RH_de, GENERIC_SHORT_CH_OR_RH_en, GENERIC_LONG_CH_OR_RH_de, GENERIC_LONG_CH_OR_RH_en }, // CommingHome Orientation, |
{ 'G', 3, 3, GENERIC_SHORT_CH_OR_SO_de, GENERIC_SHORT_CH_OR_SO_en, GENERIC_LONG_CH_OR_SO_de, GENERIC_LONG_CH_OR_SO_en }, // CommingHome Orientation, |
{ EOF, 0, 0, 0,0,0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_NoYes[] PROGMEM = "01"; // Nein, Ja (z.B. (fast) alle Checkboxen) |
static const char GENERIC_Value[] PROGMEM = "v"; // value mit min,max aus paramEditItem (in paramEditItemTable) (z.B. Nick/Roll P (0..20) / Min. Gas (0..247) ) |
static const char GENERIC_ValueACCZ[] PROGMEM = "D"; // value mit min,max aus paramEditItem (in paramEditItemTable) x 4 (ACC Z Landing pulse ) |
static const char GENERIC_ValuePoti[] PROGMEM = "vP"; // value, Poti (z.B. Gyro Gier P) |
static const char GENERIC_ValueOutPoti[] PROGMEM = "vTP"; // value, Out, Poti (z.B. Servo 3 & 4) |
static const char GENERIC_Cannel[] PROGMEM = "C"; // Channel (z.B. Gas / Gier / Nick / Roll) |
static const char GENERIC_AusChSerWpeMMM[] PROGMEM = "ACSWUMX"; // Aus, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum (z.B. Poti 1..8) |
static const char GENERIC_DisChSerWpeOff[] PROGMEM = "DCSWO"; // Disabled, Channel, Ser. Channel, WP Event, Off (z.B. Motors-Sicherheitsschalter) |
static const char GENERIC_DisChSerWpeOffOn[] PROGMEM = "DCSWON"; // Disabled, Channel, Ser. Channel, WP Event, Off, On (z.B. Carefree) |
static const char GENERIC_DisChSerWpeFreeCHPH[] PROGMEM = "DCSWFHQ"; // Disabled, Channel, Ser. Channel, WP Event, Free, CH, PH (z.B. GPS Modus Steuerung) |
static const char GENERIC_DisChSer[] PROGMEM = "DCS"; // Disabled, Channel, Ser. Channel (z.B. Auto Start / Land) |
static const char GENERIC_CH_Orientation[] PROGMEM = "KVRG"; // Keine Änderung, vorne Home, Heck Home, gleiche Richtung (Comming Home Orientation |
static const char GENERIC_InaChSerWpeMMM[] PROGMEM = "ICSWUMX"; // Inactiv, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum(z.B. Menu Key Channel |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion - z.B. editGeneric(); cmd = MKPARAM_EDIT oder MKPARAM_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
*************************************************/ |
//---------------------- |
// HINWEIS! |
// Die unten stehende Aufgliederung in die verschiedenen Menue-Bereiche ist nur zur ORIENTIERUNG! |
// Jede paramID wird nur EINMAL deklariert - die Zuordnung in die Menues erfolgt in den |
// nachfolgenden ID_MENU_xyz_Items Strukturen! |
//---------------------- |
paramEditItem_t const paramEditItemTable[] PROGMEM = |
{ |
//----------------------------------- |
// Menue: ID_MENU_KANAELE (Kanäle) |
//----------------------------------- |
{ param_Kanalbelegung_Gas , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gas_de, param_Kanalbelegung_Gas_en }, |
{ param_Kanalbelegung_Gear , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gear_de, param_Kanalbelegung_Gear_en }, |
{ param_Kanalbelegung_Nick , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Nick_de, param_Kanalbelegung_Nick_en }, |
{ param_Kanalbelegung_Roll , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Roll_de, param_Kanalbelegung_Roll_en }, |
{ param_Kanalbelegung_Poti1 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti1_de, param_Kanalbelegung_Poti1_en }, |
{ param_Kanalbelegung_Poti2 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti2_de, param_Kanalbelegung_Poti2_en }, |
{ param_Kanalbelegung_Poti3 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti3_de, param_Kanalbelegung_Poti3_en }, |
{ param_Kanalbelegung_Poti4 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti4_de, param_Kanalbelegung_Poti4_en }, |
{ param_Kanalbelegung_Poti5 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti5_de, param_Kanalbelegung_Poti5_en }, |
{ param_Kanalbelegung_Poti6 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti6_de, param_Kanalbelegung_Poti6_en }, |
{ param_Kanalbelegung_Poti7 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti7_de, param_Kanalbelegung_Poti7_en }, |
{ param_Kanalbelegung_Poti8 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti8_de, param_Kanalbelegung_Poti8_en }, |
{ param_MotorSafetySwitch , &editGeneric, GENERIC_DisChSerWpeOff , 0, 0, param_MotorSafetySwitch_de, param_MotorSafetySwitch_en }, |
{ param_GlobalConfig3_MotorSwitchMode , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_MotorSwitchMode_de, param_GlobalConfig3_MotorSwitchMode_en }, |
{ param_ExtraConfig_SensitiveRc , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_SensitiveRc_de, param_ExtraConfig_SensitiveRc_en }, |
{ param_GlobalConfig3_SpeakAll , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_SpeakAll_de, param_GlobalConfig3_SpeakAll_en }, |
//----------------------------------- |
// Menue: ID_MENU_KONFIGURATION (Konfiguration) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en }, |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_GlobalConfig_KompassAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassAktiv_de, param_GlobalConfig_KompassAktiv_en }, |
{ param_GlobalConfig_KompassFix , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassFix_de, param_GlobalConfig_KompassFix_en }, |
// param_ExtraConfig_SensitiveRc |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GlobalConfig_HeadingHold , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HeadingHold_de, param_GlobalConfig_HeadingHold_en }, |
//----------------------------------- |
// Menue: ID_MENU_STICK (Stick) |
//----------------------------------- |
{ param_Stick_P , &editGeneric, GENERIC_Value , 0, 20, param_Stick_P_de, param_Stick_P_en }, |
{ param_Stick_D , &editGeneric, GENERIC_Value , 0, 20, param_Stick_D_de, param_Stick_D_en }, |
{ param_StickGier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_ExternalControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ExternalControl_de, param_ExternalControl_en }, |
//----------------------------------- |
// Menue: ID_MENU_LOOPING (Looping) |
//----------------------------------- |
{ param_BitConfig_LoopOben , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopOben_de, param_BitConfig_LoopOben_en }, |
{ param_BitConfig_LoopUnten , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopUnten_de, param_BitConfig_LoopUnten_en }, |
{ param_BitConfig_LoopLinks , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopLinks_de, param_BitConfig_LoopLinks_en }, |
{ param_BitConfig_LoopRechts , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopRechts_de, param_BitConfig_LoopRechts_en }, |
{ param_LoopGasLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_LoopGasLimit_de, param_LoopGasLimit_en }, |
{ param_LoopThreshold , &editGeneric, GENERIC_Value , 0, 247, param_LoopThreshold_de, param_LoopThreshold_en }, |
{ param_LoopHysterese , &editGeneric, GENERIC_Value , 0, 247, param_LoopHysterese_de, param_LoopHysterese_en }, |
{ param_WinkelUmschlagNick , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagNick_de, param_WinkelUmschlagNick_en }, |
{ param_WinkelUmschlagRoll , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagRoll_de, param_WinkelUmschlagRoll_en }, |
//----------------------------------- |
// Menue: ID_MENU_HOEHE (Höhe) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en}, |
{ param_ExtraConfig_HeightLimit , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightLimit_de, param_ExtraConfig_HeightLimit_en }, |
{ param_ExtraConfig_HeightVario , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightVario_de, param_ExtraConfig_HeightVario_en }, // negiertes param_ExtraConfig_HeightLimit |
{ param_GlobalConfig_HoehenSchalter , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenSchalter_de, param_GlobalConfig_HoehenSchalter_en}, |
{ param_ExtraConfig_VarioBeep , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_VarioBeep_de, param_ExtraConfig_VarioBeep_en }, |
{ param_HoeheChannel , &editGeneric, GENERIC_DisChSerWpeOffOn, 0, 0, param_HoeheChannel_de, param_HoeheChannel_en }, |
{ param_Hoehe_MinGas , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_MinGas_de, param_Hoehe_MinGas_en }, |
{ param_Hoehe_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_P_de, param_Hoehe_P_en }, |
{ param_Luftdruck_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Luftdruck_D_de, param_Luftdruck_D_en }, |
{ param_Hoehe_ACC_Wirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_ACC_Wirkung_de, param_Hoehe_ACC_Wirkung_en }, |
{ param_MaxAltitude , &editGeneric, GENERIC_ValuePoti , 0, 247, param_MaxAltitude_de, param_MaxAltitude_en }, |
{ param_Hoehe_Verstaerkung , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_Verstaerkung_de, param_Hoehe_Verstaerkung_en }, |
{ param_Hoehe_HoverBand , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_HoverBand_de, param_Hoehe_HoverBand_en }, |
{ param_Hoehe_GPS_Z , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_GPS_Z_de, param_Hoehe_GPS_Z_en }, |
{ param_Hoehe_TiltCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_Tilt_Comp_de, param_Hoehe_Tilt_Comp_en }, |
{ param_Hoehe_StickNeutralPoint , &editGeneric, GENERIC_Value , 0, 160, param_Hoehe_StickNeutralPoint_de, param_Hoehe_StickNeutralPoint_en }, |
{ param_StartLandChannel , &editGeneric, GENERIC_DisChSer , 0, 0, param_StartLandChannel_de, param_StartLandChannel_en }, |
{ param_LandingSpeed , &editGeneric, GENERIC_Value , 0, 247, param_LandingSpeed_de, param_LandingSpeed_en }, |
{ param_LandingPulse , &editACCZLandingPulse, GENERIC_ValueACCZ , 0, 0, param_LandingPulse_de, param_LandingPulse_en }, |
//----------------------------------- |
// Menue: ID_MENU_KAMERA (Kamera) |
//----------------------------------- |
{ param_ServoNickControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickControl_de, param_ServoNickControl_en }, |
{ param_ServoNickComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickComp_de, param_ServoNickComp_en }, |
{ param_ServoNickFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickFails_de, param_ServoNickFails_en }, |
{ param_GlobalConfig3_ServoNickCompOff , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_ServoNickCompOff_de, param_GlobalConfig3_ServoNickCompOff_en }, // TODO: pruefen: ab welcher Version ist das drin ??? |
{ param_ServoCompInvert_SERVO_NICK_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVNick_de, param_ServoCompInvert_SVNick_en }, |
{ param_ServoCompInvert_SERVO_RELATIVE , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRelMov_de, param_ServoCompInvert_SVRelMov_en }, |
{ param_ServoNickMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMin_de, param_ServoNickMin_en }, |
{ param_ServoNickMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMax_de, param_ServoNickMax_en }, |
{ param_ServoFilterNick , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterNick_de, param_ServoFilterNick_en }, |
{ param_ServoRollControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollControl_de, param_ServoRollControl_en }, |
{ param_ServoRollComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollComp_de, param_ServoRollComp_en }, |
{ param_ServoRollFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollFails_de, param_ServoRollFails_en }, |
{ param_ServoCompInvert_SERVO_ROLL_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRoll_de, param_ServoCompInvert_SVRoll_en }, |
{ param_ServoRollMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMin_de, param_ServoRollMin_en }, |
{ param_ServoRollMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMax_de, param_ServoRollMax_en }, |
{ param_ServoFilterRoll , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterRoll_de, param_ServoFilterRoll_en }, |
{ param_ServoNickRefresh , &editGeneric, GENERIC_Value , 2, 8, param_ServoNickRefresh_de, param_ServoNickRefresh_en }, |
{ param_ServoManualControlSpeed , &editGeneric, GENERIC_Value , 0, 247, param_ServoManualControlSpeed_de, param_ServoManualControlSpeed_en }, |
{ param_Servo3 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo3_de, param_Servo3_en }, |
{ param_Servo3Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo3Fails_de, param_Servo3Fails_en }, |
{ param_Servo4 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo4_de, param_Servo4_en }, |
{ param_Servo4Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo4Fails_de, param_Servo4Fails_en }, |
{ param_Servo5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Servo5_de, param_Servo5_en }, |
{ param_Servo5Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo5Fails_de, param_Servo5Fails_en }, |
{ param_Servo3OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OnValue_de, param_Servo3OnValue_en }, |
{ param_Servo3OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OffValue_de, param_Servo3OffValue_en }, |
{ param_Servo4OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OnValue_de, param_Servo4OnValue_en }, |
{ param_Servo4OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OffValue_de, param_Servo4OffValue_en }, |
//{ param_CamOrientation , &editNA , 0 , 0, 0, param_CamOrientation_de, param_CamOrientation_en }, // wird evtl. nicht mehr unterstuetzt |
//----------------------------------- |
// Menue: ID_MENU_NAVICTRL (NaviCtrl) |
//----------------------------------- |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_NaviGpsModeChannel , &editGeneric, GENERIC_DisChSerWpeFreeCHPH , 0, 0, param_NaviGpsModeChannel_de, param_NaviGpsModeChannel_en }, |
{ param_NaviGpsGain , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsGain_de, param_NaviGpsGain_en }, |
{ param_NaviStickThreshold , &editGeneric, GENERIC_Value , 0, 247, param_NaviStickThreshold_de, param_NaviStickThreshold_en }, |
{ param_NaviGpsMinSat , &editGeneric, GENERIC_Value , 0, 247, param_NaviGpsMinSat_de, param_NaviGpsMinSat_en }, |
{ param_NaviGpsP , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsP_de, param_NaviGpsP_en }, |
{ param_NaviGpsI , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsI_de, param_NaviGpsI_en }, |
{ param_NaviGpsD , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsD_de, param_NaviGpsD_en }, |
{ param_NaviGpsPLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsPLimit_de, param_NaviGpsPLimit_en }, |
{ param_NaviGpsILimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsILimit_de, param_NaviGpsILimit_en }, |
{ param_NaviGpsDLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsDLimit_de, param_NaviGpsDLimit_en }, |
{ param_NaviGpsA , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsA_de, param_NaviGpsA_en }, |
{ param_NaviWindCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviWindCorrection_de, param_NaviWindCorrection_en }, |
{ param_NaviAccCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAccCompensation_de, param_NaviAccCompensation_en }, |
{ param_NaviMaxFlyingRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviMaxFlyingRange_de, param_NaviMaxFlyingRange_en }, |
{ param_NaviOperatingRadius , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOperatingRadius_de, param_NaviOperatingRadius_en }, |
{ param_NaviAngleLimitation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAngleLimitation_de, param_NaviAngleLimitation_en }, |
{ param_NaviPH_LoginTime , &editGeneric, GENERIC_Value , 0, 247, param_NaviPH_LoginTime_de, param_NaviPH_LoginTime_en }, |
{ param_ExtraConfig_GpsAid , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_GpsAid_de, param_ExtraConfig_GpsAid_en }, |
{ param_GlobalConfig3_DphMaxRadius , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_DphMaxRadius_de, param_GlobalConfig3_DphMaxRadius_en }, |
{ param_ComingHomeAltitude , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeAltitude_de, param_ComingHomeAltitude_en }, |
{ param_ComingHomeOrientation , &editGeneric, GENERIC_CH_Orientation , 0, 3, param_ComingHomeOrientation_de, param_ComingHomeOrientation_en }, |
{ param_SingleWpControlChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_SingleWpControlChannel_de, param_SingleWpControlChannel_en }, //ab Rev.106 |
{ param_MenuKeyChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_MenuKeyChannel_de, param_MenuKeyChannel_en }, |
{ param_SingleWpSpeed , &editGeneric, GENERIC_ValuePoti , 0, 247, param_SingleWpSpeed_de, param_SingleWpSpeed_en }, // ab Rev. 100 |
{ param_NaviDescendRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviDescendRange_de, param_NaviDescendRange_en }, |
//----------------------------------- |
// Menue: ID_MENU_AUSGAENGE (Ausgänge) |
//----------------------------------- |
{ param_J16Bitmask , &editBitmask, 0 , 0, 0, param_J16Bitmask_de, param_J16Bitmask_en }, |
{ param_J16Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J16Timing_de, param_J16Timing_en }, |
{ param_BitConfig_MotorBlink1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink1_de, param_BitConfig_MotorBlink1_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed1_de, param_BitConfig_MotorOffLed1_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_GlobalConfig3_UseNcForOut1 , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_UseNcForOut1_de, param_GlobalConfig3_UseNcForOut1_en }, // "mit WP-Event verknüpfen" |
{ param_NaviOut1Parameter , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOut1Parameter_de, param_NaviOut1Parameter_en }, // "AutoTrigger alle...[meter]" (bis Rev.98) |
{ param_AutoPhotoDistance , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoDistance_de, param_AutoPhotoDistance_en }, // "AutoTrigger alle [meter] in Distance" (ab Rev.100) |
{ param_AutoPhotoAtitudes , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoAtitudes_de, param_AutoPhotoAtitudes_en }, // "AutoTrigger alle [meter] in Altitude" (ab Rev.100) |
{ param_J17Bitmask , &editBitmask, 0 , 0, 0, param_J17Bitmask_de, param_J17Bitmask_en }, |
{ param_J17Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J17Timing_de, param_J17Timing_en }, |
{ param_BitConfig_MotorBlink2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink2_de, param_BitConfig_MotorBlink2_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed2_de, param_BitConfig_MotorOffLed2_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_WARN_J16_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J16_Bitmask_de, param_WARN_J16_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
{ param_WARN_J17_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J17_Bitmask_de, param_WARN_J17_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
//----------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES (Verschiedenes) |
//----------------------------------- |
{ param_Gas_Min , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Min_de, param_Gas_Min_en }, |
{ param_Gas_Max , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Max_de, param_Gas_Max_en }, |
{ param_KompassWirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_KompassWirkung_de, param_KompassWirkung_en }, |
//{ param_CompassOffset , &editGeneric, GENERIC_Value , 0, 255, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset , &editCompassOffset, 0 , 0, 0, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset_DisableDeclCalc , &editGeneric, GENERIC_NoYes , 0, 0, param_CompassOffset_DisableDeclCalc_de, param_CompassOffset_DisableDeclCalc_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CareFreeChannel , &editGeneric, GENERIC_DisChSerWpeOffOn , 0, 0, param_CareFreeChannel_de, param_CareFreeChannel_en }, |
{ param_ExtraConfig_LearnableCarefree , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_LearnableCarefree_de, param_ExtraConfig_LearnableCarefree_en }, |
{ param_UnterspannungsWarnung , &editGeneric, GENERIC_Value , 0, 247, param_UnterspannungsWarnung_de, param_UnterspannungsWarnung_en }, |
{ param_ComingHomeVoltage , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeVoltage_de, param_ComingHomeVoltage_en }, |
{ param_AutoLandingVoltage , &editGeneric, GENERIC_Value , 0, 247, param_AutoLandingVoltage_de, param_AutoLandingVoltage_en }, |
//{ param_ExtraConfig_33vReference , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_33vReference_de, param_ExtraConfig_33vReference_en }, // nicht mehr unterstuetzt! |
{ param_NotGasZeit , &editGeneric, GENERIC_Value , 0, 247, param_NotGasZeit_de, param_NotGasZeit_en }, |
{ param_GlobalConfig3_VarioFailsafe , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_VarioFailsafe_de, param_GlobalConfig3_VarioFailsafe_en }, |
{ param_NotGas , &editGeneric, GENERIC_Value , 0, 247, param_NotGas_de, param_NotGas_en }, |
{ param_FailSafeTime , &editGeneric, GENERIC_Value , 0, 247, param_FailSafeTime_de, param_FailSafeTime_en }, |
{ param_FailsafeChannel , &editGeneric, GENERIC_Value , 0, 16, param_FailsafeChannel_de, param_FailsafeChannel_en }, |
{ param_ExtraConfig_NoRcOffBeeping , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_NoRcOffBeeping_de, param_ExtraConfig_NoRcOffBeeping_en }, |
{ param_ExtraConfig_IgnoreMagErrAtStartup , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_IgnoreMagErrAtStartup_de, param_ExtraConfig_IgnoreMagErrAtStartup_en }, |
{ param_GlobalConfig3_NoSdCardNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoSdCardNoStart_de, param_GlobalConfig3_NoSdCardNoStart_en }, |
{ param_GlobalConfig3_NoGpsFixNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoGpsFixNoStart_de, param_GlobalConfig3_NoGpsFixNoStart_en }, |
//----------------------------------- |
// Menue: ID_MENU_GYRO (Gyro) |
//----------------------------------- |
{ param_Gyro_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_P_de, param_Gyro_P_en }, |
{ param_Gyro_Gier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_Gyro_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_I_de, param_Gyro_I_en }, |
{ param_Gyro_Gier_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_I_de, param_Gyro_Gier_I_en }, |
{ param_Gyro_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_D_de, param_Gyro_D_en }, |
{ param_DynamicStability , &editGeneric, GENERIC_ValuePoti , 0, 247, param_DynamicStability_de, param_DynamicStability_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GyroAccFaktor , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccFaktor_de, param_GyroAccFaktor_en }, |
{ param_GyroAccAbgleich , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccAbgleich_de, param_GyroAccAbgleich_en }, |
{ param_I_Faktor , &editGeneric, GENERIC_ValuePoti , 0, 247, param_I_Faktor_de, param_I_Faktor_en }, |
{ param_Driftkomp , &editGeneric, GENERIC_Value , 0, 247, param_Driftkomp_de, param_Driftkomp_en }, |
{ param_Gyro_Stability , &editGeneric, GENERIC_Value , 0, 247, param_Gyro_Stability_de, param_Gyro_Stability_en }, |
{ param_MotorSmooth , &editGeneric, GENERIC_Value , 0, 247, param_MotorSmooth_de, param_MotorSmooth_en }, |
//----------------------------------- |
// Menue: ID_MENU_BENUTZER (Benutzer) |
//----------------------------------- |
{ param_UserParam1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam1_de, param_UserParam1_en }, |
{ param_UserParam2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam2_de, param_UserParam2_en }, |
{ param_UserParam3 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam3_de, param_UserParam3_en }, |
{ param_UserParam4 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam4_de, param_UserParam4_en }, |
{ param_UserParam5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam5_de, param_UserParam5_en }, |
{ param_UserParam6 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam6_de, param_UserParam6_en }, |
{ param_UserParam7 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam7_de, param_UserParam7_en }, |
{ param_UserParam8 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam8_de, param_UserParam8_en }, |
//----------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG (Achskopplung) |
//----------------------------------- |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_AchsKopplung1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung1_de, param_AchsKopplung1_en }, |
{ param_AchsKopplung2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung2_de, param_AchsKopplung2_en }, |
{ param_CouplingYawCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_CouplingYawCorrection_de, param_CouplingYawCorrection_en }, |
//----------------------------------- |
// ENDE |
//----------------------------------- |
{ param_EOF, NOFUNC, 0,0,0, 0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
/************************************************************ |
NICHT MEHR UNTERSTUEZT: |
param_Receiver (Empfänger Typ: Hott, Jeti, Spektrum,...) |
************************************************************/ |
//############################################################################################# |
//# Menue's und Menuezuordnung |
//############################################################################################# |
//----------------------------------- |
// Menue ID's: Parameters Hauptmenue |
//----------------------------------- |
#define ID_MENU_FAVORITEN 1 // fuer spaeter reserviert.... |
#define ID_MENU_KANAELE 2 |
#define ID_MENU_KONFIGURATION 3 |
#define ID_MENU_STICK 4 |
#define ID_MENU_LOOPING 5 |
#define ID_MENU_HOEHE 6 |
#define ID_MENU_KAMERA 7 |
#define ID_MENU_NAVICTRL 8 |
#define ID_MENU_AUSGAENGE 9 |
#define ID_MENU_VERSCHIEDENES 10 |
#define ID_MENU_GYRO 11 |
#define ID_MENU_BENUTZER 12 |
#define ID_MENU_ACHSKOPPLUNG 13 |
#define ID_MENU_MIXERSETUP 14 // nicht verwendet / unterstuetzt |
#define ID_MENU_EASYSETUP 15 |
#define ID_MENU_TEST 66 // TEST / DEBUG |
//----------------------------------- |
// Zuordnungen von paramID's zu den jeweiligen Menue's |
// |
// Eintrag "SEPARATOR" - damit wird eine Trennlinie |
// im Menue dargestellt |
//----------------------------------- |
//------------------------------- |
// Menue: ID_MENU_KANAELE |
//------------------------------- |
unsigned char const ID_MENU_KANAELE_Items[] = |
{ |
param_Kanalbelegung_Gas, |
param_Kanalbelegung_Gear, |
param_Kanalbelegung_Nick, |
param_Kanalbelegung_Roll, |
param_Kanalbelegung_Poti1, |
param_Kanalbelegung_Poti2, |
param_Kanalbelegung_Poti3, |
param_Kanalbelegung_Poti4, |
param_Kanalbelegung_Poti5, |
param_Kanalbelegung_Poti6, |
param_Kanalbelegung_Poti7, |
param_Kanalbelegung_Poti8, |
SEPARATOR, |
param_MotorSafetySwitch, |
param_GlobalConfig3_MotorSwitchMode, |
SEPARATOR, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig3_SpeakAll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KONFIGURATION |
//------------------------------- |
unsigned char const ID_MENU_KONFIGURATION_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_GlobalConfig_GpsAktiv, |
param_GlobalConfig_KompassAktiv, |
param_GlobalConfig_KompassFix, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig_AchsenkopplungAktiv, |
param_GlobalConfig_DrehratenBegrenzer, |
param_GlobalConfig_HeadingHold, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_STICK |
//------------------------------- |
unsigned char const ID_MENU_STICK_Items[] = |
{ |
param_Stick_P, |
param_Stick_D, |
param_StickGier_P, |
SEPARATOR, |
param_ExternalControl, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_LOOPING |
//------------------------------- |
unsigned char const ID_MENU_LOOPING_Items[] = |
{ |
param_BitConfig_LoopOben, |
param_BitConfig_LoopUnten, |
param_BitConfig_LoopLinks, |
param_BitConfig_LoopRechts, |
SEPARATOR, |
param_LoopGasLimit, |
param_LoopThreshold, |
param_LoopHysterese, |
param_WinkelUmschlagNick, |
param_WinkelUmschlagRoll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_HOEHE |
//------------------------------- |
unsigned char const ID_MENU_HOEHE_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_ExtraConfig_HeightLimit, |
param_ExtraConfig_HeightVario, |
param_GlobalConfig_HoehenSchalter, |
param_ExtraConfig_VarioBeep, |
SEPARATOR, |
param_HoeheChannel, |
param_Hoehe_Verstaerkung, |
param_Hoehe_MinGas, |
param_Hoehe_HoverBand, |
param_Hoehe_P, |
param_Hoehe_GPS_Z, |
param_Hoehe_TiltCompensation, |
param_Luftdruck_D, |
param_Hoehe_StickNeutralPoint, |
param_Hoehe_ACC_Wirkung, |
param_MaxAltitude, |
SEPARATOR, |
param_StartLandChannel, |
param_LandingSpeed, |
param_LandingPulse, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KAMERA |
//------------------------------- |
unsigned char const ID_MENU_KAMERA_Items[] = |
{ |
param_ServoNickControl, |
param_ServoNickComp, |
param_ServoNickFailsave, |
param_GlobalConfig3_ServoNickCompOff, // erst ab FC-Rev 96 |
param_ServoCompInvert_SERVO_NICK_INV, |
param_ServoCompInvert_SERVO_RELATIVE, |
param_ServoNickMin, |
param_ServoNickMax, |
param_ServoFilterNick, |
SEPARATOR, |
param_ServoRollControl, |
param_ServoRollComp, |
param_ServoRollFailsave, |
param_ServoCompInvert_SERVO_ROLL_INV, |
param_ServoRollMin, |
param_ServoRollMax, |
param_ServoFilterRoll, |
SEPARATOR, |
param_ServoNickRefresh, |
param_ServoManualControlSpeed, |
param_Servo3, |
param_Servo3Failsave, |
param_Servo4, |
param_Servo4Failsave, |
param_Servo5, |
param_Servo5Failsave, |
SEPARATOR, |
param_Servo3OnValue, |
param_Servo3OffValue, |
param_Servo4OnValue, |
param_Servo4OffValue, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_NAVICTRL |
//------------------------------- |
unsigned char const ID_MENU_NAVICTRL_Items[] = |
{ |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_NaviGpsGain, |
param_NaviStickThreshold, |
param_NaviGpsMinSat, |
param_NaviGpsP, |
param_NaviGpsPLimit, |
param_NaviGpsI, |
param_NaviGpsILimit, |
param_NaviGpsD, |
param_NaviGpsDLimit, |
param_NaviGpsA, |
SEPARATOR, |
param_NaviWindCorrection, |
param_NaviAccCompensation, |
param_NaviMaxFlyingRange, |
param_NaviOperatingRadius, |
param_NaviAngleLimitation, |
param_NaviPH_LoginTime, |
param_ExtraConfig_GpsAid, |
param_GlobalConfig3_DphMaxRadius, |
SEPARATOR, |
param_ComingHomeAltitude, |
param_ComingHomeOrientation, |
SEPARATOR, |
param_SingleWpControlChannel, |
param_MenuKeyChannel, |
param_SingleWpSpeed, |
SEPARATOR, |
param_NaviDescendRange, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_AUSGAENGE |
//------------------------------- |
unsigned char const ID_MENU_AUSGAENGE_Items[] = |
{ |
param_J16Bitmask, // LED 1 Bitmaske |
param_J16Timing, |
param_BitConfig_MotorBlink1, |
param_BitConfig_MotorOffLed1, |
param_GlobalConfig3_UseNcForOut1, |
param_NaviOut1Parameter, |
param_AutoPhotoDistance, |
param_AutoPhotoAtitudes, |
SEPARATOR, |
param_J17Bitmask, // LED 2 Bitmaske |
param_J17Timing, |
param_BitConfig_MotorBlink2, |
param_BitConfig_MotorOffLed2, |
SEPARATOR, |
param_WARN_J16_Bitmask, |
param_WARN_J17_Bitmask, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES |
//------------------------------- |
unsigned char const ID_MENU_VERSCHIEDENES_Items[] = |
{ |
param_Gas_Min, |
param_Gas_Max, |
param_KompassWirkung, |
param_CompassOffset, |
param_CompassOffset_DisableDeclCalc, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
SEPARATOR, |
param_UnterspannungsWarnung, |
param_ComingHomeVoltage, |
param_AutoLandingVoltage, |
SEPARATOR, |
param_NotGasZeit, |
param_GlobalConfig3_VarioFailsafe, // Nutze "Vario-Höhe" für "Not-Gas" -> beeinflusst 'param_NotGas' bzgl. "%" |
param_NotGas, |
param_FailSafeTime, |
param_FailsafeChannel, |
param_ExtraConfig_NoRcOffBeeping, |
SEPARATOR, |
param_ExtraConfig_IgnoreMagErrAtStartup, |
param_GlobalConfig3_NoSdCardNoStart, |
param_GlobalConfig3_NoGpsFixNoStart, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_GYRO |
//------------------------------- |
unsigned char const ID_MENU_GYRO_Items[] = |
{ |
param_Gyro_P, |
param_Gyro_I, |
param_Gyro_D, |
param_Gyro_Gier_P, |
param_Gyro_Gier_I, |
SEPARATOR, |
param_DynamicStability, |
param_GlobalConfig_DrehratenBegrenzer, |
SEPARATOR, |
param_GyroAccFaktor, |
param_GyroAccAbgleich, |
SEPARATOR, |
param_I_Faktor, |
param_Driftkomp, |
param_Gyro_Stability, |
param_MotorSmooth, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_BENUTZER |
//------------------------------- |
unsigned char const ID_MENU_BENUTZER_Items[] = |
{ |
param_UserParam1, |
param_UserParam2, |
param_UserParam3, |
param_UserParam4, |
param_UserParam5, |
param_UserParam6, |
param_UserParam7, |
param_UserParam8, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG |
//------------------------------- |
unsigned char const ID_MENU_ACHSKOPPLUNG_Items[] = |
{ |
param_GlobalConfig_AchsenkopplungAktiv, |
param_AchsKopplung1, |
param_AchsKopplung2, |
param_CouplingYawCorrection, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_EASYSETUP |
//------------------------------- |
unsigned char const ID_MENU_EASYSETUP_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_HoeheChannel, |
param_Hoehe_StickNeutralPoint, |
param_StartLandChannel, |
SEPARATOR, |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_ExtraConfig_GpsAid, |
param_ComingHomeAltitude, |
SEPARATOR, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
EOF // *** MUST BE THE LAST! *** |
}; |
//############################################################################################# |
//# Error-Handling |
//############################################################################################# |
//# mittels des auf dem PKT angezeigten Errocodes (z.B. "E32") kann man hier schauen was bzw. |
//# wo (in welcher Funktion) der Fehler aufgetreten ist. |
//# |
//# Im Menue wird der Fehlercode rechts bei den Values dargestellt (dabei ertönen immer wieder |
//# Fehler-Beep's). |
//# In der Vollbildanzeige wird zusaetzlich ein uebergebener Wert (value) angezeigt - im Code |
//# kann man ermitteln was der Wert darstellt. |
//############################################################################################# |
#define E01_PARAMID_MENUEDITCATEGORY 1 // Menu_EditCategory() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E11_PARAMID_EDITGENERIC 11 // editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E12_PARAMID_EDITBITMASK 12 // editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E30_CODECHAR_NOT_FOUND 30 // find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E31_PARAMEDIT_ITEM_NOT_FOUND 31 // find_genericCodeByValue() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E32_CODECHAR_NOT_FOUND 32 // find_genericCodeByValue() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E33_VALUE_NOT_IN_FORMAT 33 // find_genericCodeByValue() - FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
#define E41_VALUE_MIN_MAX_RANGE 41 // strValueGeneric() - FEHLER: fuer die Range von 'v' (Value) wurde max < min angegeben bei Generic -> nicht unterstuetzt |
//-------------------------------------------------------------- |
// _error( errorcode, value, showscreen) |
// |
// zeigt Fehlermeldungen an |
// |
// PARAMETER: |
// errorcode : siehe Error-defines |
// value : zusaetzlicher Anzeigeparameter (nur in Screen-Anzeige) |
// showscreen: true = Fehlermeldung wird vollstaendig angezeigt. |
// - er Screen wird dabei geloescht |
// - Ende mit Taste 'Ende' |
// - langer Fehler-Beep |
// |
// false = es wird nur der Buffer mkparam_strValueBuffer |
// mit dem Fehlercode befuellt |
// - Anzeige im Menue ganz recht als "Enn" |
// - kurzer Fehler-Beep der jedoch bei jeder |
// Aktion im Menue zu hoeren ist |
// => dann auf Anzeige "Enn" achten! |
//-------------------------------------------------------------- |
void _error( uint8_t errorcode, uint8_t value, uint8_t showscreen) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("E%02u"), errorcode); // in mkparam_strValueBuffer ausgeben fuer das Menue |
if( showscreen ) |
{ |
lcd_cls(); |
lcdx_printp_center( 1, PSTR(" ERROR "), MINVERS, 0,-3); |
lcdx_printp_center( 2, PSTR("mkparameters.c"), MNORMAL, 0,0); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("Code : E%02u"), errorcode ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Value: %3u"), value ); |
lcd_printp_at(11, 7, strGet(ENDE), MNORMAL ); |
set_beep( 500, 0x000f, BeepNormal); // Beep Error |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
//-------------------------------- |
// Anmerkung: OG 25.03.2014 |
// |
// Ohne diese Verzoegerung funkioniert die Update-Erkennung via PKT-UpdateTool |
// meist nicht - ist das PKT zu schnell? |
// Kann man evtl. das PKT-UpdateTool diesbezueglich mal ueberpruefen ob man |
// Timings/Timeout's anpassen kann? |
// |
// Hinweis dazu: PKT_CtrlHook() bzw. die dort enthaltene Update-Anforderungspruefung |
// ist in den letzten Wochen beschleunigt geworden da unnoetiger Code fuer nicht |
// benoetigte Bildschirmausgaben entfernt wurde - das ist grundsaetzlich fuer das PKT |
// besser. |
// |
// Um das nachzustellen: |
// Unten das Delay auskommentieren und einen Fehler in Menu_EditCategory() provozieren |
// indem "paramID = 0;" einkommentiert wird (ist dort beschrieben) |
// Dann den Fehlerscreen im PKT anzeigen lassen und ein Update via PKT-UpdateTool initiieren, |
//-------------------------------- |
_delay_ms(150); |
PKT_CtrlHook(); |
} |
} |
else |
{ |
// kurzer Fehler-Beep - der kann wiederholt im Menue auftreten bei jedem Refresh des Menues |
set_beep( 160, 0x000f, BeepNormal); // Beep Error |
} |
} |
//############################################################################################# |
//# Edit-Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = find_paramEditItem( paramID ) |
// |
// sucht in paramEditItemTable (PROGMEM) eine paramID und |
// kopiert den Eintrag in das RAM |
// |
// PARAMETER: |
// paramID |
// |
// RUECKGABE: |
// true = paramID gefunden |
// false = nicht gefunden (Fehler) |
// |
// -- globale Modulvariablen -- |
// Ergebnisse in: |
// |
// paramEditItem : der gefundene Eintrag vom Typ paramEditItem_t (im RAM) |
// paramEditFormat: der Format-String paramEditItem.format im RAM |
//-------------------------------------------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ) |
{ |
unsigned char id; |
unsigned char *p; |
p = (unsigned char *) paramEditItemTable; |
while( true ) |
{ |
id = pgm_read_byte(p); // die aktuelle paramID aus paramEditDef |
if( id == paramID || id == param_EOF ) |
break; |
p += sizeof( paramEditItem_t ); |
} |
//----- |
// wenn die gesuchte paramID nicht gefunden wurde steht |
// im RAM-Buffer paramEdit.paramID == param_EOF !! |
//----- |
memcpy_P ( ¶mEditItem , p, sizeof(paramEditItem_t) ); // die Struktur -> in den RAM-Buffer kopieren (Zugriff: paramEditItem) |
strncpy_P( paramEditFormat, paramEditItem.format, MKPARAM_STRBUFFER_LEN); // format String -> in den RAM-Buffer kopieren (Zugriff: paramEditFormat) |
return( id!=param_EOF ); |
} |
//-------------------------------------------------------------- |
// ptrP = paramEditItemTitle() |
// |
// gibt einen Zeiger in PROGMEM auf den Titel/Bezeichnung eines |
// paramID's in der richtigen Sprache zurueck |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
/* |
const char * paramEditItemTitle( void ) |
{ |
if( Config.DisplayLanguage == 0 ) |
return( paramEditItem.title_de ); |
else |
return( paramEditItem.title_en ); |
} |
*/ |
//-------------------------------------------------------------- |
// print_paramEditItemTitle() |
// |
// gibt Titel/Bezeichnung auf den Bildschirm bei x=0, y=2 aus |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
void print_paramEditItemTitle( void ) |
{ |
lcd_printp_at( 0, 2, (Config.DisplayLanguage == 0 ? paramEditItem.title_de : paramEditItem.title_en ), MNORMAL); // Bezeichnung des paramID's |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( code ) |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCode( unsigned char code ) |
{ |
unsigned char codeIdx; |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != code ) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//----------------------------- |
// find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//----------------------------- |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E30_CODECHAR_NOT_FOUND, code, false); |
return false; |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
return true; |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( paramID, &v, &min, &max, pFormat) |
// |
// PARAMETER: |
// paramID |
// *v |
// *min |
// *max |
// *pFormat |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// paramEditItem |
// paramEditFormat: der zugehoerige Format-String |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCodeByValue( unsigned char paramID, unsigned char *v, unsigned char *min, unsigned char *max ) |
{ |
unsigned char codeIdx; |
char *pFormat; |
//------------------------------------------------- |
// 1. finde paramID in der paramEditItem-Tabelle |
// |
// sollte der RAM-Buffer paramEditItem bereits das |
// gesuchte enthalten dann ok |
//------------------------------------------------- |
if( paramEditItem.paramID != paramID ) |
find_paramEditItem( paramID ); |
//--- |
// FEHLER: paramEditItem nicht in Tabelle paramEditItemTable gefunden |
//--- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E31_PARAMEDIT_ITEM_NOT_FOUND, paramID, false); |
return false; |
} |
pFormat = paramEditFormat; // Zeiger auf das erste Zeichen vom Format-String (z.B. "DCSWFHQ") |
*v = paramGet( paramID ); // der aktuelle Wert des paraID |
while( true ) |
{ |
//------ |
// suche editGenericCode mittels aktuelles Zeichens *pFormat |
//------ |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//------ |
// FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//------ |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E32_CODECHAR_NOT_FOUND, *pFormat, false); |
return false; |
} |
//------ |
// suche den Value-Bereich (min/max) fuer das Code-Zeichen |
//------ |
if( editGenericCode[codeIdx].code == 'v' ) |
{ |
// bei code 'v' kommt min/max aus paramEditItem |
*min = paramEditItem.min; |
*max = paramEditItem.max; |
} |
else |
{ |
// ansonsten kommt min/max aus dem Code selber |
*min = editGenericCode[codeIdx].min; |
*max = editGenericCode[codeIdx].max; |
} |
// value gefunden (innerhalb von min/max)? |
// beruecksichtigt 'gedrehte' min/max (z.B. Poti1..8) |
if( (*min <= *max && *v >= *min && *v <= *max) || (*min > *max && *v >= *max && *v <= *min) ) |
{ |
// gefunden - Schleife verlassen |
break; |
} |
//------ |
// naechstes Code-Zeichen aus der Format-Maske |
//------ |
pFormat++; |
//------ |
// FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
//------ |
if( *pFormat == 0 ) // Ende des Format-Strings erreicht - Fehler... |
{ |
_error( E33_VALUE_NOT_IN_FORMAT, *v, false); |
return false; |
} |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
genericCode.min = *min; // ggf. min/max ueberschreiben durch min/max von paramEditItem |
genericCode.max = *max; |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * get_genericCodeText( uint8_t cmd ) |
{ |
if( cmd == MKPARAM_LONGVALUE ) |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.longText_de; |
else return genericCode.longText_en; |
} |
else |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.shortText_de; |
else return genericCode.shortText_en; |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// ok = strValueGeneric( paramID, cmd ) |
// |
// erstellt die String Representation des Wertes von paramID |
// im Buffer mkparam_strValueBuffer. |
// |
// Mit 'cmd' wird gesteuert ob es der Lang-Text oder der |
// Kurztext (3 Zeichen) ist. |
// |
// PARAMETER: |
// paramID: |
// cmd: MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
uint8_t strValueGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
unsigned char min; |
unsigned char max; |
if( find_genericCodeByValue( paramID, &v, &min, &max) ) |
{ |
//------------------- |
// value gefunden! |
//------------------- |
if( genericCode.code == 'v' ) // Code 'v' (Value) - keine Umrechnung von min/max |
{ |
if( max < min ) |
{ |
// max < min bei 'v' (Value) wird aktuell nicht unterstuezt da |
// kein Beispiel vorhanden ist was damit ggf. gemeint ist. |
// Wenn das irgendwann benoetigt wird muss das hier gecoded werden. |
// Bis dahin wird das als Fehler angesehen (ggf. vertippt beim coden). |
_error( E41_VALUE_MIN_MAX_RANGE, v, false); |
return false; |
} |
} |
else // alles andere ausser 'v' - der Wert wird in die Range von min/max umgerechnet |
{ |
if( min <= max ) |
v = v-min; |
else |
v = min-v; // OG Notiz: v = 248 = 8 # 248 = 8, 255 = 1 # min 255 max = 248 |
v++; // fuer Anzeige +1: statt '0' eine '1' anzeigen - Beispiel: statt "Poti0" zeige "Poti1" |
} |
//------------------- |
// ertstelle String |
//------------------- |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, get_genericCodeText(cmd), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// ok = getGenericNextValue( paramID, vInc ) |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t getGenericNextValue( unsigned char paramID, int8_t vInc ) |
{ |
int v; // value von paramID - signed int: damit -1 und >255 ausgewertet werden kann |
unsigned char value; // zum holen von v via find_genericCodeByValue() |
unsigned char min; |
unsigned char max; |
unsigned char codeIdx; |
char *pFormat; |
if( find_genericCodeByValue( paramID, &value, &min, &max) ) |
{ |
v = (int)value; // value vom paramID wird gecastet auf int um -1 und >255 fuer edit zu erkennen |
//------------------------------------------------------ |
// Pointer auf das Zeichen im Format-String ermitteln |
//------------------------------------------------------ |
pFormat = paramEditFormat; |
while( *pFormat != genericCode.code ) pFormat++; |
if( min > max ) // 'umgedrehte' min/max Werte wie bei Poti1..8 |
vInc = vInc * -1; |
v = v + vInc; |
//------------------------------------------------------ |
// min/max Grenzen des Code-Zeichens ueberschritten? |
//------------------------------------------------------ |
if( (min <= max && v < min) || (min > max && v > min) ) |
{ |
if( pFormat == paramEditFormat ) // Anfang vom Code-String? -> dann gehe zum Ende (letztes Zeichen) |
{ |
while( *(pFormat+1) ) pFormat++; |
} |
else |
{ |
pFormat--; // vorheriges Format-Zeichen = vorheriger Code |
} |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.max; |
else |
v = editGenericCode[codeIdx].max; |
} |
else if( (min <= max && v > max) || (min > max && v < max) ) |
{ |
pFormat++; // naechstes Format-Zeichen = naechster Code |
if( *pFormat == 0 ) // Ende des Format-Strings? |
pFormat = paramEditFormat; |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.min; |
else |
v = editGenericCode[codeIdx].min; |
} |
//------------------------------------------------------ |
// neuen Wert setzen |
//------------------------------------------------------ |
paramSet( paramID, (unsigned char)v ); |
return true; |
} // end: if( find_genericCode( paramID, &v, &min, &max) ) |
return false; |
} |
//-------------------------------------------------------------- |
// editGeneric( paramID, cmd ) |
// |
// PARAMETER: |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v_org; |
uint8_t redraw; |
int8_t vInc; |
if( cmd != MKPARAM_EDIT ) |
{ |
// wenn cmd = MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
strValueGeneric( paramID, cmd); // Ergebnis in: mkparam_strValueBuffer |
return; |
} |
find_paramEditItem( paramID ); |
//------------------------ |
// editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//------------------------ |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E11_PARAMID_EDITGENERIC, paramID, true); |
return; |
} |
lcd_cls(); |
v_org = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
vInc = 0; // 0, +1, -1 (inc/dec) |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
strValueGeneric( paramID, MKPARAM_LONGVALUE); // Lang-Text des Values anzeigen |
// nach strValueGeneric() ist auch genericCode initialisiert! |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %18s"), mkparam_strValueBuffer ); |
// DEBUG... |
#ifdef DEBUG_PARAMEDIT |
//lcd_printf_at_P( 16, 1, MNORMAL, PSTR("%c %3u"), genericCode.code, paramGet(paramID) ); |
#endif |
//lcd_printf_at_P( 16, 4, MNORMAL, PSTR("= %03u"), v ); |
if( abs(genericCode.max-genericCode.min) <= 20 ) |
_delay_ms( 200 ); // Verzoegerung |
vInc = 0; |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_press(1 << KEY_ESC) ) // Key: Cancel |
{ |
paramSet( paramID, v_org ); // org. Wert wieder herstellen |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // Key: OK |
{ |
break; |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) // Key: rechts / +1 |
{ |
vInc = +1; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) // Key: links / -1 |
{ |
vInc = -1; |
} |
if( vInc != 0 ) |
{ |
getGenericNextValue( paramID, vInc ); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
// editBitmask( paramID, cmd ) |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editBitmask( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
uint8_t redraw; |
uint8_t i; |
int8_t pos; |
find_paramEditItem( paramID ); |
//----------------------- |
// editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
v = paramGet( paramID ); |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%03u"), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
lcd_cls(); |
v = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
pos = 0; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 1, 7, PSTR("0/1 \x19 OK"), MNORMAL); // Keyline: -> 0/1 OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
lcdx_printp_at( 7, 6, PSTR("\x18"), MNORMAL, -2,2); // Keyline langer Tastendruck: -> 0/1 OK |
if( (paramID == param_J16Bitmask) || (paramID == param_J17Bitmask) ) |
{ |
lcdx_printp_at( 10, 5, PSTR("(Bit1=Idle)"), MNORMAL, 0,2); // |
} |
print_paramEditItemTitle(); // Bezeichnung des paramID's (Stelle x=0, y=2) |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
for(i = 0; i < 8; i++) |
{ |
lcd_putc( 8-i, 4, ( (v & (1 << i)) ? '1' : '0'), MNORMAL); |
} |
lcd_frect( 0, (8*5), 125-(11*6), 7, 0); // clear: Eingabemarkierung |
lcd_printp_at (pos+1, 5,PSTR("\x12"), MNORMAL); // Eingabemarkierung (Pfeil nach oben) |
lcd_printf_at_P( 15, 4, MNORMAL, PSTR("= %03u"), v ); // Anzeige des aktuellen Wertes Dezimal |
// _delay_ms( 200 ); // Verzoegerung |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if (get_key_short(1 << KEY_ENTER)) // Key: OK |
{ |
paramSet( paramID, v ); // Wert speichern |
break; // und verlassen |
} |
if( get_key_short(1 << KEY_PLUS) ) |
{ |
if(pos == 7) pos = 0; |
else pos++; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if(pos == 0) pos = 7; |
else pos--; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short (1 << KEY_MINUS) ) |
{ |
v ^= (1<<(7-pos)); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editCompassOffset( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
int v; // signed |
uint8_t redraw; |
uint8_t bit7; |
uint8_t bit8; |
uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editCompassOffset() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// Wert ermitteln (-60 bis 60) |
//----------------------- |
vu = paramGet( paramID ); |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
bit8 = ((vu & 0x80) ? true : false); // Bit 8 in Kombination mit Bit 7 = "Disable dec. Calc" |
// Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// Bit 8 != Bit 7: "Disable dec. Calc" AN |
lDisDeclCalc = ((bit8 == bit7) ? 0 : 1); // merken um das spaeter wieder in den Wert einzubauen |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
if( bit7 ) // Wert negativ? |
{ |
vu = vu - 1; // Umrechnung: 7-Bit Zweierkomplement |
vu = (vu ^ 0xff); // invertieren |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
v = (int)vu; |
v = v * -1; |
} |
else // Wert ist positiv |
{ |
v = (int)vu; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%3d"), v ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %3d"), v ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
//------------------------------------- |
// signed int in unsigned byte wandeln |
//------------------------------------- |
if( v < 0 ) // Wert negativ? |
{ |
// Umwandeln in 7-Bit Zweierkomplement |
v = v * -1; // Wert positiv machen |
vu = (unsigned char)v; // in unsigned Byte speichern |
vu = (vu ^ 0xff); // invertieren |
vu = vu + 1; // +1 |
} |
else // Wert ist positiv |
{ |
vu = (unsigned char)v; // in unsigned Byte speichern |
} |
//------------------------------------- |
// "Disable declination calc" wieder einrechnen |
//------------------------------------- |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
if( v <= -60 ) v = 60; |
else v = v - 1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
if( v >= 60 ) v = -60; |
else v = v + 1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
// int v; // signed |
uint8_t redraw; |
// uint8_t bit7; |
// uint8_t bit8; |
// uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editACCZLandingPulse() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
vu = paramGet( paramID ); |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%4d"), vu*4 ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %4d"), vu *4 ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
// //------------------------------------- |
// // signed int in unsigned byte wandeln |
// //------------------------------------- |
// if( v < 0 ) // Wert negativ? |
// { |
// // Umwandeln in 7-Bit Zweierkomplement |
// v = v * -1; // Wert positiv machen |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// vu = (vu ^ 0xff); // invertieren |
// vu = vu + 1; // +1 |
// } |
// else // Wert ist positiv |
// { |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// } |
// |
// //------------------------------------- |
// // "Disable declination calc" wieder einrechnen |
// //------------------------------------- |
// bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
// |
// if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
// else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// |
// if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
// else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
// if( v <= -60 ) v = 60; |
// else v = v - 1; |
// |
if( vu < 191 ) vu = 190; |
else vu = vu -1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
// if( v >= 60 ) v = -60; |
// else v = v + 1; |
if( vu >= 255 ) vu = 255; |
// if( vu == 1 ) vu = 191; |
else vu = vu +1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// editNA( paramID, cmd ) |
// |
// Hilfsfunktion die nur "nicht verfügbar" anzeigt wenn die |
// paramID noch nicht implementiert ist |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editNA( unsigned char paramID, uint8_t cmd ) |
{ |
if( cmd != MKPARAM_EDIT ) |
{ |
strncpy_P( mkparam_strValueBuffer, PSTR("NA!"), MKPARAM_STRBUFFER_LEN); // "NA!" bzw, "not available" |
return; |
} |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 2000, true, true ); // "nicht verfügbar" |
} |
//############################################################################################# |
//# TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editGenericTEST( unsigned char paramID ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
find_paramEditItem( paramID ); // Ergebnis in paramEditItem (RAM) |
if( paramEditItem.paramID == paramID ) |
{ |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("%S"), paramEditItem.title_de ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("format: %s"), paramEditFormat ); |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("value:%u"), paramGet(paramID) ); |
strValueGeneric( paramID, MKPARAM_LONGVALUE); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
strValueGeneric( paramID, MKPARAM_SHORTVALUE); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
} |
if( paramEditItem.paramID == param_EOF ) |
{ |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("! ERROR !") ); |
} |
//----------------------------------------- |
// Tasten... |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void TEST(void) |
{ |
//editGeneric( param_Gyro_Gier_P, MKPARAM_EDIT ); |
editGeneric( param_Kanalbelegung_Gas, MKPARAM_EDIT ); |
} |
//############################################################################################# |
//# Menues & Favoriten |
//############################################################################################# |
//-------------------------------------------------------------- |
// fav_add() |
// |
// einen Favoriten hinzufuegen |
// |
// Aufruf durch: wird durch den Menu-Controller aufgerufen |
//-------------------------------------------------------------- |
void fav_add( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//------------------------------ |
// welche paramID wurde gewaehlt? |
//------------------------------ |
paramID = MenuCtrl_GetItemId(); // gewaehlter Menuepunkt bzw. paramID (0 = keine itemID) |
//------------------------------ |
// ungueltige paramID? |
//------------------------------ |
if( paramID==0 || paramID==EOF ) |
{ |
return; // keine gueltige paramID gewaehlt -> ZURUECK |
} |
//------------------------------ |
// Duplikatsuche bzgl. paramID |
// (gilt nicht fuer SEPARATOR) |
//------------------------------ |
if( paramID!=SEPARATOR ) |
{ |
//------------------------------ |
// suche Duplikate fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=paramID); i++); |
//------------------------------ |
// Favoriten bereits vorhanden? |
// -> exit |
//------------------------------ |
if( Config.MKParam_Favs[i] == paramID ) |
{ |
set_beep( 300, 0x000f, BeepNormal); // Error Beep kurz |
PKT_Popup_P( 400, strGet(STR_FAV_EXIST),0,0,0); // "* Fav vorhanden *" (ca. 4 sec max.) |
return; |
} |
} // end: if( paramID!=SEPARATOR ) |
//------------------------------ |
// suche freien Speicherplatz fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0); i++); |
//------------------------------ |
// Favoriten voll? |
// -> exit |
//------------------------------ |
if( i >= MAX_MKPARAM_FAVORITES ) |
{ |
set_beep( 500, 0x000f, BeepNormal); // Error Beep lang |
PKT_Popup_P( 400, strGet(STR_FAV_FULL),0,0,0); // "* Fav ist voll *" (ca. 4 sec max.) |
return; |
} |
//------------------------------ |
// neuen Fav merken |
//------------------------------ |
Config.MKParam_Favs[i] = paramID; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
PKT_Popup_P( 400, strGet(STR_FAV_ADD),0,0,0); // "Fav hinzugefügt!" (ca. 4 sec max.) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Favoriten( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
if( Config.MKParam_Favs[0] == 0 ) // Favoriten vorhanden |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_FAV_NOTEXIST), false, 500, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
MenuCtrl_SetDelete( true ); // Menueeintraege loeschen (langer Druck auf "OK") |
MenuCtrl_SetMove( true ); // Menueeintraege verschieben (langer Druck auf hoch/runter) |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menu-Items |
//--------------- |
i = 0; |
while( (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0) ) |
{ |
paramID = Config.MKParam_Favs[i]; |
if( paramID == SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
} |
else |
{ |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
MenuCtrl_PushParamEdit( paramID ); |
// existiert die paramID in der Firmware des Kopters? |
if( !paramExist(paramID) ) |
{ |
MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} |
// DEBUG! (Simulation eines deaktivierten Fav's) |
// if( i==2 ) MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} // end: else: if( paramID == SEPARATOR ) |
i++; |
} // end: while(..) |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // KEY_ESC = Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// ggf. neue Menuereihenfolge |
// in der Config speichern |
//--------------- |
for( i=0; (i<MAX_MKPARAM_FAVORITES); i++) |
{ |
Config.MKParam_Favs[i] = MenuCtrl_GetItemIdByIndex( i ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_EditCategory( const unsigned char *menuitems ) |
{ |
unsigned char paramID; |
unsigned char lastmenuitem = 0; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
MenuCtrl_SetKey( KEY_ENTER_LONG, 0, &fav_add ); |
//--------------- |
// Menuitems |
//--------------- |
while( *menuitems != EOF ) |
{ |
paramID = *menuitems; |
if( paramID == SEPARATOR ) |
{ |
// wenn der vorherige Menueeintrag bereits ein SEPARATOR war |
// dann nicht erneut einen Separator anzeigen! |
// |
// Das kann ggf. passieren wenn z.B. aufgrund der FC-Revision |
// alle Zwischeneintraege ausgeblendet wurden |
if( lastmenuitem != SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
lastmenuitem = paramID; |
} |
} |
else |
{ |
//------------------------------------------------------ |
// TEST / DEBUG fuer beschriebenes Problem in _error() |
// |
// Wenn man das dortige Problem nachstellen will kann |
// man hier einen Fehlerscreen provozieren! |
//------------------------------------------------------ |
//paramID = 0; |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
// existiert die paramID in der Firmware des Kopters? |
if( paramExist(paramEditItem.paramID) ) |
{ |
MenuCtrl_PushParamEdit( paramEditItem.paramID ); |
lastmenuitem = paramID; |
} |
} |
menuitems++; |
} |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Parameters_MenuMain( uint8_t setting, char *settingname ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle( settingname ); |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
//MenuCtrl_PushML2_P( ID_MENU_TEST , MENU_ITEM, &TEST, ID_MENU_TEST_de , ID_MENU_TEST_en ); |
MenuCtrl_PushML2_P( ID_MENU_FAVORITEN , MENU_ITEM, NOFUNC, ID_MENU_FAVORITEN_de , ID_MENU_FAVORITEN_en ); // Favoriten: noch nicht implementiert... |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_MENU_KANAELE , MENU_ITEM, NOFUNC, ID_MENU_KANAELE_de , ID_MENU_KANAELE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KONFIGURATION, MENU_ITEM, NOFUNC, ID_MENU_KONFIGURATION_de, ID_MENU_KONFIGURATION_en ); |
MenuCtrl_PushML2_P( ID_MENU_STICK , MENU_ITEM, NOFUNC, ID_MENU_STICK_de , ID_MENU_STICK_en ); |
MenuCtrl_PushML2_P( ID_MENU_LOOPING , MENU_ITEM, NOFUNC, ID_MENU_LOOPING_de , ID_MENU_LOOPING_en ); |
MenuCtrl_PushML2_P( ID_MENU_HOEHE , MENU_ITEM, NOFUNC, ID_MENU_HOEHE_de , ID_MENU_HOEHE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KAMERA , MENU_ITEM, NOFUNC, ID_MENU_KAMERA_de , ID_MENU_KAMERA_en ); |
MenuCtrl_PushML2_P( ID_MENU_NAVICTRL , MENU_ITEM, NOFUNC, ID_MENU_NAVICTRL_de , ID_MENU_NAVICTRL_en ); |
MenuCtrl_PushML2_P( ID_MENU_AUSGAENGE , MENU_ITEM, NOFUNC, ID_MENU_AUSGAENGE_de , ID_MENU_AUSGAENGE_en ); |
MenuCtrl_PushML2_P( ID_MENU_VERSCHIEDENES, MENU_ITEM, NOFUNC, ID_MENU_VERSCHIEDENES_de, ID_MENU_VERSCHIEDENES_en ); |
MenuCtrl_PushML2_P( ID_MENU_GYRO , MENU_ITEM, NOFUNC, ID_MENU_GYRO_de , ID_MENU_GYRO_en ); |
MenuCtrl_PushML2_P( ID_MENU_BENUTZER , MENU_ITEM, NOFUNC, ID_MENU_BENUTZER_de , ID_MENU_BENUTZER_en ); |
MenuCtrl_PushML2_P( ID_MENU_ACHSKOPPLUNG , MENU_ITEM, NOFUNC, ID_MENU_ACHSKOPPLUNG_de , ID_MENU_ACHSKOPPLUNG_en ); |
//MenuCtrl_PushML2_P( ID_MENU_MIXERSETUP , MENU_ITEM, NOFUNC, ID_MENU_MIXERSETUP_de , ID_MENU_MIXERSETUP_en ); // nicht mehr unterstuetzt! |
MenuCtrl_PushML2_P( ID_MENU_EASYSETUP , MENU_ITEM, NOFUNC, ID_MENU_EASYSETUP_de , ID_MENU_EASYSETUP_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
switch( itemid ) |
{ |
case ID_MENU_FAVORITEN: Menu_Favoriten(); break; |
case ID_MENU_KANAELE: Menu_EditCategory( ID_MENU_KANAELE_Items ); break; |
case ID_MENU_KONFIGURATION: Menu_EditCategory( ID_MENU_KONFIGURATION_Items ); break; |
case ID_MENU_STICK: Menu_EditCategory( ID_MENU_STICK_Items ); break; |
case ID_MENU_LOOPING: Menu_EditCategory( ID_MENU_LOOPING_Items ); break; |
case ID_MENU_HOEHE: Menu_EditCategory( ID_MENU_HOEHE_Items ); break; |
case ID_MENU_KAMERA: Menu_EditCategory( ID_MENU_KAMERA_Items ); break; |
case ID_MENU_NAVICTRL: Menu_EditCategory( ID_MENU_NAVICTRL_Items ); break; |
case ID_MENU_AUSGAENGE: Menu_EditCategory( ID_MENU_AUSGAENGE_Items ); break; |
case ID_MENU_VERSCHIEDENES: Menu_EditCategory( ID_MENU_VERSCHIEDENES_Items ); break; |
case ID_MENU_GYRO: Menu_EditCategory( ID_MENU_GYRO_Items ); break; |
case ID_MENU_BENUTZER: Menu_EditCategory( ID_MENU_BENUTZER_Items ); break; |
case ID_MENU_ACHSKOPPLUNG: Menu_EditCategory( ID_MENU_ACHSKOPPLUNG_Items ); break; |
case ID_MENU_EASYSETUP: Menu_EditCategory( ID_MENU_EASYSETUP_Items ); break; |
default: //PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 200, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
break; |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# MK_Parameters() - Main-Function |
//############################################################################################# |
//-------------------------------------------------------------- |
// changed = MK_Parameters( setting, settingname ) |
//-------------------------------------------------------------- |
uint8_t MK_Parameters( uint8_t setting, char *settingname ) |
{ |
int changed; |
unsigned char *org_parameters; |
uint8_t size = paramsetSize(); |
//----------------------------------------------------------------- |
// Erkennung ob Aenderungen durch den Benutzer vorgenommen wurde |
// -> das aktuelle Paramset wird gespeichert um es spaeter mit |
// der bearbeiteten Version via memcmp zu vergleichen |
//----------------------------------------------------------------- |
org_parameters = malloc( size+1 ); // +1 fuer das erste settings-byte |
if( !org_parameters ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return false; |
} |
memcpy( org_parameters, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
//----------------------------------------------------------------- |
// MK-Parameter bearbeiten |
//----------------------------------------------------------------- |
MK_Parameters_MenuMain( setting, settingname ); |
//----------------------------------------------------------------- |
// Vergleich: Orginal-Daten <-> ggf. geaenderte Daten |
//----------------------------------------------------------------- |
changed = memcmp( org_parameters, (unsigned char *)(pRxData), size+1 ); |
free( org_parameters ); |
return( changed!=0 ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/mkparameters.h |
---|
0,0 → 1,95 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.h |
//# |
//# 27.03.2014 OG |
//# etliche neue Funktionen / Strukturen |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
// fuer cmd-Parameter der Edit-Funktionen |
#define MKPARAM_EDIT 1 // Wert bearbeiten |
#define MKPARAM_SHORTVALUE 2 // erzeugt einen 3-Zeichen String der den Wert darstellt (z.B. "Dis"); Ergebnis in strValueBuffer |
#define MKPARAM_LONGVALUE 3 // erzeugt einen String der den Wert in langer Form darstellt (z.B. "Disabled"); Ergebnis in strValueBuffer |
#define MKPARAM_STRBUFFER_LEN 20 |
//-------------------------------------------- |
// paramEditItem_t |
// deklariert die Edit-Daten einer paramID |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
extern paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
extern char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
extern char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ); |
void editGeneric( unsigned char paramID, uint8_t cmd ); |
void editBitmask( unsigned char paramID, uint8_t cmd ); |
void editDisableDeclCalc( unsigned char paramID, uint8_t cmd ); |
void editCompassOffset( unsigned char paramID, uint8_t cmd ); |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ); |
void editNA( unsigned char paramID, uint8_t cmd ); |
uint8_t MK_Parameters( uint8_t setting, char *settingname ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/mkparameters_messages.h |
---|
0,0 → 1,618 |
/***************************************************************************** |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters_messages.h |
//# |
//# 16.07.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.11a inkl. der ID's |
//# const char param_singleWpControlChannel_de |
//# const char param_MenuKeyChannel_de |
//# |
//# 09.04.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.09j |
//# const char param_ServoNickFails_de[],const char param_ServoRollFails_de[], const char param_Servo3Fails_de[], |
//# const char param_Servo4Fails_de[], const char param_Servo5Fails_de[] |
//# |
//# 26.09.2014 Cebra |
//# - add: Text Höhe Tilt Compensation (FC207f) |
//# |
//# 04.06.2014 OG |
//# - chg: Text von ID_MENU_FAVORITEN etwas gekuerzt |
//# |
//# 07.05.2014 OG |
//# - chg: ID_MENU_FAVORITEN_en |
//# |
//# 18.04.2014 OG |
//# - chg: Textanpassung: param_NaviGpsMinSat_de, param_NaviStickThreshold_de |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Textanpassung: param_WARN_J16_Bitmask_de, param_WARN_J17_Bitmask_de |
//# - chg: Textanpassung: param_J16Bitmask, param_J16Timing |
//# - chg: Textanpassung: param_J17Bitmask, param_J17Timing |
//# - chg: Textanpassung: param_SingleWpSpeed_de |
//# - chg: Textanpassung: param_ServoNickRefresh_de, param_ServoManualControlSpeed_de |
//# - chg: Textanpassung: param_ServoRollControl_de |
//# |
//# 07.04.2014 OG |
//# - chg: kleine Aenderungen an englischen Texten |
//# |
//# 06.04.2014 CB |
//# - add: englische Menütexte ergänzt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# |
//# 28.03.2014 OG |
//# - add: paramID-Texte fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 16.03.2014 OG |
//# erste groesstenteils fertige Version |
//# |
//# 23.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_MESSAGES_H |
#define _MKPARAMETERS_MESSAGES_H |
//############################################################################################# |
//# Texte: Menues von mkparameters |
//############################################################################################# |
static const char ID_MENU_TEST_de[] PROGMEM = "TEST/DEBUG"; |
#define ID_MENU_TEST_en ID_MENU_TEST_de |
static const char ID_MENU_FAVORITEN_de[] PROGMEM = "Favoriten"; |
static const char ID_MENU_FAVORITEN_en[] PROGMEM = "Favorites"; |
static const char ID_MENU_KANAELE_de[] PROGMEM = "Kanäle"; |
static const char ID_MENU_KANAELE_en[] PROGMEM = "Channel"; |
static const char ID_MENU_KONFIGURATION_de[] PROGMEM = "Konfiguration"; |
static const char ID_MENU_KONFIGURATION_en[] PROGMEM = "Configuration"; |
static const char ID_MENU_STICK_de[] PROGMEM = "Stick"; |
#define ID_MENU_STICK_en ID_MENU_STICK_de |
static const char ID_MENU_LOOPING_de[] PROGMEM = "Looping"; |
#define ID_MENU_LOOPING_en ID_MENU_LOOPING_de |
static const char ID_MENU_HOEHE_de[] PROGMEM = "Höhe"; |
static const char ID_MENU_HOEHE_en[] PROGMEM = "Altitude"; |
static const char ID_MENU_KAMERA_de[] PROGMEM = "Kamera"; |
static const char ID_MENU_KAMERA_en[] PROGMEM = "Camera"; |
static const char ID_MENU_NAVICTRL_de[] PROGMEM = "Navi-Ctrl"; |
#define ID_MENU_NAVICTRL_en ID_MENU_NAVICTRL_de |
static const char ID_MENU_AUSGAENGE_de[] PROGMEM = "Ausgänge"; |
static const char ID_MENU_AUSGAENGE_en[] PROGMEM = "Outputs"; |
static const char ID_MENU_VERSCHIEDENES_de[] PROGMEM = "Verschiedenes"; |
static const char ID_MENU_VERSCHIEDENES_en[] PROGMEM = "Miscellaneous"; |
static const char ID_MENU_GYRO_de[] PROGMEM = "Gyro"; |
#define ID_MENU_GYRO_en ID_MENU_GYRO_de |
static const char ID_MENU_BENUTZER_de[] PROGMEM = "Benutzer"; |
static const char ID_MENU_BENUTZER_en[] PROGMEM = "User"; |
static const char ID_MENU_ACHSKOPPLUNG_de[] PROGMEM = "Achskopplung"; |
static const char ID_MENU_ACHSKOPPLUNG_en[] PROGMEM = "Coupl Axes"; |
/* |
static const char ID_MENU_MIXERSETUP_de[] PROGMEM = "Mixer-Setup"; |
static const char ID_MENU_MIXERSETUP_en[] PROGMEM = "Config Mix"; |
*/ |
static const char ID_MENU_EASYSETUP_de[] PROGMEM = "Easy Setup"; |
#define ID_MENU_EASYSETUP_en ID_MENU_EASYSETUP_de |
//############################################################################################# |
//# Bezeichnungen der paramID's |
//############################################################################################# |
// Hier ist erstmal alles deklariert was auch in paramset.h vorhanden ist. |
// Wenn der Compiler richtig eingestellt ist, dann wird alles nicht benoetigte wegoptimiert! |
static const char param_Hoehe_MinGas_de[] PROGMEM = "Min. Gas"; |
static const char param_Hoehe_MinGas_en[] PROGMEM = "min. throttle"; |
static const char param_Luftdruck_D_de[] PROGMEM = "Luftdruck D"; |
static const char param_Luftdruck_D_en[] PROGMEM = "barometric D"; |
static const char param_HoeheChannel_de[] PROGMEM = "Höhe Sollwert"; |
static const char param_HoeheChannel_en[] PROGMEM = "hight setpoint"; |
static const char param_Hoehe_P_de[] PROGMEM = "Höhe P"; |
static const char param_Hoehe_P_en[] PROGMEM = "altitude P"; |
static const char param_Hoehe_Verstaerkung_de[] PROGMEM = "Verstärk./Rate"; |
static const char param_Hoehe_Verstaerkung_en[] PROGMEM = "gain rate"; |
static const char param_Hoehe_ACC_Wirkung_de[] PROGMEM = "Z-ACC"; |
#define param_Hoehe_ACC_Wirkung_en param_Hoehe_ACC_Wirkung_de |
static const char param_Hoehe_HoverBand_de[] PROGMEM = "Schwebe-Gas"; |
static const char param_Hoehe_HoverBand_en[] PROGMEM = "hoover throttle"; |
static const char param_Hoehe_GPS_Z_de[] PROGMEM = "GPS Z"; |
#define param_Hoehe_GPS_Z_en param_Hoehe_GPS_Z_de |
static const char param_Hoehe_Tilt_Comp_de[] PROGMEM = "Tilt Compensation"; |
#define param_Hoehe_Tilt_Comp_en param_Hoehe_Tilt_Comp_de |
static const char param_Hoehe_StickNeutralPoint_de[] PROGMEM = "Stick Neutr.Punkt"; |
static const char param_Hoehe_StickNeutralPoint_en[] PROGMEM = "stick neutr.point"; |
static const char param_Stick_P_de[] PROGMEM = "Nick/Roll P"; |
static const char param_Stick_D_de[] PROGMEM = "Nick/Roll D"; |
static const char param_StickGier_P_de[] PROGMEM = "Gier P"; |
#define param_Stick_P_en param_Stick_P_de |
#define param_Stick_D_en param_Stick_D_de |
#define param_StickGier_P_en param_StickGier_P_de |
static const char param_Gas_Min_de[] PROGMEM = "Min. Gas"; |
static const char param_Gas_Min_en[] PROGMEM = "min. throttle"; |
static const char param_Gas_Max_de[] PROGMEM = "Max. Gas"; |
static const char param_Gas_Max_en[] PROGMEM = "max. throttle"; |
static const char param_GyroAccFaktor_de[] PROGMEM = "ACC/Gyro Faktor"; |
#define param_GyroAccFaktor_en param_GyroAccFaktor_de |
static const char param_KompassWirkung_de[] PROGMEM = "Kompasswirkung"; |
static const char param_KompassWirkung_en[] PROGMEM = "compass effect"; |
static const char param_Gyro_P_de[] PROGMEM = "Gyro P"; |
static const char param_Gyro_I_de[] PROGMEM = "Gyro I"; |
static const char param_Gyro_D_de[] PROGMEM = "Gyro D"; |
static const char param_Gyro_Gier_P_de[] PROGMEM = "Gier P"; |
static const char param_Gyro_Gier_I_de[] PROGMEM = "Gier I"; |
static const char param_Gyro_Stability_de[] PROGMEM = "Gyro stab."; |
#define param_Gyro_P_en param_Gyro_P_de |
#define param_Gyro_I_en param_Gyro_I_de |
#define param_Gyro_D_en param_Gyro_D_de |
#define param_Gyro_Gier_P_en param_Gyro_Gier_P_de |
#define param_Gyro_Gier_I_en param_Gyro_Gier_I_de |
#define param_Gyro_Stability_en param_Gyro_Stability_de |
static const char param_UnterspannungsWarnung_de[] PROGMEM = "Unterspannung [0.1V]"; |
static const char param_UnterspannungsWarnung_en[] PROGMEM = "undervoltage [0.1V]"; |
static const char param_NotGas_de[] PROGMEM = "NOT-Gas"; |
static const char param_NotGas_en[] PROGMEM = "Emerg.Throttle"; |
static const char param_NotGasZeit_de[] PROGMEM = "NOT-Gas Zeit [0.1s]"; |
static const char param_NotGasZeit_en[] PROGMEM = "Emerg.Thro.Time[0.1s]"; |
static const char param_Receiver_de[] PROGMEM = "Receiver"; |
#define param_Receiver_en param_Receiver_de |
static const char param_I_Faktor_de[] PROGMEM = "Hauptregler I"; |
static const char param_I_Faktor_en[] PROGMEM = "main I"; |
static const char param_UserParam1_de[] PROGMEM = "Parameter 1"; |
static const char param_UserParam2_de[] PROGMEM = "Parameter 2"; |
static const char param_UserParam3_de[] PROGMEM = "Parameter 3"; |
static const char param_UserParam4_de[] PROGMEM = "Parameter 4"; |
#define param_UserParam1_en param_UserParam1_de |
#define param_UserParam2_en param_UserParam2_de |
#define param_UserParam3_en param_UserParam3_de |
#define param_UserParam4_en param_UserParam4_de |
static const char param_ServoNickControl_de[] PROGMEM = "Nick Ansteuerung"; |
static const char param_ServoNickControl_en[] PROGMEM = "nick servo ctrl"; |
static const char param_ServoNickComp_de[] PROGMEM = "Nick Kompensation"; |
static const char param_ServoNickComp_en[] PROGMEM = "nick servo comp."; |
static const char param_ServoNickFails_de[] PROGMEM = "Nick Failsave Wert"; |
static const char param_ServoNickFails_en[] PROGMEM = "nick failsave value"; |
static const char param_ServoNickMin_de[] PROGMEM = "Nick Servo Minimum"; |
static const char param_ServoNickMax_de[] PROGMEM = "Nick Servo Maximum"; |
#define param_ServoNickMin_en param_ServoNickMin_de |
#define param_ServoNickMax_en param_ServoNickMax_de |
static const char param_ServoRollControl_de[] PROGMEM = "Roll Ansteuerung"; |
static const char param_ServoRollControl_en[] PROGMEM = "roll servo ctrl."; |
static const char param_ServoRollComp_de[] PROGMEM = "Roll Kompensation"; |
static const char param_ServoRollComp_en[] PROGMEM = "roll compensation"; |
static const char param_ServoRollFails_de[] PROGMEM = "Roll Failsave Wert"; |
static const char param_ServoRollFails_en[] PROGMEM = "roll failsave value"; |
static const char param_ServoRollMin_de[] PROGMEM = "Roll Minimum"; |
static const char param_ServoRollMax_de[] PROGMEM = "Roll Maximum"; |
#define param_ServoRollMin_en param_ServoRollMin_de |
#define param_ServoRollMax_en param_ServoRollMax_de |
//static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschw."; // ALT |
static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschwindigkeit"; |
static const char param_ServoNickRefresh_en[] PROGMEM = "servo refresh"; |
static const char param_ServoManualControlSpeed_de[] PROGMEM = "Manu. Geschwindigkeit."; |
static const char param_ServoManualControlSpeed_en[] PROGMEM = "manuell Speed"; |
static const char param_CamOrientation_de[] PROGMEM = "Cam Richtung"; |
static const char param_CamOrientation_en[] PROGMEM = "cam orientation"; |
static const char param_Servo3_de[] PROGMEM = "Servo 3"; |
#define param_Servo3_en param_Servo3_de |
static const char param_Servo3Fails_de[] PROGMEM = "Servo 3 Failsave Wert"; |
static const char param_Servo3Fails_en[] PROGMEM = "Servo 3 failsave value"; |
static const char param_Servo4_de[] PROGMEM = "Servo 4"; |
#define param_Servo4_en param_Servo4_de |
static const char param_Servo4Fails_de[] PROGMEM = "Servo 4 Failsave Wert"; |
static const char param_Servo4Fails_en[] PROGMEM = "Servo 4 failsave value"; |
static const char param_Servo5_de[] PROGMEM = "Servo 5"; |
#define param_Servo5_en param_Servo5_de |
static const char param_Servo5Fails_de[] PROGMEM = "Servo 5 Failsave Wert"; |
static const char param_Servo5Fails_en[] PROGMEM = "Servo 5 failsave value"; |
static const char param_Servo3OnValue_de[] PROGMEM = "Servo 3 On Out1/2"; |
static const char param_Servo3OffValue_de[] PROGMEM = "Servo 3 Off Out1/2"; |
static const char param_Servo4OnValue_de[] PROGMEM = "Servo 4 On Out1/2"; |
static const char param_Servo4OffValue_de[] PROGMEM = "Servo 4 Off Out1/2"; |
#define param_Servo3OnValue_en param_Servo3OnValue_de |
#define param_Servo3OffValue_en param_Servo3OffValue_de |
#define param_Servo4OnValue_en param_Servo4OnValue_de |
#define param_Servo4OffValue_en param_Servo4OffValue_de |
static const char param_LoopGasLimit_de[] PROGMEM = "Looping Gas Limit"; |
static const char param_LoopGasLimit_en[] PROGMEM = "loop throttle limit"; |
static const char param_LoopThreshold_de[] PROGMEM = "Ansprechschwelle"; |
static const char param_LoopThreshold_en[] PROGMEM = "response threshold"; |
static const char param_LoopHysterese_de[] PROGMEM = "Hysterese"; |
static const char param_LoopHysterese_en[] PROGMEM = "hysteresis"; |
static const char param_AchsKopplung1_de[] PROGMEM = "Gier pos. Kopplung"; |
static const char param_AchsKopplung1_en[] PROGMEM = "gier pos. coupling"; |
static const char param_AchsKopplung2_de[] PROGMEM = "Nick/Roll Kopplung"; |
static const char param_AchsKopplung2_en[] PROGMEM = "nick/roll coupling"; |
static const char param_CouplingYawCorrection_de[] PROGMEM = "Gier Korrektur"; |
static const char param_CouplingYawCorrection_en[] PROGMEM = "gier correction"; |
static const char param_WinkelUmschlagNick_de[] PROGMEM = "Nick Umkehrpunkt"; |
static const char param_WinkelUmschlagNick_en[] PROGMEM = "nick turnover"; |
static const char param_WinkelUmschlagRoll_de[] PROGMEM = "Roll Umkehrpunkt"; |
static const char param_WinkelUmschlagRoll_en[] PROGMEM = "roll turnover"; |
static const char param_GyroAccAbgleich_de[] PROGMEM = "ACC/Gyro Komp. [1/x]"; |
static const char param_GyroAccAbgleich_en[] PROGMEM = "ACC/Gyro Comp. [1/x]"; |
static const char param_Driftkomp_de[] PROGMEM = "Drift-Kompensation"; |
static const char param_Driftkomp_en[] PROGMEM = "drift compensation"; |
static const char param_DynamicStability_de[] PROGMEM = "Dynamische Stabilität"; |
static const char param_DynamicStability_en[] PROGMEM = "dynamic stabiliy"; |
static const char param_UserParam5_de[] PROGMEM = "Parameter 5"; |
static const char param_UserParam6_de[] PROGMEM = "Parameter 6"; |
static const char param_UserParam7_de[] PROGMEM = "Parameter 7"; |
static const char param_UserParam8_de[] PROGMEM = "Parameter 8"; |
#define param_UserParam5_en param_UserParam5_de |
#define param_UserParam6_en param_UserParam6_de |
#define param_UserParam7_en param_UserParam7_de |
#define param_UserParam8_en param_UserParam8_de |
static const char param_J16Bitmask_de[] PROGMEM = "Out1: Bitmaske"; |
static const char param_J16Bitmask_en[] PROGMEM = "Out1: bitmask"; |
static const char param_J16Timing_de[] PROGMEM = "Out1: Timing [10ms]"; |
#define param_J16Timing_en param_J16Timing_de |
static const char param_J17Bitmask_de[] PROGMEM = "Out2: Bitmaske"; |
static const char param_J17Bitmask_en[] PROGMEM = "Out2: bitmask"; |
static const char param_J17Timing_de[] PROGMEM = "Out2: Timing [10ms]"; |
#define param_J17Timing_en param_J17Timing_de |
static const char param_WARN_J16_Bitmask_de[] PROGMEM = "Out1 Warn UBat"; |
static const char param_WARN_J16_Bitmask_en[] PROGMEM = "Out1 undervolt warn"; |
static const char param_WARN_J17_Bitmask_de[] PROGMEM = "Out2 Warn UBat"; |
static const char param_WARN_J17_Bitmask_en[] PROGMEM = "Out2 undervolt warn"; |
static const char param_AutoPhotoDistance_de[] PROGMEM = "Auto Trigger Dist.[m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoDistance_en param_AutoPhotoDistance_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_AutoPhotoAtitudes_de[] PROGMEM = "Auto Trigger Alt. [m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoAtitudes_en param_AutoPhotoAtitudes_de // ab Rev. 100 (z.B. FC 2.05e) |
#define param_NaviOut1Parameter_de param_AutoPhotoDistance_de // bis Rev. 98 |
#define param_NaviOut1Parameter_en param_AutoPhotoDistance_de // bis Rev. 98 |
static const char param_SingleWpSpeed_de[] PROGMEM = "SingleWP Speed 0.1m/s"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_SingleWpSpeed_en param_SingleWpSpeed_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_LandingPulse_de[] PROGMEM = "ACC Z Landing Pulse"; // ab Rev. 104 (FC 2.09d) |
#define param_LandingPulse_en param_LandingPulse_de // ab Rev. 104 (FC 2.09d) |
static const char param_NaviGpsModeChannel_de[] PROGMEM = "GPS Modus Steuerung"; |
static const char param_NaviGpsModeChannel_en[] PROGMEM = "GPS mode control"; |
static const char param_NaviGpsGain_de[] PROGMEM = "GPS Verstärkung [%]"; |
static const char param_NaviGpsGain_en[] PROGMEM = "GPS gain [%]"; |
static const char param_NaviGpsP_de[] PROGMEM = "GPS-P"; |
static const char param_NaviGpsI_de[] PROGMEM = "GPS-I"; |
static const char param_NaviGpsD_de[] PROGMEM = "GPS-D"; |
static const char param_NaviGpsPLimit_de[] PROGMEM = "GPS-P Limit"; |
static const char param_NaviGpsILimit_de[] PROGMEM = "GPS-I Limit"; |
static const char param_NaviGpsDLimit_de[] PROGMEM = "GPS-D Limit"; |
static const char param_NaviGpsA_de[] PROGMEM = "GPS Acc"; |
#define param_NaviGpsP_en param_NaviGpsP_de |
#define param_NaviGpsI_en param_NaviGpsI_de |
#define param_NaviGpsD_en param_NaviGpsD_de |
#define param_NaviGpsPLimit_en param_NaviGpsPLimit_de |
#define param_NaviGpsILimit_en param_NaviGpsILimit_de |
#define param_NaviGpsDLimit_en param_NaviGpsDLimit_de |
#define param_NaviGpsA_en param_NaviGpsA_de |
static const char param_NaviGpsMinSat_de[] PROGMEM = "Min. Satelliten"; |
static const char param_NaviGpsMinSat_en[] PROGMEM = "minimum satelite"; |
static const char param_NaviStickThreshold_de[] PROGMEM = "GPS Stick-Schwelle"; |
static const char param_NaviStickThreshold_en[] PROGMEM = "GPS stick threshold"; |
static const char param_NaviWindCorrection_de[] PROGMEM = "GPS Windkorrektur [%]"; |
static const char param_NaviWindCorrection_en[] PROGMEM = "GPS wind correct. [%]"; |
static const char param_NaviAccCompensation_de[] PROGMEM = "ACC Kompensation"; |
static const char param_NaviAccCompensation_en[] PROGMEM = "ACC compensation"; |
static const char param_NaviOperatingRadius_de[] PROGMEM = "max. Radius"; |
static const char param_NaviOperatingRadius_en[] PROGMEM = "max. radius"; |
static const char param_NaviAngleLimitation_de[] PROGMEM = "Winkel Limit"; |
static const char param_NaviAngleLimitation_en[] PROGMEM = "angle limit"; |
static const char param_NaviPH_LoginTime_de[] PROGMEM = "PH Login Zeit [s]"; |
static const char param_NaviPH_LoginTime_en[] PROGMEM = "PH login time [s]"; |
static const char param_ExternalControl_de[] PROGMEM = "Ext. Kontrolle"; |
static const char param_ExternalControl_en[] PROGMEM = "ext. control"; |
static const char param_OrientationAngle_de[] PROGMEM = "OrientationAngle"; |
#define param_OrientationAngle_en param_OrientationAngle_de |
static const char param_CareFreeChannel_de[] PROGMEM = "CareFree Steuerung"; |
static const char param_CareFreeChannel_en[] PROGMEM = "careFree control"; |
static const char param_MotorSafetySwitch_de[] PROGMEM = "Motor Sicherh.Schalt."; |
static const char param_MotorSafetySwitch_en[] PROGMEM = "motor safety switch"; |
static const char param_MotorSmooth_de[] PROGMEM = "Motor Glättung"; |
static const char param_MotorSmooth_en[] PROGMEM = "motor smooth"; |
static const char param_ComingHomeAltitude_de[] PROGMEM = "Com.Home Höhe [m]"; |
static const char param_ComingHomeAltitude_en[] PROGMEM = "coming home alti. [m]"; |
static const char param_FailSafeTime_de[] PROGMEM = "Fails. CH Zeit [s]"; |
static const char param_FailSafeTime_en[] PROGMEM = "fails. CH time [s]"; |
static const char param_MaxAltitude_de[] PROGMEM = "Max. Höhe [m]"; |
static const char param_MaxAltitude_en[] PROGMEM = "max. altitude [m]"; |
static const char param_FailsafeChannel_de[] PROGMEM = "Fails. Channel 0=Aus"; |
static const char param_FailsafeChannel_en[] PROGMEM = "fails. channel 0=off"; |
static const char param_ServoFilterNick_de[] PROGMEM = "Nick Filter"; |
static const char param_ServoFilterNick_en[] PROGMEM = "nick filter"; |
static const char param_ServoFilterRoll_de[] PROGMEM = "Roll Filter"; |
static const char param_ServoFilterRoll_en[] PROGMEM = "roll filter"; |
static const char param_StartLandChannel_de[] PROGMEM = "Auto StartLand Kanal"; |
static const char param_StartLandChannel_en[] PROGMEM = "auto start/land chan."; |
static const char param_LandingSpeed_de[] PROGMEM = "Landing Speed 0.1m/s"; |
#define param_LandingSpeed_en param_LandingSpeed_de |
static const char param_CompassOffset_de[] PROGMEM = "Compass Offset [\x0B]"; |
#define param_CompassOffset_en param_CompassOffset_de |
static const char param_AutoLandingVoltage_de[] PROGMEM = "Autoland. Volt [0.1V]"; |
#define param_AutoLandingVoltage_en param_AutoLandingVoltage_de |
static const char param_ComingHomeVoltage_de[] PROGMEM = "Coming H. Volt [0.1V]"; |
#define param_ComingHomeVoltage_en param_ComingHomeVoltage_de |
static const char param_BitConfig_de[] PROGMEM = "BitConfig"; |
static const char param_ServoCompInvert_de[] PROGMEM = "ServoCompInvert"; |
static const char param_ExtraConfig_de[] PROGMEM = "ExtraConfig"; |
static const char param_GlobalConfig3_de[] PROGMEM = "GlobalConfig3"; |
static const char param_Name_de[] PROGMEM = "Setting Name"; |
#define param_BitConfig_en param_BitConfig_de |
#define param_ServoCompInvert_en param_ServoCompInvert_de |
#define param_ExtraConfig_en param_ExtraConfig_de |
#define param_GlobalConfig3_en param_GlobalConfig3_de |
#define param_Name_en param_Name_de |
static const char param_ComingHomeOrientation_de[] PROGMEM = "CH Ausrichtung"; |
static const char param_ComingHomeOrientation_en[] PROGMEM = "CH orientation"; |
static const char param_SingleWpControlChannel_de[] PROGMEM = "Single WP Ctrl Chan."; |
#define param_SingleWpControlChannel_en param_SingleWpControlChannel_de |
static const char param_MenuKeyChannel_de[] PROGMEM = "Next WP Channel"; |
#define param_MenuKeyChannel_en param_MenuKeyChannel_de |
// subitems (Bit / Byte-Felder) |
static const char param_ServoCompInvert_SVNick_de[] PROGMEM = "Nick Umkehren"; |
static const char param_ServoCompInvert_SVNick_en[] PROGMEM = "nick inv. direction"; |
static const char param_ServoCompInvert_SVRoll_de[] PROGMEM = "Roll Umkehren"; |
static const char param_ServoCompInvert_SVRoll_en[] PROGMEM = "Roll inv. direction"; |
static const char param_ServoCompInvert_SVRelMov_de[] PROGMEM = "Nick Relativ"; |
static const char param_ServoCompInvert_SVRelMov_en[] PROGMEM = "nick relativ"; |
static const char param_ExtraConfig_HeightLimit_de[] PROGMEM = "Höhenbegrenzung"; |
static const char param_ExtraConfig_HeightLimit_en[] PROGMEM = "heigth limitation"; |
static const char param_ExtraConfig_HeightVario_de[] PROGMEM = "Vario-Höhe"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_HeightVario_en[] PROGMEM = "vario heigth"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_VarioBeep_de[] PROGMEM = "akust. Vario"; |
#define param_ExtraConfig_VarioBeep_en param_ExtraConfig_VarioBeep_de |
static const char param_ExtraConfig_SensitiveRc_de[] PROGMEM = "Erw. Empf.Sig.Prüfung"; |
static const char param_ExtraConfig_SensitiveRc_en[] PROGMEM = "enh. rec. sign. check"; |
static const char param_ExtraConfig_33vReference_de[] PROGMEM = "33vReference"; |
#define param_ExtraConfig_33vReference_en param_ExtraConfig_33vReference_de |
static const char param_ExtraConfig_NoRcOffBeeping_de[] PROGMEM = "k.Summer o.Sender"; |
static const char param_ExtraConfig_NoRcOffBeeping_en[] PROGMEM = "no beep without TX"; |
static const char param_ExtraConfig_GpsAid_de[] PROGMEM = "Dynamic PH"; |
#define param_ExtraConfig_GpsAid_en param_ExtraConfig_GpsAid_de |
static const char param_ExtraConfig_LearnableCarefree_de[] PROGMEM = "Teachable CF"; |
#define param_ExtraConfig_LearnableCarefree_en param_ExtraConfig_LearnableCarefree_de |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_de[] PROGMEM = "Kompass Fehler ignor."; |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_en[] PROGMEM = "ignore compass error"; |
static const char param_BitConfig_LoopOben_de[] PROGMEM = "Looping Oben"; |
static const char param_BitConfig_LoopOben_en[] PROGMEM = "looping up"; |
static const char param_BitConfig_LoopUnten_de[] PROGMEM = "Looping Unten"; |
static const char param_BitConfig_LoopUnten_en[] PROGMEM = "looping down"; |
static const char param_BitConfig_LoopLinks_de[] PROGMEM = "Looping Links"; |
static const char param_BitConfig_LoopLinks_en[] PROGMEM = "looping left"; |
static const char param_BitConfig_LoopRechts_de[] PROGMEM = "Looping Rechts"; |
static const char param_BitConfig_LoopRechts_en[] PROGMEM = "looping right"; |
static const char param_BitConfig_MotorBlink1_de[] PROGMEM = "nur wenn Motor An"; |
static const char param_BitConfig_MotorBlink1_en[] PROGMEM = "only with motor on"; |
static const char param_BitConfig_MotorOffLed1_de[] PROGMEM = " \x19 sofort an"; |
static const char param_BitConfig_MotorOffLed1_en[] PROGMEM = " \x19 immediately on"; |
#define param_BitConfig_MotorOffLed2_de param_BitConfig_MotorOffLed1_de |
#define param_BitConfig_MotorBlink2_de param_BitConfig_MotorBlink1_de |
#define param_BitConfig_MotorOffLed2_en param_BitConfig_MotorOffLed1_en |
#define param_BitConfig_MotorBlink2_en param_BitConfig_MotorBlink1_en |
static const char param_GlobalConfig3_NoSdCardNoStart_de[] PROGMEM = "k.Start o.SD-Karte"; |
static const char param_GlobalConfig3_NoSdCardNoStart_en[] PROGMEM = "no start w/o SD-card"; |
static const char param_GlobalConfig3_DphMaxRadius_de[] PROGMEM = "Max.Radius dPH"; |
#define param_GlobalConfig3_DphMaxRadius_en param_GlobalConfig3_DphMaxRadius_de |
static const char param_GlobalConfig3_VarioFailsafe_de[] PROGMEM = "NOT-Gas Vario Höhe"; |
static const char param_GlobalConfig3_VarioFailsafe_en[] PROGMEM = "Emerg.thr.vario h."; |
static const char param_GlobalConfig3_MotorSwitchMode_de[] PROGMEM = "Motor Swi.Mode"; |
#define param_GlobalConfig3_MotorSwitchMode_en param_GlobalConfig3_MotorSwitchMode_de |
static const char param_GlobalConfig3_NoGpsFixNoStart_de[] PROGMEM = "k.Start o.GPS Fix"; |
static const char param_GlobalConfig3_NoGpsFixNoStart_en[] PROGMEM = "no start w/o GPS Fix"; |
static const char param_GlobalConfig3_UseNcForOut1_de[] PROGMEM = "mit WP-Event verkn."; |
static const char param_GlobalConfig3_UseNcForOut1_en[] PROGMEM = "combine with WP-Event"; |
static const char param_GlobalConfig3_SpeakAll_de[] PROGMEM = "Alles Ansagen"; |
static const char param_GlobalConfig3_SpeakAll_en[] PROGMEM = "speak all"; |
static const char param_GlobalConfig3_ServoNickCompOff_de[] PROGMEM = "Nick Komp. Aus"; |
static const char param_GlobalConfig3_ServoNickCompOff_en[] PROGMEM = "nick compensation off"; |
static const char param_GlobalConfig_HoehenRegelung_de[] PROGMEM = "Höhenregelung"; |
static const char param_GlobalConfig_HoehenRegelung_en[] PROGMEM = "height control"; |
static const char param_GlobalConfig_HoehenSchalter_de[] PROGMEM = "Schalter Höhe"; |
static const char param_GlobalConfig_HoehenSchalter_en[] PROGMEM = "height switch"; |
static const char param_GlobalConfig_HeadingHold_de[] PROGMEM = "Heading Hold"; |
#define param_GlobalConfig_HeadingHold_en param_GlobalConfig_HeadingHold_de |
static const char param_GlobalConfig_KompassAktiv_de[] PROGMEM = "Kompass Aktiv"; |
static const char param_GlobalConfig_KompassAktiv_en[] PROGMEM = "compass aktiv"; |
static const char param_GlobalConfig_KompassFix_de[] PROGMEM = "Kompass Fix"; |
static const char param_GlobalConfig_KompassFix_en[] PROGMEM = "compass fix"; |
static const char param_GlobalConfig_GpsAktiv_de[] PROGMEM = "GPS Aktiv"; |
#define param_GlobalConfig_GpsAktiv_en param_GlobalConfig_GpsAktiv_de |
static const char param_GlobalConfig_AchsenkopplungAktiv_de[] PROGMEM = "Achs(ent)kopplung"; |
static const char param_GlobalConfig_AchsenkopplungAktiv_en[] PROGMEM = "(De)Coupl Axes"; |
static const char param_GlobalConfig_DrehratenBegrenzer_de[] PROGMEM = "Drehratenbegrenzung"; |
static const char param_GlobalConfig_DrehratenBegrenzer_en[] PROGMEM = "rotary rate limit."; |
static const char param_Kanalbelegung_Nick_de[] PROGMEM = "Nick"; |
static const char param_Kanalbelegung_Roll_de[] PROGMEM = "Roll"; |
#define param_Kanalbelegung_Nick_en param_Kanalbelegung_Nick_de |
#define param_Kanalbelegung_Roll_en param_Kanalbelegung_Roll_de |
static const char param_Kanalbelegung_Gas_de[] PROGMEM = "Gas"; |
static const char param_Kanalbelegung_Gas_en[] PROGMEM = "throttle"; |
static const char param_Kanalbelegung_Gear_de[] PROGMEM = "Gear"; |
#define param_Kanalbelegung_Gear_en param_Kanalbelegung_Gear_de |
static const char param_Kanalbelegung_Poti1_de[] PROGMEM = "Poti 1"; |
static const char param_Kanalbelegung_Poti2_de[] PROGMEM = "Poti 2"; |
static const char param_Kanalbelegung_Poti3_de[] PROGMEM = "Poti 3"; |
static const char param_Kanalbelegung_Poti4_de[] PROGMEM = "Poti 4"; |
static const char param_Kanalbelegung_Poti5_de[] PROGMEM = "Poti 5"; |
static const char param_Kanalbelegung_Poti6_de[] PROGMEM = "Poti 6"; |
static const char param_Kanalbelegung_Poti7_de[] PROGMEM = "Poti 7"; |
static const char param_Kanalbelegung_Poti8_de[] PROGMEM = "Poti 8"; |
#define param_Kanalbelegung_Poti1_en param_Kanalbelegung_Poti1_de |
#define param_Kanalbelegung_Poti2_en param_Kanalbelegung_Poti2_de |
#define param_Kanalbelegung_Poti3_en param_Kanalbelegung_Poti3_de |
#define param_Kanalbelegung_Poti4_en param_Kanalbelegung_Poti4_de |
#define param_Kanalbelegung_Poti5_en param_Kanalbelegung_Poti5_de |
#define param_Kanalbelegung_Poti6_en param_Kanalbelegung_Poti6_de |
#define param_Kanalbelegung_Poti7_en param_Kanalbelegung_Poti7_de |
#define param_Kanalbelegung_Poti8_en param_Kanalbelegung_Poti8_de |
static const char param_CompassOffset_DisableDeclCalc_de[] PROGMEM = "Disable Decl. Calc"; |
#define param_CompassOffset_DisableDeclCalc_en param_CompassOffset_DisableDeclCalc_de |
static const char param_NaviMaxFlyingRange_de[] PROGMEM = "Max. Flugradius [10m]"; |
static const char param_NaviMaxFlyingRange_en[] PROGMEM = "Max. Flightrad. [10m]"; |
static const char param_NaviDescendRange_de[] PROGMEM = "Fail SinkRadius [10m]"; |
#define param_NaviDescendRange_en param_NaviDescendRange_de |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/mksettings.c |
---|
0,0 → 1,707 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: MKSettings_Menu() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 26.03.2014 OG |
//# - add: etliche Aenderungen in allen Bereichen fuer das erste Release |
//# der neuen MK-Settings |
//# |
//# 27.02.2014 OG |
//# - chg: MKSettings_AskAction() Unterstuetzung von param_DUMMY |
//# |
//# 26.02.2014 OG |
//# - chg: MKSettings_Copy() auf KEYLINE2 geaendert |
//# |
//# 23.02.2014 OG |
//# - chg: MKSettings_Menu() Aufruf von MK_Parameters() geaendert |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mksettings.h" |
//--------------------------------------------------------------------------------------------- |
#define F_EXIT 0 |
#define F_REFRESH 1 |
char mksettings_menu_names[6][18]; // befuellt von: MKSettings_read_Names() |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char paramsetRevision; // Revision FC-Parameterset fuer die das Temp-Setting gueltig ist |
unsigned char *paramset; // malloc: Pointer auf kopierte Parameter-Daten |
} MKSetting_TMP_t; |
MKSetting_TMP_t MKSetting_TMP; |
#define ID_SETTING_COPY 10 // fuer MKSettings_Menu() |
#define ID_EDIT 20 // fuer MKSettings_AskAction() |
#define ID_ACTIVATE 21 // fuer MKSettings_AskAction() |
#define ID_COPY 22 // fuer MKSettings_AskAction() |
#define ID_SAVE 30 // fuer MKSettings_AskSaveSetting() |
#define ID_DISCARD 31 // fuer MKSettings_AskSaveSetting() |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init0() |
// |
// nur fuer main.c |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init0( void ) |
{ |
memset( &MKSetting_TMP, 0, sizeof(MKSetting_TMP_t) ); |
strcpy( mksettings_menu_names[5], "PKT: --empty--"); |
} |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init() |
// |
// loeschen / initialisieren vom PKT Temp-Setting |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init( void ) |
{ |
if( MKSetting_TMP.paramset != NULL ) |
{ |
free( MKSetting_TMP.paramset ); |
} |
MKSettings_TMP_Init0(); |
} |
//-------------------------------------------------------------- |
// from_setting = 6 : von TMP zu einem MK-Setting |
// from_setting <= 5: von MK-Setting zu TMP |
//-------------------------------------------------------------- |
uint8_t MKSettings_TMP_copy( uint8_t to_setting, uint8_t timeout ) |
{ |
uint8_t written; |
uint8_t size = paramsetSize(); |
if( MKSetting_TMP.paramset == NULL ) |
{ |
MKSetting_TMP.paramset = malloc( size+1 ); // +1 fuer das erste settings-byte |
} |
if( !MKSetting_TMP.paramset ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return 0; // -> EXIT |
} |
// von MK-Setting 1..5 zu TMP |
if( to_setting == 6 ) |
{ |
MKSetting_TMP.paramsetRevision = MKVersion.paramsetRevision; |
memcpy( MKSetting_TMP.paramset, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
return 6; |
} |
// von TMP zu MK-Setting 1..5 |
memcpy( (unsigned char *)(pRxData), MKSetting_TMP.paramset , size+1 ); // memcpy( dst, src, size)) |
written = MK_Setting_write( to_setting, timeout); |
return written; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_read_Names( void ) |
{ |
uint8_t setting; |
// die 5 Settings vom Kopter laden um die |
// Setting-Namen zu speichern |
for( setting=1; setting <= 5; setting++ ) |
{ |
if( !MK_Setting_load( setting, 20 ) ) |
return(0); // Fehler - setting konnte nicht geladen werden (timeout = 20) |
xsnprintf( mksettings_menu_names[setting-1], 16, "%1d: %s", setting, paramGet_p(param_Name) ); |
} |
// TMP-Setting |
if( MKSetting_TMP.paramset != NULL ) |
{ |
if( MKSetting_TMP.paramsetRevision != MKVersion.paramsetRevision ) |
{ |
// wenn die FC-Revision vom TMP-Setting abweicht vom zuletzt geladenen |
// dann wurde ggf. der Kopter gewechselt -> TMP-Setting verwerfen |
MKSettings_TMP_Init(); |
} |
else |
{ |
// den Namen aus dem TMP-Setting holen |
paramsetInit( MKSetting_TMP.paramset ); |
xsnprintf( mksettings_menu_names[5], 16, "PKT: %s", paramGet_p(param_Name) ); |
} |
} |
setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
return setting; |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskAction( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_EDIT, ID_ACTIVATE, ID_COPY |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskAction( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
if( !paramExist(param_DUMMY) ) // param_DUMMY -> das Parameterset wird nicht zum bearbeiten unterstuetzt |
MenuCtrl_Push_P( ID_EDIT , MENU_ITEM, NOFUNC, strGet(EDIT_SETTING) ); // "Setting ändern" |
if( setting != 6) |
MenuCtrl_Push_P( ID_ACTIVATE, MENU_ITEM, NOFUNC, strGet(PARA_AKTIVI) ); // "aktivieren" |
MenuCtrl_Push_P( ID_COPY , MENU_ITEM, NOFUNC, strGet(STR_COPY) ); // "kopieren" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskSaveSetting( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_SAVE, ID_DISCARD |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskSaveSetting( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
MenuCtrl_Push_P( ID_SAVE , MENU_ITEM, NOFUNC, strGet(STR_SAVE) ); // "speichern" |
MenuCtrl_Push_P( ID_DISCARD , MENU_ITEM, NOFUNC, strGet(STR_DISCARD) ); // "verwerfen" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_Copy( uint8_t from_setting ) |
{ |
const char *mask = PSTR("%15s"); |
uint8_t redraw = true; |
uint8_t loaded = 0; |
uint8_t to_setting; |
uint8_t written; |
uint8_t key; |
int8_t yoffs; |
lcd_cls(); |
to_setting = from_setting; |
while( true ) |
{ |
//------------------------ |
// anzeigen |
//------------------------ |
if( redraw ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, strGet(PARA_COPY), MINVERS); |
yoffs = -1; |
lcdx_printp_at( 0, 2, strGet(STR_VON), MNORMAL, 0,yoffs); // "von" |
lcdx_printf_at_P( 5, 2, MNORMAL, 3,yoffs, mask, mksettings_menu_names[from_setting-1] ); |
yoffs = -7; |
lcd_frect( 5*6, 4*8+yoffs, 15*6, 7, 0); |
lcdx_printp_at( 0, 4, strGet(STR_NACH), MNORMAL, 0,yoffs); // "nach" |
lcdx_printf_at_P( 5, 4, MNORMAL, 3,yoffs, mask, mksettings_menu_names[to_setting-1] ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
redraw = false; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
return 99; // nur "ENDE" |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
if( to_setting == 6 ) to_setting = 1; |
else to_setting++; |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
if( to_setting == 1 ) to_setting = 6; |
else to_setting--; |
redraw = true; |
} |
//------------------------------- |
// Taste: OK = Setting kopieren? |
//------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
lcdx_printp_center( 5, strGet(PARA_COPYQ), MNORMAL, 0,1); // "Wirklich kopieren?" (zentriert) |
lcd_rect_round( 0, 5*8-3, 127, 7+7, 1, R2); // Rahmen um die Frage |
lcd_frect( 0, 7*8, 127, 7, 0); // Keyline loeschen |
lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // neue Keyline: "Nein Ja" |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
key = 0; |
while( !key ) // Abfrage: Ja / Nein |
{ |
key = get_key_press(1 << KEY_ENTER) ? KEY_ENTER : key; // => "Ja" (=Setting kopieren) |
key = get_key_press(1 << KEY_ESC) ? KEY_ESC : key; // => "Nein" |
PKT_CtrlHook(); |
} |
//--------------------------- |
// "Ja" -> Setting kopieren! |
//--------------------------- |
if( key == KEY_ENTER ) // => "Ja" -> Setting kopieren! |
{ |
if( from_setting != 6 ) // kein PKT TMP-Setting |
{ |
loaded = MK_Setting_load( from_setting, 20 ); // timeout = 20 |
if( loaded != from_setting ) // Fehler beim laden - Datenverlust? |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return 0; // 0 = Ende/Abbruch |
} |
} |
if( to_setting == 6 ) // von Setting 1..5 nach TMP speichern |
{ |
MKSettings_TMP_copy( 6, 10 ); |
return loaded; |
} |
// Anzeige "speichern..." |
lcd_frect( 0, (8*4), 128, (8*4), 0); // Anzeigebereich löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
if( from_setting == 6 ) // von TMP nach Setting 1..5 |
{ |
written = MKSettings_TMP_copy( to_setting, 30 ); |
return written; |
} |
if( to_setting <= 5 ) // 'echtes' MK Setting speichern |
{ |
written = MK_Setting_write( to_setting, 30 ); // Timeout = 40 |
return written; |
} |
} |
if( key == KEY_ESC ) // => "Nein" -> nicht kopieren |
{ |
lcd_cls(); |
redraw = true; |
} |
} //end: if( get_key_press(1 << KEY_ENTER) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = true; |
} |
} // end: while( true ) |
return 0; |
} |
//-------------------------------------------------------------- |
// ret = MKSettings_Menu() |
// |
// Rueckgabe: |
// 0 = Ende/Fehler/Abbruch |
// 1 = Refresh |
//-------------------------------------------------------------- |
uint8_t MKSettings_Menu( void ) |
{ |
uint8_t i; |
uint8_t active_setting; |
uint8_t setting; |
uint8_t wahl; |
uint8_t wahl2; |
uint8_t changed; |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_LOADSETTINGS), false, 0, true, true ); // "lade Settings..." |
active_setting = MKSettings_read_Names(); |
if( !active_setting ) // Fehler: settings konnten nicht geladen werden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "MK Settings" |
//MenuCtrl_SetTitle_P( PSTR("MK Settings") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menue-Punkte |
//--------------- |
for( i=0; i< ( MKSetting_TMP.paramset!=NULL ? 6 : 5); i++ ) |
{ |
MenuCtrl_Push( i+1, MENU_SUB, NOFUNC, mksettings_menu_names[i] ); // Setting 1..5 |
} |
MenuCtrl_ItemSelect( active_setting ); // Menucursor auf aktives Setting setzen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) // Ende? |
{ |
break; // Ende |
} |
setting = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( setting >=1 && setting <= 6 ) |
{ |
wahl = MKSettings_AskAction( setting ); |
//-------------- |
// bearbeiten |
//-------------- |
if( wahl == ID_EDIT ) |
{ |
// TODO: Fehler abfangen! |
MK_Setting_load( setting, 25 ); // timeout = 25 |
changed = MK_Parameters( setting, mksettings_menu_names[setting-1] ); |
if( changed && (setting!=6) ) |
{ |
wahl2 = MKSettings_AskSaveSetting( setting ); |
if( wahl2 == ID_SAVE ) |
{ |
lcd_frect( 0, (8*7), 128, 7, 0); // Keyline löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
setting = MK_Setting_write( setting, 50); |
if( !setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
} |
} |
} |
//-------------- |
// aktivieren |
//-------------- |
if( wahl == ID_ACTIVATE ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_ACTIVATESETTING), false, 0, true, true ); // "aktiviere Setting..." |
active_setting = MK_Setting_change( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
else |
{ |
// neues Setting markieren |
for( i=1; i<=5; i++ ) MenuCtrl_ItemMark( i, false); // Markierungen loeschen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
} |
} // end: if( wahl == ID_ACTIVATE ) |
//-------------- |
// kopieren |
//-------------- |
if( wahl == ID_COPY ) |
{ |
active_setting = MKSettings_Copy( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
if( active_setting != 99 ) // =99 bedeutet: User hat die Funktion abgebrochen... |
{ |
MenuCtrl_Destroy(); // ok, kein Abbruch durch den User -> Menue neu aufbauen |
return F_REFRESH; // da sich ggf. Settings-Namen geaendert haben |
} |
} // end: if( wahl == ID_COPY ) |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
return F_EXIT; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// das muss ueberarbeitet werden.... |
//-------------------------------------------------------------- |
static uint8_t check_motorOff(void) |
{ |
NaviData_t *naviData; |
if( hardware == NC ) // Prüfung funktioniert nur mit NC |
{ |
SwitchToNC(); |
SendOutData( 'o', ADDRESS_NC, 1, 10, 1); |
mode = 'O'; |
rxd_buffer_locked = FALSE; |
timer = 200; |
while( !rxd_buffer_locked && timer>0 ); |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
// timer = MK_TIMEOUT; |
Decode64(); |
naviData = (NaviData_t *) pRxData; |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
return false; |
else |
return true; |
} |
return false; |
} |
return true; // hmm, wenn man nur eine FC hat dann wird hier immer gemeldet "Motoren sind aus" ? |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Settings( void ) |
{ |
//MKSettings_TMP_Init(); |
//if( true ) |
if( !check_motorOff() ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_SWITCHMOTOROFF), false, 400, true, true ); // "Motoren ausschalten!" |
return; |
} |
while( MKSettings_Menu() == F_REFRESH ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/mksettings.h |
---|
0,0 → 1,55 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.h |
//# |
//# 19.02.2014 OG - NEU |
//# - add: MKSettings_TMP_Init(), MKSettings_TMP_Init0() |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
void MK_Settings( void ); |
void MKSettings_TMP_Init0( void ); |
void MKSettings_TMP_Init( void ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/paramset.c |
---|
0,0 → 1,2583 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.c |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: unsigned char SingleWpControlChannel; (FC2.11a) |
//# unsigned char MenuKeyChannel; (FC2.11a) |
//# - add: paramset_106 (FC2.11a) |
//# |
//# 09.04.2015 Cebra (PKT384a) |
//# - add: unsigned char ServoFS_Pos[5] (FC 2.09i) |
//# - add: paramset_105 (FC2.09j) |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - add: param_LandingPulse (FC 2.09d) |
//# |
//# 26.01.2015 Cebra |
//# - add: Function transform_ComingHomeOrientation |
//# Ändert die Bits 4+5 für ServoCompInvert |
//# in Konfigurationstabelle der paramSubID: param_ComingHomeOrientation neu |
//# |
//# 26.09.2014 Cebra |
//# - add: Parameterset Rev. 103 (FC 2.07f) |
//# - chg: param_Hoehe_GPS_Z zu param_Hoehe_Verstaerkung im Paramset 103 |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 10.05.2014 OG |
//# - add: transform_CompassOffset_DisableDeclCalc() - transformiert true/false |
//# in das Byte von param_CompassOffset |
//# |
//# 09.05.2014 OG |
//# - chg: paramSet() - angepasst auf Transform-Funktionen |
//# - chg: paramGet() - angepasst auf Transform-Funktionen |
//# - add: eine neue Transformations-Zwischenschicht implementiert - abgebildet |
//# ueber die neue Tabelle paramTransform. |
//# Hier koennen Transform-Funktionen hinterlegt werden die einen |
//# ubyte8 Wert veraendern. |
//# Die Transformation wird von paramSet()/paramGet() aufgerufen. |
//# |
//# 17.04.2014 OG |
//# - FIX: _paramset_getsubitemid(): Pruefung von_Revision/bis_Revision |
//# korrigiert - fuehrte vorher ggf. zu falschen Anzeigen von SubItems! |
//# - chg: param_BitConfig_MotorOffLed1 auf bis_Revision 101 begrenzt |
//# - chg: param_BitConfig_MotorOffLed2 auf bis_Revision 101 begrenzt |
//# |
//# 09.04.2014 OG |
//# - chg: Rev. 101 auf param_DUMMY umgestellt um Platz zu sparen |
//# |
//# 08.04.2014 OG |
//# - add: Parameterset Rev. 102 (FC 2.05g) |
//# - fix: _paramset_getsubitemid() - Vergleich bzgl. von/bis_Revision umgedreht |
//# und equal zugefuegt |
//# |
//# 06.04.2014 OG |
//# - add: Parameterset Rev. 101 (FC 2.05f) |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: Parameterset Rev. 100 (FC 2.05e) |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 24.03.2014 OG |
//# - chg: _paramset_getsubitemid() erweitert Unterstuetzung von/bis FC-Revisionen |
//# - add: paramSubItem_t mit Unterstuetzung fuer von/bis FC-Revisionen |
//# |
//# 27.02.2014 OG |
//# - chg: die Revisions-Tabellen 90 bis 94 mittels param_DUMMY gekuerzt |
//# -> keine Parameter-Bearbeitung bei denen moeglich |
//# - add: vollstaendige Unterstuetzung von paramSubID's via paramGet(), paramSet() |
//# |
//# 26.02.2014 OG |
//# - add: paramSubItem[] - Zugriffstabelle auf Bit- und Bytefelder innerhalb |
//# einer paramID |
//# - chg: bei den paramset_nnn[] Tabellen 'const' ergaenzt |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 19.02.2014 OG |
//# - fix: paramsetInit() Parameter pData wurde nicht konsequent verwendet |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# - add: Rev-Tabellen von FC v0.88e bis FC v2.03d |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
//############################################################################ |
//# INHALT |
//# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//# 3a. interne Zugriffsfunktionen |
//# 3b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# 3c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# x. TEST / DEBUG |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
//--------------------------- |
// fuer transform_... |
//--------------------------- |
#define GETVALUE 1 // Wert setzen |
#define SETVALUE 2 // Wert lesen |
paramRevItem_t *paramsetRevTable; // Zeiger auf aktive Revision-Table oder 0 wenn nicht gesetzt -> wird gesetzt druch paramsetInit() / paramset.c |
unsigned char *mkparamset; // nur temp. gueltig solange der PKT-RX-Buffer (pRxData) noch nicht wieder ueberschrieben -> wird gesetzt druch paramsetInit() / paramset.c |
//############################################################################################# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//############################################################################################# |
//--------------------------------------------------------------------------------------- |
// die nachfolgenden Tabellen entsprechen der Struktur von paramRevItem_t aus paramset.h |
//--------------------------------------------------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 90 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88e |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_091 |
// |
// STRUKTUR-DIFF zu 0: |
// |
// DEFINE-DIFF zu 0: |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 91 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88m, 0.88n |
// |
// STRUKTUR-DIFF zu 090: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 090: |
// - add: #define PID_SPEAK_HOTT_CFG |
// - add: #define CFG3_MOTOR_SWITCH_MODE |
// - add: #define CFG3_NO_GPSFIX_NO_START |
//----------------------------------------------- |
// |
// |
// + paramID (paramRevItem_t -> paramID) |
// | |
// | + size in Bytes (paramRevItem_t -> size) |
// | | |
// | | |
// | | |
paramRevItem_t const paramset_091[] PROGMEM = |
{ |
{ param_DUMMY , 111 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_091[] |
//############################################################################################################################################################ |
//----------------------------------------------- |
// FC-Parameter Revision: 92 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90d |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_093 |
// |
// STRUKTUR-DIFF zu 091: |
// -add: param_NaviOut1Parameter |
//# |
// DEFINE-DIFF zu 091: |
// -add: #define CFG3_USE_NC_FOR_OUT1 0x20 |
// -add: #define CFG3_SPEAK_ALL |
// -add: #define SERVO_NICK_INV 0x01 |
// -add: #define SERVO_ROLL_INV 0x02 |
// -add: #define SERVO_RELATIVE 0x04 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 93 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90e, 0.90g, 0.90j |
// |
// STRUKTUR-DIFF zu 092: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 092: |
// - keine Aenderung erkennbar! |
//----------------------------------------------- |
paramRevItem_t const paramset_093[] PROGMEM = |
{ |
{ param_DUMMY , 112 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_093[] |
//----------------------------------------------- |
// FC-Parameter Revision: 94 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.91a |
// |
// STRUKTUR-DIFF zu 093: |
// - add: param_StartLandChannel |
// - add: param_LandingSpeed |
// |
// DEFINE-DIFF zu 093: |
// etwas undurchsichtig... |
// - add: #define EE_DUMMY |
// - add: #define PID_HARDWARE_VERSION |
//----------------------------------------------- |
paramRevItem_t const paramset_094[] PROGMEM = |
{ |
{ param_DUMMY , 114 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_094[] |
//----------------------------------------------- |
// FC-Parameter Revision: 95 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90L, 2.00a, , 2.00e |
// |
// STRUKTUR-DIFF zu 094: |
// - rename: param_MaxHoehe -> param_HoeheChannel |
// - rename: param_NaviGpsModeControl -> param_NaviGpsModeChannel |
// - rename: param_NaviGpsACC -> param_NaviGpsA |
// - rename: param_CareFreeModeControl -> param_CareFreeChannel |
// |
// DEFINE-DIFF zu 094: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_095[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_095[] |
//----------------------------------------------- |
// FC-Parameter Revision: 96 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: ??? |
// |
// STRUKTUR-DIFF zu 095: |
// - KEINE AENDERUNG der INTERNEN DATEN-STRUKTUR! |
// |
// DEFINE-DIFF zu 095: |
// GlobalConfig3 |
// - add: #define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 97 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.02b |
// |
// STRUKTUR-DIFF zu 096: |
// - add: param_CompassOffset |
// - add: param_AutoLandingVoltage |
// |
// DEFINE-DIFF zu 096: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_097[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_097[] |
//----------------------------------------------- |
// FC-Parameter Revision: 98 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.03d |
// |
// STRUKTUR-DIFF zu 097: |
// - add: param_ComingHomeVoltage |
// |
// DEFINE-DIFF zu 097: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_098[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_098[] |
//----------------------------------------------- |
// FC-Parameter Revision: 99 |
// keine Inormationen zu Rev. 99 vorhanden. |
// |
// Kommentar dazu von Holger Buss: |
// "Die Revision 0.99 kannst Du ignorieren." |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 100 |
// |
// ab FC-Version: ab ??? bis 2.05e |
// gefunden in: 2.05e |
// |
// STRUKTUR-DIFF zu 098: |
// - del: NaviOut1Parameter |
// - add: AutoPhotoDistance (ersetzt NaviOut1Parameter) |
// - add: AutoPhotoAtitudes |
// - add: SingleWpSpeed |
// |
// DEFINE-DIFF zu 098: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine kurze Zeit in Betaversionen |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_100[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_100[] |
//----------------------------------------------- |
// FC-Parameter Revision: 101 |
// |
// ab FC-Version: ab 2.05f bis 2.05f |
// gefunden in: 2.05f |
// |
// STRUKTUR-DIFF zu 100: |
// - add: Servo3OnValue |
// - add: Servo3OffValue |
// - add: Servo4OnValue |
// - add: Servo4OffValue |
// |
// DEFINE-DIFF zu 100: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine einzige Betaversion |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_101[] PROGMEM = |
{ |
{ param_DUMMY , 123 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_101[] |
//----------------------------------------------- |
// FC-Parameter Revision: 102 |
// |
// ab FC-Version: ab 2.05g bis ??? (min. 2.06b) |
// gefunden in: 2.05g, 2.06a |
// |
// STRUKTUR-DIFF zu 101: |
// - add: NaviMaxFlyingRange (ersetzt NaviOperatingRadius) |
// - add: NaviDescendRange |
// - del: NaviOperatingRadius |
// |
// DEFINE-DIFF zu 101: |
// - del: CFG3_DPH_MAX_RADIUS |
//----------------------------------------------- |
paramRevItem_t const paramset_102[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_102[] |
//----------------------------------------------- |
// FC-Parameter Revision: 103 |
// |
// ab FC-Version: ab 2.07f bis ??? |
// gefunden in: 2.07f |
// |
// STRUKTUR-DIFF zu 102: |
// keine |
// |
// DEFINE-DIFF zu 102: |
// - chg: Hoehe_GPS_Z zu Hoehe_TiltCompensation |
//----------------------------------------------- |
paramRevItem_t const paramset_103[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_103[] |
//----------------------------------------------- |
// FC-Parameter Revision: 104 |
// |
// ab FC-Version: ab 2.09d bis ??? |
// gefunden in: 2.09d |
// |
// STRUKTUR-DIFF zu 103: |
// add: LandingPulse |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_104[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_104[] |
//----------------------------------------------- |
// FC-Parameter Revision: 105 |
// |
// ab FC-Version: ab 2.09j bis ??? |
// gefunden in: 2.09j |
// |
// STRUKTUR-DIFF zu 104: |
// add: unsigned char ServoFS_Pos[5]; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_105[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_105[] |
//----------------------------------------------- |
// FC-Parameter Revision: 106 |
// |
// ab FC-Version: ab 2.11a bis ??? |
// gefunden in: 2.11a |
// |
// STRUKTUR-DIFF zu 105: |
// add: unsigned char SingleWpControlChannel; |
// unsigned char MenuKeyChannel; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_106[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_SingleWpControlChannel , 1 }, // unsigned char SingleWpControlChannel; // ab Rev. 106 (FC2.11a) |
{ param_MenuKeyChannel , 1 }, // unsigned char MenuKeyChannel; // ab Rev. 106 (FC2.11a) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_106[] |
//############################################################################################# |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//############################################################################################# |
typedef struct |
{ |
unsigned char Revision; |
const paramRevItem_t *RevisionTable; |
} paramsetRevMap_t; |
#define MAPEOF 255 |
//---------------------------------------------------------------- |
// Mappingtabelle zwischen FC-Revision und Paramset-Tabelle |
// |
// Anmerkung bzgl. Groesse: |
// Angenommen man wuerde die Rev 97 bis 90 weglassen, dann |
// wuerde man ca. 1 KByte sparen |
//---------------------------------------------------------------- |
paramsetRevMap_t paramsetRevMap[] = |
{ |
{ 106 , paramset_106 }, // 2.11a |
{ 105 , paramset_105 }, // 2.09j |
{ 104 , paramset_104 }, // 2.09d |
{ 103 , paramset_103 }, // 2.07f |
{ 102 , paramset_102 }, // 2.05g (bis min. 2.06b) |
{ 101 , paramset_101 }, // 2.05f (eingeschraenkte Unterstuetzung - ) nur in einer einzigen Betaversion vorhanden!) |
{ 100 , paramset_100 }, // 2.05e (Anmerkung OG 06.04.2014: ggf. spaeter loeschen um Platz zu sparen da diese Version nur in einigen wenigen Betaversionen vorhanden ist) |
// Rev. 99: nicht vorhanden; ignorieren |
{ 98 , paramset_098 }, // 2.03d |
{ 97 , paramset_097 }, // 2.02b |
{ 96 , paramset_095 }, // ??? (keine Struktur-Aenderung zu 095) |
{ 95 , paramset_095 }, // 0.90L, 2.00a, 2.00e |
{ 94 , paramset_094 }, // eingeschraenkte Unterstuetzung; 0.91a |
{ 93 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90e, 0.90g, 0.90j |
{ 92 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90d (keine Struktur-Aenderung zu 093) |
{ 91 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88m, 0.88n |
{ 90 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88e (keine Struktur-Aenderung zu 091) |
{ MAPEOF, 0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramSubID; // groesser/gleich PARAMSUBITEMS |
unsigned char paramID; // -> mapping auf entsprechende paramID |
unsigned char subType; // SUBTYPE_BIT oder SUBTYPE_BYTE |
unsigned char subIndex; // bei SUBTYPE_BIT: Bitmask z.b. 0x02 |
// bei SUBTYPE_BYTE: index des Bytes (siehe param_Kanalbelegung) |
unsigned char von_Revision; // ab welcher FC-Revision ist das SubItem vorhanden (0=immer) |
unsigned char bis_Revision; // bis zu welcher FC-Revision ist das SubItem vorhanden (0=immer) |
} paramSubItem_t; |
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
// +- von_Revision |
// | |
// +- paramSubID +- gehoert zu? +- Bit/Byte? +- Bit/Byte der paramID | +- bis_Revision |
// | | | | | | |
//--|-----------------------------------------|----------------------|--------------|----------------------------------|--|---------------------------------------------- |
paramSubItem_t const paramSubItem[] PROGMEM = |
{ |
{ param_ServoCompInvert_SERVO_NICK_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_NICK_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
{ param_ServoCompInvert_SERVO_ROLL_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_ROLL_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
{ param_ServoCompInvert_SERVO_RELATIVE , param_ServoCompInvert, SUBTYPE_BIT, SERVO_RELATIVE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
{ param_ExtraConfig_HeightLimit , param_ExtraConfig, SUBTYPE_BIT, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
{ param_ExtraConfig_HeightVario , param_ExtraConfig, SUBTYPE_BITN, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 negiert zu param_ExtraConfig_HeightLimit |
{ param_ExtraConfig_VarioBeep , param_ExtraConfig, SUBTYPE_BIT, CFG2_VARIO_BEEP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
{ param_ExtraConfig_SensitiveRc , param_ExtraConfig, SUBTYPE_BIT, CFG_SENSITIVE_RC , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
{ param_ExtraConfig_33vReference , param_ExtraConfig, SUBTYPE_BIT, CFG_3_3V_REFERENCE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
{ param_ExtraConfig_NoRcOffBeeping , param_ExtraConfig, SUBTYPE_BIT, CFG_NO_RCOFF_BEEPING , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
{ param_ExtraConfig_GpsAid , param_ExtraConfig, SUBTYPE_BIT, CFG_GPS_AID , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
{ param_ExtraConfig_LearnableCarefree , param_ExtraConfig, SUBTYPE_BIT, CFG_LEARNABLE_CAREFREE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
{ param_ExtraConfig_IgnoreMagErrAtStartup , param_ExtraConfig, SUBTYPE_BIT, CFG_IGNORE_MAG_ERR_AT_STARTUP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
{ param_BitConfig_LoopOben , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_OBEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
{ param_BitConfig_LoopUnten , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_UNTEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
{ param_BitConfig_LoopLinks , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_LINKS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
{ param_BitConfig_LoopRechts , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_RECHTS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
{ param_BitConfig_MotorBlink1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
{ param_BitConfig_MotorOffLed1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED1 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
{ param_BitConfig_MotorOffLed2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED2 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
{ param_BitConfig_MotorBlink2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK2 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
{ param_GlobalConfig3_NoSdCardNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_SDCARD_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
{ param_GlobalConfig3_DphMaxRadius , param_GlobalConfig3, SUBTYPE_BIT, CFG3_DPH_MAX_RADIUS , 0,100}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 |
{ param_GlobalConfig3_VarioFailsafe , param_GlobalConfig3, SUBTYPE_BIT, CFG3_VARIO_FAILSAFE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
{ param_GlobalConfig3_MotorSwitchMode , param_GlobalConfig3, SUBTYPE_BIT, CFG3_MOTOR_SWITCH_MODE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
{ param_GlobalConfig3_NoGpsFixNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_GPSFIX_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
{ param_GlobalConfig3_UseNcForOut1 , param_GlobalConfig3, SUBTYPE_BIT, CFG3_USE_NC_FOR_OUT1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
{ param_GlobalConfig3_SpeakAll , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SPEAK_ALL , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
{ param_GlobalConfig3_ServoNickCompOff , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SERVO_NICK_COMP_OFF , 96, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
{ param_GlobalConfig_HoehenRegelung , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHENREGELUNG , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
{ param_GlobalConfig_HoehenSchalter , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHEN_SCHALTER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
{ param_GlobalConfig_HeadingHold , param_GlobalConfig, SUBTYPE_BIT, CFG_HEADING_HOLD , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
{ param_GlobalConfig_KompassAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
{ param_GlobalConfig_KompassFix , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_FIX , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
{ param_GlobalConfig_GpsAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_GPS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
{ param_GlobalConfig_AchsenkopplungAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_ACHSENKOPPLUNG_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
{ param_GlobalConfig_DrehratenBegrenzer , param_GlobalConfig, SUBTYPE_BIT, CFG_DREHRATEN_BEGRENZER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
{ param_Kanalbelegung_Nick , param_Kanalbelegung, SUBTYPE_BYTE, 0 , 0, 0}, // -> Kanalbelegung[12] |
{ param_Kanalbelegung_Roll , param_Kanalbelegung, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Kanalbelegung_Gas , param_Kanalbelegung, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Kanalbelegung_Gear , param_Kanalbelegung, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Kanalbelegung_Poti1 , param_Kanalbelegung, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_Kanalbelegung_Poti2 , param_Kanalbelegung, SUBTYPE_BYTE, 5 , 0, 0}, |
{ param_Kanalbelegung_Poti3 , param_Kanalbelegung, SUBTYPE_BYTE, 6 , 0, 0}, |
{ param_Kanalbelegung_Poti4 , param_Kanalbelegung, SUBTYPE_BYTE, 7 , 0, 0}, |
{ param_Kanalbelegung_Poti5 , param_Kanalbelegung, SUBTYPE_BYTE, 8 , 0, 0}, |
{ param_Kanalbelegung_Poti6 , param_Kanalbelegung, SUBTYPE_BYTE, 9 , 0, 0}, |
{ param_Kanalbelegung_Poti7 , param_Kanalbelegung, SUBTYPE_BYTE, 10 , 0, 0}, |
{ param_Kanalbelegung_Poti8 , param_Kanalbelegung, SUBTYPE_BYTE, 11 , 0, 0}, |
{ param_CompassOffset_DisableDeclCalc , param_CompassOffset, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 8 und 7 von CompassOffset kodiert |
{ param_ComingHomeOrientation , param_ServoCompInvert, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 5 und 4 von CervoCompInvert |
{ param_ServoNickFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 0 , 0, 0}, |
{ param_ServoRollFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Servo3Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Servo4Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Servo5Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_EOF , 0,0,0,0,0 } // END - MUST BE THE LAST!! |
}; // end: paramSubItem[] |
//############################################################################################# |
//# 3a. Transformations-Tabelle |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID oder paramSubID |
unsigned char (*transformfunc)( uint8_t cmd, unsigned char value, unsigned char newvalue ); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
} paramTransform_t; |
//--------------------------------------- |
// forward Deklarationen fuer transform |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, uint16_t newvalue ); |
//--------------------------------------- |
//--------------------------------------- |
paramTransform_t const paramTransform[] = |
{ |
{ param_CompassOffset_DisableDeclCalc , &transform_CompassOffset_DisableDeclCalc }, |
{ param_ComingHomeOrientation , &transform_ComingHomeOrientation }, |
// { param_LandingPulse, &transform_ValueACCZ}, |
{ param_EOF , NULL } // END - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 3b. Transformations-Funktionen |
//############################################################################################# |
//--------------------------------------- |
// transform_CompassOffset_DisableDeclCalc() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// Der true/false (Ja/Nein) Wert von param_CompassOffset_DisableDeclCalc |
// ist in dem Byte von param_CompassOffset in den Bits 7 & 8 einkodiert. |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
uint8_t bit7; |
uint8_t bit8; |
bit7 = ((value & 0x40) ? true : false); |
bit8 = ((value & 0x80) ? true : false); |
if( cmd == GETVALUE ) |
{ |
// Bit 8 == Bit 7: Disable dec. Calc AUS |
// Bit 8 != Bit 7: Disable dec. Calc AN |
value = ((bit8 == bit7) ? 0 : 1); |
} |
if( cmd == SETVALUE ) |
{ |
if( newvalue ) bit8 = !bit7; // Bit 8 != Bit 7: Disable dec. Calc AN |
else bit8 = bit7; // Bit 8 == Bit 7: Disable dec. Calc AUS |
if( bit8 ) value = (value | 0x80); // Bit 8 setzen |
else value = (value & (0x80 ^ 0xff)); // Bit 8 loeschen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
//--------------------------------------- |
// transform_ComingHomeOrientation() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// |
// ist in dem Byte von param_ServoCompInvert in den Bits 4 & 5 einkodiert, Bits 1-3 bleiben unverändert |
//--------------------------------------- |
uint8_t save =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
if( cmd == GETVALUE ) |
{ |
save = value; // altes "gemeinsames" Byte ServoCompInvert sichern |
value =((value & 0x18) >> 3); // CominghomeOrientation Bits nach rechts verschieben |
} |
if( cmd == SETVALUE ) |
{ |
value = ((newvalue << 3)|(save & 0x07)); // CominghomeOrientation Bits nach links verschieben und "alte" Bits 1-3 wieder dazufügen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
// |
// |
////--------------------------------------- |
//// transform_ValueACCZ() |
//// |
//// PARAMETER: |
//// cmd : GETVALUE || SETVALUE |
//// value : |
//// newvalue: nur wenn cmd == SETVALUE |
//// |
//// |
//// ACC Z Landing Pulse Wert für die Anzeige x 4 |
////--------------------------------------- |
// |
// |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, unint16_t newvalue ) |
// |
//uint16_t save16 =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
// |
//{ |
// if( cmd == GETVALUE ) |
// { |
// save16 = value; // altes Byte ACC Z Landing Pulse sichern |
// value = (value * 4); // Wert x 4 |
// } |
// |
// if( cmd == SETVALUE ) |
// { |
// value = (newvalue / 4); // Neuer Wert ACC Z Landingpulse /4 |
// } |
// |
// |
// return value; |
//} |
//############################################################################################# |
//# 4a. interne Zugriffsfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// paramSubID: 'echter' Parameter zum Suchen in paramSubItem[] zum |
// nachfolgeden Parameter geben das Suchergebnis zurueck |
// und sind alle 0 wenn nichts gefunden wurde |
// |
// Rueckgabe: |
// true/false |
//-------------------------------------------------------------- |
unsigned char _paramset_getsubitemid( unsigned char paramSubID, unsigned char *paramID, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char id; |
unsigned char *p; |
unsigned char von_Revision; |
unsigned char bis_Revision; |
*paramID = 0; |
*subType = 0; |
*subIndex = 0; |
p = (unsigned char *) paramSubItem; |
while( true ) |
{ |
id = pgm_read_byte(p+0); // paramSubItem[..].paramSubID - die aktuelle paramSubID |
if( (id == paramSubID) ) // gefunden? |
{ |
von_Revision = pgm_read_byte(p+4); // paramSubItem[..].von_Revision; |
bis_Revision = pgm_read_byte(p+5); // paramSubItem[..].bis_Revision; |
// in aktueller FC-Revision vorhanden? |
if( von_Revision && (MKVersion.paramsetRevision < von_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
if( bis_Revision && (MKVersion.paramsetRevision > bis_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
*paramID = pgm_read_byte(p+1); // paramSubItem[..].paramID; |
*subType = pgm_read_byte(p+2); // paramSubItem[..].subType; |
*subIndex = pgm_read_byte(p+3); // paramSubItem[..].subIndex; |
return true; |
} |
if( id == param_EOF ) break; |
p += sizeof( paramSubItem_t ); |
} |
return false; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize: false = offset zurueckgeben (normaler Modus) |
// true = size von paramID zurueckgeben (fuer paramSize()) |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffsetX( unsigned char paramID, unsigned char retItemSize, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char *p; |
unsigned char offset; |
unsigned char size; |
unsigned char this_paramID; |
unsigned char paramSubID; |
*subType = SUBTYPE_NO; // =0 |
*subIndex = 0; |
size = 0; |
offset = param_NOTFOUND; |
if( MKVersion.paramsetOK ) |
{ |
//----------------------- |
// eine paramSubID wurde uebergeben |
// -> ermittle zugehoerige paramID! |
//----------------------- |
if( (paramID >= param_SUBITEM) && (paramID != param_EOF) ) |
{ |
paramSubID = paramID; |
if( !_paramset_getsubitemid( paramSubID, ¶mID, subType, subIndex ) ) |
{ |
// keine paramID gefunden... |
if( retItemSize ) return size; // size == 0 |
else return offset; // offset == param_NOTFOUND |
} |
} |
//----------------------- |
// paramID suchen |
//----------------------- |
offset = 0; |
p = (unsigned char *) paramsetRevTable; |
while( true ) |
{ |
this_paramID = pgm_read_byte(p); // die aktuelle paramID |
size = pgm_read_byte(p+1); // size von paramID |
// gefunden oder Ende |
if( (this_paramID == paramID) || (this_paramID == param_EOF) ) |
{ |
break; |
} |
offset += size; |
p += sizeof( paramRevItem_t ); |
} |
// paramID nicht gefunden? |
if( (this_paramID == param_EOF) && (paramID != param_EOF) ) |
{ |
offset = param_NOTFOUND; |
size = 0; |
} |
} |
if( retItemSize ) return size; |
else return offset; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize == false: offset zurueckgeben (normaler Modus) |
// retItemSize == true : size von paramID zurueckgeben |
// --> fuer paramSize() |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffset( unsigned char paramID, unsigned char retItemSize ) |
{ |
unsigned char subType; |
unsigned char subIndex; |
return _paramset_getoffsetX( paramID, retItemSize, &subType, &subIndex ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// sucht/prueft ob eine Transform-Funktion vorhanden ist |
// |
// RUECKGABE: |
// Index: (Num 0..n) auf paramTransform[] |
// => param_EOF (=255) wenn keine transform-Funktion vorhanden |
//-------------------------------------------------------------- |
unsigned char _paramset_gettransformIndex( unsigned char paramID ) |
{ |
unsigned char i; |
i = 0; |
while( paramTransform[i].paramID != param_EOF ) |
{ |
if( paramTransform[i].paramID == paramID ) return i; |
if( paramTransform[i].paramID == param_EOF ) return param_EOF; |
i++; |
} |
return param_EOF; |
} |
//############################################################################################# |
//# 4b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# |
//# Hier sind die Getter/Setter um auf die einzelnen Werte der paramset-Struktur zuzugreifen. |
//# Dazu muss vorher das richtige paramset initialisert worden sein! |
//# |
//# ACHTUNG! |
//# Fuer die Daten wird direkt auf den RX-Buffer vom PKT zugegriffen! |
//# ==> der Buffer darf NICHT durch andere Datenpakete bereits wieder ueberschrieben worden sein! |
//# |
//# Anmerkung: |
//# Das obige 'Achtung' koennte man aendern durch eine Kopie des RX-Buffers. Das verbraucht |
//# zusaetzlichen RAM-Speicher -> probieren wir es erstmal so... |
//############################################################################################# |
//-------------------------------------------------------------- |
// size = paramSize( paramID ) |
//-------------------------------------------------------------- |
unsigned char paramSize( unsigned char paramID ) |
{ |
return( _paramset_getoffset(paramID,true) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// exist = paramExist( paramID ) |
// |
// Rueckgabe: |
// true : die paramID existiert im aktuellen paramset |
// false : die paramID existiert nicht |
//-------------------------------------------------------------- |
unsigned char paramExist( unsigned char paramID ) |
{ |
return( (_paramset_getoffset(paramID,true) != 0) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// pointer = paramGet_p( paramID ) |
// |
// ACHTUNG! Nicht faehig fuer Transform! |
// |
// RUECKGABE: |
// pointer direkt auf~ein Byte im Parameterset |
// (paramSubID nicht moeglich!) |
//-------------------------------------------------------------- |
unsigned char *paramGet_p( unsigned char paramID ) |
{ |
unsigned char offset; |
offset = _paramset_getoffset( paramID, false ); // false = offset zurueckgeben |
if( offset != param_NOTFOUND ) |
{ |
return( (unsigned char *) (mkparamset + offset) ); // Rueckgabe: Pointer auf die Daten |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// value = paramGet( paramID ) |
// |
// holt den Wert von paramID. paramSubID's werden dabei |
// unterstuetzt (Bit- und Bytefelder). |
// |
// Bei Bit-Feldern (z.B. GlobalConfig3) wird eine enstprechende |
// Bit-Maskierung automatisch durchgefuehrt |
// Ergebnis: true (=1) oder false (=0)false (=0) |
// |
// Bei Byte-Feldern (z.B. Kanalbelegung) wird der Wert des |
// entsprechenden Byte's zurueckgegeben |
// |
// Hinweis: anhand des Rueckgabewertes kann nicht ueberprueft |
// werden ob die paramID gefunden wurde bzw. existiert! |
// -> ggf. erst mit paramExist() pruefen ob der Wert existiert! |
//-------------------------------------------------------------- |
unsigned char paramGet( unsigned char paramID ) |
{ |
unsigned char offset; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char value; |
unsigned char transformIdx; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
value = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen (paramID) |
//------------------- |
// subIndex: Bit |
//------------------- |
if( subType == SUBTYPE_BIT ) |
{ |
value = ((value & subIndex) ? true : false); // Rueckgabe: true/false des gewaehlten Bit's |
} |
//------------------- |
// subIndex: Bit negiert |
//------------------- |
if( subType == SUBTYPE_BITN ) |
{ |
value = ((value & subIndex) ? false : true); // Rueckgabe: true/false des gewaehlten Bit's (negiert) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
value = (unsigned char) *(mkparamset + (offset+subIndex)); // subIndex Inhalt lesen |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
value = paramTransform[transformIdx].transformfunc( GETVALUE, value, value); |
} |
return value; // ubyte zurueckgeben |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ) |
{ |
unsigned char offset; |
unsigned char subOffset = 0; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char byte; |
unsigned char value; |
unsigned char transformIdx; |
value = newvalue; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
//------------------- |
// subIndex: Bit |
//------------------- |
if( (subType == SUBTYPE_BIT) || (subType == SUBTYPE_BITN) ) |
{ |
byte = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen |
if( subType == SUBTYPE_BITN ) // SUBTYPE_BITN = negiertes BIT |
value = (value ? false : true); // negieren |
if(value) byte = byte | subIndex; // Bit setzen |
else byte = byte & (subIndex ^ 0xff); // Bit loeschen |
value = byte; // neues Value fuer das gesamte Byte (mit eingerechnetem BIT) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
subOffset = subIndex; // das 'verschobene' Byte (hier nur den Offset verschieben) |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
byte = (unsigned char) *(mkparamset + offset + subOffset); // offset Inhalt lesen |
value = paramTransform[transformIdx].transformfunc( SETVALUE, byte, newvalue); |
} |
//------------------- |
// Byte speichern |
//------------------- |
*(mkparamset + offset + subOffset) = value; |
return true; // OK |
} |
//############################################################################################# |
//# 4c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# |
//# Init und Abfrage der Groesse in Bytes ddes gesamten Paramset's |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = paramsetInit( *pData ) |
// |
// Ermittelt zu einer empfangenen Revision die passende paramset-Tabelle |
// und setzt entsprechende Werte in MKVersion |
// |
// Wird von MK_load_setting() aufgerufen nach dem Einlesen der |
// MK-Parameter (Setting-Request 'q') |
// |
// Parameter: |
// *pData : Zeiger direkt auf den RX-Buffer des PKT |
// |
// Rueckgabe: |
// true : OK - Paramset ist initialisert; die aktuelle FC-Revision |
// steht in MKVersion.paramsetRevision |
// false : Fehler - keine passende paramset-Tabelle gefunden |
//-------------------------------------------------------------- |
unsigned char paramsetInit( unsigned char *pData ) |
{ |
unsigned char i; |
unsigned char *p; |
paramsetRevTable = 0; |
mkparamset = 0; |
MKVersion.paramsetOK = false; |
MKVersion.paramsetRevision = 0; |
MKVersion.mksetting = 0; |
MKVersion.mksettingName[0] = 0; |
//-------------------------------------------------- |
// pData == 0 -> nur 'reset' der Daten |
//-------------------------------------------------- |
if( pData == 0 ) |
{ |
return 0; |
} |
//-------------------------------------------------- |
// diese beiden Werte koennen in jedem Fall gesetzt werden |
// unabhaengig ob das gefundene Parameterset vom PKT |
// unterstuetzt wird oder nicht |
//-------------------------------------------------- |
MKVersion.mksetting = (unsigned char) *pData; |
MKVersion.paramsetRevision = (unsigned char) *(pData + 1); |
//-------------------------------------------------- |
// FC-Revision in paramsetRevMap suchen |
//-------------------------------------------------- |
i = 0; |
while( true ) |
{ |
if( (paramsetRevMap[i].Revision == MKVersion.paramsetRevision) || (paramsetRevMap[i].Revision == MAPEOF) ) |
break; |
i++; |
} |
//-------------------------------------- |
// Revision nicht gefunden -> return 0 |
//-------------------------------------- |
if( paramsetRevMap[i].Revision == MAPEOF ) |
{ |
return 0; |
} |
//-------------------------------------- |
// Revision gefunden |
//-------------------------------------- |
mkparamset = (unsigned char *) (pData + 1); |
paramsetRevTable = (paramRevItem_t *) paramsetRevMap[i].RevisionTable; |
MKVersion.paramsetOK = true; |
//-------------------------------------- |
// den aktuellen Setting-Namen kopieren |
// wird u.a. vom OSD verwendet |
//-------------------------------------- |
p = paramGet_p( param_Name ); |
if( p ) memcpy( MKVersion.mksettingName, p, 12 ); |
return MKVersion.paramsetOK; |
} |
//-------------------------------------------------------------- |
// size = paramsetSize() |
// |
// Gibt die Groesse des gesamten aktuellen Parameterset's |
// zurueck. Fuer Revision 098 z.B. 130 = 130 Bytes. |
// |
// Rueckgabe: |
// =0 : Fehler; nicht unterstuetzte FC-Revision oder nicht |
// initialisiert |
//-------------------------------------------------------------- |
unsigned char paramsetSize( void ) |
{ |
if( MKVersion.paramsetOK ) |
return _paramset_getoffset( param_EOF, false ); // false = offset zurueckgeben; der offset von param_EOF entspricht der Groesse des paramSet's! |
else |
return 0; |
} |
//############################################################################################# |
//# x. TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
// TEST / DEBUG |
//-------------------------------------------------------------- |
#ifdef DEBUG_PARAMSET |
void paramsetDEBUG( void ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
//memcpy( &MKVersion.NCVersion, (Version_t *) (pRxData), sizeof(MKVersion_t) ); |
/* |
typedef struct |
{ |
Version_t NCVersion; // |
Version_t FCVersion; // |
unsigned char paramsetRevision; // von der FC |
unsigned char paramsetOk; // true wenn Revision in paramset.c vorhanden |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
*/ |
//setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
//---------------------------------- |
// Anzeige: FC/NC Version |
//---------------------------------- |
lcdx_printp_at( 0, 0, PSTR("FC:"), MNORMAL, 0,0); |
MKVersion_print_at( 3, 0, FC, MNORMAL, 0,0); |
lcdx_printp_at( 13, 0, PSTR("NC:"), MNORMAL, 0,0); |
MKVersion_print_at( 16, 0, NC, MNORMAL, 0,0); |
//---------------------------------- |
// Anzeige: FC Revision |
//---------------------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("Rev: %03u"), MKVersion.paramsetRevision ); |
if( MKVersion.paramsetOK ) |
lcdx_printp_at( 8, 1, PSTR(" ok!"), MINVERS, 0,0); |
else |
lcdx_printp_at( 8, 1, PSTR(" err"), MINVERS, 0,0); |
// Size von paramset |
lcd_printf_at_P( 13, 1, MNORMAL, PSTR("Size=%3u"), paramsetSize() ); |
//---------------------------------- |
// Anzeige: aktuelles MK Setting |
//---------------------------------- |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("Set:%2u %s"), MKVersion.mksetting, MKVersion.mksettingName ); |
/* |
unsigned char *p; |
p = paramGet_p( param_Name ); |
if( p ) |
{ |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 1, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 2, 3, MNORMAL, PSTR("%c"), *p ); |
} |
*/ |
//---------------------------------- |
//---------------------------------- |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("Size Name:%3u"), paramSize(param_Name) ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
paramSet_UByte( param_Name, 'x' ); |
lcd_printf_at_P( 3, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
*/ |
/* |
uint8_t v; |
v = MKVersion_Cmp( MKVersion.FCVer, 2,0,'f' ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("v:%3u"), v ); |
*/ |
/* |
unsigned char paramID = 1; |
unsigned char paramSubType = 1; |
unsigned char paramSubIndex =1; |
_paramset_getsubitemid( param_GlobalConfig_DrehratenBegrenzer, ¶mID, ¶mSubType, ¶mSubIndex ); |
//lcd_printf_at_P( 0, 4, MNORMAL, PSTR("ID:%u T:%u I:%u"), paramID, paramSubType, paramSubIndex ); |
*/ |
//#define param_ExtraConfig_NoRcOffBeeping 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
//#define param_ExtraConfig_GpsAid 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
//#define param_ExtraConfig_LearnableCarefree 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
//#define param_ExtraConfig_IgnoreMagErrAtStartup 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
//#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
//#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
//paramGet( unsigned char paramID ); |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
//#define param_Kanalbelegung_Gas 242 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
//#define param_Kanalbelegung_Gear 243 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
//#define param_Kanalbelegung_Nick 240 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
//#define param_Kanalbelegung_Roll 241 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
// lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), paramGet(param_Kanalbelegung_Gas), paramGet(param_Kanalbelegung_Gear), paramGet(param_Kanalbelegung_Nick), paramGet(param_Kanalbelegung_Roll) ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u"), paramGet(param_Kanalbelegung) ); |
/* |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
unsigned char v; |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
*/ |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%u %u"), |
paramGet( param_Kanalbelegung_Gas ), |
paramGet( param_Kanalbelegung_Gear ) |
); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti1 ), |
paramGet( param_Kanalbelegung_Poti2 ), |
paramGet( param_Kanalbelegung_Poti3 ), |
paramGet( param_Kanalbelegung_Poti4 ) |
); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti5 ), |
paramGet( param_Kanalbelegung_Poti6 ), |
paramGet( param_Kanalbelegung_Poti7 ), |
paramGet( param_Kanalbelegung_Poti8 ) |
); |
*/ |
// lcd_printf_at_P( 0, 3, MNORMAL, PSTR("HR:%u H-Limit:%u"), |
// paramGet( param_GlobalConfig_HoehenRegelung ), |
// paramGet( param_ExtraConfig_HeightLimit ) |
// ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Auto-S-L:%u"), |
// paramGet( param_StartLandChannel ) |
// ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("CH Orientation:%x"), paramGet( param_ServoCompInvert)); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("ACC Land pulse:%x"), paramGet( param_LandingPulse)); |
; |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
#endif // #ifdef DEBUG_PARAMSET |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings/paramset.h |
---|
0,0 → 1,357 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.h |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: #define param_SingleWpControlChannel (FC2.11a) |
//# #define param_MenuKeyChannel (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: #define param_ServoNickFailsave, #define param_ServoRollFailsave, #define param_Servo3Failsave |
//# #define param_Servo4Failsave, #define param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: #define param_ComingHomeOrientation 241 ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: paramID fuer Rev.103 (FC 2.07f) |
//# -> param_Hoehe_TiltCompensation |
//# |
//# 08.04.2014 OG |
//# - add: paramID's fuer Rev.102 (FC 2.05g) |
//# -> param_NaviMaxFlyingRange, param_NaviDescendRange |
//# |
//# 06.04.2014 OG |
//# - add: paramID's fuer Rev.101 |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: paramID's fuer Rev.100 |
//# -> param_AutoPhotoDistance, param_AutoPhotoAtitudes |
//# -> param_SingleWpSpeed |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 27.02.2014 OG |
//# - del: paramID's fuer FC-Revisionen < 95 entfernt |
//# |
//# 27.02.2014 OG |
//# - chg: paramGet(), paramSet() - Unterstuetzen auch Bit- und Bytefelder |
//# |
//# 26.02.2014 OG |
//# - add: defines fuer die Unterstuetzung von paramSubID's ergaenzt |
//# paramSubItems bieten direkten Zugriff auf Bit- und Bytefelder |
//# von paramID's - die paramSubItems wurde in die defines der |
//# paramID's integriert indem sie am Ende gelistet werden (>=160) |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 25.02.2014 OG |
//# - add: defines zu param_Kanalbelegung_xyz (240 bis 251) |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
#ifndef _PARAMSET_H |
#define _PARAMSET_H |
//--------------------------- |
// Typ einer paramSubID |
//--------------------------- |
#define SUBTYPE_NO 0 // keine paramSubID |
#define SUBTYPE_BIT 1 // Bit-Feld (z.B. param_GlobalConfig) |
#define SUBTYPE_BITN 2 // Bit-Feld (z.B. param_GlobalConfig) negiert! |
#define SUBTYPE_BYTE 3 // Byte-Feld (z.B. Kanalbelegung) |
//######################################################################################################################################################## |
//---------------------------------------------------------------------------------------- |
// Die Reihenfolge ist egal - aber(!) darauf achten das keine ID doppelt vergeben wird! |
// Wenn neue Feleder hinzukommen an das Ende (vor EOF - keine ID 255!) hinzufuegen! |
// Bitte die Datentypen im Kommentar notieren! |
// |
// -> paramID |
//---------------------------------------------------------------------------------------- |
#define param_Revision 1 // unsigned char |
#define param_Kanalbelegung 2 // unsigned char [12] GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
#define param_GlobalConfig 3 // unsigned char 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
#define param_Hoehe_MinGas 4 // unsigned char Wert : 0-100 |
#define param_Luftdruck_D 5 // unsigned char Wert : 0-250 |
#define param_HoeheChannel 6 // unsigned char Wert : 0-32 |
#define param_Hoehe_P 7 // unsigned char Wert : 0-32 |
#define param_Hoehe_Verstaerkung 8 // unsigned char Wert : 0-50 |
#define param_Hoehe_ACC_Wirkung 9 // unsigned char Wert : 0-250 |
#define param_Hoehe_HoverBand 10 // unsigned char Wert : 0-250 |
#define param_Hoehe_GPS_Z 11 // unsigned char Wert : 0-250 |
#define param_Hoehe_StickNeutralPoint 12 // unsigned char Wert : 0-250 |
#define param_Stick_P 13 // unsigned char WERT : 0..20 |
#define param_Stick_D 14 // unsigned char WERT : 0..20 |
#define param_StickGier_P 15 // unsigned char Wert : |
#define param_Gas_Min 16 // unsigned char Wert : 0-32 |
#define param_Gas_Max 17 // unsigned char Wert : 33-250 |
#define param_GyroAccFaktor 18 // unsigned char Wert : 1-64 |
#define param_KompassWirkung 19 // unsigned char Wert : 0-32 |
#define param_Gyro_P 20 // unsigned char Wert : 10-250 |
#define param_Gyro_I 21 // unsigned char Wert : 0-250 |
#define param_Gyro_D 22 // unsigned char Wert : 0-250 |
#define param_Gyro_Gier_P 23 // unsigned char Wert : 10-250 |
#define param_Gyro_Gier_I 24 // unsigned char Wert : 0-250 |
#define param_Gyro_Stability 25 // unsigned char Wert : 0-16 |
#define param_UnterspannungsWarnung 26 // unsigned char Wert : 0-250 |
#define param_NotGas 27 // unsigned char Wert : 0-250 //Gaswert bei Empängsverlust |
#define param_NotGasZeit 28 // unsigned char Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
#define param_Receiver 29 // unsigned char 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
#define param_I_Faktor 30 // unsigned char Wert : 0-250 |
#define param_UserParam1 31 // unsigned char Wert : 0-250 |
#define param_UserParam2 32 // unsigned char Wert : 0-250 |
#define param_UserParam3 33 // unsigned char Wert : 0-250 |
#define param_UserParam4 34 // unsigned char Wert : 0-250 |
#define param_ServoNickControl 35 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoNickComp 36 // unsigned char Wert : 0-250 // Einfluss Gyro/Servo |
#define param_ServoNickMin 37 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoNickMax 38 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoRollControl 39 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoRollComp 40 // unsigned char Wert : 0-250 |
#define param_ServoRollMin 41 // unsigned char Wert : 0-250 |
#define param_ServoRollMax 42 // unsigned char Wert : 0-250 |
#define param_ServoNickRefresh 43 // unsigned char Speed of the Servo |
#define param_ServoManualControlSpeed 44 // unsigned char |
#define param_CamOrientation 45 // unsigned char |
#define param_Servo3 46 // unsigned char Value or mapping of the Servo Output |
#define param_Servo4 47 // unsigned char Value or mapping of the Servo Output |
#define param_Servo5 48 // unsigned char Value or mapping of the Servo Output |
#define param_LoopGasLimit 49 // unsigned char Wert: 0-250 max. Gas während Looping |
#define param_LoopThreshold 50 // unsigned char Wert: 0-250 Schwelle für Stickausschlag |
#define param_LoopHysterese 51 // unsigned char Wert: 0-250 Hysterese für Stickausschlag |
#define param_AchsKopplung1 52 // unsigned char Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
#define param_AchsKopplung2 53 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_CouplingYawCorrection 54 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_WinkelUmschlagNick 55 // unsigned char Wert: 0-250 180°-Punkt |
#define param_WinkelUmschlagRoll 56 // unsigned char Wert: 0-250 180°-Punkt |
#define param_GyroAccAbgleich 57 // unsigned char 1/k (Koppel_ACC_Wirkung) |
#define param_Driftkomp 58 // unsigned char |
#define param_DynamicStability 59 // unsigned char |
#define param_UserParam5 60 // unsigned char Wert : 0-250 |
#define param_UserParam6 61 // unsigned char Wert : 0-250 |
#define param_UserParam7 62 // unsigned char Wert : 0-250 |
#define param_UserParam8 63 // unsigned char Wert : 0-250 |
#define param_J16Bitmask 64 // unsigned char for the J16 Output |
#define param_J16Timing 65 // unsigned char for the J16 Output |
#define param_J17Bitmask 66 // unsigned char for the J17 Output |
#define param_J17Timing 67 // unsigned char for the J17 Output |
#define param_WARN_J16_Bitmask 68 // unsigned char for the J16 Output |
#define param_WARN_J17_Bitmask 69 // unsigned char for the J17 Output |
#define param_NaviOut1Parameter 70 // unsigned char for the J16 Output |
#define param_NaviGpsModeChannel 71 // unsigned char Parameters for the Naviboard |
#define param_NaviGpsGain 72 // unsigned char |
#define param_NaviGpsP 73 // unsigned char |
#define param_NaviGpsI 74 // unsigned char |
#define param_NaviGpsD 75 // unsigned char |
#define param_NaviGpsPLimit 76 // unsigned char |
#define param_NaviGpsILimit 77 // unsigned char |
#define param_NaviGpsDLimit 78 // unsigned char |
#define param_NaviGpsA 79 // unsigned char |
#define param_NaviGpsMinSat 80 // unsigned char |
#define param_NaviStickThreshold 81 // unsigned char |
#define param_NaviWindCorrection 82 // unsigned char |
#define param_NaviAccCompensation 83 // unsigned char New since 0.86 -> was: SpeedCompensation |
#define param_NaviOperatingRadius 84 // unsigned char bis Rev. 101 |
#define param_NaviAngleLimitation 85 // unsigned char |
#define param_NaviPH_LoginTime 86 // unsigned char |
#define param_ExternalControl 87 // unsigned char for serial Control |
#define param_OrientationAngle 88 // unsigned char Where is the front-direction? |
#define param_CareFreeChannel 89 // unsigned char switch for CareFree |
#define param_MotorSafetySwitch 90 // unsigned char |
#define param_MotorSmooth 91 // unsigned char |
#define param_ComingHomeAltitude 92 // unsigned char |
#define param_FailSafeTime 93 // unsigned char |
#define param_MaxAltitude 94 // unsigned char |
#define param_FailsafeChannel 95 // unsigned char if the value of this channel is > 100, the MK reports "RC-Lost" |
#define param_ServoFilterNick 96 // unsigned char |
#define param_ServoFilterRoll 97 // unsigned char |
#define param_StartLandChannel 98 // unsigned char |
#define param_LandingSpeed 99 // unsigned char |
#define param_CompassOffset 100 // unsigned char |
#define param_AutoLandingVoltage 101 // unsigned char in 0,1V 0 -> disabled |
#define param_ComingHomeVoltage 102 // unsigned char in 0,1V 0 -> disabled |
#define param_BitConfig 103 // unsigned char (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
#define param_ServoCompInvert 104 // unsigned char 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
#define param_ExtraConfig 105 // unsigned char bitcodiert |
#define param_GlobalConfig3 106 // unsigned char bitcodiert |
#define param_Name 107 // char [12] Name vom Parameterset |
#define param_crc 108 // unsigned char |
#define param_AutoPhotoDistance 109 // unsigned char ab Rev. 100 (ersetzt NaviOut1Parameter) // Auto Photo |
#define param_AutoPhotoAtitudes 110 // unsigned char ab Rev. 100 |
#define param_SingleWpSpeed 111 // unsigned char ab Rev. 100 |
#define param_Servo3OnValue 112 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo3OffValue 113 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OnValue 114 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OffValue 115 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_NaviMaxFlyingRange 116 // unsigned char ab Rev. 102 (FC 2.05g) (ersetzt NaviOperatingRadius) // in 10m |
#define param_NaviDescendRange 117 // unsigned char ab Rev. 102 (FC 2.05g) |
#define param_Hoehe_TiltCompensation 118 // unsigned char ab Rev. 103 (FC 2.07f) |
#define param_LandingPulse 119 // unsigned char ab Rev. 104 (FC 2.09d) |
#define param_ServoFS_Pos 120 // unsigned char [5] ServoNickFailsave[0],ServoRollFailsave[1],Servo3Failsave[2],Servo4Failsave[3],Servo5Failsave[4] ab Rev. 105 (FC 2.09i) |
#define param_SingleWpControlChannel 121 // unsigned char ab Rev. 106 (FC 2.11a) |
#define param_MenuKeyChannel 122 // unsigned char ab Rev. 106 (FC 2.11a) |
//------- ^^^ HIER NEUE paramID's einfuegen! ^^^ |
// ab hier sind Sub-Items zu einer paramID definiert |
// - einige paramID's werden nochmals untergliedert in Bit- oder Bytefelder |
// - die Zuordnung der Sub-Items zu paramID's erfolgt in paramset.c |
#define param_SUBITEM 160 // <- ab diesem Wert handelt es sich um ein paramSubID und nicht um eine paramID |
#define param_Kanalbelegung_Nick 170 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
#define param_Kanalbelegung_Roll 171 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
#define param_Kanalbelegung_Gas 172 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
#define param_Kanalbelegung_Gear 173 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
#define param_Kanalbelegung_Poti1 174 // SUBTYPE_BYTE - Kanalbelegung [4] (unsigned char) |
#define param_Kanalbelegung_Poti2 175 // SUBTYPE_BYTE - Kanalbelegung [5] (unsigned char) |
#define param_Kanalbelegung_Poti3 176 // SUBTYPE_BYTE - Kanalbelegung [6] (unsigned char) |
#define param_Kanalbelegung_Poti4 177 // SUBTYPE_BYTE - Kanalbelegung [7] (unsigned char) |
#define param_Kanalbelegung_Poti5 178 // SUBTYPE_BYTE - Kanalbelegung [8] (unsigned char) |
#define param_Kanalbelegung_Poti6 179 // SUBTYPE_BYTE - Kanalbelegung [9] (unsigned char) |
#define param_Kanalbelegung_Poti7 180 // SUBTYPE_BYTE - Kanalbelegung [10] (unsigned char) |
#define param_Kanalbelegung_Poti8 181 // SUBTYPE_BYTE - Kanalbelegung [11] (unsigned char) |
#define param_ServoCompInvert_SERVO_NICK_INV 190 // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
#define param_ServoCompInvert_SERVO_ROLL_INV 191 // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
#define param_ServoCompInvert_SERVO_RELATIVE 192 // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
#define param_ExtraConfig_HeightLimit 200 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
#define param_ExtraConfig_HeightVario 201 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 ist negiert zu param_ExtraConfig_HeightLimit |
#define param_ExtraConfig_VarioBeep 202 // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
#define param_ExtraConfig_SensitiveRc 203 // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
#define param_ExtraConfig_33vReference 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
#define param_ExtraConfig_NoRcOffBeeping 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
#define param_ExtraConfig_GpsAid 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
#define param_ExtraConfig_LearnableCarefree 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
#define param_ExtraConfig_IgnoreMagErrAtStartup 208 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
#define param_BitConfig_LoopOben 210 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
#define param_BitConfig_LoopUnten 211 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
#define param_BitConfig_LoopLinks 212 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
#define param_BitConfig_LoopRechts 213 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
#define param_BitConfig_MotorBlink1 214 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
#define param_BitConfig_MotorOffLed1 215 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
#define param_BitConfig_MotorOffLed2 216 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
#define param_BitConfig_MotorBlink2 217 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
#define param_GlobalConfig3_DphMaxRadius 221 // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 nur bis Rev 101 |
#define param_GlobalConfig3_VarioFailsafe 222 // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
#define param_GlobalConfig3_MotorSwitchMode 223 // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
#define param_GlobalConfig3_UseNcForOut1 225 // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
#define param_GlobalConfig3_SpeakAll 226 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
#define param_GlobalConfig3_ServoNickCompOff 227 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
#define param_GlobalConfig_HoehenRegelung 230 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
#define param_GlobalConfig_HoehenSchalter 231 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
#define param_GlobalConfig_HeadingHold 232 // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
#define param_GlobalConfig_KompassAktiv 233 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
#define param_GlobalConfig_KompassFix 234 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
#define param_GlobalConfig_GpsAktiv 235 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
#define param_GlobalConfig_AchsenkopplungAktiv 236 // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define param_GlobalConfig_DrehratenBegrenzer 237 // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
#define param_ServoNickFailsave 238 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[0] ab FC 2.09j |
#define param_ServoRollFailsave 239 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[1] ab FC 2.09j |
#define param_Servo3Failsave 240 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[2] ab FC 2.09j |
#define param_Servo4Failsave 241 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[3] ab FC 2.09j |
#define param_Servo5Failsave 242 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[4] ab FC 2.09j |
// hier: spezielle Subitems |
#define param_CompassOffset_DisableDeclCalc 243 // wird gemappt auf: param_CompassOffset - der Wert ist dort in Bit 8 und 7 kodiert |
#define param_ComingHomeOrientation 244 // unsigned char 0x08, 0x10, Bits im Feld ServoCompInvert |
//--------------------------------------------------------------------------------------- |
// IMMER am Ende!! |
#define param_DUMMY 254 // DO NOT CHANGE! DUMMY => spacer werte |
#define param_EOF 255 // DO NOT CHANGE! EOF => markiert das Ende von paramID-Listen |
#define param_NOTFOUND 255 // DO NOT CHANGE! NOTFOUND => kein paramID gefunden |
//######################################################################################################################################################## |
//--------------------------- |
// struct: einzelnes Parameter-Item |
//--------------------------- |
typedef struct |
{ |
unsigned char paramID; |
unsigned char size; |
} paramRevItem_t; |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char paramsetInit( unsigned char *pData ); |
unsigned char paramsetSize( void ); |
unsigned char paramExist( unsigned char paramID ); |
unsigned char paramSize( unsigned char paramID ); |
unsigned char paramGet( unsigned char paramID ); |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ); |
unsigned char *paramGet_p( unsigned char paramID ); |
// TEST / DEBUG |
void paramsetDEBUG( void ); |
#endif // _PARAMSET_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mksettings |
---|
Property changes: |
Added: svn:ignore |
+_FC_eeprom_Versions_Archiv |
+ |
+_old_source |
+ |
+mkparameters.c_sic |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/motortest/motortest.c |
---|
0,0 → 1,396 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY motortest.c |
//# |
//# 13.05.2014 OG |
//# - chg: motor_test() - #ifdef USE_I2CMOTORTEST erweitert um unsigned int SerLoop; |
//# wegen Warning: variable set but not used |
//# |
//# 12.02.2014 OG |
//# - chg: motor_test() Verschiebung von var 'buffer' wegen "unused variable 'buffer'" |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "motortest.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../motortest/twimaster.h" |
//#include "menu.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../messages.h" |
uint8_t m; |
uint8_t mmode; // 0=Value 1=Motor |
uint8_t v; |
volatile uint8_t i2c_state; |
volatile uint8_t motor_addr = 0; |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// Senden der Motorwerte per I2C-Bus |
// |
void SendMotorData(uint8_t m,uint8_t v) |
{ |
if (m==0) |
for(m=0;m<MAX_MOTORS;m++) // alle Motoren |
{ |
// Motor[m].SetPoint = MotorTest[m]; |
Motor[m].SetPoint = v; |
Motor[m].SetPointLowerBits = 0; |
// Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution |
// Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
} |
else |
{ |
Motor[m-1].SetPoint = v; |
Motor[m-1].SetPointLowerBits = 0; |
} |
if(I2C_TransferActive) |
I2C_TransferActive = 0; // enable for the next time |
else |
{ |
motor_write = 0; |
I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
} |
} |
//-------------------------------------------------------------- |
// |
void Search_BL (void) |
{ |
uint8_t i = 0; |
unsigned int timer; |
lcd_cls (); |
MotorenEin =0; |
MotorTest[i] = 0; |
lcd_printp (PSTR("Suche BL-Ctrl"), 0); |
// Check connected BL-Ctrls |
BLFlags |= BLFLAG_READ_VERSION; |
motor_read = 0; // read the first I2C-Data |
SendMotorData(0,0); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
timer = SetDelay(1); |
for(i=0; i < MAX_MOTORS; i++) |
{ |
SendMotorData(i,0); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
{ |
lcd_printp_at (0, 1, PSTR("Found BL-Ctrl:"), 0); |
lcd_print_hex_at (18,1,i,0); |
lcd_printp_at (0, 2, PSTR("Version:"), 0); |
lcd_print_hex_at (8,2,Motor[i].Version,0); |
lcd_printp_at (11, 2, PSTR("SetPoi:"), 0); |
lcd_print_hex_at (18,2,Motor[i].SetPoint,0); |
lcd_printp_at (0, 3, PSTR("SetPoiL:"), 0); |
lcd_print_hex_at (8,3,Motor[i].SetPointLowerBits,0); |
lcd_printp_at (11, 3, PSTR("State :"), 0); |
lcd_print_hex_at (18,3,Motor[i].State,0); |
lcd_printp_at (0, 4, PSTR("ReadMod:"), 0); |
lcd_print_hex_at (8,4,Motor[i].ReadMode,0); |
lcd_printp_at (11, 4, PSTR("Currnt:"), 0); |
lcd_print_hex_at (18,4,Motor[i].Current,0); |
lcd_printp_at (0, 5, PSTR("MaxPWM :"), 0); |
lcd_print_hex_at (8,5,Motor[i].MaxPWM,0); |
lcd_printp_at (11, 5, PSTR("Temp :"), 0); |
write_ndigit_number_u (18,5,Motor[i].Temperature,3,1,0); |
} |
} //End For I |
} |
#endif |
//-------------------------------------------------------------- |
// |
void motor (uint8_t m,uint8_t v) |
{ |
memset (buffer, 0, 16); |
if(m == 0) |
{ |
memset (buffer, v, 16); |
} |
else |
{ |
buffer[m-1] = v; |
} |
SendOutData('t', ADDRESS_FC, 1, buffer, 16); |
} |
//-------------------------------------------------------------- |
// |
void motor_test (uint8_t MotorMode) |
{ |
lcd_cls (); |
mmode = 1; // 1=Motor |
m = 1; |
v = 0; |
#ifdef USE_I2CMOTORTEST |
unsigned int SerLoop; |
SerLoop = 10; |
char buffer[7]; |
if (MotorMode == I2C_Mode) |
{ |
Search_BL(); |
do |
{ |
lcd_printp_at (11, 7, PSTR("Ende Check"), 0); |
if (get_key_press (1 << KEY_ESC)) |
{ |
get_key_press(KEY_ALL); |
return; |
} |
} |
while (!get_key_press (1 << KEY_ENTER)); |
} |
#endif |
lcd_cls(); |
lcd_printp (PSTR(" BL-Ctrl Test "), 2); |
lcd_printp_at (2, 2, PSTR("Motor: 1"), 0); |
lcd_printp_at (2, 3, PSTR("Value: 0"), 0); |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x1a \x1b"), 0); |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
uart1_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r"); |
#endif |
if (MotorMode == FC_Mode) |
{ |
if (hardware == NC && current_hardware == NC) |
{ |
SwitchToFC(); |
} |
} |
do |
{ |
// mmode 0=Value 1=Motor |
if ((mmode == 0) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (v < 254)) |
{ |
v++; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 0) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (v > 0)) |
{ |
if (MotorMode == FC_Mode) |
lcd_frect (((v * (14*8) / 255) + 8), (8*5), ((14*8) / 255), 6, 0); |
v--; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (m < 16)) |
{ |
m++; |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (m > 0)) |
{ |
m--; |
if(m > 0) |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) |
lcd_printp_at (9, 2, PSTR("All"), 0); |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
if (v > 0) |
{ |
m = 0; |
v=0; |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
lcd_cls_line (0, 5, 21); |
if(m > 0) write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) lcd_printp_at (9, 2, PSTR("All"), 0); |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
SendMotorData(m,v); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
} |
#endif |
if(mmode == 0) // 0=Value |
{ |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
lcd_printp_at (0, 3, PSTR(" "), 0); |
mmode = 1; // 1=Motor |
} |
else |
{ |
lcd_printp_at (0, 2, PSTR(" "), 0); |
lcd_printp_at (0, 3, PSTR("\x1d"), 0); |
mmode = 0; // 0=Value |
} |
} |
//if (get_key_press (1 << KEY_ENTER))// |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
SendMotorData(m,v); |
timer = SetDelay(1); |
lcd_printp_at (0, 3, PSTR("SetPoint :"), 0); |
write_ndigit_number_u (13,3,Motor[m-1].SetPoint,3,0,0); |
lcd_printp_at (0, 4, PSTR("Current :"), 0); |
lcd_print_hex_at (13,4,Motor[m-1].Current,0); |
write_ndigit_number_u (13,4,Motor[m-1].Current,3,0,0); |
lcd_printp_at (0, 5, PSTR("Temperature:"), 0); |
write_ndigit_number_u (13,5,Motor[m-1].Temperature,3,0,0); |
lcd_printp_at (0, 6, PSTR("Version:"), 0); |
lcd_print_hex_at (8,6,Motor[m-1].Version,0); |
lcd_printp_at (11, 6, PSTR("State :"), 0); |
lcd_print_hex_at (18,6,Motor[m-1].State,0); |
if (Motor[m-1].SetPoint > 0) |
{ |
if (SerLoop == 0) |
{ |
itoa( m-1, buffer, 10); // convert interger into string (decimal format) |
uart1_puts(buffer); // and transmit string to UART |
uart1_puts(";"); |
itoa( Motor[m-1].Version, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPoint, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPointLowerBits, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].State, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].ReadMode, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Current, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].MaxPWM, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Temperature, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts("\r"); |
uart1_puts("\n"); |
SerLoop =200; |
} |
else |
SerLoop--; |
} |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
else |
#endif |
motor (m,v); //if (MotorMode == I2C_Mode)// |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
if (MotorMode == FC_Mode) |
{ |
motor(0,0); // switch all engines off at exit |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/motortest/motortest.h |
---|
0,0 → 1,46 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _MOTORTEST_H |
#define _MOTORTEST_H |
#define I2C_Mode 1 // Motortest Lokal |
#define FC_Mode 2 // Motortest ueber FC |
void motor_test (uint8_t MotorMode); |
void SendMotorData(uint8_t m,uint8_t v); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/motortest/twimaster.c |
---|
0,0 → 1,523 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) Holger Buss, Ingo Busker |
// + Nur f?r den privaten Gebrauch |
// + www.MikroKopter.com |
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt f?r das gesamte Projekt (Hardware, Software, Bin?rfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur f?r den privaten (nicht-kommerziellen) Gebrauch zul?ssig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best?ckung und Verkauf von Platinen oder Baus?tzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver?ffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m?ssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien ver?ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gew?hr auf Fehlerfreiheit, Vollst?ndigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir ?bernehmen keinerlei Haftung f?r direkte oder indirekte Personen- oder Sachsch?den |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zul?ssig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY twimaster.c |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/twi.h> |
#include <util/delay.h> |
#include "../eeprom/eeprom.h" |
#include "../motortest/twimaster.h" |
#include "../timer/timer.h" |
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint8_t I2C_TransferActive = 0; |
volatile uint16_t I2CTimeout = 100; |
uint8_t MissingMotor = 0; |
uint8_t RequiredMotors = 1; |
char MotorenEin = 0; |
volatile uint8_t BLFlags = 0; |
MotorData_t Motor[MAX_MOTORS]; |
// bit mask for witch BL the configuration should be sent |
volatile uint16_t BLConfig_WriteMask = 0; |
// bit mask for witch BL the configuration should be read |
volatile uint16_t BLConfig_ReadMask = 0; |
// buffer for BL Configuration |
BLConfig_t BLConfig; |
#define I2C_WriteByte(byte) {TWDR = byte; TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define I2C_ReceiveByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);} |
#define I2C_ReceiveLastByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define TWI_BASE_ADDRESS 0x52 |
uint8_t RAM_Checksum(uint8_t* pBuffer, uint16_t len) |
{ |
uint8_t crc = 0xAA; |
uint16_t i; |
for(i=0; i<len; i++) |
{ |
crc += pBuffer[i]; |
} |
return crc; |
} |
//-------------------------------------------------------------- |
// Initialize I2C (TWI) |
// |
void I2C_Init(char clear) |
{ |
#ifdef USE_I2CMOTORTEST |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
// SDA is INPUT |
DDRC &= ~(1<<DDC1); |
// SCL is output |
DDRC |= (1<<DDC0); |
// pull up SDA |
//PORTC |= (1<<PORTC0)|(1<<PORTC1); |
// TWI Status Register |
// prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
// set TWI Bit Rate Register |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; |
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
if(clear) for(i=0; i < MAX_MOTORS; i++) |
{ |
Motor[i].Version = 0; |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
Motor[i].State = 0; |
Motor[i].ReadMode = BL_READMODE_STATUS; |
Motor[i].Current = 0; |
Motor[i].MaxPWM = 0; |
Motor[i].Temperature = 0; |
} |
sei(); |
SREG = sreg; |
#endif |
} |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
void I2C_Reset(void) |
{ |
// stop i2c bus |
I2C_Stop(TWI_STATE_MOTOR_TX); |
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
I2C_TransferActive = 0; |
I2C_Init(0); |
I2C_WriteByte(0); |
BLFlags |= BLFLAG_READ_VERSION; |
} |
#endif |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// I2C ISR |
// |
ISR (TWI_vect) |
{ |
static uint8_t missing_motor = 0, motor_read_temperature = 0; |
static uint8_t *pBuff = 0; |
static uint8_t BuffLen = 0; |
switch (twi_state++) |
{ |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
I2C_TransferActive = 1; |
// skip motor if not used in mixer |
// while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
BLConfig_WriteMask = 0; // reset configuration bitmask |
motor_write = 0; // reset motor write counter for next cycle |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(TWI_BASE_ADDRESS + TW_READ + (motor_read<<1) ); // select slave address in rx mode |
} |
else I2C_WriteByte(TWI_BASE_ADDRESS + TW_WRITE + (motor_write<<1) ); // select slave address in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint |
// if old version has been detected |
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
{ |
twi_state = 4; //jump over sending more data |
} |
// the new version has been detected |
else if(!( (Motor[motor_write].SetPointLowerBits && (RequiredMotors < 7)) || BLConfig_WriteMask || BLConfig_ReadMask ) ) |
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
twi_state = 4; //jump over sending more data |
} |
break; |
case 2: // lower bits of setpoint (higher resolution) |
if ((0x0001<<motor_write) & BLConfig_ReadMask) |
{ |
Motor[motor_write].ReadMode = BL_READMODE_CONFIG; // configuration request |
} |
else |
{ |
Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request |
} |
// send read mode and the lower bits of setpoint |
I2C_WriteByte((Motor[motor_write].ReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
// configuration tranmission request? |
if((0x0001<<motor_write) & BLConfig_WriteMask) |
{ // redirect tx pointer to configuration data |
pBuff = (uint8_t*)&BLConfig; // select config for motor |
BuffLen = sizeof(BLConfig_t); |
} |
else |
{ // jump to end of transmission for that motor |
twi_state = 4; |
} |
break; |
case 3: // send configuration |
I2C_WriteByte(*pBuff); |
pBuff++; |
if(--BuffLen > 0) |
twi_state = 3; // if there are some bytes left |
break; |
case 4: // repeat case 0-4 for all motors |
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received |
{ |
if(!missing_motor) |
missing_motor = motor_write + 1; |
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) |
Motor[motor_write].State++; // increment error counter and handle overflow |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive Data |
case 5: // TWI_STATE_MOTOR_RX |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted but no ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
if(++motor_read >= MAX_MOTORS) |
{ // all motors read |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
} |
else |
{ // motor successfully addressed |
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
{ |
// new BL found |
switch(Motor[motor_read].ReadMode) |
{ |
case BL_READMODE_CONFIG: |
pBuff = (uint8_t*)&BLConfig; |
BuffLen = sizeof(BLConfig_t); |
break; |
case BL_READMODE_STATUS: |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
else BuffLen = 1;// read Current only |
break; |
} |
} |
else // old BL version |
{ |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if((BLFlags & BLFLAG_READ_VERSION) || (motor_read == motor_read_temperature)) BuffLen = 2; // Current & MaxPwm |
else BuffLen = 1; // read Current only |
} |
if(BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else |
{ |
I2C_ReceiveByte(); // read next byte |
} |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 6: // receive bytes |
*pBuff = TWDR; |
pBuff++; |
BuffLen--; |
if(BuffLen>1) |
{ |
I2C_ReceiveByte(); // read next byte |
} |
else if (BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else // nothing left |
{ |
if(BLFlags & BLFLAG_READ_VERSION) |
{ |
// if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN) && (Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
if((Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
else Motor[motor_read].Version = 0; |
} |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_TransferActive = 0; |
return; |
} |
twi_state = 6; // if there are some bytes left |
break; |
case 21: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
case 22: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
I2CTimeout = 10; |
// repeat case 18...22 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ |
dac_channel = 0; // reset dac channel counter |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
break; |
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
I2C_TransferActive = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
uint8_t I2C_WriteBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
// return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor) |
{ |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
} |
// check BL configuration to send |
if(BLConfig.Revision != BLCONFIG_REVISION) |
return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) |
return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
if(!motor) // 0 means all |
{ |
BLConfig_WriteMask = 0xFF; // all motors at once with the same configuration |
} |
else //only one specific motor |
{ |
BLConfig_WriteMask = 0x0001<<(motor-1); |
} |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_WriteMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_write = 0; |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
while(BLConfig_WriteMask && !CheckDelay(timer)); // repeat until the BL config has been sent |
if(BLConfig_WriteMask) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); |
return(BLCONFIG_SUCCESS); |
} |
//-------------------------------------------------------------- |
uint8_t I2C_ReadBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor == 0) |
return (BLCONFIG_ERR_READ_NOT_POSSIBLE); |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
BLConfig_ReadMask = 0x0001<<(motor-1); |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_ReadMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_read = 0; |
BLConfig.Revision = 0; // bad revision |
BLConfig.crc = 0; // bad checksum |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
}while(BLConfig_ReadMask && !CheckDelay(timer)); // repeat until the BL config has been received from all motors |
// validate result |
if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
return(BLCONFIG_SUCCESS); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/motortest/twimaster.h |
---|
0,0 → 1,80 |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
+ |
+#include <inttypes.h> |
+#include "../mk-data-structs.h" |
+ |
+#define TWI_STATE_MOTOR_TX 0 |
+#define TWI_STATE_MOTOR_RX 5 |
+#define TWI_STATE_GYRO_OFFSET_TX 18 |
+ |
+extern volatile uint8_t twi_state; |
+extern volatile uint8_t motor_write; |
+extern volatile uint8_t motor_read; |
+extern volatile uint8_t I2C_TransferActive; |
+ |
+extern uint8_t MissingMotor; |
+ |
+#define MAX_MOTORS 12 |
+#define MOTOR_STATE_PRESENT_MASK 0x80 |
+#define MOTOR_STATE_ERROR_MASK 0x7F |
+#define MOTOR_STATE_NEW_PROTOCOL_MASK 0x01 |
+#define BLFLAG_TX_COMPLETE 0x01 |
+#define BLFLAG_READ_VERSION 0x02 |
+ |
+extern volatile uint8_t BLFlags; |
+extern char MotorenEin; |
+unsigned char MotorTest[16]; |
+#define BL_READMODE_STATUS 0 |
+#define BL_READMODE_CONFIG 16 |
+ |
+ |
+ |
+extern MotorData_t Motor[MAX_MOTORS]; |
+ |
+#define BLCONFIG_REVISION 2 |
+ |
+#define MASK_SET_PWM_SCALING 0x01 |
+#define MASK_SET_CURRENT_LIMIT 0x02 |
+#define MASK_SET_TEMP_LIMIT 0x04 |
+#define MASK_SET_CURRENT_SCALING 0x08 |
+#define MASK_SET_BITCONFIG 0x10 |
+#define MASK_RESET_CAPCOUNTER 0x20 |
+#define MASK_SET_DEFAULT_PARAMS 0x40 |
+#define MASK_SET_SAVE_EEPROM 0x80 |
+ |
+#define BITCONF_REVERSE_ROTATION 0x01 |
+#define BITCONF_RES1 0x02 |
+#define BITCONF_RES2 0x04 |
+#define BITCONF_RES3 0x08 |
+#define BITCONF_RES4 0x10 |
+#define BITCONF_RES5 0x20 |
+#define BITCONF_RES6 0x40 |
+#define BITCONF_RES7 0x80 |
+ |
+ |
+ |
+extern BLConfig_t BLConfig; |
+ |
+extern volatile uint16_t I2CTimeout; |
+ |
+void I2C_Init(char); // Initialize I2C |
+#define I2C_Start(start_state) {twi_state = start_state; BLFlags &= ~BLFLAG_TX_COMPLETE; TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);} |
+#define I2C_Stop(start_state) {twi_state = start_state; TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);} |
+void I2C_Reset(void); // Reset I2C |
+ |
+#define BLCONFIG_SUCCESS 0 |
+#define BLCONFIG_ERR_MOTOR_RUNNING 1 |
+#define BLCONFIG_ERR_MOTOR_NOT_EXIST 2 |
+#define BLCONFIG_ERR_HW_NOT_COMPATIBLE 3 |
+#define BLCONFIG_ERR_SW_NOT_COMPATIBLE 4 |
+#define BLCONFIG_ERR_CHECKSUM 5 |
+#define BLCONFIG_ERR_READ_NOT_POSSIBLE 6 |
+ |
+uint8_t I2C_WriteBLConfig(uint8_t motor); |
+uint8_t I2C_ReadBLConfig(uint8_t motor); |
+ |
+#endif |
+ |
+ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osd.c |
---|
0,0 → 1,4919 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
* Oliver Gemesi "olli42" for additions und changes in OSDScreen * |
*****************************************************************************/ |
//-------------------- |
// Debug |
//-------------------- |
//#define DEBUG_OSD_TIME // mit dem NC-Simulator koennen keine brauchbaren Zeit/Datum Daten ermittelt werden |
// am Anfang von osd() wird mit dieser Einstellung eine gueltige Fake-Zeit/Date gesetzt |
//#define DEBUG_OSD_STAT_MOTORRUN // erzwingt Statistik Aufzeichnung auch wenn die Motoren nicht laufen |
//############################################################################ |
//# HISTORY osd.c |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 21.06.2014 OG |
//# - chg: osd() - bei auftreten von mk_timeout wieder OSD_MK_Connect(MK_CONNECT) |
//# aktiviert -> dadurch wird u.a. wieder auf die NC umgeschaltet falls |
//# etwas anderes (z.B. ein anderes PKT) auf die FC umgeschaltet hat |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 02.06.2014 OG |
//# - fix: Beep_Waypoint() Target-Reach Beep auch bei Single-WP's (WP-Listen |
//# die nur aus einem WP bestehen) |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# - add: include "../osd/osdold_screens.h" |
//# - add: include "../osd/osdold_messages.h" |
//# - chg: osd() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# aufrufende Menue erledigt wird |
//# - add: OSD_Screen_MKDisplay() um Beep_Waypoint() ergaenzt |
//# - chg: OSD_Screen_Waypoints() umgestellt auf Beep_Waypoint() |
//# - add: Beep_Waypoint() |
//# |
//# 31.05.2014 OG |
//# - chg: OSD_Screen_MKDisplay() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 26.05.2014 OG |
//# - add: etliche Aenderungen/Erweiterung am Screen "Waypoints" (auch neues Layout) |
//# |
//# 24.05.2014 OG |
//# - fix: OSD_Screen_Waypoints(), OSD_Screen_MKDisplay() - Anzeige des akt. WP-Index |
//# - chg: OSD_Screen_3DLage() - optisches Facelift |
//# - fix: OSD_Screen_3DLage() - es wurden ggf. Kreise gezeichnet die ungueltig |
//# und in undefinierten Speicher gingen - wurde korrigiert |
//# - chg: OSD0,1,2 angepasst an OSD_Element_CompassDirection() mit xoffs,yoffs |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 20.05.2014 OG |
//# - chg: OSD_Element_Flag_Label() - Anpassung zur Rahmenbestimmung eines Flags |
//# |
//# 19.05.2014 OG |
//# - fix: osd() - Tastensteuerung bei MK-Empfangsverlust |
//# - chg: osd() - wenn MK-Setting nicht ermittelt werden konnte dann exit |
//# - add: OSD_Screen_MKDisplay() - Anzeige von akt. Waypoint und Anzahl der Waypoints |
//# - chg: OSD_Popup_MKSetting() - gibt true/false fuer Ok zurueck und Beep bei Fehler |
//# - chg: OSD_Popup_MKSetting() - timeout von 15 auf 9 reduziert |
//# - chg: OSD_Popup_MKSetting(), OSD_Popup_MKError() - Layout, Multi-Sprachenunterstuetzung |
//# |
//# 02.05.2014 OG |
//# - del: Popup_Draw() (jetzt in lcd.c) |
//# |
//# 02.05.2014 OG |
//# - chg: kleine Aenderung an Codereihenfolge von MkError_Save() wegen Compiler-Warning |
//# |
//# 28.04.2014 OG |
//# Anmerkung OSD-MK-Display: wenn der MK-Display Modus eingeschaltet wird, dann werden die Display Daten |
//# angefordert und die BL-Daten etwas reduziert. Hin und wieder kann es passieren das ein Tastendruck im |
//# OSD-MK-Display-Modus nicht durch kommt (aber nicht allzu stoerend) - kann evtl. verbessert werden wenn |
//# auch die OSD-Daten reduziert werden (waere jedoch einiges mehr Aufwand). |
//# - Unterstuetzung von MK-Display (nur NC-Modus) im OSD |
//# (einschalten mit langem Druck dritte Taste von links, abschalten ebenso) |
//# - Info-Anzeige bzgl. langem Tastendruck fuer MK-Display erweitert und deutsche Uebersetzung von "long press" |
//# - Umstrukturierung von osd.c fuer MK-Display |
//# - Aenderungen/Erweiterungen an verschiedenen weiteren Funktionen wie OSD_MK_GetData() fuer MK-Display |
//# - add: OSD_MK_ShowTimeout() - neues Design/Layout |
//# - del: OSD_Timeout() (ersetzt durch OSD_MK_ShowTimeout()) |
//# - chg: verschiedene Aenderungen an Timings |
//# - add: neuer Timer fuer MK-Display: timer_get_displaydata |
//# - fix: es gab bei einem Datenpaket-Timer einen Konflikt mit 'timer' - ein neuer Timer wurde aktiviert (timer_mk_timeout) |
//# |
//# 22.04.2014 OG |
//# - add: OSD_Element_HomeCircleX() - soll das alte OSD_Element_HomeCircle() ersetzen, wenn |
//# die alten OSD-Screens endgueltig rausfliegen |
//# - chg: Aenderung Anzeige OSD_Screen_UserGPS() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_MKStatus() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_Electric() (abgerundete Ecken und Code Einsparung) |
//# |
//# 01.03.2014 OG |
//# - chg: OSD_Popup_MKError() auf lcdx_printp_center() umgestellt |
//# |
//# 16.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - chg: OSD_MK_GetData() prueft mit MKVersion_Cmp() auf NC-Version v0.30g |
//# ob GPS-Zeit-Datenpakete ('t') von der NC angefordert werden koennen |
//# |
//# 12.02.2014 OG |
//# - chg: define MKVERSIONnnn rund um OSD_MK_UTCTime() entfernt |
//# -> TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//# -> keine Unterstuetzung mehr durch zu alte NC-Versionen |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 10.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MKVersion_Setting_print() |
//# |
//# 08.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf neue MKVersion Struktur |
//# |
//# 04.02.2014 OG |
//# - fix: OSD_Element_BattLevel2() Aufruf von writex_ndigit_number_u_100th() |
//# |
//# 03.02.2014 OG |
//# - chg: OSD_Element_BattLevel2() unterstuetzt Config.OSD_ShowCellU zur Anzeige |
//# der MK-Spannung als Einzelzelle (kalk. auf Basis der ermittelten Zellenzahl) |
//# |
//# 29.01.2014 OG |
//# - add: neue MK-Errorcodes 32,33,34 |
//# - add: #include "mk/mkbase.h" |
//# |
//# 24.01.2014 OG |
//# - chg: OSD_Popup_MKSetting(): MK-Settings wird beim Start vom OSD wieder angezeigt |
//# auch wenn die falsche FC-Revision vorhanden ist mit folgender Regel: |
//# a) wenn falsche FC-Settings-Revision dann nur die Nummer |
//# b) wenn richtige FC-Revision dann mit Nummer und Namen |
//# |
//# 06.01.2014 Cebra |
//# - add: MK-Settingsname wird bei falscher FC Version nicht angezeigt |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screens koennen vom Benutzer an-/ausgeschaltet werden (osd(), ScreenCtrl_Push()) |
//# - chg: OSD-Screen Namen jetzt in messages.c |
//# - fix: MK-Error Check prueft ob die Fehlernummer gueltig ist |
//# - del: alten Screen Electric (der ohne Nachkomma bei den Stroemen) |
//# |
//# 03.07.2013 OG |
//# - chg: OSD_Popup_MKSetting() - zentrierte Anzeige des Setting-Namens |
//# - chg: OSD_Popup_MKSetting() - timing |
//# - chg: osd() - Screen-Umschaltung beschleunigt |
//# - chg: timinigs bei OSD_MK_Connect(), osd() (werden Verbindungsfehler reduziert?) |
//# |
//# 02.07.2013 OG |
//# - chg: CheckMKLipo() - MK-Unterspannungs-Beep nur wenn Motoren laufen |
//# - del: unbenutzte Variablen |
//# |
//# 30.06.2013 OG |
//# - add: grafische Akku-Anzeige auch fuer Screen Navigation und ElectricN |
//# |
//# 30.06.2013 OG |
//# - add: Screen General: das Batt-Symbol zeigt grafisch den Fuellstand |
//# des MK-Lipos |
//# - add: Mittelwert fuer Gesamtstrom |
//# - add: calc_avg() - Mittelwertberechnung |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - add: USE_OSD_PKTHOOK in osd() - aktuell noch nicht klar ob PKT_Hook() |
//# die Kommunikation stoert |
//# |
//# 27.06.2013 OG |
//# - chg: numerische Winkelanzeige im Screen "Navigation" von -180 bis 180 Grad |
//# 0 Grad = vorderer Ausleger zeigt zur Startposition |
//# - fix: Homesicht des MK's (Winkelanzeige, Grafik) |
//# ab nun: keine Benutzereinstellung mehr dafuer |
//# es gilt: 0 Grad bzw. Strich nach unten -> der vordere Ausleger des MK |
//# zeigt auf seine Startposition |
//# |
//# 19.06.2013 OG |
//# - fix: redundante PKT-Error 40 eliminiert durch merken via old_PKTErrorcode |
//# -> hilft wenn der MK ausgeschaltet wird waehrend man in der PKT-OSD-Anzeige ist |
//# - fix: last_FlyTime in osd() |
//# |
//# 18.06.2013 OG |
//# - chg: OSD_Timeout() erweitert um Errorlogging (Code 40 = "PKT RX lost") |
//# - add: Fehlerliste "PKT RX lost" (Code: 40) |
//# |
//# 15.06.2013 OG |
//# - chg: Anzeige OSD_Popup_MKSetting() kann durch Benutzer via Setup an/ausgeschaltet werden |
//# |
//# 15.06.2013 OG |
//# - chg: OSD_Popup_MKSetting() optimiert, Layout, fix |
//# - chg: PKT_CtrlHook erstmal wieder disabled in osd() (evtl. Probleme mit Timings?) |
//# |
//# 13.06.2013 cebra |
//# - add: Settings Popup beim Start des OSD-Screens |
//# |
//# 11.06.2013 OG |
//# - add: Mittelwertberechnung BL-Strom [MK_GetData()] |
//# - chg: Config.OSD_ScreenMode wird vor PKT_CtrlHook() gesetzt [osd()] |
//# |
//# 27.05.2013 OG |
//# - fix: OSD_MK_UTCTime() (neue "t" Version) Problem wegen cast fuer Sekunden |
//# |
//# 19.05.2013 OG |
//# - chg: osd() erweitert um PKT_CtrlHook() um u.a. PKT-Updates zu ermoeglichen |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: OSD_Screen_ElectricN() - zeigt BL-Stroeme mit Nachkomma an |
//# - del: define SHOW_OSD_BLCURRENT_10TH (ersetzt durch neue Screen-Variante) |
//# - chg: osd() umgestellt auf OSD Screen Controler; Anpassungen dafuer an allen Screens |
//# - add: Funktionen fuer Screen Controler (Screen_*()) |
//# |
//# 15.05.2013 OG |
//# - chg: OSD_Screen_Electric() zeigt BL-Strom mit Nachkomma an |
//# (einstellbar via define SHOW_OSD_BLCURRENT_10TH bzgl. neue/alte Anzeige) |
//# - add: define DEBUG_OSD_STAT_MOTORRUN erzwingt Statistik Aufzeichnung auch wenn |
//# die Motoren nicht laufen |
//# - fix: osd() Statistik-Ende Zeit wird nach Landung gesetzt |
//# - chg: diverse Aufraeumarbeiten |
//# - chg: OSD_Screen_Statistics() umgetsellt auf calc_BLmax(() |
//# - add: calc_BLmax() ermittelt max. BL Current & Temp |
//# - del: draw_symbol_degree(), draw_symbol_rc() - wurden ersetzt durch |
//# Zeichen im font 8x6 (SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY) |
//# |
//# 14.05.2013 OG |
//# - add: OSD_MK_Connect() vereinheitlicht MK-Verbindungsinitialisierung |
//# und MK-Abo-Renew |
//# - chg: OSD_MK_UTCTime() atomisiert via ATOMIC_BLOCK() |
//# - chg: OSD_Screen_Debug_RX() verschiedene Optimierungen und Beschreibungen |
//# |
//# 14.05.2013 OG |
//# - chg: OSD_MK_UTCTime() wird mittels define jetzt mit neuem oder mit altem |
//# Algorithmus einkompiliert. |
//# ALT bei: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
//# NEU bei: alles andere (also neuer): die neue Zeit/Datumsermittlung |
//# |
//# 13.05.2013 OG |
//# - fix: an allen Stellen mit naviData->CompassHeading ein Modulo 360 ergaenzt |
//# um Winkelangaben der FC geradezuziehen falls der Kopter beim Mixersetup |
//# 'verdreht' wurde (Problem von helitron im Forum - Winkelanzeige > 360 Grad) |
//# Ist ggf. ab Firmware NC 0.30h nicht mehr notwendig da Holger das |
//# korrigieren wollte (muesste noch geprueft werden) |
//# |
//# 11.05.2013 OG |
//# - add: OSD_MK_UTCTime_NEW() - ab NC 0.30g (Alpha, noch nicht im Einsatz) |
//# |
//# 05.05.2013 OG |
//# - chg: OSD_Popup_Info() & OSD_Popup_MKError() wurden vereinheitlicht |
//# mittels Popup_Draw() |
//# - chg: Anzeigezeiten der Popup's etwas verkuerzt |
//# |
//# 03.05.2013 OG |
//# - fix: OSD_MK_GetData() - griff beim lesen der BL's auf naviData fuer |
//# die Statistik - relevante naviData-Werte werden nun vor dem |
//# wechseln auf BL-Data gemerkt |
//# - chg: Erfassung der Statistik Start/Ende-Zeit verbessert |
//# - chg: bei Aufrufen von writex_datetime_time()/writex_datetime_time() den |
//# Parameter 'timeoffset' entfernt |
//# - add: define DEBUG_OSD_TIME um eine Fake-Zeit/Datum bei Gebrauch des |
//# NC-Simulators zu setzen |
//# |
//# 29.04.2013 OG |
//# - chg: Plausibilitaetscheck in OSD_MK_UTCTime() bzgl. des Datums |
//# |
//# 28.04.2013 OG |
//# - chg: die alten OSD-Screens angepasst auf define USE_FONT_BIG |
//# |
//# 24.04.2013 OG |
//# - chg: directions_p[] geaendert auf Kompatibilitaet mit Atmel Studio 6 |
//# |
//# 21.04.2013 OG |
//# - chg: define AltimeterAdjust nach osd.h verschoben |
//# |
//# 14.04.2013 OG |
//# - add: weitere Statistik-Aufzeichnungen |
//# - fix: MK_GetData() - Statistik-Aufzeichnung nur fuer wirklich vorhandene BL's |
//# |
//# 04.04.2013 OG |
//# - fix: 3 warnings |
//# |
//# 04.04.2013 OG |
//# - chg: MK-Errortext-Anzeige umgestellt auf naviData->Errorcode |
//# weniger Datenkomminkation mit dem MK und schnelleres Ansprechverhalten |
//# |
//# 03.04.2013 OG |
//# - chg: defines OSD_DEMO, OSD_SCREEN_DEBUG, OSD_SCREEN_OLD |
//# umbenannt nach: USE_OSD_DEMO, USE_OSD_SCREEN_DEBUG, USE_OSD_SCREEN_OLD |
//# und verschoben nach: main.h |
//# |
//# 28.03.2013 OG |
//# - fix: MKErr_Log_Init() - es fehlte Multiplikation mit MAX_MKERR_LOG |
//# - chg: Code Formatierungen |
//# |
//# 28.03.2013 CB |
//# - add: replace OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//# |
//# 28.03.2013 OG |
//# - fix: abo_timer wieder aktiviert fuer 'o' und 'k' |
//# |
//# 28.03.2013 OG |
//# - chg: osd_statistic_t - erweitert um diverse Werte und Timestamps (werden apeter implementiert) |
//# - add: mkerror_t MKErr_Log[MAX_MKERR_LOG] - Vorbereitung um MK-Errors zu loggen |
//# |
//# 27.03.2013 OG |
//# - add: Datum / Zeit vom MK lesen |
//# - chg: Kommunikation mit dem MK optimiert |
//# - chg: auf neue messages.c angepasst |
//# - struct von den Statistiken auf PKTdatetime PKTdatetime_t |
//# - verschiedene andere Aenderungen |
//# |
//# 22.03.2013 OG |
//# erhebliche Aenderungen / Ergaenzungen - hier nur das Wichtigste |
//# - neue Funktion "User GPS" (UGPS) - mit langem Druck auf Taste KEY_ENTER werden die aktuellen GPS-Daten abgespeichert |
//# * gespeichert wird nur wenn wenn der MK GPS-Ok meldet |
//# * wenn Ok erfolgt ein bestaetigungs Beep; wenn nicht Ok erfolgt ein Error-Beep |
//# - neuer Screen "User GPS" - Anzeige der letzten der 3 UGPS-Koordinaten (intern wird mehr gespeichert) |
//# - Anzeige von MK-Error Messages als Popup mit einstellbarer Anzeigezeit (kann mit Taste abgebrochen werden) |
//# * kann getestet werden z.B. durch Ausschalten der Funke (= "RC Signal lost") |
//# * siehe: http://www.mikrokopter.de/ucwiki/ErrorCodes |
//# - OSD-Info ist nun ein Popup - autom. ausblenden nach einstellbarer Zeit; im Hintergrund werden weiterhin MK-Daten empfangen und ausgewertet |
//# - neuer Debug-Screen "Debug RXPackages" - zeigt die Anzahl empfangener Datenpakete der verschiedenen Bereiche (um z.B. Timings einzustellen) |
//# - erweiterte Kommunikation mit dem MK fuer BL-Daten und Error-Messages |
//# - Timings der MK-Datenkommunikation weitreichend einstellbar durch defines |
//# - Datenstrukturen fuer OSD-Statistiken und BL-Statistiken |
//# - neue timer: timer_get_erdata, timer_get_bldata, timer_osd_refresh, timer_pkt_uptime |
//# - mit dem define OSD_SCREEN_OLD koennen ggf. die alten OSD-Screens ausgeblendet werden (spart ein paar KByte's) |
//# - verschiedene Optimierungen |
//# |
//# 12.03.2013 OG |
//# - add: Get_BL_Data() - BL-Ctrl via NC auslesen (BETA) (siehe dort auch Kommentare im func-Header) |
//# - add: neuer Screen "Electric" |
//# - chg: Layout von Screen "Navigation" - Pixelverschiebungen |
//# - chg: Layout von Screen "MK-Status" - Pixelverschiebungen und Anzeige 'Strom' ersetzt durch 'entn. Kapazitaet' |
//# - add: in osd() LiPO-Cell Erkennung hinzugefuegt (fuer Screen "Electric") |
//# |
//# 10.03.2013 OG |
//# - fix: doppelte Degree-Anzeige in OSD_Element_CompassDegree() |
//# - add: neuer Screen "MK-Status" |
//# - add: 7 neue OSD-Flags |
//# - chg: Screen-Refresh Zeit via timer2 (einstellbar durch define TIME_OSD_REFRESH) |
//# - chg: mit define OSD_DEBUG_SCREEN kann ein zusaetzlicher Screen verwendet werden zum testen/entwickeln |
//# - del: entfernt CFG2_HEIGHT_LIMIT in OSD_Element_AltitudeControl() (bis bessere Loesung gefunden ist) |
//# |
//# 08.03.2013 OG |
//# - del: OSD_Screen_Element() und cleanup in osd.h |
//# - add: OSD_Element_UpDown() (steigend/sinken via Pfeilen) |
//# - chg: OSD_Element_UpDown() in Screen "General" und "Navigation" hinzugefuegt (rechts neben der Hoehenanzeige) |
//# - chg: Screen "General" die Sat-Warnung wurde auf OSD_Element_Flag(OSD_FLAG_S0) geaendert |
//# - chg: Anzeige von Flag 'nicht genug GPS-Sateliten' (OSD_FLAG_S0) auf "S!" geändert |
//# |
//# 07.03.2013 OG |
//# - Hinweis bzgl. LowBatt-Anzeige in den Screens "General" und "Navigation": |
//# Es gibt zwei unabhängige LowBatt-Warnungen. |
//# 1. die PKT LowBatt-Warnung: sie arbeitet mit der im PKT hinterlegten |
//# LowBatt Spannung und stellt den Spannungswert inkl. der Einheit "V" |
//# invers dar wenn der Warnwert erreicht wurde (inkl. lautem PKT-Peepen) |
//# 2. die MK-LowBatt Warnung: hierbeit wird das Flag "BA" angezeigt wenn |
//# der MK eine LowBatt Warnung sendet |
//# Dadurch hat man nun zwei verschiedene LowBatt Warnungen die man auf Wunsch |
//# verschieden einstellen kann. Bei mir ist die PKT-LowBatt etwas niedriger |
//# eingestellt als die MK-Warnung und bedeutet "es ist aller hoechste Zeit zu landen". |
//# Die Spannung der MK-LowBat ist ein wenig hoeher eingestellt und |
//# zeigt mir "es ist bald Zeit zu landen". |
//# - del: Kommunikation zu FC - siehe Kommentare in osd() |
//# - chg: Kommunikation zu NC - siehe Kommentare in osd() |
//# - add: neuer Screen "Navigation" |
//# - chg: Layout Screen "Statistics" - Einheiten um zwei Pixel nach rechts verschoben |
//# - chg: Layout von Screen "General" modifiziert (u.a. xoffs,yoffs Pixelverschiebungen) |
//# - add: OSD_FLAG_BA in Screen "General" |
//# - add: die OSD_Element_xyz() Funktionen in osd.h aufgenommen |
//# - chg: an verschiedenen Stellen die Char-Drawmode defines MNORMAL, MINVERS, usw. eingebaut |
//# - del: Kompatibilitaetscode fuer "3D-Lage" ueber Hauptmenue entfernt |
//# - chg: die Funktionen OSD_Element_Switch() und OSD_Element_SwitchLabel() heissen |
//# nun OSD_Element_Flag() und OSD_Element_Flag_Label() |
//# - chg: die defines OSD_SWITCH_xy heissen jetzt OSD_FLAG_xy |
//# - fix: letzte GPS-Koordinaten werden jetzt permanent Config.LastLatitude, Config.LastLongitude gespeichert |
//# |
//# 03.03.2013 OG |
//# - add: delay in Mainloop von osd() um springende Werte abzudaempfen (TEST) |
//# - add: Startverzoegerung der Screens bis NaviData sich etwas stabilisiert hat (TEST) |
//# - add: OSD Startup Message "connecting MK..." |
//# - add: 'Emergency Landing' (EL) Anzeige in Screen "General" |
//# - del: OUT1/OUT2 Anzeige in Screen "General" |
//# - add: RC-Quality in Screen "General" |
//# - add: func: draw_symbol_rc() (alternative RC-Quality Symbol) |
//# - fix: Hoehenanzeige fuer Screens "OSD0" und "OSD1" |
//# - fix: OSD_Element_SwitchLabel() angepasst fuer x=0 und y=0 |
//# - add: OSD_Element_Switch/Label() erweitert um OSD_SWITCH_FS |
//# - fix: Screen-Redraw nach OSD_MK_TIMEOUT() und anderen Fehlermeldungen |
//# - chg: messages.c: STATS_ITEM_0 bis STATS_ITEM_6 angepasst (1 char kuerzer) |
//# - chg: Layout von OSD_Info() - mehr background-clear und etwas kleiner |
//# |
//# 02.03.2013 OG |
//# - chg: keine internal func in Screen's wegen warnings bei anderen |
//# - del: Screen "OSD3" |
//# - fix: Hoehenanzeige in Screen "General" (Minuszeichen) |
//# - add: MK LowBat Warning in Screen "General" |
//# - add: neues Degree Symbol (als func) in Screen General (kleiner als das Char 0x1E) |
//# - add: weitere Flags in OSD_Element_Flag() |
//# |
//# 01.03.2013 OG |
//# - Reskrukturierung Code (+ neuer OSD-Screens und einiges mehr) |
//############################################################################ |
//############################################################################ |
//# HINWEISE: |
//# |
//# 1. define: USE_OSD_DEMO (main.h) |
//# mit define OSD_DEMO wird ein Demo-Modus bei den neuen Screens einge- |
//# schaltet - damit werden u.a. alle Flag's angezeigt fuer Scree-Fotos |
//# |
//# 2. define: USE_OSD_SCREEN_DEBUG (main.h) |
//# mit diesem define wird ein zusaetzlicher Screen "Debug" einkompiliert |
//# fuer Test / Experimente / Debug von OSD-Elementen |
//# |
//# 3. define: USE_OSD_SCREEN_OLD (main.h) |
//# ein-/ ausschalten der alten OSD-Screens OSD0, OSD1, OSD2 (spart ca. 3 KByte) |
//# |
//# 4. Timings der MK-Datenkommunikation |
//# die Timings sind weitreichend enstellbar ueber die define im Abschnitt 'Timings' |
//# der Debug-Screen 'Debug RXPackages' informiert ueber die Anzahl eingegangener |
//# Datenpakete der verschiedenen Bereiche (OSD, BL-Daten, Error-Message) |
//# |
//# 5. Informationen zum Display |
//# DOG: 128 x 64 Pixel with 6x8 Font => 21 x 8 |
//# |
//# 6. MK-Kommunikationsprotokoll Referenz |
//# http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=%28%28----%28-*%29%28\r%29%3F\n%29%28.*%29CategoryCoding%29#en.2BAC8-SerialCommands.Flight-Ctrl |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
//-------------------- |
// Funktionen ein-/ausbinden |
//-------------------- |
// -> siehe main.h |
//-------------------- |
// Timings |
//-------------------- |
#define TIME_OSD_REFRESH 45 // Screen Refresh (Steuerung via 'timer_osd_refresh') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_POPUP_INFO 400 // 4 Sekunden Popup-Info zeigen (kann mit Taste abgebrochen werden) |
#define TIME_POPUP_MKERROR 700 // 7 Sekunden Popup-MK-Error zeigen (kann mit Taste abgebrochen werden) |
#define TXINTERVAL_OSDDATA 10 // Intervall mit der der MK OSD-Daten senden soll (n*10 = ms) |
#define TIME_GET_BLDATA 35 // Zeitintervall in der BL-Daten vom MK geholt werden (Steuerung via 'timer_get_bldata') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_READ_BLDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
#define TXINTERVAL_BLDATA 7 // Intervall mit der der MK BL-Daten senden soll (n*10 = ms) |
#define TIME_GET_TIDATA 12000 // alles 120 Sekunden Zeit/Datum vom MK lesen (die Zwischenzeit wird von einem PKT-Timer uebernommen) |
#define TIME_GET_DISPLAYDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
// weiteres |
//-------------------- |
#define OSD_POPUP_NONE 0 |
#define OSD_POPUP_INFO 1 |
#define OSD_POPUP_MKERROR 2 |
#define MK_CONNECT 1 |
#define MK_ABORENEW 2 |
// global definitions and global vars |
NaviData_t *naviData; |
uint16_t heading_home; |
uint8_t drawmode; |
uint8_t OSDScreenRefresh; |
// flags from last round to check for changes |
uint8_t old_AngleNick = 0; |
uint8_t old_AngleRoll = 0; |
uint16_t old_hh = 0; |
// aktuell nicht benoetigt - siehe Kommentar in osd.c |
//mk_param_struct_t *mk_param_struct; |
//uint8_t Flags_ExtraConfig; |
//uint8_t Flags_GlobalConfig; |
//uint8_t Flags_GlobalConfig3; |
// cache old vars for blinking attribute, checkup is faster than full |
// attribute write each time |
volatile uint8_t OSD_active = 0; |
uint8_t Vario_Beep_Up = 0; |
uint8_t Vario_Beep_Down = 0; |
uint8_t Vario_Beep_Up_Interval = 9; |
uint8_t Vario_Beep_Down_Interval = 6; |
uint8_t Vario_Threshold = 0; |
uint8_t Vario_Threshold_Value = 7; |
uint8_t OldWP = 0; |
uint8_t NextWP = 0; |
uint8_t WP_old = 0; // Screen: "Waypoints" |
uint8_t WP_last = false; // Screen: "Waypoints" |
#define MAX_CELL_VOLTAGE 43 // max cell voltage for LiPO |
#define MIN_CELL_VOLTAGE 32 // min cell voltage for LiPO |
// Flags |
volatile uint8_t error = 0; |
uint8_t cells = 0; |
uint8_t BattLowVoltageWarning = 0; |
uint8_t CellIsChecked = 0; |
uint8_t AkkuWarnThreshold = 0; |
uint16_t duration = 0; |
//----------------------------------------------------------- |
// Buffer |
//----------------------------------------------------------- |
BLData_t blData[OSD_MAX_MOTORS]; // speichert gelesene BL-Datenpakete |
pkt_gpspos_t GPS_Current; // aktuelle GPS-Position |
u8 old_PKTErrorcode; // speichert den letzen Errorcode vom PKT damit dieser nicht wiederholt gespeichert wird |
u8 old_MKErrorcode; // speichert den letzen Errorcode vom MK damit dieser nicht wiederholt angezeigt wird |
//----------------------------------------------------------- |
// OSD Daten |
//----------------------------------------------------------- |
NaviData_t osdData; // Buffer |
//----------------------------------------------------------- |
// MK-DISPLAY |
//----------------------------------------------------------- |
uint8_t mkdisplayMode = false; |
uint8_t mkdisplayCmd = 0xff; |
char mkdisplayData[81]; // Buffer (80 +1 fuer term. Char) |
//--------------------- |
// DEBUG |
//--------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
uint16_t readCounterOSD; // Anzahl gelesener Datenpakete von NC Modus 'o' (Request OSD-Data) |
uint16_t readCounterTIME; // Anzahl gelesener Datenpakete von NC (Time) |
uint16_t readCounterDISPLAY; |
uint16_t readCounterBL[OSD_MAX_MOTORS]; // Anzahl gelesener Datenpakete pro BL via NC Modus 'k' (BL Ctrl Status) |
#endif // USE_OSD_SCREEN_DEBUG |
//--------------------- |
// Strings & Co. |
//--------------------- |
static const char mkerror00[] PROGMEM = "No Error"; |
static const char mkerror01[] PROGMEM = "FC not compatible"; |
static const char mkerror02[] PROGMEM = "MK3Mag not compati."; |
static const char mkerror03[] PROGMEM = "no FC communication"; |
static const char mkerror04[] PROGMEM = "no MK3Mag communic."; |
static const char mkerror05[] PROGMEM = "no GPS communication"; |
static const char mkerror06[] PROGMEM = "bad compass value"; |
static const char mkerror07[] PROGMEM = "RC Signal lost"; |
static const char mkerror08[] PROGMEM = "FC spi rx error"; |
static const char mkerror09[] PROGMEM = "no NC communication"; |
static const char mkerror10[] PROGMEM = "FC Nick Gyro"; |
static const char mkerror11[] PROGMEM = "FC Roll Gyro"; |
static const char mkerror12[] PROGMEM = "FC Yaw Gyro"; |
static const char mkerror13[] PROGMEM = "FC Nick ACC"; |
static const char mkerror14[] PROGMEM = "FC Roll ACC"; |
static const char mkerror15[] PROGMEM = "FC Z-ACC"; |
static const char mkerror16[] PROGMEM = "Pressure sensor"; |
static const char mkerror17[] PROGMEM = "FC I2C"; |
static const char mkerror18[] PROGMEM = "Bl Missing"; |
static const char mkerror19[] PROGMEM = "Mixer Error"; |
static const char mkerror20[] PROGMEM = "Carefree Error"; |
static const char mkerror21[] PROGMEM = "GPS lost"; |
static const char mkerror22[] PROGMEM = "Magnet Error"; |
static const char mkerror23[] PROGMEM = "Motor restart"; |
static const char mkerror24[] PROGMEM = "BL Limitation"; |
static const char mkerror25[] PROGMEM = "Waypoint range"; |
static const char mkerror26[] PROGMEM = "No SD-Card"; |
static const char mkerror27[] PROGMEM = "SD Logging aborted"; |
static const char mkerror28[] PROGMEM = "Flying range!"; |
static const char mkerror29[] PROGMEM = "Max Altitude"; |
static const char mkerror30[] PROGMEM = "No GPS Fix"; |
static const char mkerror31[] PROGMEM = "compass not calibr."; |
static const char mkerror32[] PROGMEM = "BL selftest"; |
static const char mkerror33[] PROGMEM = "no ext. compass"; |
static const char mkerror34[] PROGMEM = "compass sensor"; |
#define MAX_MKERROR_NUM 34 // maximale Error-Nummer vom MK (verwendet in osd()) |
static const char mkerror35[] PROGMEM = ""; // free for MK |
static const char mkerror36[] PROGMEM = ""; // free for MK |
static const char mkerror37[] PROGMEM = ""; // free for MK |
static const char mkerror38[] PROGMEM = ""; // free for MK |
static const char mkerror39[] PROGMEM = ""; // free for MK |
static const char pkterror40[] PROGMEM = "PKT RX lost"; |
//--------------------------------------- |
// wenn die Liste erweitert wird, |
// MAX_MKERROR_NUM in osd.h anpassen! |
//--------------------------------------- |
const char * const mkerrortext[] PROGMEM= |
{ |
mkerror00, |
mkerror01, |
mkerror02, |
mkerror03, |
mkerror04, |
mkerror05, |
mkerror06, |
mkerror07, |
mkerror08, |
mkerror09, |
mkerror10, |
mkerror11, |
mkerror12, |
mkerror13, |
mkerror14, |
mkerror15, |
mkerror16, |
mkerror17, |
mkerror18, |
mkerror19, |
mkerror20, |
mkerror21, |
mkerror22, |
mkerror23, |
mkerror24, |
mkerror25, |
mkerror26, |
mkerror27, |
mkerror28, |
mkerror29, |
mkerror30, |
mkerror31, |
mkerror32, |
mkerror33, |
mkerror34, |
mkerror35, |
mkerror36, |
mkerror37, |
mkerror38, |
mkerror39, |
pkterror40 |
}; |
//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W"; |
const char rose[48] PROGMEM = { |
0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', |
0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', |
0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', |
0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', |
}; |
// the center is char 19 (north), we add the current heading in 8th |
// which would be 22.5 degrees, but float would bloat up the code |
// and *10 / 225 would take ages... so we take the uncorrect way |
static const char str_NE[] PROGMEM = "NE"; |
static const char str_E[] PROGMEM = "E "; |
static const char str_SE[] PROGMEM = "SE"; |
static const char str_S[] PROGMEM = "S "; |
static const char str_SW[] PROGMEM = "SW"; |
static const char str_W[] PROGMEM = "W "; |
static const char str_NW[] PROGMEM = "NW"; |
static const char str_N[] PROGMEM = "N "; |
const char * const directions_p[] PROGMEM= |
{ |
str_NE, |
str_E, |
str_SE, |
str_S, |
str_SW, |
str_W, |
str_NW, |
str_N |
}; |
//########################################################### |
//# OSD Screen Controler |
//########################################################### |
#define SCREENCTRL_MAX 15 // Anzahl max. zu verwaltender Screens |
//--------------------------- |
// typedef: einzelner Screen |
//--------------------------- |
typedef struct |
{ |
const char *screenname; |
void (*screen)(void); |
} screen_t; |
//--------------------------- |
// typedef: Screenliste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; |
uint8_t count; |
screen_t screen[SCREENCTRL_MAX]; |
} screenlist_t; |
screenlist_t osdscreens; |
//-------------------------------------------------------------- |
// ScreenCtrl_Init() |
// |
// initialisiert die Screenliste - muss vor dem ersten |
// ScreenCtrl_Push() aufgerufen werden |
//-------------------------------------------------------------- |
void ScreenCtrl_Init( void ) |
{ |
osdscreens.active = 0; |
osdscreens.count = 0; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Push( *screenname, *screenfunc) |
// |
// fuegt einen Screen der Screenliste hinzu (siehe osd()) |
//-------------------------------------------------------------- |
void ScreenCtrl_Push( uint8_t screenid, const char *screenname, void (*screenfunc)(void)) |
{ |
if( osdscreens.count < SCREENCTRL_MAX ) |
{ |
// wenn screenid = 0 dann immer anzeigen (Screen ist nicht vom Benutzer auswaehlbar) |
if( (screenid==0) || ((Config.OSD_UseScreen & (1 << screenid)) != 0) ) |
{ |
osdscreens.screen[osdscreens.count].screenname = screenname; |
osdscreens.screen[osdscreens.count].screen = screenfunc; |
osdscreens.count++; |
} |
} |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Set( screennum) |
// |
// aktiviert einen bestimmten Screen aus der Screenliste |
//-------------------------------------------------------------- |
void ScreenCtrl_Set( uint8_t screennum ) |
{ |
osdscreens.active = 0; |
if( screennum < osdscreens.count ) |
{ |
osdscreens.active = screennum; |
} |
} |
//-------------------------------------------------------------- |
// num = ScreenCtrl_GetNum() |
// |
// gibt die Nummer des aktuell aktivierten Screens zurueck |
//-------------------------------------------------------------- |
uint8_t ScreenCtrl_GetNum( void ) |
{ |
return osdscreens.active; |
} |
//-------------------------------------------------------------- |
// name = ScreenCtrl_GetName() |
// |
// gibt den Namen des aktuell aktivierten Screens zurueck |
// Rueckgabe ist ein Pointer auf einen String im PROGMEM |
//-------------------------------------------------------------- |
const char * ScreenCtrl_GetName( void ) |
{ |
return osdscreens.screen[osdscreens.active].screenname; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Next() |
// |
// zum naechsten Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Next( void ) |
{ |
osdscreens.active++; |
if( osdscreens.active >= osdscreens.count ) |
{ |
osdscreens.active = 0; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Previous() |
// |
// zum vorherigen Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Previous( void ) |
{ |
if( osdscreens.active == 0 ) |
{ |
osdscreens.active = osdscreens.count-1; |
} |
else |
{ |
osdscreens.active--; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Show() |
// |
// ruft den aktuell Screen auf |
//-------------------------------------------------------------- |
void ScreenCtrl_Show( void ) |
{ |
osdscreens.screen[osdscreens.active].screen(); |
} |
//########################################################### |
//########################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_BLmax( osd_BLmax_t *blmax ) |
{ |
uint8_t i; |
memset( blmax, 0, sizeof(osd_BLmax_t) ); |
for( i=0; i<Config.OSD_Statistic.BL_Count; i++) |
{ |
if( Config.OSD_Statistic.BL[i].max_Current > blmax->max_BL_Current ) |
{ |
blmax->max_BL_Current = Config.OSD_Statistic.BL[i].max_Current; |
blmax->max_BL_Current_Index = i; |
} |
if( Config.OSD_Statistic.BL[i].max_Temp > blmax->max_BL_Temp ) |
{ |
blmax->max_BL_Temp = Config.OSD_Statistic.BL[i].max_Temp; |
blmax->max_BL_Temp_Index = i; |
} |
} |
} |
//-------------------------------------------------------------- |
// STAT_Init() |
// |
// initialisiert die Statistic Werte neu |
//-------------------------------------------------------------- |
void STAT_Init(void) |
{ |
// init: statistic |
memset( &Config.OSD_Statistic, 0, sizeof(osd_statistic_t) ); |
Config.OSD_Statistic.min_UBat = 255; |
Config.OSD_Statistic.min_RCQuality = 255; |
Config.OSD_Statistic.min_AngleNick = 126; |
Config.OSD_Statistic.min_AngleRoll = 126; |
} |
//-------------------------------------------------------------- |
// MKErr_Log_Init() |
// |
// initialisiert die MK Errorcodes neu |
//-------------------------------------------------------------- |
void MKErr_Log_Init(void) |
{ |
// init: MK Errorlog |
memset( &Config.MKErr_Log, 0, sizeof(mkerror_t)*MAX_MKERR_LOG ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MkError_Save( uint8_t Errorcode ) |
{ |
uint8_t i; |
for( i=MAX_MKERR_LOG-1; i>0; i--) |
{ |
Config.MKErr_Log[i] = Config.MKErr_Log[i-1]; |
} |
Config.MKErr_Log[0].Errorcode = Errorcode; |
memcpy( &Config.MKErr_Log[0].set_Time, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
// OSD_MK_Connect() |
// |
// Verbindung zum MK herstellen oder ABO-Renew |
// |
// modus: MK_CONNECT oder MK_ABORENEW |
//-------------------------------------------------------------- |
void OSD_MK_Connect( uint8_t modus ) |
{ |
uint8_t tx_interval; |
if( modus == MK_CONNECT ) |
{ |
SwitchToNC(); |
// switch off: 3d data => kompatibel mit SmartOSD???? |
//tx_interval = 0; |
//SendOutData ('c', ADDRESS_ANY, 1, &tx_interval, 1); |
//_delay_ms(25); |
tx_interval = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tx_interval, 1); // switch off: debug |
timer = 3; while( timer > 0 ); // short delay |
} |
tx_interval = TXINTERVAL_OSDDATA; // switch on: sending osd-data |
SendOutData( 'o', ADDRESS_NC, 1, &tx_interval, 1); // request: OSD Data from NC every ..ms |
timer = 3; while( timer > 0 ); // short delay |
// switch on: sending bl-data |
tx_interval = TXINTERVAL_BLDATA; // 5 => 50 ms (send packet every n*10 ms) |
SendOutData( 'k', ADDRESS_ANY, 1, &tx_interval, 1); // request: BL Ctrl Status |
mode = 'O'; |
abo_timer = ABO_TIMEOUT; |
rxd_buffer_locked = FALSE; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_MK_ShowTimeout( void ) |
{ |
if( old_PKTErrorcode != 40 ) |
{ |
MkError_Save( 40 ); // Logge: "PKT RX lost" |
old_PKTErrorcode = 40; |
} |
lcd_cls (); |
lcd_frect_round( 0, 0, 127,10, 1, R1); // Rect: Invers (Titel) |
lcdx_printp_center( 0, strGet(OSD_ERROR), MINVERS, 1,2); // "FEHLER: Datenverlust!" |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,6); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,6); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+5, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,9); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,11); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,11); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+8, 127, (2*8)+4, 1); // Rahmen |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline: "Ende" |
set_beep ( 0, 0, BeepOff); // Falls Spannungswarnung an war Beeper aus (ist das notwendig?) |
} |
/*************************************************************************************************************************************** |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// Fuer: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
// -> erstmal keine Unterstuetzung mehr... |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// ALTE Funktion fuer FC < 0.90h (und dementsprechend NC < 0.30h) |
// |
// Setzt die PKT globale UTCTime mithilfe des MK. |
// |
// Foraussetzung: |
// - der NC-Modus muss aktiv sein (SwitchToNC) |
// - aktuell nur fuer osd.c |
// |
// Rueckgabe: |
// true = ok, UTCTime gespeichert |
// false = Zeit/Datum nicht gelesen |
// |
// Hack: |
// Gelesen wird die Seite 3 des NC-Display's. Dort wird Zeit |
// und Datum an den entsprechnenden Positionen via Zeichen an- |
// gezeigt. Die Zeichen werden ausgelesen und in die interne UTCTime |
// gespeichert. |
// |
// HINWEIS: |
// Ab NC > v0.30b (evtl. v0.30c) wird die NC ein neues Datenpaket |
// senden ("T") mit strukturierten Informationen zu Datum/Zeit. |
// Das wurde von Holger eingebaut. |
// Wird spaeter auch im PKT implementiert. |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
uint32_t sec; |
uint32_t min; |
uint32_t hour; |
uint16_t year; |
uint8_t month; |
uint8_t day; |
uint8_t page; |
uint8_t ok = false; |
mode = 'L'; // read: MK-Display Page |
page = 3; // anfordern von Seite 3 (der NC) |
SendOutData ('l', ADDRESS_ANY, 1, &page, 1); // request: MK-Display Page 3 |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
sec = (uint32_t)(pRxData[66+7+2] - '0') + 10 * (uint32_t)(pRxData[66+6+2] - '0'); // seconds: sec |
min = (uint32_t)(pRxData[66+4+2] - '0') + 10 * (uint32_t)(pRxData[66+3+2] - '0'); // seconds: min |
hour = (uint32_t)(pRxData[66+1+2] - '0') + 10 * (uint32_t)(pRxData[66+0+2] - '0'); // seconds: hour |
sec += (min * 60) + (hour * 3600); |
year = (uint16_t)(pRxData[46+9+2] - '0') + 10 * (uint16_t)(pRxData[46+8+2] - '0'); // year |
year += 100 * (uint16_t)(pRxData[46+7+2] - '0') + 1000 * (uint16_t)(pRxData[46+6+2] - '0'); // year |
day = (uint8_t)(pRxData[46+4+2] - '0') + 10 * (uint8_t)(pRxData[46+3+2] - '0'); // day |
month = (uint8_t)(pRxData[46+1+2] - '0') + 10 * (uint8_t)(pRxData[46+0+2] - '0'); // month |
// in der globalen UTCTime speichern (hoffentlich funkt kein timer dazwischen) |
if( year > 2000 && year < 2200 ) // plausibilitaets check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = sec; |
UTCTime.day = day; |
UTCTime.month = month; |
UTCTime.year = year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
***************************************************************************************************************************************/ |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// NEUE Ermittlung der Zeit vom MK |
// ab NC v0.30g (bzw. NC 0.30h fuer FC 0.90h) |
// |
// TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
DateTime_t *rx_DateTime; |
uint8_t tx_interval; |
uint8_t ok = false; |
mode = 'T'; // read: MK-Display Page |
tx_interval = 1; // |
SendOutData ('t', ADDRESS_NC, 1, &tx_interval, 1); // request: DateTime |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
rx_DateTime = (DateTime_t *) (pRxData); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
if( rx_DateTime->Year > 2000 && rx_DateTime->Year < 2200 ) // Plausibilitaets Check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = ((uint32_t)(rx_DateTime->Hour))*3600 + ((uint32_t)(rx_DateTime->Min))*60 + (uint32_t)(rx_DateTime->Sec); |
UTCTime.day = rx_DateTime->Day; |
UTCTime.month = rx_DateTime->Month; |
UTCTime.year = rx_DateTime->Year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int32_t calc_avg( int32_t avg, int32_t nvalue, int32_t count, int32_t factor) |
{ |
avg = avg + (( ( nvalue * factor) - avg) / count); |
return avg; |
} |
//-------------------------------------------------------------- |
// OSD_MK_GetData() |
// |
// Holt Daten der BL-Ctrl via NC. |
// |
// Die Werte der BL's 1-12 kommen in mehr oder weniger beliebiger |
// Reihenfolge an. Die Daten fuer vorhandene BL's werden oefter |
// gesendet als die von nicht vorhandenen BL's. |
// |
// Diese Funktioen liest fuer die Zeit BL_READ_TIME die BL-Daten |
// vom MK ein und ordnet diese der PKT-internen Speichstruktur zu. |
// |
// Hierbei ist nicht gewaehrleistet, dass die Daten von jedem BL |
// in einem festen Zeitrahmen ermittelt werden. Die Folge ist |
// ein 'zufaelliger' Aufbau der Anzeige der BL-Daten und auch |
// eine nicht exakt bestimmbare Refreshtime der Werte. |
// |
// Optimieren kann man ggf. die Refreshzeit aller BL-Werte durch |
// tx_intervall fuer Kommando "k" und durch BL_READ_TIME. |
// Brauchbare Startwerte sind (noch experimentell): |
// tx_interval = 5 (fuer "k") = 50 ms |
// BL_READ_TIME = 25 = 250 ms |
// |
// Wenn dieses Verhalten verbessert werden soll muss ggf. |
// die ISR() (usart.c) fuer Kommando "k" angepasst werden um |
// in einem Schwung mehrere gesendete BL-Pakete aufeinmal |
// aufzunehmen und als Paket zur Verfuegung zu stellen. |
//-------------------------------------------------------------- |
void OSD_MK_GetData( void ) |
{ |
BLData_t *rx_blData; |
uint8_t blIndex; |
uint8_t FCStatusFlags; |
uint8_t v; |
uint8_t skipBL = false; |
FCStatusFlags = naviData->FCStatusFlags; // save naviData->FCStatusFlags for use with BL-Data |
//###################################### |
//# ZEIT/DATUM |
//###################################### |
// lese UTC-Time vom MK |
//-------------------------------------- |
if( timer_get_tidata == 0 ) |
{ |
//-------------------------------------------------- |
// das 'T' Datenpaket der NC fuer OSD_MK_UTCTime() |
// gibt es erst ab NC v0.30g (!) |
// |
// --> Versionspruefung der NC-Firmware |
//-------------------------------------------------- |
v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version >= "0.30g" |
if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
{ |
if( !OSD_MK_UTCTime(20) ) |
timer_get_tidata = 50; // erfolglos: versuche es nach einer 1/2 Sekunde erneut |
else |
timer_get_tidata = TIME_GET_TIDATA; // alle 60 Sekunden refresh - den Rest uebernimmt ein Timer des PKT |
} |
} |
//###################################### |
//# MK-DISPLAY |
//###################################### |
// switch to: (h) |
//-------------------------------------- |
//if( timer_get_displaydata == 0 ) |
if( mkdisplayMode && timer_get_displaydata == 0 ) |
{ |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
/* |
if( mkdisplayCmd != 0xff ) |
{ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 1, &mkdisplayCmd, 1); |
} |
*/ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
mkdisplayCmd = 0xff; // 0xff = aktuelle Seite |
timer = 20; while( (timer>0) && !rxd_buffer_locked ); // |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
memcpy( mkdisplayData, (const void *)&rxd_buffer[3+ 0], 80 ); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterDISPLAY++; |
#endif // USE_OSD_SCREEN_DEBUG |
} |
timer_get_displaydata = TIME_GET_DISPLAYDATA; // n*10 ms |
skipBL = true; |
} // end: if( mkdisplayMode && timer_get_displaydata == 0 ) |
//###################################### |
//# BL-Daten |
//###################################### |
// switch to: 'BL Ctrl Status' (k) |
//-------------------------------------- |
if( !skipBL && timer_get_bldata == 0 ) |
{ |
mode = 'K'; // read: BL Ctrl Status |
rxd_buffer_locked = FALSE; |
timer = TIME_READ_BLDATA; // fuer x Zeit werden BL-Daten gelesen |
while( timer>0 ) // lese Daten-Pakete fuer die angegebene Zeit |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
rx_blData = (BLData_t *) (pRxData); |
// die BL-Daten kommen in beliebiger Reihenfolge an |
// Hier werden sie entsprechend ihres Index gesichert |
blIndex = rx_blData->Index; |
if( blIndex >= 0 && blIndex < OSD_MAX_MOTORS ) |
{ |
memcpy( &blData[blIndex], rx_blData, sizeof(BLData_t)); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterBL[blIndex]++; |
#endif // USE_OSD_SCREEN_DEBUG |
// Statistiken |
if( (blData[blIndex].Status & 0xf0) && (FCStatusFlags & FC_STATUS_MOTOR_RUN) ) // nur wenn BL/Motor vorhanden und Motoren laufen |
{ |
// BL Statistik: Anzahl empf. Datenpakete (fuer Mittelwert) |
Config.OSD_Statistic.BL[blIndex].count++; |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t) calc_avg( (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current, |
(int32_t)blData[blIndex].Current, |
(int32_t)Config.OSD_Statistic.BL[blIndex].count, |
100); |
// ALT |
// BL Statistik: Mittelwert: Strom (*100 um Rundungsfehler zu reduzieren) |
//avg = (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current; |
//avg = avg + (( ( (int32_t)blData[blIndex].Current * 100) - avg) / (int32_t)Config.OSD_Statistic.BL[blIndex].count); |
//Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t)avg; |
// BL Statistik: Max: Strom |
if( blData[blIndex].Current > Config.OSD_Statistic.BL[blIndex].max_Current) Config.OSD_Statistic.BL[blIndex].max_Current = blData[blIndex].Current; |
// BL Statistik: Max: Temperatur |
if( blData[blIndex].Temperature > Config.OSD_Statistic.BL[blIndex].max_Temp) Config.OSD_Statistic.BL[blIndex].max_Temp = blData[blIndex].Temperature; |
if( blIndex+1 > Config.OSD_Statistic.BL_Count ) Config.OSD_Statistic.BL_Count = blIndex+1; |
} |
} |
rxd_buffer_locked = FALSE; |
} |
} |
timer_get_bldata = TIME_GET_BLDATA; // n*10 ms |
} // end: if( timer_get_bldata == 0 ) |
//-------------------------------------- |
// back to: OSD-Data |
//-------------------------------------- |
mode = 'O'; // read: OSD-Data |
rxd_buffer_locked = FALSE; // ready to receive new data |
timer_mk_timeout = MK_TIMEOUT; // reset osd MK_TIMEOUT timer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void variobeep(int16_t vario) |
{ |
#ifdef USE_SOUND |
{ //start Beep |
if (vario >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
// set_beep ( 100, 0xffff, BeepNormal); |
duration = 52 -vario; |
// if (duration =0); duration = 1; |
// write_ndigit_number_u (0,6,duration,5,0); |
playTone(300+vario*2,duration,Config.Volume); |
// playTone(300,duration,volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
if (vario <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
duration = 50 -vario; |
// write_ndigit_number_u (0,7,duration,5,0); |
// if (duration < vario) ; duration = 0; |
// playTone(300,50,volume); |
playTone(300+vario*2,duration,Config.Volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (vario == 0) Vario_Threshold = 0; //Startverzögerung löschen |
} // end Beep |
#endif |
} |
//-------------------------------------------------------------- |
// Diese Funktion Beept unabhaengig von der Einstellung Config.OSD_VarioBeep |
// Aufruf ggf. mit: if( Config.OSD_VarioBeep ) Beep_Vario(); |
// |
// Ansonten: |
// -> hier noch aufräumen in Zusammenhang mit func variobeep() |
//-------------------------------------------------------------- |
void Beep_Vario(void) |
{ |
if ( (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) && (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL)) |
{ //start Beep |
if (naviData->Variometer <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 300, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (naviData->Variometer == 0) Vario_Threshold = 0; //Startverzögerung löschen |
if (naviData->Variometer >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 100, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
} // end Beep |
} |
//-------------------------------------------------------------- |
// Quelle Mikrokopter FC-Software Holger + Ingo |
//-------------------------------------------------------------- |
void CheckMKLipo( void ) |
{ |
if( Config.MK_LowBat < 50 ) // automatische Zellenerkennung |
{ |
if( CellIsChecked <= 2 ) // nur beim Start 1x prüfen |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE) |
break; |
} |
BattLowVoltageWarning = cells * Config.MK_LowBat; |
CellIsChecked++; |
} |
} |
else BattLowVoltageWarning = Config.MK_LowBat; |
if( naviData->UBat < BattLowVoltageWarning) |
{ |
if( AkkuWarnThreshold <= 4) |
{ |
AkkuWarnThreshold++; |
} |
else if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
{ |
// MK-Unterspannungs-Beep nur wenn Motoren laufen |
set_beep ( 1000, 0x0020, BeepSevere); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_heading_home(void) |
{ |
int16_t degree; |
//--------------------------------------- |
// warum modulo 360: |
// |
// die NC kann unter gewissen Umstaenden Winkel > 360 Grad |
// senden -> fragt H&I warum |
//--------------------------------------- |
degree = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
if( degree < 0 ) |
heading_home = (int16_t)(360 + degree); |
else |
heading_home = (int16_t)degree; |
} |
//-------------------------------------------------------------- |
// convert the <heading> gotton from NC into an index |
uint8_t heading_conv (uint16_t heading) |
{ |
if (heading > 23 && heading < 68) |
return 0; //direction = "NE"; |
else if (heading > 67 && heading < 113) |
return 1; //direction = "E "; |
else if (heading > 112 && heading < 158) |
return 2; //direction = "SE"; |
else if (heading > 157 && heading < 203) |
return 3; //direction = "S "; |
else if (heading > 202 && heading < 248) |
return 4; //direction = "SW"; |
else if (heading > 247 && heading < 293) |
return 5; //direction = "W "; |
else if (heading > 292 && heading < 338) |
return 6; //direction = "NW"; |
return 7; //direction = "N "; |
} |
//-------------------------------------------------------------- |
// draw a compass rose at <x>/<y> for <heading> |
void draw_compass (uint8_t x, uint8_t y, uint16_t heading, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t front = 19 + (heading / 22); |
for (uint8_t i = 0; i < 9; i++) |
lcdx_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// variometer |
// x, y in Pixel! |
//-------------------------------------------------------------- |
void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
lcd_rect (x, y - ((hight) / 2), width, hight, 1); |
lcd_frect (x + 1, y - ((hight) / 2) + 1, width - 2, hight - 2, 0); |
lcd_line (x, y, x + width, y, 1); |
if (variometer > 0) // steigend |
{ |
switch (variometer / 5) |
{ |
case 0: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
break; |
case 1: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
break; |
case 2: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
break; |
default: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
lcd_frect (x + 1, y - 6, 8, 1, 1); // 15 > |
break; |
} |
} |
else if (variometer < 0) // fallend |
{ |
switch ((variometer) / -5) |
{ |
case 0: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
break; |
case 1: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
break; |
case 2: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
break; |
default: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
lcd_frect (x + 1, y + 5, 8, 1, 1); // -15 > |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
// variometer 2 |
// |
// x, y in Pixel |
// x, y top, left |
//-------------------------------------------------------------- |
/* |
void draw_variometer2( uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
uint8_t max = 5; // max: 5 m/sec == 100% |
lcd_rect (x, y, width, hight, 1); |
lcd_frect(x + 1, y + 1, width - 2, hight - 2, 0); |
lcd_line (x, y + ((hight) / 2), x + width, y + ((hight) / 2), 1); |
} |
*/ |
//-------------------------------------------------------------- |
// Home symbol |
// draw Homesymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_homesymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_plot (x,y-4,1); |
lcd_line (x+1,y-1,x+1,y-5,1); |
lcd_plot (x+2,y-6,1); |
lcd_plot (x+3,y-7,1); |
lcd_plot (x+4,y-6,1); |
lcd_line (x+5,y-1,x+5,y-5,1); |
lcd_plot (x+6,y-4,1); |
lcd_plot (x+3,y-1,1); |
lcd_plot (x+3,y-2,1); |
lcd_line (x+1,y,x+5,y,1); |
} |
//-------------------------------------------------------------- |
// Target symbol |
// draw Targetsymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_targetsymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_circle (x+3, y-3, 4, 1); |
lcd_line (x,y-3,x+6,y-3,1); |
lcd_line (x+3,y,x+3,y-6,1); |
lcd_circle (x+3, y-3, 2, 1); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
if (Altimeter > (300 / AltimeterAdjust) || Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
writex_ndigit_number_s ( x, y, (Altimeter / (30 / AltimeterAdjust)), 4, 0, mode, xoffs,yoffs); |
else // up to 10m write meters.dm |
writex_ndigit_number_s_10th( x, y, (Altimeter / (3 / AltimeterAdjust)), 3, 0, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcd_o_circle (uint16_t x, uint16_t y, int16_t breite, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 2; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x+xoffs, y+yoffs, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcd_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcdx_o_circle( uint8_t x, uint8_t y, int8_t width, uint8_t mode) |
{ |
int8_t radius = width / 2; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x, y, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcdx_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_mk( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: rund |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_round( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_ellipse( x+5, y+5, 5+1, 5, 1); // Aussenkreis |
//lcd_line( x+2, y+5, x+8, y+5, 1); // Innenkreuz Horizontal (laenger) |
lcd_line( x+3, y+5, x+7, y+5, 1); // Innenkreuz Horizontal (kuerzer) |
lcd_line( x+5, y+3, x+5, y+7, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: eckig |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_diamond( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_home( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+5, 10, 8, 1); // Mitte |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach Rechts |
lcd_rect( x+4, y+10, 2, 3, 1); // Tuere |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_sat( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+2, 4, 2, 1); // linker Fluegel |
lcd_rect( x+8, y+2, 4, 2, 1); // rechter Fluegel |
lcd_rect( x+4, y+0, 4, 6, 1); // Mitte, oben |
lcd_line( x+6, y+7, x+2, y+11, 1); // Strahl Links |
lcd_line( x+6, y+7, x+10, y+11, 1); // Strahl Rechts |
lcd_line( x+1, y+12, x+11, y+12, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini2( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y-1, x+4, y-1, 1); // |
//lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+0, x+6, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+0, 0); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_battery( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+2, y+0, 2, 2, 1); // der kleine Knubbel oben |
lcd_rect( x+0, y+2, 6, 15, 1); // body |
} |
//-------------------------------------------------------------- |
// RC symbol |
// alternatives Symbol fuer RC-Quality |
//-------------------------------------------------------------- |
void draw_symbol_rc( uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 1; |
x += 1; |
lcd_plot( x+3, y+4, 1); |
lcd_line( x+2, y+2, x+4, y+2, 1); |
lcd_line( x+1, y+0, x+5, y+0, 1); |
} |
//############################################################## |
//# spezielle Beeps |
//############################################################## |
//-------------------------------------------------------------- |
// Beep_Waypoint() |
// |
// Beep bei Waypoint Wechsel und wenn die WP-Liste abgearbeitet |
// ist |
//-------------------------------------------------------------- |
void Beep_Waypoint( void ) |
{ |
//----------------- |
// BEEP: WP erreicht |
//----------------- |
if( naviData->WaypointIndex > WP_old ) |
{ |
set_beep( 30, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( (naviData->WaypointIndex == naviData->WaypointNumber) |
&& (naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
&& (!WP_last) |
) |
{ |
set_beep( 400, 0xffff, BeepNormal ); // langer Bestaetigungs-Beep |
WP_last = true; |
} |
if( naviData->WaypointIndex != WP_old ) |
{ |
WP_old = naviData->WaypointIndex; |
} |
if( (naviData->WaypointIndex != naviData->WaypointNumber) // wenn aktueller WP != WP-Anzahl -> WP_last abschalten |
|| !(naviData->NCFlags & NC_FLAG_TARGET_REACHED) // wenn kein TR mehr an ist -> WP_last abschalten |
) |
{ |
WP_last = false; |
} |
// alter Code zur Orientierung |
//if( OldWP != naviData->WaypointIndex ) |
//{ |
// OldWP = naviData->WaypointIndex; |
// NextWP = true; |
//} |
// |
//if( (NextWP==true) && (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ) |
//{ |
// set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
// NextWP = false; |
//} |
} |
//############################################################## |
//# OSD-ELEMENTS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs, yoffs) |
// |
// xC, yC : x,y in Characters |
// item : OSD_FLAG_AH, OSD_FLAG_PH, usw. |
// lOn : true / false |
// xoffs,yoffs : x,y Pixelverschiebung |
//-------------------------------------------------------------- |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs) |
{ |
int8_t x = (xC*6)-2; |
int8_t y = (yC*8)-1; |
uint8_t w = 14; |
uint8_t h = 8; |
const char *labels[OSD_FLAG_COUNT] = |
{ |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!"), // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
}; |
//lcd_plot( x-2, y-2, 1); // Referenz |
if( yC==0 && yoffs<=0 ) { y = 0; h = 7; } |
if( xC==0 && xoffs<=0 ) { x = 0; w = 12; } |
if( lOn ) |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 1); // Filler |
lcdx_printp_at( xC, yC, labels[item], MINVERS, xoffs,yoffs); // Label |
} |
else |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 0); // clear |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t lOn = 0; |
// FC_StatusFlags 0.88 |
switch( item ) |
{ |
// Altitue Hold |
case OSD_FLAG_AH : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL); |
break; |
// Position Hold |
case OSD_FLAG_PH : lOn = (naviData->NCFlags & NC_FLAG_PH); |
break; |
// Coming Home |
case OSD_FLAG_CH : lOn = (naviData->NCFlags & NC_FLAG_CH); |
break; |
// Care Free |
case OSD_FLAG_CF : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE); |
break; |
// Out1 |
case OSD_FLAG_O1 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE); |
break; |
// Out2 |
case OSD_FLAG_O2 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE); |
break; |
// LowBat warning (MK) |
case OSD_FLAG_BA : lOn = (naviData->FCStatusFlags & FC_STATUS_LOWBAT); |
break; |
// Calibrate |
case OSD_FLAG_CA : lOn = (naviData->FCStatusFlags & FC_STATUS_CALIBRATE); |
break; |
// Start |
case OSD_FLAG_ST : lOn = (naviData->FCStatusFlags & FC_STATUS_START); |
break; |
// Motor Run |
case OSD_FLAG_MR : lOn = (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN); |
break; |
// Fly |
case OSD_FLAG_FY : lOn = (naviData->FCStatusFlags & FC_STATUS_FLY); |
break; |
// Emergency Landing |
case OSD_FLAG_EL : lOn = (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING); |
break; |
// RC Failsave Active |
case OSD_FLAG_FS : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE); |
break; |
// GPS ok |
case OSD_FLAG_GP : lOn = (naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// GPS-Sat not ok (GPS NOT ok) |
case OSD_FLAG_S0 : lOn = !(naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// Vario Trim Up |
case OSD_FLAG_TU : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_UP); |
break; |
// Vario Trim Down |
case OSD_FLAG_TD : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_DOWN); |
break; |
// Free |
case OSD_FLAG_FR : lOn = (naviData->NCFlags & NC_FLAG_FREE); |
break; |
// Range Limit |
case OSD_FLAG_RL : lOn = (naviData->NCFlags & NC_FLAG_RANGE_LIMIT); |
break; |
// No Serial Link |
case OSD_FLAG_SL : lOn = (naviData->NCFlags & NC_FLAG_NOSERIALLINK); |
break; |
// Target Reached |
case OSD_FLAG_TR : lOn = (naviData->NCFlags & NC_FLAG_TARGET_REACHED); |
break; |
// Manual Control |
case OSD_FLAG_MC : lOn = (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL); |
break; |
} |
OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// Anzeige von Steigen / Sinken |
//-------------------------------------------------------------- |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs) |
{ |
x = (x*6) + xoffs; |
y = (y*8) + yoffs; |
lcd_frect( x, y, 6, 7, 0); // clear |
if( naviData->Variometer > 2 ) // steigen mehr als 0.2 m/sec (ein guter Wert muss noch in der Praxis ermittelt werden) |
{ |
lcd_line( x+2, y+0, x+0, y+2, 1); |
lcd_line( x+2, y+0, x+4, y+2, 1); |
} |
else if( naviData->Variometer < -2 ) // sinken mehr als 0.2 m/sec |
{ |
lcd_line( x+2, y+6, x+0, y+4, 1); |
lcd_line( x+2, y+6, x+4, y+4, 1); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Altitude( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
switch( nStyle ) |
{ |
case 0 : |
case 1 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
break; |
case 2 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x+4, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MBIG); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x+4, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MBIG); |
lcd_putc( x+8, y, 'm', MBIG); |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
break; |
} |
#else |
if( nStyle == 2 ) |
{ |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
x += 4; |
} |
else |
{ |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
} |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
#endif |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==1 |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ) |
{ |
uint16_t Balken = 0; |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); |
if( cells > 0 ) // LipobargraphAnzeige nur wenn Anzahl der Lipozellen bekannt sind |
{ |
write_ndigit_number_u (x+6, y, cells, 1, 0, drawmode); |
lcd_printp_at (x+7, y, PSTR("S"), drawmode); |
if( cells==3 ) |
{ |
lcd_rect(x*6, y*8, 28, 7, 1); // Rahmen |
Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/12; |
if((Balken > 0) && (Balken <28)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if(Balken <= 26) lcd_frect(Balken+(x*6)+1, (y*8)+1, 26-Balken, 5, 0); // loeschen |
} |
if( cells==4 ||cells==5 ) |
{ |
lcd_rect(x*6, y*8, 30, 7, 1); // Rahmen |
if (cells == 4) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/15; |
if (cells == 5) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/19; |
if ((Balken > 0) && (Balken <=29)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if (Balken <= 27) lcd_frect(Balken+(x*6)+1, (y*8)+1, 28-Balken, 5, 0); // loeschen |
} |
} // end if: cells > 0 (TODO: Anzeige fuer cells==0 implementieren) |
} |
//-------------------------------------------------------------- |
// fuer die neuen OSD-Screens |
//-------------------------------------------------------------- |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
drawmode = (naviData->UBat < BattLowVoltageWarning ? MINVERS : MNORMAL); |
if( Config.OSD_ShowCellU ) |
{ |
// kalk. Einzelzellen Spannungsanzeige |
writex_ndigit_number_u_100th( x, y, (uint16_t)((naviData->UBat*10)/cells), 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("v"), drawmode, xoffs+1,yoffs); // Einheit (Einzelzellenanzeige ca., berechnet) |
} |
else |
{ |
// Gesamtspannungsanzeige |
writex_ndigit_number_u_10th( x, y, naviData->UBat, 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("V"), drawmode, xoffs+1,yoffs); // Einheit (Gesamtspannung) |
} |
lcd_line( (x+4)*6, y*8, (x+4)*6, y*8+7, (drawmode==MINVERS ? 1 : 0) ); // filler zwischen Spannung und "V" |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==0 |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
if( nStyle <= 1) |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 4 : 3); // Fett-Schrift |
if( nStyle <= 1) |
{ |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
} |
else |
{ |
write_ndigit_number_u_10th (x-2, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR("V"), drawmode); |
} |
#else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
if( nStyle == 2) |
x += 3; |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
#endif |
} |
//-------------------------------------------------------------- |
// OSD_Element_Battery_Bar( x, y, length, width, orientation) |
// |
// neue Variante (OG 06/2013) |
// |
// Parameter: |
// length : Gesamtlaenge des Bar's |
// width : Breite in Pixel (fest) |
// orientation: ORIENTATION_V - Vertikal (x,y oberer,linker Punkt) |
// ORIENTATION_H |
//-------------------------------------------------------------- |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation) |
{ |
uint8_t bat_umax; |
uint8_t bat_umin; |
uint8_t bat_uact; |
int8_t bat_p; |
// die Min/Max Spannungswerte sind jetzt erstmal fest vorgegeben bzw. berechnet, eine individuelle |
// Anpassung waere moeglich aber wenn besser ist es, wenn der User das nicht machen muesste... |
bat_umax = cells * 42; |
//bat_umin = cells * 32; |
bat_umin = cells * 34; // 3.4 Volt pro Zelle Minimum |
bat_uact = naviData->UBat; |
bat_p = length * (bat_uact-bat_umin) / (bat_umax-bat_umin); |
if( bat_p < 0 ) bat_p = 0; |
if( bat_p > length ) bat_p = length; |
if( width == 1 ) width = 0; // eine kleine Eigenart von frect um es zu ueberreden eine Linie zu zeichnen |
if( orientation == ORIENTATION_V ) |
{ |
lcd_frect( x, y , width, length-bat_p, 0); // clear: vertical |
lcd_frect( x, y+length-bat_p, width, bat_p, 1); // draw: vertical |
} |
else |
{ |
lcd_frect( x+bat_p, y, length-bat_p, width, 0); // clear: |
lcd_frect( x , y, bat_p , width, 1); // draw: |
} |
} |
//-------------------------------------------------------------- |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( Config.OSD_LipoBar ) |
OSD_Element_BatteryLevel_Bar( x, y); |
else |
OSD_Element_BatteryLevel_Text( x, y, nStyle); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Capacity( uint8_t x, uint8_t y ) |
{ |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? 2 : 0); |
write_ndigit_number_u (x, y, naviData->UsedCapacity, 5, 0, drawmode); |
lcd_printp_at (x+5, y, PSTR("mAh"), drawmode); |
// BeepTime = 3000; |
// BeepMuster = 0x0020; |
} |
//-------------------------------------------------------------- |
// OSD_Element_CompassDegree( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
x += 3; |
break; |
case 2 : |
#ifdef USE_FONT_BIG |
write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MBIG); |
#else |
write_ndigit_number_u (x+5, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
#endif |
x += 8; |
break; |
} |
lcd_putc( x, y, 0x1E, MNORMAL); // degree symbol |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printp_at (x, y, (const char *) (pgm_read_word ( &(directions_p[heading_conv((naviData->CompassHeading)%360)]))), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ) |
{ |
draw_compass (x, y, (naviData->CompassHeading)%360, 0,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Current( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u_10th (x, y, naviData->Current, 3, 0,0); |
lcd_printp_at (x+4, y, PSTR("A"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ) |
{ |
write_time (x, y, naviData->FlyingTime); |
lcd_printp_at (x+5, y, PSTR("m"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,0); |
lcd_printp_at (x+3, y, PSTR("Kmh"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ) |
{ |
calc_heading_home(); |
lcd_o_circle( x, y, breite, 1, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (old_hh + 180), rOffset, 0, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (heading_home + 180), rOffset, 1, xoffs,yoffs); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
// NEU! 22.04.2014 OG |
// soll das alte OSD_Element_HomeCircle() komplett ersetzen wenn |
// die alten OSD-Screens endgueltig rausfliegen |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
{ |
calc_heading_home(); |
lcd_ellipse_line( px, py, rx, ry, (old_hh + 180), 0); |
lcd_ellipse_line( px, py, rx, ry, (heading_home + 180), 1); |
lcd_ellipse( px, py, rx, ry, 1); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, heading_home, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//-------------------------------------------------------------- |
// OSD_Element_HomeDistance( x, y, nStyle) |
//-------------------------------------------------------------- |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_homesymbol(x+4, y); |
break; |
case 2 : lcd_printp_at (x, y, PSTR("Home"), 0); |
write_ndigit_number_u (x+5, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_LEDOutput( x, y, bitmask) |
// |
// bitmask: LED1 = FC_STATUS2_OUT1_ACTIVE |
// LED2 = FC_STATUS2_OUT2_ACTIVE |
//-------------------------------------------------------------- |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ) |
{ |
uint8_t lOn; |
lOn = (naviData->FCStatusFlags2 & bitmask ? 1 : 0); // Bit gesetzt? |
lOn = (Config.OSD_InvertOut ? !lOn : lOn); // Invertieren? |
lOn = (lOn ? 1 : 0); // auf 0 oder 1 setzen |
lcd_fcircle (x * 6 + 5, y * 8 + 3, Config.OSD_LEDform, lOn); |
lcd_circle (x * 6 + 5, y * 8 + 3, 3, 1); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED1Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT1_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED2Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT2_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Manuell( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Manuell( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL) |
lcd_putc (x, y, 'M', 0); // rc transmitter |
else |
lcd_putc (x, y, 'X', 0); // clear |
} |
//-------------------------------------------------------------- |
// OSD_Element_RCIntensity( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->RC_Quality, 3, 0,0); |
//lcd_printp_at (x+3, y, PSTR("\x1F"), 0); // RC-transmitter |
if (naviData->NCFlags & NC_FLAG_NOSERIALLINK) |
{ |
lcd_printpns_at(x+3, y, PSTR(" "), 0); // Clear |
} |
else |
{ |
lcd_printpns_at(x+3, y, PSTR("PC"), 0); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_SatsInUse( x, y, nStyle) |
// |
// nStyle == 0: "00s" |
// nStyle == 1: wie 0 |
// nStyle == 2: "00 Sat" |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
drawmode = (naviData->NCFlags & NC_FLAG_GPS_OK ? 0 : 2); |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_putc (x+2, y, 0x08, drawmode); |
break; |
case 2 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR(" Sat"), drawmode); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Variometer( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Variometer( uint8_t x, uint8_t y ) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
draw_variometer (x, y, 10, 14, naviData->Variometer); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Target( x, y, nStyle) |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
// nStyle = 0,1: "000m" |
// nStyle = 2,3: "Ziel 000m -" |
//-------------------------------------------------------------- |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( nStyle <= 1 ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_targetsymbol(x+4,y); |
} |
else |
{ |
lcd_printp_at (x, y, PSTR("Ziel"), 0); |
write_ndigit_number_u (x+5, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
} |
} |
//-------------------------------------------------------------- |
// TODO: |
// - pruefen ob beep hier an richtiger Stelle ist |
//-------------------------------------------------------------- |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ) |
{ |
uint8_t FC_Fallspeed; |
FC_Fallspeed = (unsigned int)naviData->Variometer; |
FC_Fallspeed = 255-FC_Fallspeed; |
drawmode = ( (naviData->Variometer < 0) && (FC_Fallspeed > Config.OSD_Fallspeed) ? 2 : 0); |
if( Config.OSD_VarioBeep ) |
Beep_Vario(); // Beep ??? |
if( drawmode == 2 ) |
{ |
if( !Config.HWSound ) |
set_beep ( 1000, 0x0060, BeepNormal); // muss ein Beep hier hin???? |
else |
variobeep(naviData->Variometer); // muss ein Beep hier hin???? |
} |
write_ndigit_number_s_10th (x, y, naviData->Variometer, 3,0, drawmode); |
lcd_printpns_at(x+4, y, PSTR("ms"), drawmode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ) |
{ |
if (!OldWP == naviData->WaypointIndex) |
{ |
// BeepTime = 500; |
// BeepMuster = 0x0080; |
OldWP = naviData->WaypointIndex; |
NextWP = true; |
} |
if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
{ |
set_beep ( 500, 0x0080, BeepNormal); |
NextWP = false; |
} |
write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
lcd_printp_at (x, y, PSTR("WP"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Bearing/ 10, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//############################################################## |
//# OSD-SCREENS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD-Screen "General" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
void OSD_Screen_General( void ) |
{ |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// Display: 128 x 64 with 6x8 Font => 21 x 8 |
// Linien: Horizontal |
lcd_line (0, 28, 127, 28, 1); // mitte |
lcd_line (0, 51, 127, 51, 1); // unten |
// Linien: Vertikal |
lcd_line (65, 0, 65, 50, 1); // mitte |
//----------------------------------------- |
// Block: Oben - Links |
//----------------------------------------- |
draw_icon_battery(0,4); |
lcdx_printp_at( 7, 2, PSTR(" mA"), MNORMAL, 0,2); |
//----------------------------------------- |
// Block: Oben - Rechts |
//----------------------------------------- |
lcdx_printp_at( 12, 0, PSTR("Alt:") , MNORMAL, 0,0); |
lcdx_printp_at( 12, 1, PSTR("Dir:") , MNORMAL, 0,1); |
lcdx_putc( 20, 1, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
lcdx_printp_at( 12, 2, PSTR(" I:") , MNORMAL, 0,2); |
lcdx_putc( 20, 2, 'A', MNORMAL, 2,2); |
//----------------------------------------- |
// Block: Unten - Links |
//----------------------------------------- |
draw_icon_sat(0,33); |
lcdx_printp_at( 6, 5, PSTR(" kmh"), MNORMAL, 0,1); |
//----------------------------------------- |
// Block: Unten - Rechts |
//----------------------------------------- |
draw_icon_home( 70, 32); |
lcdx_putc( 20, 4, 'm', MNORMAL, 2,0); |
lcdx_putc( 20, 5, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
//----------------------------------------- |
// unterste Zeile |
//----------------------------------------- |
draw_symbol_rc( 20, 7); // RC-transmitter |
//lcdx_putc( 20, 7, SYMBOL_RCQUALITY, MNORMAL, 2,0); |
} |
//----------------- |
// Batt Level (Graph) |
//----------------- |
OSD_Element_Battery_Bar( 2, 8, 11, 2, ORIENTATION_V); |
//----------------- |
// Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 2, 0, 0,0 ); |
//----------------- |
// LowBat Warnung MK |
//----------------- |
OSD_Element_Flag( 8, 0, OSD_FLAG_BA, 0,0 ); |
//----------------- |
// Flugzeit |
//----------------- |
writex_time(2, 1, naviData->FlyingTime, MNORMAL, 0,1); |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 2, 2, naviData->UsedCapacity, 5, 0, drawmode, 0,2); |
//----------------- |
// Höhe |
//----------------- |
writex_altimeter( 16, 0, naviData->Altimeter, MNORMAL, 0,0 ); |
/* |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s ( 16, 0, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0, MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th( 16, 0, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0, MNORMAL); |
*/ |
//----------------- |
// steigen / sinken |
//----------------- |
OSD_Element_UpDown( 20, 0, 2,0); |
//----------------- |
// Compass Degree |
//----------------- |
writex_ndigit_number_u (17, 1, (naviData->CompassHeading)%360, 3, 0,MNORMAL, 0,1); |
//----------------- |
// Strom |
//----------------- |
//write_ndigit_number_u_10th( 16, 2, naviData->Current, 3, 0,0); // alternativ mit Nachkomma |
writex_ndigit_number_u( 17, 2, naviData->Current/10, 3, 0,MNORMAL, 0,2); |
//----------------- |
// Sat Anzahl |
//----------------- |
write_ndigit_number_u (4, 4, naviData->SatsInUse, 2, 0,MNORMAL); |
//----------------- |
// Sat Warnung "!" |
//----------------- |
/* |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) |
lcd_printp_at( 9, 4, PSTR(" "), MNORMAL); |
else |
lcd_printp_at( 9, 4, PSTR("!"), MNORMAL); |
*/ |
OSD_Element_Flag( 8, 4, OSD_FLAG_S0, -1,0 ); // Sat Warnung (GPS not ok) |
//----------------- |
// Geschwindigkeit |
//----------------- |
writex_ndigit_number_u( 3, 5, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL, 0,1); |
//----------------- |
// Home Distance |
//----------------- |
write_ndigit_number_u( 17, 4, naviData->HomePositionDeviation.Distance / 10, 3, 0,MNORMAL); |
//----------------- |
// Home Winkel |
//----------------- |
writex_ndigit_number_u( 16, 5, heading_home, 4, 0,MNORMAL, 0,1); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 1, 7, OSD_FLAG_CF, 0,0 ); // Care Free |
OSD_Element_Flag( 4, 7, OSD_FLAG_AH, 0,0 ); // Altitude Hold |
OSD_Element_Flag( 7, 7, OSD_FLAG_PH, 0,0 ); // Position Hold |
OSD_Element_Flag( 10, 7, OSD_FLAG_CH, 0,0 ); // Coming Home |
OSD_Element_Flag( 13, 7, OSD_FLAG_EL, 0,0 ); // Emergency Landing |
//----------------- |
// RC-Quality (MK) |
//----------------- |
write_ndigit_number_u( 17, 7, naviData->RC_Quality, 3, 0,MNORMAL); |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 8, 0, OSD_FLAG_BA, true, 0,0 ); // DEMO: Batterie Warnung |
OSD_Element_Flag_Label( 8, 4, OSD_FLAG_S0, true, -1,0 ); // DEMO: Sat Warnung (GPS not ok) |
OSD_Element_Flag_Label( 1, 7, OSD_FLAG_CF, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 4, 7, OSD_FLAG_AH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 7, 7, OSD_FLAG_PH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 10, 7, OSD_FLAG_CH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 13, 7, OSD_FLAG_EL, true, 0,0 ); // DEMO |
#endif |
} |
//-------------------------------------------------------------- |
// OSD-Screen "Navigation" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
void OSD_Screen_Navigation( void ) |
{ |
int8_t xoffs, yoffs; |
int16_t degree; |
uint8_t minus; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcdx_printp_at( 0, 2, PSTR("Alt:"), MNORMAL, 0,2); // Hoehe |
lcdx_printp_at( 0, 5, PSTR("Home:"), MNORMAL, 0,3); // Home Distance |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 0; |
yoffs = 3; |
writex_altimeter( 0, 3, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 4, 3, 1,yoffs); |
//----------------- |
// Home Distance |
//----------------- |
yoffs = 3; |
writex_ndigit_number_u( 0, 6, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, 0,yoffs+1); |
lcdx_printp_at( 4, 6, PSTR("m"), MNORMAL, 2,yoffs+1); // Home |
//----------------- |
// Home Circle |
//----------------- |
//void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( 64, 38, 25, 22 ); // entspricht dem ehemaligem Huellkreis |
OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
lcd_frect( (9*6)-3+xoffs, (4*8)-2+yoffs, (3*6)+4, (1*8)+2, 0); // inner clear |
lcd_rect ( (9*6)-4+xoffs, (4*8)-3+yoffs, (3*6)+6, (1*8)+4, 1); // inner rect |
lcd_frect( 61+xoffs, 57+yoffs, 2, 2, 1); // bottom mini rect |
degree = (int16_t)heading_home; |
minus = false; |
if( degree >= 180 ) degree = 360 - degree; |
else minus = true; |
writex_ndigit_number_u( 9, 4, degree, 3, 0,MNORMAL, xoffs+1,yoffs); // Degree (Winkel) |
if( minus && degree != 0 ) |
lcd_line( (9*6)-2+xoffs, (4*8)+3+yoffs, (9*6)-1+xoffs, (4*8)+3+yoffs, 1); |
//----------------- |
// Variometer |
//----------------- |
// OG: Variometer wird erstmal nicht angezeigt weil es den Screen zu voll macht |
// wenn doch muessen erst grafische Anpassungen an den Variometer-Code gemacht |
// werden |
//void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
//draw_variometer( 95, 38, 7, 30, naviData->Variometer); |
//draw_variometer( 94, 38, 7, 21, naviData->Variometer); |
//draw_variometer2( 94, 28, 7, 21, naviData->Variometer); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3, 0); // MK Batt Warning |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0, 0); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0,-3); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0,-6); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0,-1); // Coming Home |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3,-1); // GPS-Sat not ok (GPS NOT ok) |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3,0); // DEMO |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0,0); // DEMO |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0,-3); // DEMO |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0,-6); // DEMO |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0,-1); // DEMO |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3,-1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_NAVIGATION |
//-------------------------------------------------------------- |
// OSD-Screen "OSD_Screen_Waypoints_OLD" |
// |
// alte, alternative Variante - wenn sich die neue Variante |
// durchsetzt kann das hier geloescht werden |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
/* |
void OSD_Screen_Waypoints_OLD( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, 127, 12+25, 1); // Trennlinie in der Mitte |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
//draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 6; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+2,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -2,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( px, py, rx, ry ) |
//OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
OSD_Element_HomeCircleX( 112, 24, 10, 8 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-1, 33, 2, 1, 1); // bottom mini rect |
lcd_frect( 112-1, 33, 2, 0, 1); // bottom mini rect |
lcd_frect( 112-0, 33, 0, 1, 1); // bottom mini rect |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 6; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+2,yoffs); |
uint8_t wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//----------------- |
// Sat Anzahl |
//----------------- |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, 0,2); |
draw_icon_satmini( 117, 42); |
//----------------- |
// WP: Distance |
//----------------- |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// Oben: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
// Variante: TR neben Waypoint-Anzahl |
//OSD_Element_Flag( 13, 5, OSD_FLAG_TR, 6,2); // Target Reached |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
*/ |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "Waypoints" |
// |
// aktuelle Variante! |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
void OSD_Screen_Waypoints( void ) |
{ |
int8_t xoffs, yoffs; |
uint8_t v; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, (15*6+5), 12+25, 1); // Trennlinie in der Mitte |
lcd_line( (15*6+5), 12+25, (15*6+5), 63, 1); // Linie Vertikal rechts, unten |
lcd_plot ((15*6+5), 12+25, 0); |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben,Links: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben,Links: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben,Rechts: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Oben,Mitte: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 5; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+3,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -3,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
// orginal |
//OSD_Element_HomeCircleX( 110, 25, 11, 9 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-3, 35, 2, 1, 1); // bottom mini rect |
// 1 pixel weiter links |
OSD_Element_HomeCircleX( 109, 25, 11, 9 ); // leicht erweiterter Huellkreis |
lcd_frect( 112-4, 35, 2, 1, 1); // bottom mini rect |
// etwas groesser |
//OSD_Element_HomeCircleX( 109, 26, 12, 10 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-4, 37, 2, 1, 1); // bottom mini rect |
//----------------- |
// Home: Sat Anzahl |
//----------------- |
//yoffs = -2; // alternativ |
yoffs = 0; |
//naviData->SatsInUse = 10; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, -1,2+yoffs); |
draw_icon_satmini( 115, 42+yoffs); |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 5; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+3,yoffs); |
v = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) v = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), v, naviData->WaypointNumber ); |
//----------------- |
// WP: Countdown Target-Holdtime |
//----------------- |
xoffs = 6; |
yoffs = 2; |
v = naviData->TargetHoldTime; |
if( v > 0 ) |
{ |
lcdx_printf_at_P( 12, 5, MINVERS, xoffs+2,yoffs, PSTR("%2d"), v ); |
lcd_line( (12*6)+xoffs+2, (5*8)+yoffs-1, (12*6)+xoffs+2+11, (5*8)+yoffs-1, 1); |
lcd_line( (12*6)+xoffs+1, (5*8)+yoffs-1, (12*6)+xoffs+1, (5*8)+yoffs+7, 1); |
} |
else |
{ |
lcd_frect( (12*6)+xoffs+1, (5*8)+yoffs-1, (2*8)-3, 8, 0); |
} |
//----------------- |
// WP: Distance |
//----------------- |
xoffs = 5; |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "User GPS" |
// |
// OSDScreenRefresh: OSD_SCREEN_REFESH = update values |
// OSD_SCREEN_REDRAW = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_USERGPS |
void OSD_Screen_UserGPS( void ) |
{ |
uint8_t y, i; |
int8_t yoffs; |
uint8_t show_gps_altitude = 0; // 1=GPS-Höhe anzeigen; 0=barymetrische Höhe anzeigen |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 9, 127, 63-9, 1, R2); // Rahmen |
lcdx_printp_at( 6, 0, PSTR("UGPS"), MNORMAL, 2,0); |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Sat Ok |
//----------------- |
OSD_Element_Flag( 12, 0, OSD_FLAG_S0, 5,0); // GPS-Sat not ok (GPS NOT ok) |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// die letzen 3 User-GPS Daten anzeigen |
//----------------- |
yoffs = -4; |
for(i=0; i<3; i++) |
{ |
y = (i*2)+2; |
lcd_frect( 0, (y*8)+yoffs-1, 127, 7, 1); // inverser Hintergrund |
writex_ndigit_number_u( 0, y+0, (i+1), 1, 0 , MINVERS, 4,yoffs); // Index |
writex_datetime_time( 4, y+0, Config.GPS_User[i].timestamp, MINVERS, 0,yoffs); |
//writex_time( 3, y+0, GPS_User[i].time_pkt, MINVERS, 0,yoffs); |
//lcdx_printp_at( 12, y+0, PSTR("Alt:") , MINVERS, 0,yoffs); |
lcdx_printp_at( 20, y+0, PSTR("m") , MINVERS, 0,yoffs); |
if( show_gps_altitude ) // GPS-Hoehe oder barymetrische Hoehe? |
{ |
// GPS Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("G"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_ndigit_number_s( 16, y+0, Config.GPS_User[i].GPSData.Altitude/1000, 4, 0, MINVERS, 0,yoffs); // GPS Hoehe in Meter |
} |
else |
{ |
// barymetrische Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("B"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_altimeter( 16, y+0, Config.GPS_User[i].Altimeter, MINVERS, 0,yoffs ); |
} |
lcd_line( 1, (y*8)+yoffs+7, 125, (y*8)+yoffs+7, 0); // clear: Invers unterste Linie |
writex_gpspos( 1, y+1, Config.GPS_User[i].GPSData.Latitude , MNORMAL, 0,yoffs); // GPS Lat |
writex_gpspos( 12, y+1, Config.GPS_User[i].GPSData.Longitude, MNORMAL, 0,yoffs); // GPS Long |
//writex_gpspos( 1, y+1, GPS_User[i].GPSData.Latitude + 12867000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
//writex_gpspos( 12, y+1, GPS_User[i].GPSData.Longitude+ 8568000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
yoffs++; |
} |
} |
#endif // USE_OSD_SCREEN_USERGPS |
//-------------------------------------------------------------- |
// OSD-Screen "Status" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_MKSTATUS |
void OSD_Screen_MKStatus( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 10, 127, 63-10, 1, R2); // Rahmen |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // "mAh" (entnommene Kapazität) |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Strom |
//----------------- |
//writex_ndigit_number_u_10th( 7, 0, naviData->Current, 4, 0,MNORMAL, 1,0); // Strom mit Nachkomma |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
//writex_ndigit_number_u( 7, 0, naviData->UsedCapacity * 10, 5, 0, drawmode, -3,0); // DEBUG |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Flags |
//----------------- |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0+xoffs, 0+yoffs); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0+xoffs,-3+yoffs); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0+xoffs,-6+yoffs); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0+xoffs,-1+yoffs); // Coming Home |
xoffs -= 4; |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3+xoffs, 0+yoffs); // MK Batt Warning |
OSD_Element_Flag( 16, 4, OSD_FLAG_EL, -3+xoffs,-3+yoffs); // Emergency Landing |
OSD_Element_Flag( 16, 6, OSD_FLAG_RL, -3+xoffs,-6+yoffs); // Range Limit |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3+xoffs,-1+yoffs); // GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag( 12, 2, OSD_FLAG_CA, 0+xoffs, 0+yoffs); // Calibrate |
OSD_Element_Flag( 12, 4, OSD_FLAG_ST, 0+xoffs,-3+yoffs); // Start |
OSD_Element_Flag( 12, 6, OSD_FLAG_MR, 0+xoffs,-6+yoffs); // Motor Run |
OSD_Element_Flag( 12, 7, OSD_FLAG_FY, 0+xoffs,-1+yoffs); // Fly |
xoffs -= 4; |
OSD_Element_Flag( 9, 2, OSD_FLAG_O1, -2+xoffs, 0+yoffs); // Out1 |
OSD_Element_Flag( 9, 4, OSD_FLAG_O2, -2+xoffs,-3+yoffs); // Out2 |
OSD_Element_Flag( 9, 6, OSD_FLAG_TR, -2+xoffs,-6+yoffs); // Target Reached |
OSD_Element_Flag( 9, 7, OSD_FLAG_MC, -2+xoffs,-1+yoffs); // Manual Control |
xoffs -= 4; |
OSD_Element_Flag( 6, 2, OSD_FLAG_TU, -4+xoffs, 0+yoffs); // Vario Trim Up |
OSD_Element_Flag( 6, 4, OSD_FLAG_TD, -4+xoffs,-3+yoffs); // Vario Trim Down |
OSD_Element_Flag( 6, 6, OSD_FLAG_FR, -4+xoffs,-6+yoffs); // Free |
OSD_Element_Flag( 6, 7, OSD_FLAG_SL, -4+xoffs,-1+yoffs); // No Serial Link |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
/* |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!") // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
*/ |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0+xoffs, 0+yoffs); // DEMO: Carefree |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0+xoffs,-3+yoffs); // DEMO: Altitude Hold |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0+xoffs,-6+yoffs); // DEMO: Position Hold |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0+xoffs,-1+yoffs); // DEMO: Coming Home |
xoffs -= 4; |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3+xoffs, 0+yoffs); // DEMO: MK Batt Warning |
OSD_Element_Flag_Label( 16, 4, OSD_FLAG_EL, true, -3+xoffs,-3+yoffs); // DEMO: Emergency Landing |
OSD_Element_Flag_Label( 16, 6, OSD_FLAG_RL, true, -3+xoffs,-6+yoffs); // DEMO: Range Limit |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3+xoffs,-1+yoffs); // DEMO: GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag_Label( 12, 2, OSD_FLAG_CA, true, 0+xoffs, 0+yoffs); // DEMO: Calibrate |
OSD_Element_Flag_Label( 12, 4, OSD_FLAG_ST, true, 0+xoffs,-3+yoffs); // DEMO: Start |
OSD_Element_Flag_Label( 12, 6, OSD_FLAG_MR, true, 0+xoffs,-6+yoffs); // DEMO: Motor Run |
OSD_Element_Flag_Label( 12, 7, OSD_FLAG_FY, true, 0+xoffs,-1+yoffs); // DEMO: Fly |
xoffs -= 4; |
OSD_Element_Flag_Label( 9, 2, OSD_FLAG_O1, true, -2+xoffs, 0+yoffs); // DEMO: Out1 |
OSD_Element_Flag_Label( 9, 4, OSD_FLAG_O2, true, -2+xoffs,-3+yoffs); // DEMO: Out2 |
OSD_Element_Flag_Label( 9, 6, OSD_FLAG_TR, true, -2+xoffs,-6+yoffs); // DEMO: Target Reached |
OSD_Element_Flag_Label( 9, 7, OSD_FLAG_MC, true, -2+xoffs,-1+yoffs); // DEMO: Manual Control |
xoffs -= 4; |
OSD_Element_Flag_Label( 6, 2, OSD_FLAG_TU, true, -4+xoffs, 0+yoffs); // DEMO: Vario Trim Up |
OSD_Element_Flag_Label( 6, 4, OSD_FLAG_TD, true, -4+xoffs,-3+yoffs); // DEMO: Vario Trim Down |
OSD_Element_Flag_Label( 6, 6, OSD_FLAG_FR, true, -4+xoffs,-6+yoffs); // DEMO: Free |
OSD_Element_Flag_Label( 6, 7, OSD_FLAG_SL, true, -4+xoffs,-1+yoffs); // DEMO: No Serial Link |
#endif |
} |
#endif // USE_OSD_SCREEN_MKSTATUS |
//-------------------------------------------------------------- |
// OSD_Screen_Electric() |
// |
// Anzeige BL-Temperaturen und Stroeme |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_ELECTRIC |
void OSD_Screen_Electric( void ) |
{ |
uint8_t x, y, x0, y0; |
int8_t yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line (33, 0, 33, 19, 1); // Linie vertikal oben, links |
lcd_line (93, 0, 93, 19, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 19, 127, 63-19, 1, R2); // Rahmen |
lcd_line (0, 41, 127, 41, 1); // Linie horizontal mitte |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // entnommene Kapazität |
lcdx_printp_at( 12,1 , PSTR("A") , MNORMAL, -1,2); // aktueller Strom |
writex_ndigit_number_u( 19, 1, cells, 1, 0, MNORMAL, 2,2); // LiPO Cells Wert |
lcdx_printp_at( 20,1 , PSTR("s") , MNORMAL, 2,2); // LiPO Cells "s" |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
//writex_time(16, 0, naviData->FlyingTime+900, MNORMAL, 2,0); // DEBUG |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Strom |
//----------------- |
writex_ndigit_number_u_10th( 7, 1, naviData->Current, 4, 0,MNORMAL, -3,2); // Strom mit Nachkomma |
//----------------- |
// BL 1-8 Temp & Strom |
//----------------- |
x0 = 1; |
y0 = 4; |
for( y=0; y<2; y++) // 2 Zeilen (mit je 4 BL's/Motoren) |
{ |
if( y==0 ) yoffs = -9; |
else yoffs = -3; |
for( x=0; x<4; x++) // und 4 BL's/Motoren pro Zeile |
{ |
if( blData[y*4+x].Status & 0xf0 ) // BL/Motor vorhanden? |
{ |
if( blData[y*4+x].Temperature != 0 ) // Anzeige nur wenn Temp != 0 wegen BL-Ctrl v1 die keine Temperatur senden |
{ |
writex_ndigit_number_u( (x*5)+x0, (y*2)+y0+0, ( blData[y*4+x].Temperature ), 3, 0,MNORMAL, 0,yoffs); // Temperatur |
lcdx_putc ( (x*5)+3+x0, (y*2)+y0+0, SYMBOL_SMALLDEGREE ,MNORMAL, 1,yoffs); |
} |
// Variante: mit Nachkommastellen |
writex_ndigit_number_u_10th ( (x*5)+x0, (y*2)+y0+1, ( blData[y*4+x].Current), 3, 0, MNORMAL, 0,yoffs); // Strom |
} |
} |
} |
#ifdef USE_OSD_DEMO |
OSD_Element_Flag_Label( 19, 1, OSD_FLAG_BA, true, 0,1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_ELECTRIC_N |
//-------------------------------------------------------------- |
// OSD_Screen_Statistics() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_STATISTIC |
void OSD_Screen_Statistics( void ) |
{ |
osd_BLmax_t blmax; |
uint8_t line = 0; |
// max. der BL's ermitteln |
calc_BLmax( &blmax ); |
//--------------------------- |
// max Altitude |
lcd_printp_at (0, line, strGet(STATS_ITEM_0), MNORMAL); |
write_ndigit_number_s (14, line, Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// max Speed |
// max_GroundSpeed = 1; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_1), MNORMAL); |
write_ndigit_number_u (15, line, (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("kmh"), MNORMAL, 2,0); |
//--------------------------- |
// max Distance |
// max_Distance = 64512; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_2), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Distance / 10, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// min voltage |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_3), MNORMAL); |
if( Config.OSD_Statistic.min_UBat==255 ) |
lcd_printp_at(14, line, PSTR(" 0"), MNORMAL); |
else |
write_ndigit_number_u_10th (14, line, Config.OSD_Statistic.min_UBat, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'V', MNORMAL, 2,0); |
//--------------------------- |
// Used Capacity |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_6), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Capacity, 4, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("mAh"), MNORMAL, 2,0); |
//--------------------------- |
// max Current |
// max_Current = 1000; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_5), MNORMAL); |
write_ndigit_number_u_10th (13, line, Config.OSD_Statistic.max_Current, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Current |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Curr:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Current_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u_10th (14, line, blmax.max_BL_Current, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Temp |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Temp:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Temp_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u (14, line, blmax.max_BL_Temp, 4, 0,MNORMAL); |
lcdx_printp_at( 18, line, PSTR("\013C"), MNORMAL, 2,0); |
} |
#endif // USE_OSD_SCREEN_STATISTIC |
//-------------------------------------------------------------- |
// OSD_Screen_3DLage() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_3DLAGE |
void OSD_Screen_3DLage( void ) |
{ |
uint16_t head_home; |
uint8_t Nick; |
uint8_t Roll; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_rect_round( 0, 0, 127, 63-0, 1, R2); // Rahmen |
} |
head_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_line(26,32,100,32,1); // horizontal // |
lcd_line(63,0,63,63,1); // vertical // |
// 45' Angel |
lcd_line(61,11,65,11,1); // -- // |
lcd_line(40,30,40,34,1); // | // |
lcd_line(86,30,86,34,1); // | // |
lcd_line(61,53,65,53,1); // -- // |
lcdx_printp_at( 9, 0, strGet(OSD_3D_V), MNORMAL, 0,4); // V |
lcdx_printp_at( 3, 3, strGet(OSD_3D_L), MNORMAL, 0,0); // L |
lcdx_printp_at(17, 3, strGet(OSD_3D_R), MNORMAL, 0,0); // R |
lcdx_printp_at( 9, 7, strGet(OSD_3D_H), MNORMAL, 0,-3); // H |
// Oben, Links: Ni |
lcdx_printp_at(0, 0, strGet(OSD_3D_NICK), MNORMAL, 5,4); // Ni |
writex_ndigit_number_s (2, 0, naviData->AngleNick, 3, 0, MNORMAL, 7,4); |
lcdx_putc (5, 0, SYMBOL_SMALLDEGREE, MNORMAL, 7,4); |
// Unten, Links: Ro |
lcdx_printp_at(0, 7, strGet(OSD_3D_ROLL), MNORMAL, 5,-3); // Ro |
writex_ndigit_number_s (2, 7, naviData->AngleRoll, 3, 0, MNORMAL, 7,-3); |
lcdx_putc (5, 7, SYMBOL_SMALLDEGREE, MNORMAL, 7,-3); |
// Oben, Rechts: Ko |
//lcdx_printp_at(13, 0, strGet(OSD_3D_COMPASS), MNORMAL, -4,3); // Ko |
writex_ndigit_number_u (15, 0, (naviData->CompassHeading)%360, 3, 0, MNORMAL, -4,4); |
lcdx_putc (18, 0, SYMBOL_SMALLDEGREE, MNORMAL, -4,4); |
OSD_Element_CompassDirection( 19, 0, -2,4 ); |
Roll = ((-naviData->AngleRoll/2)+63); |
Nick = ((-naviData->AngleNick/2)+32); |
if( Roll < (9+1) ) Roll = (9+1); // nicht ausserhalb des Screens zeichnen! |
if( Roll > 127-(9+1) ) Roll = 127-(9+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick < (8+1) ) Nick = (8+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick > 63-(8+1) ) Nick = 63-(8+1); // nicht ausserhalb des Screens zeichnen! |
if( old_AngleRoll != 0 ) // nicht ausserhalb des Screens zeichnen! |
{ |
lcd_ellipse ( old_AngleRoll, old_AngleNick, 9, 8, 0); |
lcd_ellipse_line( old_AngleRoll, old_AngleNick, 8, 7, old_hh, 0); |
} |
lcd_ellipse ( Roll, Nick, 9, 8, 1); |
lcd_ellipse_line( Roll, Nick, 8, 7, head_home, 1); |
// remember last values (3DL) |
old_hh = head_home; |
old_AngleNick = Nick; |
old_AngleRoll = Roll; |
} |
#endif // USE_OSD_SCREEN_3DLAGE |
//-------------------------------------------------------------- |
// OSD_Screen_MKDisplay() |
// |
// das ist ein Spezialscreen der ausserhalb der regulaeren |
// OSD-Screens aufgerufen und bedient wird! |
//-------------------------------------------------------------- |
//#ifdef USE_OSD_SCREEN_MKDISPLAY |
void OSD_Screen_MKDisplay( void ) |
{ |
uint8_t wpindex; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal Oben: links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal Oben: rechts |
lcd_rect_round( 0, 2*7-2, 127, 5*7+3+3, 1, R2); // Rahmen unten fuer Inhalt Display |
//lcd_frect_round( 6*6+0, 0, 9*6+2, 9, 1, R1); // Umrahmung fuer "OSD-Displ" |
//lcdx_printp_at( 7, 0, PSTR("OSD-Disp"), MINVERS, -3,1); // "OSD-Displ" |
//lcdx_printp_at(15, 0, PSTR("l"), MINVERS, -4,1); // das "l" von "OSD-Displ" (1 Pixel nach links) |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 0, MNORMAL, -1,0, PSTR("WP:%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//lcdx_printf_at_P(13, 0, MNORMAL, -3,1, PSTR("R:%d"), (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("WP:%d"), naviData->WaypointIndex); |
//write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
//lcd_printp_at (x, y, PSTR("WP"), 0); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Navi-Kreis |
//----------------- |
//OSD_Element_HomeCircleX( 64, 5, 6, 5, true ); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
//------------------------------------------ |
mkdisplayData[80] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &mkdisplayData[60], MNORMAL, 5,3); |
mkdisplayData[60] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &mkdisplayData[40], MNORMAL, 5,2); |
mkdisplayData[40] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &mkdisplayData[20], MNORMAL, 5,1); |
mkdisplayData[20] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &mkdisplayData[0], MNORMAL, 5,0); |
Beep_Waypoint(); |
} |
//#endif // USE_OSD_SCREEN_MKDISPLAY |
//############################################################## |
#ifdef USE_OSD_SCREEN_DEBUG |
//############################################################## |
//************************************************************** |
//* OSD_DEBUG_SCREEN - Experimental-Code usw. |
//* - nicht fuer die Oeffentlichkeit bestimmt |
//* - gesteuert ueber define OSD_DEBUG_SCREEN |
//************************************************************** |
//-------------------------------------------------------------- |
// OSD_Screen_Debug() |
//-------------------------------------------------------------- |
void OSD_Screen_Debug( void ) |
{ |
//char buffer[80]; |
static uint16_t debug_count = 0; |
//char buffer[30]; |
//uint8_t y, i; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_printp_at( 0, 0, PSTR("Debug"), 0); |
//timer_pkt_uptime = 0; |
} |
debug_count++; |
write_ndigit_number_u( 17, 0, (debug_count), 4, 0,MNORMAL); // |
// act. Current |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("act Current%3.1u A"), naviData->Current); |
// max. Current |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("max Current%3.1u A"), Config.OSD_Statistic.max_Current); |
// avg. Current |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("avg Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Current/100)); |
// avg. Current DEBUG |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("avgD Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Altimeter/100)); |
// stat. Packages |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("stat Pkg's%7u"), Config.OSD_Statistic.count_osd); |
/* |
// DEBUG: Heading |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("NC-Errorcode:%3u"), naviData->Errorcode); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("CH:%6d%6d"), naviData->CompassHeading, naviData->CompassHeading%360); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("HD:%6d%6d"), naviData->HomePositionDeviation.Bearing, naviData->HomePositionDeviation.Bearing%360); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)) + 180; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
calc_heading_home(); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("xx:%6d"), heading_home); |
*/ |
/* |
lcd_putc( 0, 3, 0x08, MNORMAL); // ASCII - 8 08 SAT Symbol |
lcd_putc( 2, 3, 0x09, MNORMAL); // ASCII - 9 09 Altitude Symbol |
lcd_putc( 4, 3, 0x0C, MNORMAL); // ASCII - 12 0C Enter Symbol |
lcd_putc( 6, 3, 0x1F, MNORMAL); // ASCII - 31 1F Antenne |
lcd_putc( 8, 3, 10, MNORMAL); // 'o' |
lcd_putc(10, 3, 13, MNORMAL); // 'o' |
//lcd_putc(10, 3, 0x06, MNORMAL); |
//lcd_putc(12, 3, 0x07, MNORMAL); |
lcd_putc( 0, 5, 0x1E, MNORMAL); |
lcd_putc( 4, 5, 0x7e, MNORMAL); |
lcd_putc( 6, 5, 0x7f, MNORMAL); |
lcd_putc( 8, 5, 0x18, MNORMAL); |
lcd_putc(10, 5, 0x19, MNORMAL); |
*/ |
//lcd_printp_at( 0, 1, PSTR("Free RAM:"), 0); |
//writex_ndigit_number_u ( 9, 1, get_freeRam(), 9, 0,MNORMAL, 0,0); // |
} |
//-------------------------------------------------------------- |
// OSD_Screen_Debug_RX() |
// |
// Anzeige gelesener Datenpakete (fuer Feinabstimmung) und weitere |
// Werte wie Zeit/Datum (mit/ohne Abgleich zur NC) |
// |
// Anzeige oben 1. Zeile: |
// Screenname : "Debug-RX" |
// PKT-Uptime : Minuten, Sekunden die das PKT aktuell eingeschaltet ist |
// Screen-Count: Anzahl der Aufrufe des Screens (abhaengig von der Refreshtime) |
// |
// Anzeige Zeit: |
// "N" oder "O": neuer(N) oder alter(O) Zeit-Algo fuer die NC (OSD_MK_UTCTime()) |
// 00:00:00 : Stunde, Minute, Sekunde (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// dd.mm.yyyy : Tag, Monat, Jahr (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// Solange keine richtige Zeit von der NC gemeldet wird, wird die PKT-Uptime |
// seit einschalten des PKT angezeigt. |
// |
// Anzeige unten: |
// OSD: Anzahl gelesener OSD-Pakete der NC |
// Time: Anzahl gelesener Time-Pakete der NC (Aktualisierung ca. jede Minute) |
// |
// BL: es werden die gelesenen Datenpakete der BL's angezeigt |
// von BL1 (links oben) bis BL8 (rechts unten) |
//-------------------------------------------------------------- |
void OSD_Screen_Debug_RX( void ) |
{ |
static uint16_t debug_count = 0; |
uint8_t y; |
int8_t yoffs; |
//uint8_t status; // FC Kommunikation |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_frect( 0, 0, 1, 8, 1); // title spacer |
lcdx_printp_at( 0, 0, PSTR("Debug-RX"), MINVERS, 1,0); // title |
lcd_line (0, 9, 127, 9, 1); // Linie horizontal |
lcd_line (0, 22, 127, 22, 1); // Linie horizontal |
//lcd_line (0, 31, 127, 31, 1); // Linie horizontal |
} |
//----------------------- |
// Zeile 0 |
//----------------------- |
writex_time( 11, 0, timer_pkt_uptime/100, MNORMAL, 0,0); |
debug_count++; |
write_ndigit_number_u ( 17, 0, (debug_count), 4, 0,MNORMAL); // |
//----------------------- |
// Anzeige Zeit / Datum |
//----------------------- |
yoffs = 5; |
writex_datetime_time( 1, 1, UTCTime, MNORMAL, 5,yoffs); // Zeit |
writex_datetime_date( 11, 1, UTCTime, MNORMAL, 0,yoffs); // Datum |
//----------------------- |
// gelesene Pakete: OSD und Zeit |
//----------------------- |
y = 4; |
yoffs = -5; |
lcdx_printp_at( 0, y+0, PSTR("OSD:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+0, ( readCounterOSD), 5, 0,MNORMAL, 0,yoffs); // |
lcdx_printp_at( 13, y+0, PSTR("Time:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 18, y+0, ( readCounterTIME), 3, 0,MNORMAL, 0,yoffs); // |
//---- |
lcdx_printp_at( 0, y+1, PSTR("Dis:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+1, ( readCounterDISPLAY), 5, 0,MNORMAL, 0,yoffs); // |
//----------------------- |
// gelesene Pakete: BL |
//----------------------- |
y = 6; |
lcd_frect( 0, y*8-1, 8, 17, 1); // Box |
lcdx_printp_at( 0, y+0, PSTR("B"), MINVERS, 2,0); |
lcdx_printp_at( 0, y+1, PSTR("L"), MINVERS, 2,0); |
write_ndigit_number_u ( 2, y+0, ( readCounterBL[0]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+0, ( readCounterBL[1]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+0, ( readCounterBL[2]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+0, ( readCounterBL[3]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 2, y+1, ( readCounterBL[4]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+1, ( readCounterBL[5]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+1, ( readCounterBL[6]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+1, ( readCounterBL[7]), 4, 0,MNORMAL); // |
} |
//############################################################## |
#endif // USE_OSD_SCREEN_DEBUG |
//############################################################## |
//----------------------------------------------------------- |
// ok = OSD_Popup_MKSetting() |
// |
// zeigt das aktuelle FC-Setting beim Start vom OSD an |
// |
// RUECKGABE: |
// true = Setting konnte gelesen werden |
// false = Fehler |
//----------------------------------------------------------- |
uint8_t OSD_Popup_MKSetting( void ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printp_center( 2, PSTR("PKT OSD") , MNORMAL, 0, 0); |
lcdx_printp_center( 6, PSTR("MK Setting"), MINVERS, 0,-8); |
MK_Setting_load( 0xff, 9 ); // 0xff == aktuelles Parameterset holen; 9 == timeout |
MKVersion_Setting_print( 7, MINVERS, 0,-4); // aus: mkbase.c |
if( MKVersion.mksetting == 0 ) |
set_beep( 500, 0xffff, BeepNormal ); // kein Setting - langer Beep ERROR |
clear_key_all(); |
timer = 300; // ca. 3 Sekunden zeigen; Abbruch mit einer Taste moeglich |
while( timer > 0 && !get_key_press(0xff) ); |
clear_key_all(); |
lcd_cls(); |
return (MKVersion.mksetting != 0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_MKError( u8 mkerrorcode ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printf_center_P( 6, MINVERS, 0,-8, PSTR("** MK-%S %02d **"), strGet(STR_ERROR), mkerrorcode ); // "MK-FEHLER" und Fehlernummer |
lcdx_printp_center( 7, (const char*) pgm_read_word(&mkerrortext[mkerrorcode]), MINVERS, 0,-4); // MK-Fehlertext |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_Info( uint8_t ScreenNum, const char *ScreenName) |
{ |
Popup_Draw( 5 ); // 5 Zeilen von unten nach oben fuer's Popup |
lcd_line( 3, 53-21, 124, 53-21, 0); // Linie: oben |
lcd_line( 3, 53, 124, 53, 0); // Linie: unten |
//----------------------- |
// ScreenNummer: ScreenName |
//----------------------- |
lcdx_printf_at_P( 0, 3, MINVERS, 5,-2, PSTR("%02d: %S"), ScreenNum, ScreenName ); |
//----------------------- |
// longpress Key's |
//----------------------- |
lcdx_printp_at( 0, 4, strGet(STR_LONGPRESS), MINVERS, 6,3); // "langer Tastendruck:" |
lcdx_printp_at(12, 5, PSTR("Disp"), MINVERS, 0,4); |
lcdx_printp_at(17, 5, PSTR("UGps"), MINVERS, 0,4); |
//----------------------- |
// shortpress Key's |
//----------------------- |
lcd_printp_at( 0, 7, strGet(KEYLINE3), MINVERS); |
lcd_printp_at(17, 7, PSTR("Info") , MINVERS); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_Pos_Save( pkt_gpspos_t *pGPS ) |
{ |
pGPS->Altimeter = naviData->Altimeter; // barymetrische Hoehe |
pGPS->HomeDistance = naviData->HomePositionDeviation.Distance; // Entfernung Home |
memcpy( &pGPS->GPSData, &naviData->CurrentPosition, sizeof(GPS_Pos_t) ); // sichern... |
memcpy( &pGPS->timestamp, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// GPS_User_Init() |
// |
// initialisiert die GPS Positionen neu |
//-------------------------------------------------------------- |
void GPS_User_Init( void ) |
{ |
memset( Config.GPS_User, 0, sizeof(pkt_gpspos_t)*MAX_GPS_USER ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_User_Save( void ) |
{ |
uint8_t i; |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) // nur wenn MK-GPS ok ist |
{ |
for( i=MAX_GPS_USER-1; i>0; i--) |
{ |
Config.GPS_User[i] = Config.GPS_User[i-1]; |
} |
GPS_Pos_Save( &Config.GPS_User[0] ); |
set_beep( 160, 0xffff, BeepNormal ); // Beep Ok |
} |
else |
{ |
set_beep( 600, 0x000f, BeepNormal ); // Beep Error (keine gueeltigen GPS-Daten) |
} |
} |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Init( void ) |
{ |
CellIsChecked = 0; |
cells = 0; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Check( void ) |
{ |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
} |
//############################################################## |
//# OSD MAIN LOOP |
//############################################################## |
//-------------------------------------------------------------- |
// OSD MAIN LOOP |
//-------------------------------------------------------------- |
void osd( void ) |
{ |
uint8_t osdexit = false; |
uint8_t mktimeout = false; |
uint8_t flying = false; |
uint8_t popup_state = OSD_POPUP_NONE; |
lcd_cls(); |
#ifdef DEBUG_OSD_TIME |
// Fake-Zeit/Datum setzen wenn der NC-Simulator verwendet wird |
if( UTCTime.year < 2000 ) |
{ |
UTCTime.seconds = ((uint32_t)13*3600)+(15*60)+42; // 13:15:42 |
UTCTime.day = 01; |
UTCTime.month = 05; |
UTCTime.year = 2013; |
} |
#endif |
//---------------------------------------- |
// Anzeige: aktuelles MK-Setting |
//---------------------------------------- |
if( (Config.OSD_ShowMKSetting) ) |
{ |
if( !OSD_Popup_MKSetting() ) |
return; |
} |
/* |
//----------------------------------------------------------------------------------------------- |
// 07.03.2013 OG: del |
// Dieser Teil hat immer wieder Probleme bereitet bei der Verbindung des PKT-OSD zum MK da |
// MK_TIMEOUTs zustande kamen. Eine Recherche im Code ergab, dass die Nutzdaten die |
// hierueber bezogen werden sich lediglich auf Flags_ExtraConfig beschraenkten (CFG2_HEIGHT_LIMIT). |
// Siehe dazu untere Kommentare. |
// |
// Der negative Effekt moeglicher MK_TIMEOUTs und Verzoegerungen sind es aktuell nicht Wert |
// CFG2_HEIGHT_LIMIT zu unterstuetzen. Dieses Feature ist erstmal raus. |
// |
// Falls gewuenscht wird, dass CFG2_HEIGHT_LIMIT wieder in das PKT-OSD kommt muss |
// es zuverlaessig an anderer Stelle implementiert werden - und zwar nicht in osd.c |
// weil es eine statische FC-Information ist (ggf. beim Verbindungsaufbau PKT <-> MK). |
// |
// Hat auch aktuell Auswirkung auf den Code OSD_Element_AltitudeControl() |
//----------------------------------------------------------------------------------------------- |
//lcd_printp_at( 0, 3, PSTR("connecting MK..."), 0); |
// |
//SwitchToFC(); |
// |
//status = load_setting(0xff); |
// |
//if( status == 255 ) |
//{ |
// lcd_printp_at(0, 0, PSTR("Keine Settings !!"), 0); // Keine Settings |
// _delay_ms(2000); |
//} |
//Flags_ExtraConfig = mk_param_struct->ExtraConfig; // OG: wird in osd.c nur verwendet von: OSD_Element_AltitudeControl() |
//Flags_GlobalConfig = mk_param_struct->GlobalConfig; // OG: wird nicht in osd.c verwendet |
//Flags_GlobalConfig3 = mk_param_struct->GlobalConfig3; // OG: wird nicht in osd.c verwendet |
*/ |
//------------------------- |
// MK-Display initialisieren |
//------------------------- |
memset( mkdisplayData, 0, 81 ); |
mkdisplayMode = false; |
mkdisplayCmd = 0xff; |
//------------------------- |
// BL-DATA initialisieren |
//------------------------- |
memset( blData, 0, sizeof(BLData_t)*OSD_MAX_MOTORS ); |
//------------------------- |
// Debug initialisieren |
//------------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD = 0; |
readCounterTIME = 0; |
readCounterDISPLAY = 0; |
memset( readCounterBL, 0, sizeof(uint16_t)*OSD_MAX_MOTORS ); |
#endif // USE_OSD_SCREEN_DEBUG |
//------------------------- |
// NC Datenkommunikation starten |
//------------------------- |
OSD_MK_Connect( MK_CONNECT ); |
OSD_active = true; // benoetigt fuer Navidata Ausgabe an SV2 |
//------------------------- |
// Clear statistics |
//------------------------- |
//STAT_Init(); |
CellIsChecked = 0; |
cells = 0; |
AkkuWarnThreshold = 0; |
OldWP = 0; |
NextWP = false; |
old_PKTErrorcode = 0; |
old_MKErrorcode = 0; |
//------------------------- |
// Init: OSD-Screens |
//------------------------- |
ScreenCtrl_Init(); |
ScreenCtrl_Push( OSDSCREEN_GENERAL , strGet(STR_OSDSCREEN_GENERAL) , &OSD_Screen_General ); |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScreenCtrl_Push( OSDSCREEN_NAVIGATION , strGet(STR_OSDSCREEN_NAVIGATION), &OSD_Screen_Navigation ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints ); |
#endif |
// *ALTERNATIVE* |
//#ifdef USE_OSD_SCREEN_WAYPOINTS |
//ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints0 ); |
//#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScreenCtrl_Push( OSDSCREEN_ELECTRIC , strGet(STR_OSDSCREEN_ELECTRIC) , &OSD_Screen_Electric ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScreenCtrl_Push( OSDSCREEN_MKSTATUS , strGet(STR_OSDSCREEN_MKSTATUS) , &OSD_Screen_MKStatus ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScreenCtrl_Push( OSDSCREEN_USERGPS , strGet(STR_OSDSCREEN_USERGPS) , &OSD_Screen_UserGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScreenCtrl_Push( OSDSCREEN_3DLAGE , strGet(STR_OSDSCREEN_3DLAGE) , &OSD_Screen_3DLage ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScreenCtrl_Push( OSDSCREEN_STATISTICS , strGet(STR_OSDSCREEN_STATISTIC) , &OSD_Screen_Statistics ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScreenCtrl_Push( OSDSCREEN_OSD0 , strGet(STR_OSDSCREEN_OSD0) , &OSD_Screen_OSD0 ); |
ScreenCtrl_Push( OSDSCREEN_OSD1 , strGet(STR_OSDSCREEN_OSD1) , &OSD_Screen_OSD1 ); |
ScreenCtrl_Push( OSDSCREEN_OSD2 , strGet(STR_OSDSCREEN_OSD2) , &OSD_Screen_OSD2 ); |
#endif |
#ifdef USE_OSD_SCREEN_DEBUG |
ScreenCtrl_Push( 0 , PSTR("Debug") , &OSD_Screen_Debug ); |
ScreenCtrl_Push( 0 , PSTR("Debug-RX") , &OSD_Screen_Debug_RX ); |
#endif |
ScreenCtrl_Set( Config.OSD_ScreenMode ); |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
timer_mk_timeout = MK_TIMEOUT; |
timer_osd_refresh = TIME_OSD_REFRESH; |
timer_get_bldata = 0; |
timer_get_tidata = 0; |
timer_get_displaydata = 0; |
//-------------------------------- |
// OSD Main-Loop |
//-------------------------------- |
while( !osdexit ) |
{ |
//################################ |
//# Empfange/verarbeite: OSD-Daten |
//################################ |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
Decode64(); |
memcpy( &osdData, (const void *)pRxData, sizeof(NaviData_t) ); // sichern in: osdData |
naviData = &osdData; |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD++; // gelesene Datenpakete |
#endif |
mktimeout = false; |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
//---------------------------------- |
// Winkel zu Home |
//---------------------------------- |
calc_heading_home(); |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
GPS_Pos_Save( &GPS_Current ); |
Config.LastLatitude = GPS_Current.GPSData.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = GPS_Current.GPSData.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// PKT Fehler reset |
//---------------------------------- |
if( old_PKTErrorcode == 40 ) // 40 = PKT Empfangsausfall ("PKT RX lost") |
{ |
// PKT-Verbindungsfehler zuruecksetzen |
// da an dieser Stelle ja bereits wieder ein gueltiges Datenpaket |
// von der NaviCtrl empfangen wurde |
old_PKTErrorcode = 0; |
clear_key_all(); |
} |
//---------------------------------- |
// remember statistics (only when engines running) |
//---------------------------------- |
#ifdef DEBUG_OSD_STAT_MOTORRUN |
if( true ) |
#else |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) // AM FLIEGEN -> Statistik aufzeichnen |
#endif |
{ |
flying = true; |
// --- gueltige Zeit von der NC vorhanden und noch keine Start-Zeit gesetzt? |
if( UTCTime.year != 0 && Config.OSD_Statistic.begin_StatTime.year == 0 ) |
{ |
memcpy( &Config.OSD_Statistic.begin_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Start Zeit/Datum sichern... |
} |
Config.OSD_Statistic.last_FlyTime = naviData->FlyingTime; |
Config.OSD_Statistic.count_osd++; // Anzahl OSD-Statistik Pakete |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.avg_Current = (uint16_t)calc_avg( (int32_t)Config.OSD_Statistic.avg_Current, |
(int32_t)naviData->Current, |
(int32_t)Config.OSD_Statistic.count_osd, |
(int32_t)100 ); |
if( naviData->Altimeter > Config.OSD_Statistic.max_Altimeter ) Config.OSD_Statistic.max_Altimeter = naviData->Altimeter; |
if( naviData->GroundSpeed > Config.OSD_Statistic.max_GroundSpeed ) Config.OSD_Statistic.max_GroundSpeed = naviData->GroundSpeed; |
if( naviData->HomePositionDeviation.Distance > Config.OSD_Statistic.max_Distance ) Config.OSD_Statistic.max_Distance = naviData->HomePositionDeviation.Distance; |
if( naviData->Current > Config.OSD_Statistic.max_Current ) Config.OSD_Statistic.max_Current = naviData->Current; |
if( naviData->UsedCapacity > Config.OSD_Statistic.max_Capacity ) Config.OSD_Statistic.max_Capacity = naviData->UsedCapacity; |
if( naviData->UBat < Config.OSD_Statistic.min_UBat ) Config.OSD_Statistic.min_UBat = naviData->UBat; |
if( naviData->TopSpeed > Config.OSD_Statistic.max_TopSpeed ) Config.OSD_Statistic.max_TopSpeed = naviData->TopSpeed; |
if( naviData->RC_Quality > Config.OSD_Statistic.max_RCQuality ) Config.OSD_Statistic.max_RCQuality = naviData->RC_Quality; |
if( naviData->RC_Quality < Config.OSD_Statistic.min_RCQuality ) Config.OSD_Statistic.min_RCQuality = naviData->RC_Quality; |
if( naviData->AngleNick < Config.OSD_Statistic.min_AngleNick ) Config.OSD_Statistic.min_AngleNick = naviData->AngleNick; |
if( naviData->AngleNick > Config.OSD_Statistic.max_AngleNick ) Config.OSD_Statistic.max_AngleNick = naviData->AngleNick; |
if( naviData->AngleRoll < Config.OSD_Statistic.min_AngleRoll ) Config.OSD_Statistic.min_AngleRoll = naviData->AngleRoll; |
if( naviData->AngleRoll > Config.OSD_Statistic.max_AngleRoll ) Config.OSD_Statistic.max_AngleRoll = naviData->AngleRoll; |
} |
else if( flying && UTCTime.year != 0 ) // GELANDET -> Statistik beenden |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
flying = false; |
} |
//----------------------- |
// Check: Akku Warnung |
//----------------------- |
CheckMKLipo(); |
//---------------------------------- |
// Show: OSD-Screens |
//---------------------------------- |
if( popup_state == OSD_POPUP_NONE && (timer_osd_refresh == 0 || OSDScreenRefresh == OSD_SCREEN_REDRAW) ) |
{ |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) lcd_cls(); |
if( !mkdisplayMode ) |
ScreenCtrl_Show(); |
else |
OSD_Screen_MKDisplay(); |
timer_osd_refresh = TIME_OSD_REFRESH; |
} |
OSDScreenRefresh = OSD_SCREEN_REFRESH; |
//---------------------------------- |
// Check: MK-Error |
//---------------------------------- |
if( naviData->Errorcode != old_MKErrorcode && naviData->Errorcode <= MAX_MKERROR_NUM ) |
{ |
if( naviData->Errorcode > 0 ) // raise Error |
{ |
// Fehler aufzeichnen |
MkError_Save( naviData->Errorcode ); |
// Fehler Anzeigen |
OSD_Popup_MKError( naviData->Errorcode ); |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error (MK-Error vorhanden) |
timer2 = TIME_POPUP_MKERROR; |
popup_state = OSD_POPUP_MKERROR; |
} |
else // reset Error |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
old_MKErrorcode = naviData->Errorcode; |
} |
rxd_buffer_locked = FALSE; // ready to receive new data |
//------------------------------------------- |
// hole weitere Daten vom MK (BL, Time, ...) |
// |
// muss am Schluss stehen da naviData-Buffer |
// dabei ueberschrieben wird! |
//------------------------------------------- |
OSD_MK_GetData(); // holt BL-Daten und NC-Zeit |
timer_mk_timeout = MK_TIMEOUT; |
} //end: if( rxd_buffer_locked ) // OSD-Daten |
//################################ |
//# der Rest... |
//################################ |
//-------------------------------- |
// TASTEN: MK-Timeout |
//-------------------------------- |
if( mktimeout ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
} |
//-------------------------------- |
// Popup beenden |
// wenn irgendeine Taste gedrückt oder Popup-Timeout |
//-------------------------------- |
if( !mktimeout && (popup_state != OSD_POPUP_NONE) && (get_key_press(255) || !timer2) ) // get_key_press(255) == alles an Tasten abfangen was moeglich ist |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
clear_key_all(); |
} |
//-------------------------------- |
// TASTEN: KEIN mkdisplay (OSD Modus) |
//-------------------------------- |
if( !osdexit && !mktimeout && !mkdisplayMode ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
if( get_key_long(1 << KEY_ESC) ) // ÊINSCHALTEN: mkdisplayMode |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = true; |
mkdisplayCmd = 0; // lesen display-Daten erzwingen |
} |
if( get_key_long (1 << KEY_ENTER) ) // User GPS-Position sichern |
{ |
GPS_User_Save(); |
} |
if( get_key_short (1 << KEY_ENTER) ) // Popup: Info |
{ |
if( popup_state == OSD_POPUP_NONE ) |
{ |
popup_state = OSD_POPUP_INFO; |
OSD_Popup_Info( ScreenCtrl_GetNum(), ScreenCtrl_GetName() ); |
timer2 = TIME_POPUP_INFO; |
} |
} |
if( get_key_press (1 << KEY_MINUS)) // previous screen |
{ |
ScreenCtrl_Previous(); |
} |
if( get_key_press (1 << KEY_PLUS)) // next Screen |
{ |
ScreenCtrl_Next(); |
} |
} |
//-------------------------------- |
// TASTEN: mkdisplay AKTIV |
//-------------------------------- |
if( !osdexit && !mktimeout && mkdisplayMode ) |
{ |
/* |
if( get_key_long(1 << KEY_ENTER) // ABSCHALTEN mkdisplayMode: longpress ENTER, ESC, MINUS, PLUS schaltet mkdisplay aus |
|| get_key_long(1 << KEY_ESC) |
|| get_key_long(1 << KEY_MINUS) |
|| get_key_long(1 << KEY_PLUS) ) |
{ |
*/ |
if( get_key_long(1 << KEY_ESC) ) // ABSCHALTEN mkdisplayMode: longpress ESC (Taste 3 von links) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep beim Modus-Wechsel |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = false; |
clear_key_all(); |
} |
if( get_key_short (1 << KEY_MINUS) ) mkdisplayCmd = 0xfe; // MK-Key: rechts (next page) |
if( get_key_short (1 << KEY_PLUS) ) mkdisplayCmd = 0xfd; // MK-Key: links (previous page) |
if( get_key_short (1 << KEY_ESC) ) mkdisplayCmd = 0xfb; // MK-Key: runter |
if( get_key_short (1 << KEY_ENTER) ) mkdisplayCmd = 0xf7; // MK-Key: hoch |
if( mkdisplayCmd != 0xff ) // wenn eine MK-Display Taste gedrueckt worde sofort Daten |
{ // holen und darstellen um Anzeigereaktion fuer Benutzer zu verbessern |
timer_get_displaydata = 0; // lesen erzwingen |
OSD_MK_GetData(); // Daten holen |
OSD_Screen_MKDisplay(); // MK-Display Anzeigen |
} |
} |
//----------------------- |
// abo_timer |
//----------------------- |
if( abo_timer == 0 ) |
{ |
OSD_MK_Connect( MK_ABORENEW ); |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
OSD_MK_Connect( MK_CONNECT ); // 21.06.2014 OG: wieder aktviert wegen Umschaltung auf NC |
} |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
#ifdef USE_OSD_PKTHOOK |
if( PKT_CtrlHook() ) OSDScreenRefresh = OSD_SCREEN_REDRAW; // Update vom Updatetool angefordert? |
#endif |
} // END: while( !osdexit ) |
//------------------------------------------ |
// PKT-OSD beenden |
//------------------------------------------ |
Config.OSD_ScreenMode = ScreenCtrl_GetNum(); // merken letzter Screen |
//------------------------------------------ |
// ggf. Statistik Ende Zeit/Datum sichern |
//------------------------------------------ |
if( flying && UTCTime.year != 0 ) // wenn noch am fliegen |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
} |
OSD_active = false; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osd.h |
---|
0,0 → 1,309 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osd.h |
//# |
//# 21.06.2014 OG |
//# - add: writex_altimeter() |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: weitere Exporte von Funktionen draw_icon...() |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - add: OSD_Element_Battery_Bar() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# |
//# 26.05.2014 OG |
//# - add: #define OSDSCREEN_WAYPOINTS |
//# |
//# 24.05.2014 OG |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 28.04.2014 OG |
//# - del: OSD_Timeout() |
//# |
//# 07.07.2013 OG |
//# - add: defines fuer Screen-ID's (verwendet in setup.c, osd.c) |
//# |
//# 30.06.2013 OG |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# |
//# 31.05.2013 OG |
//# Achtung! Aenderung eeprom-Kompatibilitaet wegen typedef Erweiterung! |
//# - chg: typedef: osd_statistic_BL_t fuer Mittelwerte |
//# - chg: typedef: osd_statistic_t fuer Mittelwerte |
//# |
//# 15.05.2013 OG |
//# - add: calc_BLmax() |
//# - add: struct osd_BLmax_t |
//# |
//# 04.05.2013 OG |
//# - chg: aktualisiert Kommentare in 'osd_statistic_t' |
//# - del: OSD_Debug_XX() |
//# |
//# 02.05.2013 OG |
//# - fix: struct osd_statistic_t: max_Distance von int16_t nach uint16_t |
//# |
//# 28.04.2013 OG |
//# - chg: osd(OSDMode) -> osd(void) |
//# - del: OSDDATA_Statistics() |
//# |
//# vorheriges: siehe osd.c |
//############################################################################ |
#ifndef _OSD_H |
#define _OSD_H |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#define OSD_MAX_MOTORS 8 // max. Anzahl vom PKT unterstuetzer Motoren (absolut MAX ist 12 da der MK nicht mehr unterstuetzt) |
#define MAX_GPS_USER 10 // max. Anzahl der GPS-Positionen durch Benutzer gespeichert |
#define MAX_MKERR_LOG 20 // max. Anzahl Eintraege im MK-Error-Log |
#define OSD_SCREEN_REFRESH 0 // Screen: Werte anzeigen |
#define OSD_SCREEN_REDRAW 1 // Screen: Labels und statischer Elemente neu zeichen, Werte anzeigen |
#define ORIENTATION_V 1 // fuer OSD_Element_Battery_Bar() |
#define ORIENTATION_H 2 |
// OSD-Screen ID's |
// maximal: 31 (!) wegen Bitcodierung in Config.OSD_UseScreen |
#define OSDSCREEN_GENERAL 0 |
#define OSDSCREEN_NAVIGATION 1 |
#define OSDSCREEN_ELECTRIC 2 |
#define OSDSCREEN_MKSTATUS 3 |
#define OSDSCREEN_USERGPS 4 |
#define OSDSCREEN_3DLAGE 5 |
#define OSDSCREEN_STATISTICS 6 |
#define OSDSCREEN_OSD0 7 |
#define OSDSCREEN_OSD1 8 |
#define OSDSCREEN_OSD2 9 |
#define OSDSCREEN_WAYPOINTS 10 |
// Flags |
#define OSD_FLAG_AH 0 // Altitue Hold |
#define OSD_FLAG_PH 1 // Position Hold |
#define OSD_FLAG_CF 2 // Care Free |
#define OSD_FLAG_CH 3 // Coming Home |
#define OSD_FLAG_O1 4 // Out1 (LED 1) |
#define OSD_FLAG_O2 5 // Out2 (LED 2) |
#define OSD_FLAG_BA 6 // LowBat warning (MK) |
#define OSD_FLAG_CA 7 // Calibrate |
#define OSD_FLAG_ST 8 // Start |
#define OSD_FLAG_MR 9 // Motor Run |
#define OSD_FLAG_FY 10 // Fly |
#define OSD_FLAG_EL 11 // Emergency Landing |
#define OSD_FLAG_FS 12 // RS Failsave Active |
#define OSD_FLAG_GP 13 // GPS ok |
#define OSD_FLAG_S0 14 // GPS-Sat not ok (GPS NOT ok) |
#define OSD_FLAG_TU 15 // Vario Trim Up |
#define OSD_FLAG_TD 16 // Vario Trim Down |
#define OSD_FLAG_FR 17 // Free |
#define OSD_FLAG_RL 18 // Range Limit |
#define OSD_FLAG_SL 19 // No Serial Link |
#define OSD_FLAG_TR 20 // Target Reached |
#define OSD_FLAG_MC 21 // Manual Control |
#define OSD_FLAG_COUNT 22 |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
//----------------------------------------------------------- |
// typedef: Statistiken |
//----------------------------------------------------------- |
typedef struct |
{ |
uint16_t count; // Anzahl Werte BL-Daten (fuer Mittelwertberechnung) |
uint8_t max_Current; // in 0.1 A steps |
uint16_t avg_Current; // Mittelwert Current (*100 fuer Rechengenauigkeit) |
uint8_t max_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_statistic_BL_t; |
typedef struct |
{ |
PKTdatetime_t begin_StatTime; // Datum/Zeit |
PKTdatetime_t end_StatTime; // Datum/Zeit |
uint16_t total_FlyTime; // gesamt Flugzeit seit Stat-Init |
uint16_t last_FlyTime; // letzte Flugzeit |
uint16_t count_osd; // TODO: Anzahl Werte OSD-Daten (fuer Mittelwertberechnung) |
uint16_t count_Errorcode; // TODO: Anzahl gemeldeter MK-Errors |
int16_t max_Altimeter; // max. Hoehe |
int16_t avg_Altimeter; // TODO: Mittelwert Hoehe () |
s16 max_Variometer; // TODO: ... |
uint16_t max_GroundSpeed; // max. Geschwindigkeit |
uint16_t avg_GroundSpeed; // TODO: Mittelwert Geschwindigkeit () |
s16 max_TopSpeed; // max. velocity in vertical direction in cm/s |
uint16_t max_Distance; // max. Entfernung |
uint16_t avg_Distance; // TODO: Mittelwert Entfernung () |
uint16_t max_Current; // max. Strom |
uint16_t avg_Current; // Mittelwert Strom () |
u8 max_RCQuality; // max. Empfangsqualitaet |
u8 min_RCQuality; // min. Empfangsqualitaet |
uint16_t avg_RCQuality; // TODO: Mittelwert Empfangsqualitaet () |
uint16_t max_Capacity; // max. entnommene Kapazitaet |
s8 max_AngleNick; // max. Nick |
s8 min_AngleNick; // min. Nick |
s8 max_AngleRoll; // max. Roll |
s8 min_AngleRoll; // min. Roll |
uint8_t min_UBat; // min. Spannung (V) |
uint8_t LiPoCells; // Anzahl der LiPo Zellen |
uint8_t BL_Count; // Anzahl erkannter BL's (Motoren) |
osd_statistic_BL_t BL[OSD_MAX_MOTORS]; // Werte der einzelnen BL's |
} osd_statistic_t; |
typedef struct |
{ |
uint8_t max_BL_Current_Index; // BL-Nummer |
unsigned char max_BL_Current; // in 0.1 A steps |
uint8_t max_BL_Temp_Index; // BL-Nummer |
unsigned char max_BL_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_BLmax_t; |
//----------------------------------------------------------- |
// typedef: Aufzeichnung von MK-Errors |
//----------------------------------------------------------- |
typedef struct |
{ |
u8 Errorcode; // 0 --> okay |
PKTdatetime_t set_Time; // Datum/Zeit |
PKTdatetime_t clear_Time; // Datum/Zeit |
} mkerror_t; |
//----------------------------------------------------------- |
// typedef: PKT GPS-Positionen |
//----------------------------------------------------------- |
typedef struct |
{ |
PKTdatetime_t timestamp; // Zeitstempel: UTC |
s16 Altimeter; // barymetrische Hoehe (entspricht: naviData->Altimeter) |
u16 HomeDistance; // distance to home in cm (entspricht: naviData->HomePositionDeviation.Distance) |
GPS_Pos_t GPSData; // GPS-Position (mk-data-structs.h) |
} pkt_gpspos_t; |
//----------------------------------------------------------- |
// global var's |
//----------------------------------------------------------- |
extern volatile uint8_t OSD_active; |
extern volatile uint8_t error; |
//----------------------------------------------------------- |
// strings |
//----------------------------------------------------------- |
extern const char * const mkerrortext[]; |
//----------------------------------------------------------- |
// Funktionen |
//----------------------------------------------------------- |
void osd( void ); |
void vario_beep_output (void); |
void CheckMKLipo(void); |
void STAT_Init(void); |
void GPS_User_Init(void); |
void MKErr_Log_Init(void); |
void calc_BLmax( osd_BLmax_t *blmax ); |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ); |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Capacity( uint8_t x, uint8_t y ); |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ); |
void OSD_Element_Current( uint8_t x, uint8_t y ); |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ); |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ); |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ); |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ); |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ); |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ); |
void OSD_Element_Manuell( uint8_t x, uint8_t y ); |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ); |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Variometer( uint8_t x, uint8_t y ); |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ); |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ); |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ); |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation); |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void MKLiPoCells_Init( void ); |
void MKLiPoCells_Check( void ); |
void draw_icon_satmini( uint8_t x, uint8_t y); |
void draw_icon_satmini2( uint8_t x, uint8_t y); |
void draw_icon_home( uint8_t x, uint8_t y); |
void draw_icon_target_diamond( uint8_t x, uint8_t y); |
void draw_icon_target_round( uint8_t x, uint8_t y); |
void draw_icon_mk( uint8_t x, uint8_t y); |
void OSD_MK_ShowTimeout( void ); |
//----------------------------------------------------------- |
// EXPORTS NUR FUER osdold_screens.c |
//----------------------------------------------------------- |
extern NaviData_t *naviData; |
extern uint8_t OSDScreenRefresh; |
#endif // _OSD_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osddata.c |
---|
0,0 → 1,838 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.c |
//# |
//# 14.06.2014 OG |
//# - chg: Menu_OSDData() - Bezeichnung von "OSD Daten" und "BL Daten" geaendert |
//# zu "OSD Statistik" und "BL Statistik" |
//# |
//# 11.05.2014 OG |
//# - chg: Text von SHOWMKERROR_de, SHOWMKERROR_en etwas gekuerzt |
//# - chg: Titel von Scrollbox-Listen angepasst |
//# - chg: Menu_OSDData() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: Menu_OSDData() del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - chg: ask_delete_data() - Layout und Optimierung |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# - del: OSDDATA_ShowLastGPS() |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 24.01.2014 OG |
//# - fix: OSDDATA_ClearAllData(): Beep wenn Daten geloescht werden |
//# (wie bei den anderen Löschungen) |
//# |
//# 30.06.2013 OG |
//# - add: Gesamtstrom-Mittelwert in OSDDATA_Statistics() und OSDDATA_BLStatistics() |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - chg: Funktionsnamen geaendert |
//# |
//# 19.06.2013 OG |
//# - add: last Flytime in OSDDATA_Statistics() |
//# - chg: Reihenfolge in OSDDATA_Statistics() und zusaetzliche Trennline |
//# - add: max Current (Gesamt) in OSDDATA_Statistics() |
//# |
//# 16.06.2013 OG |
//# - fix: "min. Voltage" Anzeige in OSDDATA_Statistics() (war veschoben) |
//# |
//# 13.06.2013 OG |
//# - chg: Uebersetzungen von CB in ask_delete_data() aufgenommen |
//# - chg: GPS-Menuetitel geaendert |
//# - fix: include pkt.h |
//# |
//# 11.06.2013 OG |
//# - chg: ask_delete_data() erweitert um Info was geloescht wird & Layout (TODO: Uebersetzungen) |
//# - add: Mittelwertanzeige fuer BL-Strom |
//# - add: Anzahl BL-Statistik-Datenpakete (nur wenn USE_OSD_SCREEN_DEBUG) |
//# - add: OSDDATA_BLStatistic() - die BL-Daten wurden in eigene Anzeigefunktion verschoben |
//# - add: OSDDATA_ClearAllData() - alle Daten loeschen |
//# - add: OSDDATA_ShowLastGPS() - letzte GPS-Position anzeigen |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - chg: PKT_CtrlHook() noch an einigen Stellen eingefuegt |
//# - fix: ctrl_osddata() clear screen vor ScrollBox_Refresh |
//# |
//# 15.05.2013 OG |
//# - chg: OSDDATA_Statistics() umgestellt auf calc_BLmax() (aus osd.c) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - add: weitere Anzeigen in OSD_Statistics() |
//# - add: Content in OSDDATA_UserGPS() und OSDDATA_MkError() |
//# |
//# 28.04.2013 OG |
//# - chg: auf die neuen Features von xprintf angepasst (siehe utils/xstring.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: Texte "Datenlöschen" in messages.c aufgenommen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../osd/osd.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../mk/mkbase.h" |
#define INIT_STATISTIC 1 |
#define INIT_MKERROR 2 |
#define INIT_USERGPS 3 |
#define INIT_LASTPOS 4 |
#define INIT_ALLDATA 5 |
//----------------------- |
// Menu_OSDDATA |
//----------------------- |
#define ID_SHOWSTATISTIK 1 |
#define ID_SHOWBLSTATISTIK 2 |
#define ID_SHOWMKERROR 3 |
#define ID_SHOWUSERGPS 4 |
#define ID_SHOWLASTGPS 5 |
#define ID_CLEARALLDATA 6 |
//static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Daten"; |
//static const char SHOWSTATISTIK_en[] PROGMEM = "OSD data"; |
//static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Daten"; |
//static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL data"; |
static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Statistik"; |
static const char SHOWSTATISTIK_en[] PROGMEM = "OSD statistics"; |
static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Statistik"; |
static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL statistics"; |
static const char SHOWMKERROR_de[] PROGMEM = "MK Fehler"; |
static const char SHOWMKERROR_en[] PROGMEM = "MK errors"; |
static const char SHOWUSERGPS_de[] PROGMEM = "GPS Userdaten"; |
static const char SHOWUSERGPS_en[] PROGMEM = "GPS userdata"; |
static const char SHOWLASTGPS_de[] PROGMEM = "GPS letzte Pos."; |
static const char SHOWLASTGPS_en[] PROGMEM = "GPS last pos."; |
static const char CLEARALLDATA_de[] PROGMEM = "LÖSCHE alle Daten"; |
static const char CLEARALLDATA_en[] PROGMEM = "DELETE all data"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t ask_delete_data( uint8_t initcode ) |
{ |
uint8_t refresh = true; |
clear_key_all(); |
set_beep ( 60, 0xffff, BeepNormal); |
while( true ) |
{ |
//-------------------------------------- |
// screen refresh |
//-------------------------------------- |
if( refresh ) |
{ |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill Black |
lcdx_printp_at( 1, 0, strGet(DELETE), MINVERS, 0,0); // Titel |
show_Lipo(); // LiPo anzeigen |
lcd_rect_round( 0, 20, 127, 27, 1, R2); // Rahmen fuer Benutzerfrage |
switch( initcode ) |
{ |
case INIT_STATISTIC: lcdx_printp_center( 2, strGet(STAT_OSDBL), MNORMAL, 0,9); break; |
case INIT_MKERROR : lcdx_printp_center( 2, strGet(STAT_ERROR), MNORMAL, 0,9); break; |
case INIT_USERGPS : lcdx_printp_center( 2, strGet(STAT_GPS) , MNORMAL, 0,9); break; |
case INIT_LASTPOS : lcdx_printp_center( 2, strGet(STAT_POS) , MNORMAL, 0,9); break; |
case INIT_ALLDATA : lcdx_printp_center( 2, strGet(STAT_ALL) , MNORMAL, 0,9); break; |
} |
lcdx_printp_center( 4, strGet(DELETE), MNORMAL, 0,4); // "löschen?" |
lcd_printp_at( 0, 7, strGet(START_LASTPOS2) , MNORMAL); // Keyline: "löschen Nein" |
refresh = false; |
} |
//-------------------------------------- |
// Update vom Updatetool angefordert? |
//-------------------------------------- |
if( PKT_CtrlHook() ) |
{ |
refresh = true; |
} |
//-------------------------------------- |
// loeschen |
//-------------------------------------- |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
clear_key_all(); |
return true; // Rueckgabe: true = loeschen |
} |
//-------------------------------------- |
// Ende |
//-------------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return false; // Rueckgabe: false = nicht loeschen |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ctrl_osddata( uint8_t isdata, uint8_t initcode ) |
{ |
uint8_t key = 0; |
if( !isdata ) ScrollBox_Push_P( MNORMAL, PSTR(" no data...") ); |
ScrollBox_SetKey( KEY_ENTER, " Del" ); |
do |
{ |
key = ScrollBox_Show(); |
if( key == KEY_ENTER ) |
{ |
if( ask_delete_data(initcode) ) |
{ |
switch( initcode ) |
{ |
case INIT_STATISTIC: STAT_Init(); break; |
case INIT_MKERROR: MKErr_Log_Init(); break; |
case INIT_USERGPS: GPS_User_Init(); break; |
} |
set_beep ( 400, 0xffff, BeepNormal); |
key = 0; // exit |
} |
else |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
} |
} while( key ); |
} |
//-------------------------------------------------------------- |
// add_statistic_head() |
// |
// fuegt Basisdaten den OSD- und BL-Listen hinzu |
//-------------------------------------------------------------- |
void add_statistic_head( void ) |
{ |
PKTdatetime_t dtlocal; |
//--------------------------- |
// begin: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.begin_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("Begin: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
//lcd_printp_at( 3, 3, strGet(DELETEDATA), MNORMAL); // "Daten loeschen?" |
dtlocal.day, dtlocal.month |
); |
//--------------------------- |
// end: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.end_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("End: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
dtlocal.day, dtlocal.month |
); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete OSD-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_Statistics( void ) |
{ |
int16_t v; |
osd_BLmax_t blmax; |
lcd_cls(); |
if( !ScrollBox_Create(25) ) |
return; // kein RAM mehr |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" OSD Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// last Flytime |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Last Fly %02u:%02u min"), (Config.OSD_Statistic.last_FlyTime/60), (Config.OSD_Statistic.last_FlyTime%60) ); |
//--------------------------- |
// max. Altitude |
//--------------------------- |
v = Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust); |
ScrollBox_Push_P( MNORMAL, PSTR("%cAltitude%7d m"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Distance |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cDistance%7u m"), SYMBOL_MAX, Config.OSD_Statistic.max_Distance/10 ); |
//--------------------------- |
// max. Ground-Speed |
//--------------------------- |
v = (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 90) / (uint32_t) 250); |
ScrollBox_Push_P( MNORMAL, PSTR("%cGrnd.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Vert.-Speed |
//--------------------------- |
v = Config.OSD_Statistic.max_TopSpeed; |
v = (v*36/100); // cm/s -> km/h*10 |
ScrollBox_Push_P( MNORMAL, PSTR("%cVert.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// Used Capacity |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Capacity%7d mAh"), Config.OSD_Statistic.max_Capacity ); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR(" BL Detected%4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Curr.%4.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC"), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//--------------------------- |
// min. Voltage |
//--------------------------- |
v = (Config.OSD_Statistic.min_UBat == 255) ? 0 : Config.OSD_Statistic.min_UBat; |
ScrollBox_Push_P( MNORMAL, PSTR("%cVoltage%6.1d V"), SYMBOL_MIN, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// max. RC-Quality |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MAX, Config.OSD_Statistic.max_RCQuality ); |
//--------------------------- |
// min. RC-Quality |
//--------------------------- |
v = (Config.OSD_Statistic.min_RCQuality==255) ? 0 : Config.OSD_Statistic.min_RCQuality; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MIN, v ); |
//--------------------------- |
// max. Nick |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleNick, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Nick |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleNick==126) ? 0 : Config.OSD_Statistic.min_AngleNick; |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// max. Roll |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleRoll, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Roll |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleRoll==126) ? 0 : Config.OSD_Statistic.min_AngleRoll; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete BL-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_StatisticsBL( void ) |
{ |
uint8_t i; |
osd_BLmax_t blmax; |
lcd_cls(); |
#ifdef USE_OSD_SCREEN_DEBUG |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*6)) ) |
return; // kein RAM mehr |
#else |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*4)) ) |
return; // kein RAM mehr |
#endif |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR("BL Detected %4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Current%2.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC" ), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// LIST: BL-Data |
//--------------------------- |
if( Config.OSD_Statistic.BL_Count > 0 ) |
{ |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
for( i=0; i < Config.OSD_Statistic.BL_Count; i++) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("%1d:%cCurrent%4.1u A"), (i+1), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Current) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cCurrent%4.1u A" ), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.BL[i].avg_Current/100) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cTemp.%8u %cC" ), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Temp), SYMBOL_BIGDEGREE ); |
#ifdef USE_OSD_SCREEN_DEBUG |
ScrollBox_Push_P( MNORMAL, PSTR(" RX-Pkg.%7u"), Config.OSD_Statistic.BL[i].count ); |
//ScrollBox_Push_P( MNORMAL, PSTR(" MC%12u A" ), Config.OSD_Statistic.BL[i].avg_Current ); |
#endif |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
} |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete Benutzer-GPS-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_UserGPS( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_GPS_USER*4) + 5 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" GPS Userdata") ); |
for( i=0; i<MAX_GPS_USER; i++) |
{ |
if( Config.GPS_User[i].GPSData.Latitude != 0) // nur gueltige GPS-Koordinaten anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Nummer / Datum / Zeit |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.GPS_User[i].timestamp ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
dtlocal.day, dtlocal.month, dtlocal.year, |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60) |
); |
/* |
sec = (uint16_t)(Config.GPS_User[i].timestamp.seconds); // Sekunden |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
Config.MKErr_Log[i].set_Time.day, Config.MKErr_Log[i].set_Time.month, Config.MKErr_Log[i].set_Time.year, |
(uint16_t)(sec/3600), (uint16_t)(sec/60%60), (uint16_t)(sec%60) |
); |
// Alternative Anzeige von Datum/Zeit: |
min = (uint16_t)(Config.GPS_User[i].timestamp.seconds/60); // Minuten |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%02u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year%100 |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%04u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u.%02u.%04u %02u:%02u"), i+1, |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year, |
(uint16_t)(min/60), (uint16_t)(min%60) |
); |
*/ |
//----------------------------------- |
// Zeile 2: Hoehe (GPS/Barymetrisch) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%7d mG%5.1d mB"), |
(Config.GPS_User[i].GPSData.Altitude/1000), // GPS-Hoehe in m |
(Config.GPS_User[i].Altimeter / (3 / AltimeterAdjust)) ); // Barymetrische-Hoehe in dm |
//----------------------------------- |
// Zeile 3: Koordinaten (Lat/Long) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%3.6ld%3.6ld"), |
Config.GPS_User[i].GPSData.Latitude/10, |
Config.GPS_User[i].GPSData.Longitude/10 ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_USERGPS ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_MkError() |
// |
// zeigt aufgezeichnete MK-Fehler an |
//-------------------------------------------------------------- |
void OSDDATA_MkError( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_MKERR_LOG*4) + 3 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" MK Errors") ); |
for( i=0; i<MAX_MKERR_LOG; i++) |
{ |
if( Config.MKErr_Log[i].Errorcode != 0) // nur vorhandene Errorcodes anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Zeit / Datum |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.MKErr_Log[i].set_Time ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u:%02u:%02u %02u.%02u.%04u"), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60), |
dtlocal.day, dtlocal.month, dtlocal.year |
); |
//----------------------------------- |
// Zeile 2: Error Code |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Code %02u"), Config.MKErr_Log[i].Errorcode ); |
//----------------------------------- |
// Zeile 3: Error Text |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%20S"), pgm_read_word(&mkerrortext[Config.MKErr_Log[i].Errorcode]) ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_MKERROR ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_ShowLastGPSPosition() |
// |
// zeigt die letzte aufgezeichnete GPS-Position an. |
// Möglichkeit zum loeschen der GPS-Pos ist vorhanden. |
//-------------------------------------------------------------- |
void OSDDATA_ShowLastGPSPosition( void ) |
{ |
uint8_t redraw = true; |
clear_key_all(); |
while( true ) |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearcsreen) |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,4); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,4); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+4, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,10); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,10); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+7, 127, (2*8)+4, 1); // Rahmen |
//lcd_printp_at(0, 7, strGet(START_LASTPOS2), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(KEYLINE4) , MNORMAL); // Keyline |
lcd_printp_at(18, 7, PSTR("Del") , MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press (1 << KEY_ESC) ) // Ende |
{ |
break; |
} |
if( (get_key_press (1 << KEY_ENTER)) ) // Del |
{ |
if( ask_delete_data(INIT_LASTPOS) ) |
{ |
set_beep ( 400, 0xffff, BeepNormal); |
WriteLastPosition( 0x00000000, 0x00000000 ); // letzte GPS Position loeschen |
break; // und beenden |
} |
redraw = true; |
} |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// OSDDATA_ClearAllData() |
// |
// löscht alle erhobenen Daten |
//-------------------------------------------------------------- |
void OSDDATA_ClearAllData( void ) |
{ |
if( ask_delete_data( INIT_ALLDATA ) ) |
{ |
STAT_Init(); |
MKErr_Log_Init(); |
GPS_User_Init(); // loeschen: User GPS |
WriteLastPosition( 0x00000000, 0x00000000 ); // loeschen: letzte GPS Position |
set_beep ( 400, 0xffff, BeepNormal); |
} |
} |
//-------------------------------------------------------------- |
// Menue fuer osddata |
//-------------------------------------------------------------- |
#ifdef USE_OSDDATA |
void Menu_OSDData( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Daten" |
//MenuCtrl_SetTitle_P( PSTR("OSD Daten") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( ID_SHOWSTATISTIK , MENU_ITEM, &OSDDATA_Statistics, SHOWSTATISTIK_de , SHOWSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWBLSTATISTIK, MENU_ITEM, &OSDDATA_StatisticsBL, SHOWBLSTATISTIK_de, SHOWBLSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWMKERROR , MENU_ITEM, &OSDDATA_MkError, SHOWMKERROR_de , SHOWMKERROR_en ); |
MenuCtrl_PushML2_P( ID_SHOWUSERGPS , MENU_ITEM, &OSDDATA_UserGPS, SHOWUSERGPS_de , SHOWUSERGPS_en ); |
MenuCtrl_PushML2_P( ID_SHOWLASTGPS , MENU_ITEM, &OSDDATA_ShowLastGPSPosition, SHOWLASTGPS_de , SHOWLASTGPS_en ); |
MenuCtrl_PushML2_P( ID_CLEARALLDATA , MENU_ITEM, &OSDDATA_ClearAllData, CLEARALLDATA_de , CLEARALLDATA_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osddata.h |
---|
0,0 → 1,38 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.h |
//# |
//# 29.03.2014 OG |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# |
//# 11.06.2013 OG |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - del: verschiedene andere exportierte Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _OSDDATA_H |
#define _OSDDATA_H |
void OSDDATA_ShowLastGPSPosition( void ); |
void Menu_OSDData( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osdold_messages.c |
---|
0,0 → 1,117 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
#include "../eeprom/eeprom.h" |
#include "osdold_messages.h" |
//--------------------------------------------------------------------------------- |
static const char OSD_ALTI_0_de[] PROGMEM = "Höhe aus "; |
static const char OSD_ALTI_0_en[] PROGMEM = "Alti. off "; |
static const char OSD_ALTI_1_de[] PROGMEM = "Höhe begr."; |
static const char OSD_ALTI_1_en[] PROGMEM = "Alti.Limit"; |
static const char OSD_VARIO_0_de[] PROGMEM = "Vario aus "; |
static const char OSD_VARIO_0_en[] PROGMEM = "Vario off "; |
static const char OSD_VARIO_1_de[] PROGMEM = "Vario Höhe"; |
static const char OSD_VARIO_1_en[] PROGMEM = "Vario Alt."; |
static const char OSD_CARE_FREE_0_de[] PROGMEM = " "; |
#define OSD_CARE_FREE_0_en OSD_CARE_FREE_0_de |
static const char OSD_CARE_FREE_1_de[] PROGMEM = "Care Free"; |
#define OSD_CARE_FREE_1_en OSD_CARE_FREE_1_de |
static const char OSD_NAVI_MODE_0_de[] PROGMEM = "Navi aus "; |
static const char OSD_NAVI_MODE_0_en[] PROGMEM = "Navi off "; |
static const char OSD_NAVI_MODE_1_de[] PROGMEM = "Pos. halten"; |
static const char OSD_NAVI_MODE_1_en[] PROGMEM = "Pos. Hold "; |
static const char OSD_NAVI_MODE_2_de[] PROGMEM = "Coming Home"; |
#define OSD_NAVI_MODE_2_en OSD_NAVI_MODE_2_de |
static const char OSD_FLAGS_0_de[] PROGMEM = " "; |
#define OSD_FLAGS_0_en OSD_FLAGS_0_de |
static const char OSD_FLAGS_1_de[] PROGMEM = "Justieren"; |
static const char OSD_FLAGS_1_en[] PROGMEM = "Calibrate"; |
static const char OSD_FLAGS_2_de[] PROGMEM = "Start "; |
#define OSD_FLAGS_2_en OSD_FLAGS_2_de |
static const char OSD_FLAGS_3_de[] PROGMEM = "Betrieb "; |
static const char OSD_FLAGS_3_en[] PROGMEM = "Run "; |
static const char OSD_FLAGS_4_de[] PROGMEM = "Fliegen "; |
static const char OSD_FLAGS_4_en[] PROGMEM = "Fly "; |
static const char OSD_FLAGS_5_de[] PROGMEM = "Landung "; |
static const char OSD_FLAGS_5_en[] PROGMEM = "Landing "; |
static const char OSD_FLAGS_6_de[] PROGMEM = "Akku leer"; |
static const char OSD_FLAGS_6_en[] PROGMEM = "Low Bat. "; |
static const char OSD_LED_Form_de[] PROGMEM = "Out1/2 Format"; |
static const char OSD_LED_Form_en[] PROGMEM = "Out1/2 format"; |
//------------------------------------------------------------------------------ |
const char * const strings_osdold[] PROGMEM= |
{ |
OSD_ALTI_0_de, OSD_ALTI_0_en, |
OSD_ALTI_1_de, OSD_ALTI_1_en, |
OSD_VARIO_0_de, OSD_VARIO_0_en, |
OSD_VARIO_1_de, OSD_VARIO_1_en, |
OSD_CARE_FREE_0_de, OSD_CARE_FREE_0_en, |
OSD_CARE_FREE_1_de, OSD_CARE_FREE_1_en, |
OSD_NAVI_MODE_0_de, OSD_NAVI_MODE_0_en, |
OSD_NAVI_MODE_1_de, OSD_NAVI_MODE_1_en, |
OSD_NAVI_MODE_2_de, OSD_NAVI_MODE_2_en, |
OSD_FLAGS_0_de, OSD_FLAGS_0_en, |
OSD_FLAGS_1_de, OSD_FLAGS_1_en, |
OSD_FLAGS_2_de, OSD_FLAGS_2_en, |
OSD_FLAGS_3_de, OSD_FLAGS_3_en, |
OSD_FLAGS_4_de, OSD_FLAGS_4_en, |
OSD_FLAGS_5_de, OSD_FLAGS_5_en, |
OSD_FLAGS_6_de, OSD_FLAGS_6_en, |
OSD_LED_Form_de, OSD_LED_Form_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
//LAST_STR_de, LAST_STR_en, |
}; |
char const * strGetOSDOLD( int str_no ) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 0 ) return (const char*) pgm_read_word( &strings_osdold[str_no*2] ); |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings_osdold[(str_no*2)+1] ); |
return (const char*) pgm_read_word( &strings_osdold[0] ); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osdold_messages.h |
---|
0,0 → 1,40 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#ifndef OSDOLD_MESSAGES_H |
#define OSDOLD_MESSAGES_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings |
//LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
OSD_ALTI_0, OSD_ALTI_1, OSD_VARIO_0, OSD_VARIO_1, OSD_CARE_FREE_0, OSD_CARE_FREE_1, |
OSD_NAVI_MODE_0, OSD_NAVI_MODE_1, OSD_NAVI_MODE_2, |
OSD_FLAGS_0, OSD_FLAGS_1, OSD_FLAGS_2, OSD_FLAGS_3, OSD_FLAGS_4, OSD_FLAGS_5, OSD_FLAGS_6, |
OSD_LED_Form |
} OSDOLDSTR; |
char const * strGetOSDOLD( int str_no ); |
#endif // USE_OSD_SCREEN_OLD |
#endif /* OSDOLD_MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osdold_screens.c |
---|
0,0 → 1,271 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
#ifdef USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// OSD_Element_AltitudeControl( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ) |
{ |
//--------------------------------------------------------- |
// 10.03.2013 OG: |
// CFG2_HEIGHT_LIMIT im Augenblick nicht unterstuetzt |
// Siehe Anmerkungen in osd() |
//--------------------------------------------------------- |
/* |
if (Flags_ExtraConfig & CFG2_HEIGHT_LIMIT) |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGet(OSD_ALTI_1), 0); // Höhe begr. |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_ALTI_0), 0); // Höhe aus |
} |
else |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
*/ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
//-------------------------------------------------------------- |
// OSD_Element_CareFree( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_CareFree( uint8_t x, uint8_t y ) |
{ |
lcd_printp_at (x, y, strGetOSDOLD( naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE ? OSD_CARE_FREE_1 : OSD_CARE_FREE_0 ), 0); |
} |
//-------------------------------------------------------------- |
// OSD_Element_NaviMode( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_FREE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_0), 0); // Navi aus |
else if (naviData->NCFlags & NC_FLAG_PH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_1), 0); // Pos. Hold |
else if (naviData->NCFlags & NC_FLAG_CH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_2), 0); // Coming Home |
} |
//-------------------------------------------------------------- |
// OSD_Element_StatusFlags( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ) |
{ |
// FC_StatusFlags 0.88 |
// #define FC_STATUS_MOTOR_RUN 0x01 |
// #define FC_STATUS_FLY 0x02 |
// #define FC_STATUS_CALIBRATE 0x04 |
// #define FC_STATUS_START 0x08 |
// #define FC_STATUS_EMERGENCY_LANDING 0x10 |
// #define FC_STATUS_LOWBAT 0x20 |
// #define FC_STATUS_VARIO_TRIM_UP 0x40 |
// #define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
if (naviData->FCStatusFlags & FC_STATUS_CALIBRATE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_1), 0); // Calibrate |
else if (naviData->FCStatusFlags & FC_STATUS_START) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_2), 0); // Start |
else if (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_3), 0); // Run |
else if (naviData->FCStatusFlags & FC_STATUS_FLY) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_4), 0); // Fly |
else if (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_5), 0); // Landing |
else if (naviData->FCStatusFlags & FC_STATUS_LOWBAT) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_6), 0); // LowBat |
else |
// lcd_printp_at (x, y, PSTR(" "), 0); // Clear |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_0), 0); // Clear |
} |
//############################################################## |
//# OLD OSD-Screens |
//############################################################## |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 0 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD0( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 3); |
OSD_Element_Altitude ( 11, 3, 0); |
OSD_Element_BatteryLevel ( 0, 7, 0); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 5); |
OSD_Element_CompassDegree ( 13, 0, 0); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 0, 1); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 16, 4, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 12, 4); |
OSD_Element_HomeDistance ( 10, 5, 0); |
OSD_Element_Target ( 10, 6, 0); |
//OSD_Element_TargetDegree ( x, y); |
OSD_Element_WayPoint ( 5, 6); |
OSD_Element_LED1Output ( 0, 6); |
OSD_Element_LED2Output ( 3, 6); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 4); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 12, 2); |
OSD_Element_SatsInUse ( 18, 2, 0); |
OSD_Element_StatusFlags ( 0, 2); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 1 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD1( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
//OSD_Element_AltitudeControl( x, y); |
OSD_Element_Altitude ( 1, 1, 1); |
OSD_Element_BatteryLevel ( 0, 7, 1); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
//OSD_Element_CareFree ( x, y); |
OSD_Element_CompassDegree ( 13, 0, 1); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 7, 6); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 1, 3, 7, 0, 0,0); |
OSD_Element_HomeDegree ( 8, 3); |
OSD_Element_HomeDistance ( 7, 2, 1); |
//OSD_Element_Target ( x, y, 1); |
//OSD_Element_TargetDegree ( x, y); |
//OSD_Element_WayPoint ( x, y); |
//OSD_Element_LED1Output ( x, y); |
//OSD_Element_LED2Output ( x, y); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 8, 5); |
OSD_Element_RCIntensity ( 15, 6); |
OSD_Element_VarioWert ( 15, 2); |
OSD_Element_SatsInUse ( 8, 4, 1); |
//OSD_Element_StatusFlags ( x, y); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 2 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD2( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 1); |
OSD_Element_Altitude ( 0, 4, 2); |
OSD_Element_BatteryLevel ( 13, 7, 2); |
OSD_Element_Capacity ( 0, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 3); |
OSD_Element_CompassDegree ( 12, 3, 2); |
//OSD_Element_CompassDirection( x, y, 0,0); |
//OSD_Element_CompassRose ( x, y); |
OSD_Element_FlyingTime ( 15, 5); |
//OSD_Element_GroundSpeed ( x, y); |
OSD_Element_HomeCircle ( 16, 0, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 11, 5); |
OSD_Element_HomeDistance ( 0, 5, 2); |
OSD_Element_Target ( 0, 6, 2); |
OSD_Element_TargetDegree ( 11, 6); |
//OSD_Element_WayPoint ( x, y); |
OSD_Element_LED1Output ( 12, 2); |
OSD_Element_LED2Output ( 14, 2); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 2); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 15, 4); |
OSD_Element_SatsInUse ( 10, 0, 2); |
OSD_Element_StatusFlags ( 0, 0); |
//OSD_Element_Variometer ( x, y); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/osd/osdold_screens.h |
---|
0,0 → 1,35 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#ifndef _OSDOLD_SCREENS_H |
#define _OSDOLD_SCREENS_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ); |
void OSD_Element_CareFree( uint8_t x, uint8_t y ); |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ); |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ); |
void OSD_Screen_OSD0( void ); |
void OSD_Screen_OSD1( void ); |
void OSD_Screen_OSD2( void ); |
#endif // USE_OSD_SCREEN_OLD |
#endif // _OSDOLD_SCREENS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/pkt/pkt.c |
---|
0,0 → 1,1227 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.c |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_Ask(), PKT_Ask_P() auf PKT_AskX() angepasst |
//# - add: PKT_AskX() - ehemals _pkt_ask(), jetzt erweitert um zusaetzliche progmen Parameter und exportiert |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() (ehemals in setup.c) |
//# - chg: PKT_Update() auf PKT_Ask_P() umgestellt |
//# - add: PKT_Ask(), PKT_Ask_P(), _pkt_ask() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P(), _pkt_title() |
//# - chg: PKT_TitleFromMenuItem() umgestellt auf _pkt_title() |
//# - chg: PKT_TitlePKTVersion() umbenannt zu PKT_TitlePKT() |
//# - fix: PKT_TitleFromMenuItem() funktioniert jetzt auch wenn kein Menue vorhanden ist |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - fix: PKT_CtrlHook() bzgl. PKT_CheckUpdate() - seit Februar 2014 Probleme |
//# mit Update-Anforderung vom PKT-Updatetool -> PKT war zu schnell |
//# Es wurde jetzt ein Timer in PKT_CtrlHook() eingebaut der den Aufruf |
//# von PKT_CheckUpdate() etwas einbremst ohne das PKT auszubremsen. |
//# Ich hoffe, das es ist Loesung ist (bin guter Dinge nach Test's) |
//# - chg: PKT_CheckUpdate() - Code optimiert |
//# - add: PKT_Info() - Anzeige ob SV2-Patch vorhanden ist oder nicht (ganz oben) |
//# - add: PKT_Info() - Anzeige fuer USE_OSD_SCREEN_WAYPOINTS |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# - add: #include "../utils/menuctrl.h" |
//# |
//# 06.05.2014 OG |
//# - chg: PKT_CheckUpdate() - seit Februar funktioniert die Update-Anforderung |
//# vom PKT-Updatetool an das PKT nicht immer zuverlaessig - fgf. weil sich |
//# seitens des PKT's Timings etwas geaendert haben (durch weglassen von Code). |
//# Hier ein Versuch etwas daran wieder zu aendern... its zwar weiterhin |
//# nicht immer zuverlaessig aber vieleicht etwas besser. |
//# Wahrscheinlich kann man das nur richtig korrigieren in Verbindung mit |
//# Anpassungen am PKT-Updatetool - aber das ist ein anderes Thema... |
//# - chg: PKT_Update() - ein kurzer Beep wenn das PKT-Update aufgerufen wird |
//# |
//# 05.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P(), _pkt_popup() |
//# |
//# 11.04.2014 OG |
//# - chg: _pkt_message() ergaenzt um clear_key_all() |
//# |
//# 09.04.2014 OG |
//# - chg: PKT_Update() - umgestellt auf ShowTitle_P() |
//# |
//# 08.04.2014 OG |
//# - chg: PKT_Update() - Text bzgl. "Druecke Start..." zentriert |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# - add: _pkt_message() zeigt einen Titel mit PKT Version und Lipo an |
//# |
//# 03.04.2014 OG |
//# - chg: _pkt_message() abgesichert bzgl. zu langen Texten |
//# |
//# 27.03.2014 OG |
//# - chg: PKT_SwitchOff() Anzeige optimiert |
//# |
//# 21.02.2014 OG |
//# - fix: PKT_CheckUpdate() Zeile 0 flimmerte wegen einer nicht mehr |
//# benoetigten Textausgabe |
//# - chg: PKT_SwitchOff() auf PKT_TitlePKTVersion() angepasst |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P(), _pkt_message() |
//# - chg: MK_Info() auf USE_MKSETTINGS angepasst (ehemals MKPARAMETER) |
//# |
//# 13.02.2014 OG |
//# - chg: Screenlayout von PKT_Update() |
//# - chg: Screenlayout von PKT_SwitchOff() |
//# |
//# 11.02.2014 OG |
//# - chg: PKT_Info() Anzeige der FC-Version entfernt |
//# |
//# 04.02.2014 OG |
//# - fix: #include "../lipo/lipo.h" hinzugefuegt |
//# |
//# 03.02.2014 OG |
//# - chg: Layout von PKT_SwitchOff() und bessere Unterstuetzung von PKT_CtrlHook() |
//# - add: PKT_ShowTitleVersion() |
//# |
//# 30.01.2014 OG |
//# - chg: geaenderte Anzeige der Reihenfolgen unterstuetzter Module in PKT_Info() |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 24.01.2014 OG |
//# - chg: PKT_SwitchOff(): Layout; Code eliminiert; Beep; |
//# add PKT_Hook() (für PKT-Update) |
//# - chg: PKT_Update(): Layout |
//# |
//# 21.10.2013 CB |
//# - add: PKT_Info WyFly hinzugefügt |
//# |
//# 07.07.2013 OG |
//# - del: PKT_Info(): USE_OSD_SCREEN_ELECTRIC_N |
//# 26.06.2013 OG |
//# - del: PKT_Info(): USE_PKT_STARTINFO |
//# |
//# 13.06.2013 OG |
//# - add: PKT-Hardwareversion in der Titelanzeige von PKT_Info() |
//# - chg: Layout Titelanzeige von PKT_Info() |
//# - del: USE_PCFASTCONNECT in PKT_Info() (nicht mehr benoetigt) |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 20.05.2013 OG |
//# - chg: PKT_CtrlHook() bei PKT-Update um WriteParameter() ergaenzt |
//# |
//# 19.05.2013 OG |
//# - add: PKT_CtrlHook() ruft ggf. PKT_Update() auf und kann spaeter weitere |
//# PKT interne Sachen verarbeiten wie z.B. senden von Daten an den PC |
//# - chg: PKT_CheckUpdate() umgestellt auf timer_pktctrl um Konflikte mit |
//# mit z.B. osd zu vermeiden |
//# - chg: PKT_Info() bzgl. Codeverbrauch verringert |
//# - fix: PKT_Info() Anzeige von USE_OSD_SCREEN_NAVIGATION |
//# - add: PC_Fast_Connect() aus menu.c |
//# - add: PKT_* Funktionen aus main.c bzw. menu.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "pkt.h" |
//############################################################################################# |
//############################################################################################# |
uint8_t pkt_progress_act = 0; |
uint8_t pkt_progress_max = 0; |
int8_t pkt_progress_yoffs = 0; |
uint8_t pkt_progress_width = 0; |
uint8_t pkt_progress_height = 0; |
#define PROGRESS_YOFFS -8 |
#define PROGRESS_WIDTH 96 |
//############################################################################################# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height) |
{ |
pkt_progress_act = 0; |
pkt_progress_max = max; |
pkt_progress_yoffs = yoffs; |
pkt_progress_width = 0; // Parameter width - aktuell nicht unterstuetzt |
pkt_progress_height = 0; // Parameter height - aktuell nicht unterstuetzt |
if( pkt_progress_width == 0 ) |
pkt_progress_width = PROGRESS_WIDTH; |
lcd_rect_round( 13, 32+pkt_progress_yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_Progress_Next( void ) |
{ |
uint8_t width; |
pkt_progress_act++; // Progress: hochzaehlen |
width = (pkt_progress_act*pkt_progress_width) / pkt_progress_max; // Progress: Breite berechnen |
lcd_frect( 16, 34+pkt_progress_yoffs, width, 2, 1); // Progress: anzeigen |
return true; |
} |
//############################################################################################# |
//# Anzeigefunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// PKT_KeylineUpDown( xUp, xDown, xoffs,yoffs) |
// |
// gibt in der Keyline (y=7) ein Up- und Down-Zeichen aus. |
// Das Up-Zeichen wird dabei um einen Pixel versetzt damit |
// es besser aussieht |
// |
// Parameter: |
// xUp : x-Position des Up-Zeichens in Zeile 7 |
// xDown: x-Position des Down-Zeichens in Zeile 7 |
// xoffs, yoffs: normalerweise 0,0 |
//-------------------------------------------------------------- |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs) |
{ |
lcdx_printp_at( xUp , 7, PSTR("\x1a") , MNORMAL, xoffs,yoffs+1); // Up |
lcdx_printp_at( xDown, 7, PSTR("\x1b") , MNORMAL, xoffs,yoffs); // Down |
} |
//############################################################################################# |
//# Titel Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _pkt_title( uint8_t progmem, const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
const char *pMask; |
if( clearscreen ) lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( progmem ) pMask = PSTR("%19S"); |
else pMask = PSTR("%19s"); |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, pMask, text ); |
if( lShowLipo ) show_Lipo(); |
} |
//-------------------------------------------------------------- |
// PKT_Title( text, lShowLipo, clearscreen ) |
// |
// text im RAM |
//-------------------------------------------------------------- |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( false, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Title_P( text, lShowLipo, clearscreen ) |
// |
// text im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( true, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// optional: rechts die PKT Lipo-Spannung anzeigen |
//-------------------------------------------------------------- |
void PKT_TitlePKTlipo( uint8_t lShowLipo ) |
{ |
//PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), true, false ); // showlipo, kein clearscreen |
PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), lShowLipo, true ); // showlipo und clearscreen |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// und rechts die PKT Lipo-Spannung |
//-------------------------------------------------------------- |
void PKT_TitlePKT( void ) |
{ |
PKT_TitlePKTlipo( true ); |
} |
//-------------------------------------------------------------- |
// PKT_TitleFromMenuItem( lShowLipo ) |
// |
// zeigt eine PKT-Titelzeile mit Text vom aufrufenden Menuepunkt |
// optional kann die PKT LiPo-Spannung angezeigt werden |
// |
// wenn kein Menue vorhandenist wird die PKT-Version angezeigt |
//-------------------------------------------------------------- |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ) |
{ |
const char *pStr; |
uint8_t isPGM; |
if( MenuCtrl_GetMenuIndex() < 0 ) |
{ |
PKT_TitlePKT(); // kein Menue vorhanden -> PKT-Version anzeigen |
return; |
} |
pStr = MenuCtrl_GetItemText(); |
isPGM = MenuCtrl_IsItemTextPGM(); |
_pkt_title( isPGM, pStr, lShowLipo, true ); // true = clearscreen |
} |
//############################################################################################# |
//# PKT Message |
//############################################################################################# |
//-------------------------------------------------------------- |
// _pkt_message( progmem, text, error, timeout, beep, clearscreen) |
// |
// INTERN |
// fuer: PKT_Message(), PKT_Message_P() |
// Parameter? -> siehe dort... |
//-------------------------------------------------------------- |
void _pkt_message( uint8_t progmem, const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
char *pStr; |
if( clearscreen ) |
lcd_cls(); |
PKT_TitlePKT(); |
if( error ) |
{ |
//lcd_frect_round( 0+36, (2*8)-2, 127-73, 8+2, 1, R1); // Fill: Schwarz |
lcd_frect_round( 0+36, (2*8)-1, 127-74, 8+2, 1, R1); // Fill: Schwarz |
lcdx_printp_center( 2, strGet(STR_ERROR), MINVERS, 0,1); // "FEHLER" |
} |
else |
lcdx_printp_center( 2, strGet(STR_PKT) , MNORMAL, 0,1); // "PKT" |
//----------------------- |
// Ausgabe der Nachricht |
//----------------------- |
pStr = buffered_sprintf_P( ( progmem ? PSTR("%20S") : PSTR("%20s")), text); // max. 20 Zeichen |
strrtrim( pStr ); // alle Leerzeichen rechts loeschen |
lcdx_print_center( 4, (uint8_t *)pStr, MNORMAL, 0,5); // Text zentriert; String im RAM |
lcd_rect_round( 0, 32, 127, 16, 1, R2); // Rahmen |
if( beep ) |
{ |
if( error ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( timeout ) |
{ |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline... tja, welche Variante nehmen wir denn nun Final??? OK oder ENDE? |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
//if( get_key_short(1 << KEY_ENTER) ) break; |
if( get_key_short(1 << KEY_ESC) ) break; |
PKT_CtrlHook(); |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Message( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im RAM) |
// |
// Parameter: |
// text : String im RAM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clearscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( false, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im PROGMEM) |
// |
// Parameter: |
// text : String im PROGMEM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clrscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( true, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_Datenverlust( timeout, beep) |
//-------------------------------------------------------------- |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ) |
{ |
//_pkt_message( true, text, error, timeout, beep, clearscreen ); |
_pkt_message( true, strGet(ERROR_NODATA), true, timeout, beep, true ); |
} |
//############################################################################################# |
//# PKT Popup |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// PARAMETER: |
// progmem : true = Texte in PROGMEM |
// false = Texte im RAM (RAM geht noch nicht!) |
// timeout : Zeit bis zum automatischen beenden (z.B. 400 = 4 Sekunden) |
// wenn timeout == 0 dann direkt beenden ohne auf Tastendruck zu warten |
// text1..4: Text (die Hoehe des Popups berechnet sich aus der Anzahl der Texte) |
// -> ein nicht benutzer Text wird mit NULL oder Dezimal 0 uebergeben |
//-------------------------------------------------------------- |
void _pkt_popup( uint8_t progmem, uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4) |
{ |
uint8_t n = 0; |
uint8_t yoffs = 0; |
if( text1 ) { n = 1; yoffs = 1; } |
if( text2 ) { n = 2; yoffs = 2; } |
if( text3 ) { n = 3; yoffs = 2; } |
if( text4 ) { n = 4; yoffs = 3; } |
Popup_Draw(n+1); |
if( text1 ) lcdx_printp_center( 8-n, text1, MINVERS, 0,-5-yoffs); // Text zentriert; String im PROGMEM |
if( text2 ) lcdx_printp_center( 9-n, text2, MINVERS, 0,-4-yoffs); // Text zentriert; String im PROGMEM |
if( text3 ) lcdx_printp_center( 10-n, text3, MINVERS, 0,-3-yoffs); // Text zentriert; String im PROGMEM |
if( text4 ) lcdx_printp_center( 11-n, text4, MINVERS, 0,-2-yoffs); // Text zentriert; String im PROGMEM |
if( timeout ) |
{ |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
if( get_key_short(1<<KEY_ESC) |
|| get_key_short(1<<KEY_ENTER) |
|| get_key_short(1<<KEY_PLUS) |
|| get_key_short(1<<KEY_MINUS) ) |
{ |
break; |
} |
} |
lcd_frect( 0, 58-((n+1)*8), 127, 5+((n+1)*8), 0); // Box clear - der Bereich des Popup's wird wieder geloescht (nur wenn timeout > 0!) |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Popup( timeout, text1, text2, text3, text4 ) |
// |
// Texte im RAM |
//-------------------------------------------------------------- |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( false, timeout, text1, text2, text3, text4 ); |
} |
//-------------------------------------------------------------- |
// PKT_Popup_P( timeout, text1, text2, text3, text4 ) |
// |
// Texte im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( true, timeout, text1, text2, text3, text4 ); |
} |
//############################################################################################# |
//# PKT Ask |
//############################################################################################# |
//-------------------------------------------------------------- |
// wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
// |
// PARAMETER: |
// asktype : ASK_YES_NO, ASK_NO_YES, ASK_CANCEL_OK, ASK_END_OK, ASK_END_START |
// text1 : 1. Zeile Text |
// text2 : optionale 2. Zeile Text |
// text_progmem : text1 & text2 in PROGMEM oder RAM (PROGMEM = true) |
// headline : Ueberschrift |
// headline_progmem: -> PROGMEM oder RAM |
// title : ganz oben der Titel (Zeile 0, Invers) |
// title_progmem : -> PROGMEM oder RAM |
// |
// RUECKGABE: |
// true : Ja |
// false : Nein |
//-------------------------------------------------------------- |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ) |
{ |
uint8_t redraw = true; |
uint8_t keyenter = false; |
uint8_t retcode = false; |
const char *pMaskP; |
const char *pMaskR; |
const char *pMask; |
pMaskP = PSTR("%S"); |
pMaskR = PSTR("%s"); |
set_beep( 35, 0xffff, BeepNormal ); // kurzer Beep |
while( true ) |
{ |
//------------------------ |
// Screen zeichnen |
//------------------------ |
if( redraw ) |
{ |
if( title == NULL ) PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearscreen) |
else if( title_progmem ) PKT_Title_P( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (PROGMEM) |
else PKT_Title( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (RAM) |
if( text2 == NULL ) |
{ |
// 1 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,2, pMask, headline); // headline |
lcd_rect_round( 0, 3*7+2+10, 127, 16+0, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,6, pMask, text1); // text1 |
} |
else |
{ |
// 2 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,-1, pMask, headline); // headline |
lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,-1, pMask, text1); // text1 |
lcdx_printf_center_P( 5, MNORMAL, 0, 0, pMask, text2); // text2 |
} |
//------------------------ |
// Keyline |
//------------------------ |
switch( asktype ) |
{ |
case ASK_NO_YES: lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_YES_NO: lcd_printp_at(12, 7, strGet(YESNO), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_CANCEL_OK: lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr. OK" |
lcd_printp_at(19, 7, PSTR("OK") , MNORMAL); |
break; |
case ASK_END_OK: lcd_printp_at(12, 7, strGet(KEYLINE4), MNORMAL); // Keyline: "Ende OK" |
break; |
case ASK_END_START: lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline: "Ende Start" |
break; |
} |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // 4. Taste |
{ |
keyenter = true; |
break; |
} |
} // end: while( true ) |
//------------------------- |
retcode = keyenter; |
if( asktype == ASK_YES_NO ) retcode = !retcode; |
clear_key_all(); |
return retcode; |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im RAM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, false, headline, true, title, true); |
//return _pkt_ask( false, asktype, text1, text2, headline, title); |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask_P( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im PROGMEM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, true, headline, true, title, true); |
//return _pkt_ask( true, asktype, text1, text2, headline, title); |
} |
//############################################################################################# |
//# PKT Gauge |
//############################################################################################# |
uint8_t Gauge_px; |
uint8_t Gauge_py; |
uint8_t Gauge_rx; |
uint8_t Gauge_ry; |
uint8_t Gauge_step; |
volatile uint8_t Gauge_active; // wird in timer.c abgefragt |
#define GAUGE_DEFAULT_PX 64 // Default X (Pixel) |
#define GAUGE_DEFAULT_PY 40 // Default Y (Pixel) |
#define GAUGE_DEFAULT_RX 10 // Default Radius X Gauge-Kreis |
#define GAUGE_DEFAULT_RY 8 // Default Radius Y Gauge-Kreis |
#define GAUGE_TIMER 40 // alle n aktualisieren (Timer Intervall) |
#define GAUGE_DEGREE 45 // Winkel von Schritt zu Schritt |
#define GAUGE_LROFFS -2 // die Laenge der Gauge-Linie verringern/vergroessern |
//-------------------------------------------------------------- |
// PKT_Gauge_Begin( py ) |
//-------------------------------------------------------------- |
void PKT_Gauge_Begin( uint8_t py ) |
{ |
Gauge_px = GAUGE_DEFAULT_PX; |
Gauge_py = GAUGE_DEFAULT_PY; |
Gauge_rx = GAUGE_DEFAULT_RX; |
Gauge_ry = GAUGE_DEFAULT_RY; |
if( py>0 ) Gauge_py = py; |
Gauge_step = 0; |
timer_gauge = GAUGE_TIMER; |
Gauge_active = true; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (0), 1); |
lcd_ellipse( Gauge_px, Gauge_py, Gauge_rx, Gauge_ry, 1); |
} |
//-------------------------------------------------------------- |
// PKT_Gauge_End() |
//-------------------------------------------------------------- |
void PKT_Gauge_End( void ) |
{ |
Gauge_active = false; |
} |
// uint8_t old_step; |
//-------------------------------------------------------------- |
// PKT_Gauge_Next() |
// |
// INTERN fuer Timer ISR! |
//-------------------------------------------------------------- |
void PKT_Gauge_Next( void ) |
{ |
uint8_t old_step; |
old_step = Gauge_step; |
Gauge_step++; |
if( Gauge_step >= (360/GAUGE_DEGREE)) Gauge_step = 0; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (old_step*GAUGE_DEGREE) , 0); |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (Gauge_step*GAUGE_DEGREE), 1); |
timer_gauge = GAUGE_TIMER; |
} |
//############################################################################################# |
//# PKT Kontrollfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Reset_EEprom( void ) |
{ |
// Text1: "PKT-EEProm" (EEPROM1) |
// Text2: "wirklich löschen?" (EEPROM2) |
// Head : "* ACHTUNG '" (STR_ATTENTION) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_YES_NO, strGet(EEPROM1), strGet(EEPROM2), strGet(STR_ATTENTION), NULL) ) |
{ |
Delete_EEPROM(); |
clr_V_On(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_SwitchOff( void ) |
{ |
// Text1: "PKT ausschalten?" (SHUTDOWN) |
// Text2: NULL |
// Head : "PKT" (STR_PKT) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_NO_YES, strGet(SHUTDOWN), NULL, strGet(STR_PKT), NULL) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Ask_Restart( const char *title ) |
{ |
// Text1: "jetzt ausschalten?" (STR_PKT_SWITCHOFF_NOW) |
// Text2: NULL |
// Head : "PKT neu starten!" (STR_PKT_RESTART_NOW) |
// Titel: via Parameter |
if( PKT_Ask_P( ASK_NO_YES, strGet(STR_PKT_SWITCHOFF_NOW), NULL, strGet(STR_PKT_RESTART_NOW), title) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Update( void ) |
{ |
// Text1: "Drücke 'Start' für" (UPDATE2) |
// Text2: "min. 2 Sekunden" (UPDATE3) |
// Head : "Verbinde PKT mit PC!" (UPDATE1) |
// Titel: "PKT Update" |
if( PKT_Ask_P( ASK_END_START, strGet(UPDATE2), strGet(UPDATE3), strGet(UPDATE1), PSTR("PKT Update")) ) |
{ |
uart1_putc('S'); // Startzeichen zum Updatetool schicken |
wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
while(true); |
} |
} |
//void PKT_Update( void ) |
//{ |
// ShowTitle_P( PSTR("PKT Update"), true ); |
// lcdx_printp_center( 2, strGet(UPDATE1), MNORMAL, 0,-1); // "Verbinde PKT mit PC!" |
// lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
// lcdx_printp_center( 4, strGet(UPDATE2), MNORMAL, 0,-1); // "Drücke 'Start' für" |
// lcdx_printp_center( 5, strGet(UPDATE3), MNORMAL, 0, 0); // "min. 2 Sekunden" |
// lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline |
// //PKTupdate = false; // Flag löschen |
// |
// set_beep( 80, 0x000f, BeepNormal); // kurzer Beep |
// |
// do |
// { |
// if( get_key_press (1 << KEY_ESC) ) |
// { |
// //get_key_press(KEY_ALL); |
// clear_key_all(); |
// return; |
// } |
// } |
// while( !(get_key_press (1 << KEY_ENTER)) ); |
// |
// uart1_putc('S'); // Startzeichen zum Updatetool schicken |
// wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
// while(true); |
//} |
//-------------------------------------------------------------- |
// prueft im Menue ob das Updatetool ein Update machen will |
//-------------------------------------------------------------- |
uint8_t PKT_CheckUpdate(void) |
{ |
unsigned int uart_data; |
uint8_t state = 0; |
uint8_t update = false; |
uint8_t error = false; |
uint8_t SendVersion = false; |
char s[7]; |
//---------------------------- |
// empfangen |
//---------------------------- |
if( uart1_available() > 0 ) // Zeichen von der USB-Schnittstelle vorhanden? |
{ |
timer_pktupdatecheck = 100; // Timeouttimer um die Funktion abzubrechen |
while( !timer_pktupdatecheck==0 && !update && !error && !SendVersion ) |
{ |
uart_data = uart1_getc(); |
if( !((uart_data & 0xFF00) == UART_NO_DATA) ) |
{ |
switch (state) |
{ |
case 0: if( uart_data == PKTESC ) |
{ |
state = 1; |
} |
else error = true; |
break; |
case 1: if( uart_data == PKTESC ) |
{ |
state = 2; |
} |
else error = true; |
break; |
case 2: if( uart_data == PKTUPDATE ) update = true; |
else if (uart_data == PKTVERSION) SendVersion = true; |
else error = true; |
break; |
} // end: switch |
} // end: if |
} // end: while |
//---------------------------- |
// Rueckmeldungen |
//---------------------------- |
if( error ) |
{ |
uart1_putc('E'); // Rueckmeldungen zum Updatetool |
} |
else if( update ) |
{ |
uart1_putc('U'); |
} |
else if( SendVersion ) |
{ |
uart1_putc('V'); |
itoa( EEpromVersion, s, 10 ); |
uart1_puts(s); |
} |
else if( timer_pktupdatecheck==0 ) |
{ |
uart1_putc('T'); |
} |
//uart1_flush(); |
} |
return update; |
} |
//-------------------------------------------------------------- |
// retcode = PKT_CtrlHook() |
// |
// ruft ggf. PKT_Update() auf und kann spaeter weitere PKT interne |
// Sachen verarbeiten wenn der Hook (=Nagel) in entsprechende |
// Routienen eingebaut ist (z.B. senden von Daten an den PC) |
// |
// Aktuell verwendet von: menuctrl, scrollbox, osd |
// |
// Rueckgabe: |
// retcode: 0 = nichts wurde gemacht (false) |
// 1 = es wurde etwas durchgefuehrt und die Aufruffunktion |
// muss gff. einen Screenrefresh durchfuehren |
// (z.B. wenn der PKT-Update Screen gezeigt wurde) |
//-------------------------------------------------------------- |
uint8_t PKT_CtrlHook( void ) |
{ |
uint8_t retcode = 0; |
// 31.05.2014 OG: PKT_CheckUpdate() darf nicht in 'Hoechstgeschwindigkeit' |
// aufgerufen werden da es sonst seitens des PKT-Updatetools ggf. zu einer |
// Fehlermeldung kommt - deswegen hier timer_pktupdatecheck der PKT_CheckUpdate |
// etwas einbremst sich aber nicht negativ auf die PKT-Performance auswirkt. |
if( !timer_pktupdatecheck ) |
{ |
if( PKT_CheckUpdate() ) // Update vom Updatetool angefordert? |
{ |
WriteParameter(); |
PKT_Update(); |
retcode = 1; |
} |
timer_pktupdatecheck = 10; |
} |
return retcode; |
} |
//-------------------------------------------------------------- |
// zeigt PKT-Version und Credits / Entwickler |
//-------------------------------------------------------------- |
void PKT_Info( void ) |
{ |
char *HWV_39m = "3.9m"; |
char *HWV_39x = "3.9x"; |
char *HWV = "????"; |
if( PCB_Version == PKT39m ) HWV = HWV_39m; |
if( PCB_Version == PKT39x ) HWV = HWV_39x; |
lcd_cls(); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" PKT v%8S %s"), PSTR(PKTSWVersion), HWV ); // "3.6.8a..." und die PKT-Hardwareversion |
ScrollBox_Push_P( MINVERS, PSTR(" %S SV2-Patch"), (PCB_SV2RxTxPatch ? strGet(STR_WITH) : strGet(STR_WITHOUT)) ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
ScrollBox_Push_P( MNORMAL, PSTR("(C) GNU GPL License") ); |
ScrollBox_Push_P( MNORMAL, PSTR(" NO WARRANTY") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#ifdef USE_MODULEINFO |
const char *fmt = PSTR("%15S %4S"); |
//--- MODULE SUPPORT INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Modules installed") ); |
#ifdef USE_BLUETOOTH |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(NOO) ); |
#endif |
#ifdef USE_WLAN |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(NOO) ); |
#endif |
#ifdef USE_MKSETTINGS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDEBUGDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDISPLAY |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(NOO) ); |
#endif |
#ifdef USE_OSDDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(NOO) ); |
#endif |
#ifdef USE_TRACKING |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(NOO) ); |
#endif |
#ifdef USE_FOLLOWME |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(NOO) ); |
#endif |
#ifdef USE_JOYSTICK |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(NOO) ); |
#endif |
#ifdef USE_I2CMOTORTEST |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(NOO) ); |
#endif |
#ifdef USE_SOUND |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(NOO) ); |
#endif |
#ifdef USE_FONT_BIG |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(NOO) ); |
#endif |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#endif // USE_MODULEINFO |
//--- CREDITS INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Credits") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2013 Oliver Gemesi") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Chr. Brandtner" ) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" Harald Bongartz") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Martin Runkel") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 gebad") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2010 Sebastian Boehm") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2009-2010 Peter Mack") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2008 Thomas Kaiser") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/pkt/pkt.h |
---|
0,0 → 1,148 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.h |
//# |
//# 25.06.2014 OG |
//# - add: PKT_AskX() |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() |
//# - add: PKT_Ask(), PKT_Ask_P() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P() |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# |
//# 06.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P() |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# |
//# 21.02.2014 OG |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P() |
//# |
//# 13.02.2014 OG |
//# - add: PKT_print_PKT_center() |
//# |
//# 03.02.2014 OG |
//# - add: PKT_ShowTitlePKTVersion() |
//# |
//# 13.06.2013 OG |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 19.05.2013 OG |
//# - Ausgliederungen aus verschiedenen anderen Sourcen |
//############################################################################ |
#ifndef _PKT_H |
#define _PKT_H |
#define PKTESC 27 // Parameter für PKT-Updatetool |
#define PKTUPDATE 'u' // starte Update |
#define PKTVERSION 'v' // sende PKT EEpromversion |
#define PKTSENDCONF 'e' // sende PKT Konfigdaten |
#define ASK_YES_NO 1 |
#define ASK_NO_YES 2 |
#define ASK_CANCEL_OK 3 |
#define ASK_END_OK 4 |
#define ASK_END_START 5 |
extern volatile uint8_t Gauge_active; |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_TitlePKT( void ); |
void PKT_TitlePKTlipo( uint8_t lShowLipo ); |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ); |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs); |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height); |
uint8_t PKT_Progress_Next( void ); |
void PKT_Gauge_Begin( uint8_t py); |
void PKT_Gauge_End( void ); |
void PKT_Gauge_Next( void ); |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ); |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ); |
uint8_t PKT_CtrlHook(void); |
uint8_t PKT_CheckUpdate(void); // prüft im Hauptmenü ob das Updatetool ein Update machen will |
void PKT_Reset_EEprom( void ); |
void PKT_Update(void); |
void PKT_SwitchOff(void); |
void PKT_Info(void); |
void PKT_Ask_Restart( const char *title ); |
#endif // end: #ifndef _PKT_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/setup/setup.c |
---|
0,0 → 1,2651 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.c |
//# |
//# |
//# 03.08.2015 CB |
//# - add: FollowMe Setup um Distance und Azimuth erweitert |
//# |
//# 27.06.2014 OG |
//# - chg: Setup_MAIN() - Reihenfolge von GPS-Maus/FollowMe/Tracking geaendert |
//# |
//# 25.06.2014 OG |
//# - chg: Text von GPS_SHOWDEV |
//# - chg: Setup_FollowMe() - deaktiviert/auskommentiert: FME_REFRESH |
//# - chg: Setup_GPSMouse() - deaktiviert/auskommentiert: GPS_TYPE, GPS_ACTIVE |
//# |
//# 24.06.2014 OG |
//# - chg: Setup_GPSMouse() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - del: verschiedene weitere Modul-Variablen entfernt weil nicht benoetigt |
//# - del: Variable CheckGPS |
//# - del: BT_ShowGPSData() - ist jetzt als GPSMouse_ShowData() in gps/gpsmouse.c |
//# - chg: BT_ShowGPSData() - Parameter und Rueckgabe |
//# - del: BT_ShowGPSData_OLD |
//# |
//# 21.06.2014 CB |
//# - chg: BT_SearchDevices - Timeoutanzeige bei Devicesuche |
//# |
//# 16.06.2014 OG |
//# - chg: BT_ShowGPSData() - neues Layout und Anzeige fuer "XCnt" (=RX-Count) |
//# hinzugefuegt; XCnt zeigt die Anzahl empfangener GPS-Datenpakete an |
//# und ist ein Indikator wie schnell die BT-GPS Maus Daten sendet |
//# |
//# 13.06.2014 OG |
//# - chg: Setup_PKTGeneric() - neue Einstellung "PKT aus nach" |
//# - del: ChangeWi_SV2() |
//# |
//# 12.06.2014 OG |
//# - del: Reset_EEprom() - ist jetzt als PKT_Reset_EEprom() in pkt.c |
//# |
//# 11.06.2014 OG |
//# - fix: Edit_generic() funktioniert jetzt auch, wenn kein vorgelagertes |
//# Menue vorhanden ist (ggf. muss das Label angepasst werden) |
//# |
//# 10.06.2014 OG |
//# - chg: verschiedene Funktionen umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - add: Setup_MKConnection() - hier ist auch das setzen der Baudrate Wi232/BT |
//# hin verschoben worden (war vorher im Wi.232 Bereicch) |
//# |
//# 06.06.2014 OG |
//# - chg: Setup_WI232_MenuCreate()... Eintraege geloescht... |
//# |
//# 04.06.2014 OG |
//# - chg: Setup_PKTGeneric() eine Menue-Trennlinie hinter Sommerzeit eingefuegt |
//# |
//# 01.06.2014 OG |
//# - chg: Edit_LED_Form() Parameter 'Text' entfernt und auf strGetOSDOLD() |
//# umgestellt |
//# - add: include "../osd/osdold_messages.h" |
//# |
//# 31.05.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt; angepasst auf neue |
//# Titelzeile; PKT_CtrlHook integriert; PKT Lipo-Anzeige; |
//# auf 'redraw' umgestellt |
//# |
//# 30.05.2014 OG |
//# - chg: Edit_LipoOffset() - umgeschrieben auf neues Layout, Code optimiert, |
//# Increment jetzt +/-50 (statt vorher 20), Offset bis 15000 (statt 10000) |
//# - chg: Edit_LipoOffset() - keine Parameter mehr, keine Rueckgabe |
//# |
//# 29.05.2014 OG |
//# - chg: Setup_OSD() umgestellt auf Menuetexte statt strGet() mit Textaenderung |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# (Text enfernt da vom Menue geerbt; optionale Hilfstexte moeglich) |
//# |
//# 27.05.2014 OG |
//# - del: Edit_generic_int(), Edit_printout_int() - nicht mehr benoetigt da |
//# anhaengende Werte umgemappt auf Edit_generic() |
//# |
//# 26.05.2014 OG |
//# - chg: Menuetext TIME_ZONE ergaenzt um "UTC" |
//# - chg: Setup_MAIN() - neue Anordnung der Menueeintraege mit Trennlinien |
//# - chg: Setup_OSDScreens() - OSDSCREEN_WAYPOINTS hinzugefuegt |
//# - chg: Text "LCD aus nach..." nach "LCD aus nach" |
//# - chg: Text "PKT allgemein" geaendert auf "PKT Allgemein" |
//# - del: PKT_LCDINVERS inkl. Sprachen |
//# - chg: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) entfernt, |
//# nicht mehr unterstuetzt |
//# - chg: Edit_printout() - Hilfstexte CHANGENORMREV1 und 2 fuer NormRev entfernt |
//# |
//# 23.05.2014 OG |
//# - add: Edit_generic() - Anzeige Titel mit PKT_TitleFromMenuItem() |
//# - fix: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) |
//# Edit-Range umgestellt von 0,4 auf 0,1 |
//# |
//# 11.05.2014 OG |
//# - chg: die Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# - chg: ein paar Menuetexte fuer Setup_MAIN() |
//# |
//# 18.04.2014 OG |
//# - chg: Setup_PKTGeneric() - PKT_BRIGHTNESS rausgenommen da es aktuell auch |
//# nicht mehr mit PKT 3.9m Hardware geht (spart ca. 650 Bytes) |
//# - chg: Text von "Tracking Servos" gekuerzt auf "Tracking" |
//# |
//# 09.04.2014 OG |
//# - chg: Edit_printout() Text von WlanMode 'Nein' auf 'Aus' geaendert |
//# |
//# 04.04.2014 OG |
//# - chg: Edit_String() auf ShowTitle_P() umgestellt |
//# - del: Edit_ShowTitle_P() |
//# - chg: in versch. Funktionen Titel-Strings fuer Edit_String() gekuerzt |
//# - chg: Edit_String() auf Edit_ShowTitle_P() umgestellt |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: Edit_printout(): WlanMode (Aus, AP-Mode, AdHoc) |
//# - chg: Edit_printout(): Code-Optimierung |
//# |
//# 01.04.2014 OG |
//# - add: Unterstuetzung fuer BLE (Bluetooth 4 Low Energy Zusatzmodul an SV2) |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 27.03.2014 OG |
//# - chg: Setup_PKTGeneric(), Edit_printout() Sprache "Niederländisch" nicht mehr waehlbar |
//# |
//# 04.02.2014 OG |
//# - chg: kleine Layoutaenderung von 'PKT_Akku' in Edit_printout() |
//# - add: PKT_CtrlHook() in Edit_generic(), Edit_generic_int(), Edit_LipoOffset() eingebaut |
//# - add: kleiner Hilfetext in Edit_printout() bei Aenderung von Einstellung |
//# "LCD Normal/Invers" bzgl. PKT neu starten nach Aenderung |
//# - chg: Anzeige von Edit_LipoOffset() geaendert und fix vom springen der |
//# Eingabe wenn Wert geaendert wird |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU in Setup_OSD() ("zeige Zellensp.") |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.07.2013 Cebra |
//# - chg: Fehler in Setup_WI232_MenuCreate, MenuCtrl_Push_P( WI_INSTALLED.. war auskommentiert, dadurch Fehler im Menu bei nicht |
//# aktivem Wi232 |
//# |
//# 18.07.2013 Cebra |
//# - chg: Edit_PKT_Accu entfernt, Edit_generic erweitert |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: Setup_OSDScreens(), Mark_OSDScreen(), Edit_OSDScreen() |
//# |
//# 04.07.2013 Cebra |
//# - chg: Setupmenü um Wlan erweitert |
//# |
//# 02.07.2013 Cebra |
//# - chg: Edit_String, geändert für die Nutzung von SSID und Wlan Passwort |
//# |
//# 30.06.2013 OG |
//# - del: Setup_OSD(): OSDS_HOMEVIEW |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO |
//# |
//# 24.06.2013 OG |
//# - chg: vor dem editieren des Bluetooth Namens bt_fixname() eingefuegt |
//# |
//# 24.06.2013 OG |
//# - fix: Edit_String(): Fehlermeldung bei EDIT_BT_NAME wenn erstes Zeichen = Space |
//# - chg: Edit_String(): Code-Layout und diverses |
//# - chg: Setup_BTM222(): Bearbeitung von BT_NAME geaendert |
//# - chg: Setup_BTM222(): Menuetexte geaendert |
//# - chg: Setup_MAIN(): Text im Setup-Hauptmenue von "BTM-222" auf "Bluetooth" geaendert |
//# |
//# 21.06.2013 OG |
//# - fix: Menuetext GPSMouse "Zeige Geräteliste" (Anzeige "ä" korrigiert) |
//# |
//# 15.06.2013 OG |
//# - add: "zeige MK-Setting" in Setup_OSD() hinzugefuegt |
//# |
//# 15.06.2013 OG |
//# - chg: Umstrukturierungen am OSD-Setup |
//# - chg: OSD Empfangsausfall von Setup_PKTGeneric() nach Setup_OSD() verschoben |
//# |
//# 13.06.2013 OG |
//# - del: Setup_Time() - jetzt in Setup_PKTGeneric() |
//# - del: Setup_PKTAccu() - jetzt in Setup_PKTGeneric() |
//# - del: SETUP_PKTDEBUG in Setup_MAIN() entfernt (auskommentiert, wenn notwendig ggf. mit einem Debug define regeln) |
//# - chg: diverse Aenderungen in Menutexten |
//# - fix: keine LCD-Helligkeitseinstellung bei HW 3.9x mehr durch Abfrage von PCB_Version |
//# |
//# 31.05.2013 cebra |
//# - chg: Umlaute, soweit gefunden, korrigiert |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: etliche Aenderungen rund um Bluetooth/BT-GPS Maus fuer bessere |
//# Usebility, Code-Einsparung und Code-Lesbarkeit |
//# - add: Wait4KeyOK() |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: BT_SelectDevice() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Setup_Time(), Setup_FollowMe(), Setup_OSD() |
//# - chg: weitere Speicheroptimierungen fuer USE_* |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# |
//# 20.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Menu_Setup(), Setup_PKTGeneric(), Setup_WI232() |
//# Setup_BTM222(), Setup_PKTAccu(), Setup_GPSMouse() |
//# - add: GPSMouse_ShowData() |
//# |
//# 19.05.2013 OG |
//# - chg: Aufruf von Update_PKT() umgestellt auf PKT_Update() |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.05.2013 Cebra |
//# - chg: Fehler bei On/Off Settings |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup |
//# |
//# 19.04.2013 Cebra |
//# - chg: Fehlerbehandlung im GPS Menü für den Fall das kein BTM22 eingebaut oder GPS-Device konfiguriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 12.04.2013 Cebra |
//# - chg: edit_generic um Sprungfaktor erweitert, erm�glicht bei hohen Werten einen schnelleren Wertewechsel |
//# |
//# 03.04.2013 Cebra |
//# - chg: find/replace Fehler bei lcd_puts_at beseitigt, GPS und BT-Maus Bereich |
//# |
//# 02.04.2013 Cebra |
//# - chg: Textfehler bei der Einstellung "Verbindung zum MK" beseitigt |
//# |
//# 04.03.2013 Cebra |
//# - del: OSD-Sreenmode ,is no longer necessary, last OSD-Screen is saved at shutdown |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../setup/setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../wi232/Wi232.h" |
#include "../bluetooth/bluetooth.h" |
#include "../connect.h" |
#include "../lipo/lipo.h" |
#include "../messages.h" |
#include "../eeprom/eeprom.h" |
#include "../tracking/tracking.h" |
#include "../uart/uart1.h" |
#include "../sound/pwmsine8bit.h" |
#include "../tracking/servo_setup.h" |
#include "../bluetooth/error.h" |
#include "../stick/stick_setup.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../wifly/wifly_setup.h" |
#include "../gps/gpsmouse.h" |
#include "../utils/scrollbox.h" |
#include "../osd/osdold_messages.h" |
uint8_t edit = 0; |
uint16_t Pre; |
char EditString[21]; |
uint8_t length_tmp; |
uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
//############################################################################ |
//############################################################################ |
//----------------------------- |
// Setup_Main() |
//----------------------------- |
#define SETUP_MKCONNECTION 1 |
#define SETUP_PKTCONFIG 2 |
#define SETUP_WI232 3 |
#define SETUP_BTM222 4 |
#define SETUP_SERVOCONFIG 5 |
#define SETUP_OSDVIEW 6 |
#define SETUP_TIME 7 |
#define SETUP_GPSMAUS 8 |
#define SETUP_FOLLOWME 9 |
#define SETUP_JOYSTICK 10 |
#define SETUP_PKTAKKU 11 |
#define SETUP_PKTUPDATE 12 |
#define SETUP_PKTDEBUG 13 |
#define SETUP_EEPROMRESET 14 |
#define SETUP_WIFLY 15 |
#define SETUP_OSDSCREENS 16 |
#define SETUP_BLE 17 |
static const char SETUP_MKCONNECTION_de[] PROGMEM = "Verbindung MK"; |
static const char SETUP_MKCONNECTION_en[] PROGMEM = "connection MK"; |
static const char SETUP_PKTCONFIG_de[] PROGMEM = "PKT Allgemein"; |
static const char SETUP_PKTCONFIG_en[] PROGMEM = "PKT general"; |
static const char SETUP_WI232_de[] PROGMEM = "Wi.232"; |
#define SETUP_WI232_en SETUP_WI232_de |
static const char SETUP_BTM222_de[] PROGMEM = "Bluetooth"; |
#define SETUP_BTM222_en SETUP_BTM222_de |
static const char SETUP_BLE_de[] PROGMEM = "Bluetooth BLE"; |
#define SETUP_BLE_en SETUP_BLE_de |
static const char SETUP_WIFLY_de[] PROGMEM = "WLAN WiFly"; |
#define SETUP_WIFLY_en SETUP_WIFLY_de |
static const char SETUP_SERVOCONFIG_de[] PROGMEM = "Tracking"; |
#define SETUP_SERVOCONFIG_en SETUP_SERVOCONFIG_de |
static const char SETUP_OSDVIEW_de[] PROGMEM = "OSD Anzeige"; |
static const char SETUP_OSDVIEW_en[] PROGMEM = "OSD display"; |
static const char SETUP_OSDSCREENS_de[] PROGMEM = "OSD Screens"; |
static const char SETUP_OSDSCREENS_en[] PROGMEM = "OSD screens"; |
static const char SETUP_GPSMAUS_de[] PROGMEM = "GPS Maus"; |
static const char SETUP_GPSMAUS_en[] PROGMEM = "GPS mouse"; |
static const char SETUP_FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define SETUP_FOLLOWME_en SETUP_FOLLOWME_de |
static const char SETUP_JOYSTICK_de[] PROGMEM = "Joystick"; |
#define SETUP_JOYSTICK_en SETUP_JOYSTICK_de |
static const char SETUP_PKTUPDATE_de[] PROGMEM = "PKT Update"; |
#define SETUP_PKTUPDATE_en SETUP_PKTUPDATE_de |
static const char SETUP_PKTDEBUG_de[] PROGMEM = "Debug PKT"; |
#define SETUP_PKTDEBUG_en SETUP_PKTDEBUG_de |
static const char SETUP_EEPROMRESET_de[] PROGMEM = "PKT Reset"; |
#define SETUP_EEPROMRESET_en SETUP_EEPROMRESET_de |
//############################################################################ |
//----------------------------- |
// Setup_PKTGeneric() ("PKT allgemein") |
//----------------------------- |
#define PKT_LANGUAGE 3 |
#define PKT_LIGHTOFF 4 |
#define PKT_BRIGHTNESS 5 |
#define PKT_CONTRAST 6 |
#define PKT_SOUNDMODUL 9 |
#define PKT_BEEPER 10 |
#define PKT_VOLUME 11 |
#define PKT_ACCUTYPE 12 |
#define PKT_ACCUMEASURE 13 |
#define TIME_ZONE 14 |
#define TIME_SUMMER 15 |
#define PKT_PKTOFF 16 |
static const char LANGUAGE_de[] PROGMEM = "Sprache"; |
static const char LANGUAGE_en[] PROGMEM = "language"; |
static const char TIME_ZONE_de[] PROGMEM = "Zeitzone UTC"; |
static const char TIME_ZONE_en[] PROGMEM = "time zone UTC"; |
static const char TIME_SUMMER_de[] PROGMEM = "Sommerzeit"; |
static const char TIME_SUMMER_en[] PROGMEM = "summer time"; |
static const char PKT_PKTOFF_de[] PROGMEM = "PKT aus nach"; |
static const char PKT_PKTOFF_en[] PROGMEM = "PKT off after"; |
static const char LIGHTOFF_de[] PROGMEM = "LCD aus nach"; |
static const char LIGHTOFF_en[] PROGMEM = "LCD off after"; |
//static const char BRIGHTNESS_de[] PROGMEM = "LCD Helligkeit"; |
//static const char BRIGHTNESS_en[] PROGMEM = "LCD brightness"; |
static const char CONTRAST_de[] PROGMEM = "LCD Kontrast"; |
static const char CONTRAST_en[] PROGMEM = "LCD contrast"; |
static const char BEEPER_de[] PROGMEM = "Hardware Beeper"; |
#define BEEPER_en BEEPER_de |
#ifdef USE_SOUND |
static const char SOUNDMODUL_de[] PROGMEM = "Soundmodul"; |
static const char SOUNDMODUL_en[] PROGMEM = "Soundmodule"; |
static const char VOLUME_de[] PROGMEM = "Lautstärke Sound"; |
static const char VOLUME_en[] PROGMEM = "Volume Sound"; |
#endif |
static const char ACCUTYPE_de[] PROGMEM = "Akku Typ"; |
static const char ACCUTYPE_en[] PROGMEM = "Accu type"; |
static const char ACCUMEASURE_de[] PROGMEM = "Akku Messung"; |
static const char ACCUMEASURE_en[] PROGMEM = "Accu measure"; |
//############################################################################ |
//----------------------------- |
// Setup_MKConnection() |
//----------------------------- |
#define MKCONN_CONNECTION 1 |
#define MKCONN_BAUDRATE 2 |
#define MKCONN_CONNECTION_de SETUP_MKCONNECTION_de |
#define MKCONN_CONNECTION_en SETUP_MKCONNECTION_en |
static const char MKCONN_BAUDRATE_de[] PROGMEM = "Baudrate Wi232/BT"; |
#define MKCONN_BAUDRATE_en MKCONN_BAUDRATE_de |
//############################################################################ |
//----------------------------- |
// Setup_WI232() |
//----------------------------- |
#define WI_INSTALLED 1 |
#define WI_TXRX 2 |
#define WI_GROUP 3 |
#define WI_MODE 4 |
#define WI_TIMEOUT 5 |
#define WI_MTU 6 |
#define WI_INIT 7 |
#define WI_PCCONFIG 8 |
static const char WI_INSTALLED_de[] PROGMEM = "Modul eingebaut"; |
static const char WI_INSTALLED_en[] PROGMEM = "module built in"; |
static const char WI_TXRX_de[] PROGMEM = "TX/RX Kanal"; |
static const char WI_TXRX_en[] PROGMEM = "TX/RX Channel"; |
static const char WI_GROUP_de[] PROGMEM = "NetW. Gruppe"; |
static const char WI_GROUP_en[] PROGMEM = "NetW. Group"; |
static const char WI_MODE_de[] PROGMEM = "NetW. Mode"; |
#define WI_MODE_en WI_MODE_de |
static const char WI_TIMEOUT_de[] PROGMEM = "TX Timeout"; |
#define WI_TIMEOUT_en WI_TIMEOUT_de |
static const char WI_MTU_de[] PROGMEM = "TX MTU"; |
#define WI_MTU_en WI_MTU_de |
static const char WI_INIT_de[] PROGMEM = "Initialisieren"; |
static const char WI_INIT_en[] PROGMEM = "initialize"; |
static const char WI_PCCONFIG_de[] PROGMEM = "Konfig. mit PC"; |
static const char WI_PCCONFIG_en[] PROGMEM = "config. with PC"; |
//############################################################################ |
//----------------------------- |
// Setup_BTM222() (Bluetooth) |
//----------------------------- |
#define BT_INSTALLED 1 |
#define BT_NAME 2 |
#define BT_PIN 3 |
#define BT_INIT 4 |
#define BT_MAC 5 |
#define BT_REID 6 |
#define BT_PCCONFIG 7 |
#define BT_SHOWCONFIG 8 |
#define BT_INSTALLED_de WI_INSTALLED_de |
#define BT_INSTALLED_en WI_INSTALLED_en |
static const char BT_NAME_de[] PROGMEM = "BT Name"; |
static const char BT_NAME_en[] PROGMEM = "BT name"; |
static const char BT_PIN_de[] PROGMEM = "BT Pin"; |
static const char BT_PIN_en[] PROGMEM = "BT pin"; |
#define BT_INIT_de WI_INIT_de |
#define BT_INIT_en WI_INIT_en |
static const char BT_MAC_de[] PROGMEM = "zeige BT MAC"; |
static const char BT_MAC_en[] PROGMEM = "show BT MAC"; |
static const char BT_REID_de[] PROGMEM = "RE-ID Notiz"; |
#define BT_REID_en BT_REID_de |
#define BT_PCCONFIG_de WI_PCCONFIG_de |
#define BT_PCCONFIG_en WI_PCCONFIG_en |
static const char BT_SHOWCONFIG_de[] PROGMEM = "zeige BT Konfig"; |
static const char BT_SHOWCONFIG_en[] PROGMEM = "show BT config"; |
//############################################################################ |
//----------------------------- |
// Setup_BLE() (Bluetooth 4 Low Energy) |
//----------------------------- |
#define BLE_INSTALLED 1 |
// Text ist in messages.c: MODULE_EXIST |
//############################################################################ |
//----------------------------- |
// Setup_GPSMouse() |
//----------------------------- |
#define GPS_DEVICES 1 |
#define GPS_SEARCH 2 |
#define GPS_TYPE 3 |
#define GPS_ACTIVE 4 |
#define GPS_SHOWDEV 5 |
#define GPS_DATA 6 |
static const char GPS_SEARCH_de[] PROGMEM = "suche GPS-Maus"; |
static const char GPS_SEARCH_en[] PROGMEM = "search GPS-mouse"; |
static const char GPS_DEVICES_de[] PROGMEM = "Geräteliste"; |
static const char GPS_DEVICES_en[] PROGMEM = "devicelist"; |
static const char GPS_SHOWDEV_de[] PROGMEM = "aktuelle Maus"; |
static const char GPS_SHOWDEV_en[] PROGMEM = "act. GPS-mouse"; |
static const char GPS_DATA_de[] PROGMEM = "GPS-Maus Daten"; |
static const char GPS_DATA_en[] PROGMEM = "GPS-mouse data"; |
static const char GPS_TYPE_de[] PROGMEM = "GPS-Maus Typ"; // aktuell nicht verwendet |
static const char GPS_TYPE_en[] PROGMEM = "GPS-mouse typ"; |
static const char GPS_ACTIVE_de[] PROGMEM = "GPS-Maus aktiv"; // aktuell nicht verwendet |
static const char GPS_ACTIVE_en[] PROGMEM = "GPS-mouse activ"; |
//############################################################################ |
//----------------------------- |
// Setup_FollowMe() |
//----------------------------- |
#define FME_DISTANCE 1 |
#define FME_SPEED 2 |
#define FME_RADIUS 3 |
#define FME_AZIMUTH 4 |
static const char FME_DISTANCE_de[] PROGMEM = "FollowMe Abstand"; |
static const char FME_DISTANCE_en[] PROGMEM = "followMe distance"; |
static const char FME_AZIMUTH_de[] PROGMEM = "FollowMe Winkel"; |
static const char FME_AZIMUTH_en[] PROGMEM = "followMe angle"; |
static const char FME_SPEED_de[] PROGMEM = "FollowMe Speed"; |
#define FME_SPEED_en FME_SPEED_de |
static const char FME_RADIUS_de[] PROGMEM = "Toleranz Radius"; |
static const char FME_RADIUS_en[] PROGMEM = "tolerance radius"; |
//############################################################################ |
//----------------------------- |
// Setup_OSD() |
//----------------------------- |
#define OSDS_SHOWCELLU 1 |
#define OSDS_LOWBAT 2 |
#define OSDS_RCERROR 3 |
#define OSDS_MKSETTING 4 |
#define OSDS_MAH 5 |
#define OSDS_DATASV2 6 |
#define OSDS_OUTFORMAT 7 |
#define OSDS_OUTPOLARITY 8 |
#define OSDS_FALLSPEED 9 |
#define OSDS_VARIOBEEP 10 |
#define OSDS_VOLTBAR 11 |
static const char OSDS_LOWBAT_de[] PROGMEM = "Warn Unterspannung"; |
static const char OSDS_LOWBAT_en[] PROGMEM = "Warn LowBat"; |
static const char OSDS_MAH_de[] PROGMEM = "Warn mAh"; |
static const char OSDS_MAH_en[] PROGMEM = "Warn mAh"; |
static const char OSDS_RCERROR_de[] PROGMEM = "Warn RC-Fehler"; |
static const char OSDS_RCERROR_en[] PROGMEM = "Warn RC-Error"; |
static const char OSDS_SHOWCELLU_de[] PROGMEM = "zeige Zellspannung"; |
static const char OSDS_SHOWCELLU_en[] PROGMEM = "show cell voltage"; |
static const char OSDS_MKSETTING_de[] PROGMEM = "zeige MK Setting"; |
static const char OSDS_MKSETTING_en[] PROGMEM = "show mk setting"; |
static const char OSDS_DATASV2_de[] PROGMEM = "Navi Daten an SV2"; |
static const char OSDS_DATASV2_en[] PROGMEM = "Navi data to SV2"; |
#ifdef USE_OSD_SCREEN_OLD |
static const char OSDS_OUTFORMAT_de[] PROGMEM = "OUT1/2 Format"; |
static const char OSDS_OUTFORMAT_en[] PROGMEM = "OUT1/2 format"; |
static const char OSDS_FALLSPEED_de[] PROGMEM = "Max. Sinkrate m/s"; |
static const char OSDS_FALLSPEED_en[] PROGMEM = "max fallspeed m/s"; |
static const char OSDS_VARIOBEEP_de[] PROGMEM = "Variometer Beep"; |
static const char OSDS_VARIOBEEP_en[] PROGMEM = "Variometer beep"; |
static const char OSDS_OUTPOLARITY_de[] PROGMEM = "OUT1/2 Polarität"; |
static const char OSDS_OUTPOLARITY_en[] PROGMEM = "OUT1/2 polarity"; |
static const char OSDS_VOLTBAR_de[] PROGMEM = "Volt Balken"; |
static const char OSDS_VOLTBAR_en[] PROGMEM = "volt bargraph"; |
#endif |
//############################################################################ |
//############################################################################ |
//# Allgemeines |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyOK( void ) |
{ |
set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(19, 7, strGet(OK) , MNORMAL); |
while( !get_key_press(1 << KEY_ENTER) ) PKT_CtrlHook(); // PKT-Update usw... |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyENDE( uint8_t beep ) |
{ |
if( beep ) set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
PKT_CtrlHook(); // PKT-Update usw... |
} |
clear_key_all(); |
} |
//############################################################################ |
//# BT-Funktionen (GPS-Maus) |
//############################################################################ |
#ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// BT_CheckBTM222() |
// |
// prueft ob das Bluetooth Modul im PKT installiert ist |
//-------------------------------------------------------------- |
uint8_t BT_CheckBTM222( void ) |
{ |
if( !Config.UseBT ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return false; // -> kein BTM222 eingebaut |
} |
return true; |
} |
//-------------------------------------------------------------- |
// BT_SelectDevice() |
// |
// Auswahl eines BT-Device aus einer Liste von BT-Devices |
//-------------------------------------------------------------- |
void BT_SelectDevice( void ) |
{ |
uint8_t itemid; |
uint8_t event; |
uint8_t i; |
//------------------------------- |
// BTM222 Modul vorhanden? |
//------------------------------- |
if( !BT_CheckBTM222() ) return; |
//------------------------------- |
// BT Device-Suche durchgefuert? |
//------------------------------- |
if( bt_devicecount == 0 ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Suche durchführen!"), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( strGet(STR_BT_DEVICES) ); // "BT Geräte" |
//--------------- |
// Menuitems |
//--------------- |
for( i = 0; i < bt_devicecount; i++ ) |
{ |
MenuCtrl_Push( i, MENU_ITEM, NOFUNC, device_list[i].DevName ); |
if( strncmp(device_list[i].mac,Config.gps_UsedMac,14) == 0 ) |
MenuCtrl_ItemMark( i, true ); // aktuelles BT-Device markieren |
} |
//--------------- |
// Control |
//--------------- |
event = MenuCtrl_Control( MENUCTRL_EVENT ); |
if( event == MENUEVENT_ITEM ) |
{ |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
strncpy( Config.gps_UsedMac , device_list[itemid].mac , 14); |
strncpy( Config.gps_UsedDevName, device_list[itemid].DevName, 20); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// BT_SearchDevices() |
// |
// sucht BT-Devices |
//-------------------------------------------------------------- |
void BT_SearchDevices( void ) |
{ |
uint8_t wahl; |
char *title; |
//------------------- |
// 1. Frage: suchen? |
//------------------- |
if( Config.gps_UsedDevName[0] != 0 ) // ist eine BT GPS-Maus bereits vorhanden? |
title = Config.gps_UsedDevName; // dann zeige im Titel den Namen der GPS-Maus an |
else |
title = "BT Devices"; |
set_beep( 50, 0xffff, BeepNormal); |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
wahl = PKT_AskX( ASK_END_START, strGet(STR_SEARCH_BT_ASK), strGet(STR_BT_SEARCHTIME), true, MenuCtrl_GetItemText(), true, title, false); // "BT Geräte suchen?" |
//------------------- |
// 2. BT suchen |
//------------------- |
if( wahl ) |
{ |
if( BT_CheckBTM222() ) |
{ |
//PKT_Title_P( text, lShowLipo, clearscreen ) |
PKT_Title_P( PSTR("BT Suche"), true, true ); |
lcdx_printp_center( 2, strGet(STR_SEARCH_BT), MNORMAL, 0,1); // "suche BT Geräte" |
//PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
bt_downlink_init(); |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
bt_searchDevice(); |
set_BTOff(); |
//PKT_Gauge_End(); // Gauge: Ende |
if( bt_devicecount==0 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NO_BT_FOUND), true, 2000, true, true); |
} |
else |
{ |
set_beep( 50, 0xffff, BeepNormal); |
BT_SelectDevice(); |
} |
} |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//############################################################################ |
//# Edit-Funktionen |
//############################################################################ |
//-------------------------------------------------------------- |
// Ausgabe von Werten fuer Edit_generic() |
//-------------------------------------------------------------- |
void Edit_generic_printout( int16_t Value, int16_t min, int16_t max, uint8_t what, int8_t yoffs) |
{ |
const char *pStr = 0; // PGM |
const char *pstr = 0; // RAM |
//uint8_t x = 0; |
//uint8_t y = 3; |
switch( what ) |
{ |
case Show_uint3: pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Show_uint5: pstr = buffered_sprintf_P( PSTR("%5d"), Value ); |
break; |
case Show_uint10th: pstr = buffered_sprintf_P( PSTR("%2.1d"), Value ); |
break; |
case MK_Connection: switch( Value ) |
{ |
case 0: pStr = PSTR("Wi.232"); break; |
case 1: pStr = strGet(KABEL); break; |
} |
if( (Value == 0) && (Config.UseWi == false) ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strGet(STR_WI232_ACTIVATE), MNORMAL, 0,-7); // "aktiviere Wi.232!" |
} |
else |
{ |
lcd_frect( 0, 37, 127, 21-6, 0); // Hilfebereich loeschen |
} |
break; |
case GPSMOUSE: switch( Value ) |
{ |
case GPS_Bluetoothmouse1: pStr = PSTR("BT-Mouse"); break; |
case GPS_Mikrokopter: pStr = PSTR("Mikrokopter"); break; |
default: pStr = PSTR("unknown"); |
} |
break; |
case Wi_Netmode: switch( Value ) |
{ |
case false: pStr = strGet(SLAVE); break; |
case true : pStr = strGet(NORMAL); break; |
} |
break; |
case OnOff: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = strGet(ON); break; |
} |
break; |
case YesNo: switch( Value ) |
{ |
case 0: pStr = strGet(NOO); break; |
case 1: pStr = strGet(YES); break; |
} |
break; |
case WlanMode: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = PSTR("AP-Mode"); break; |
case 2: pStr = PSTR("AdHoc"); break; |
} |
break; |
case NormRev: switch( Value ) // wird noch von stick/stick_setup.c verwendet |
{ |
case 0 : pStr = strGet(NORMAL); break; |
case 1 : pStr = strGet(REVERSE); break; |
} |
break; |
case Kontrast: if( Value >= max ) |
{ |
Value = max; |
set_beep( 200, 0x0080, BeepNormal); |
} |
if( Value == min ) |
{ |
Value = min; |
set_beep( 200, 0x0080, BeepNormal); |
} |
lcd_set_contrast( Value ); |
pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Baudrate: pstr = buffered_sprintf_P( PSTR("%ld"), Baud_to_uint32(Value) ); |
break; |
case Language: switch( Value ) |
{ |
case 0: pStr = strGet(DEUTSCH); break; |
case 1: pStr = strGet(ENGLISCH); break; |
} |
break; |
case Sticktype: switch( Value ) |
{ |
case false: pStr = strGet(POTI); break; |
case true : pStr = strGet(TASTER); break; |
} |
break; |
/* |
case WlanSecurity: // Anmerkung 02.04.2014 OG: wird aktuell nicht benoetigt weil anders geloest |
switch (Value) |
{ |
case 0x0 : lcd_printp_at(15, 2, PSTR("Adhoc"), 0); |
break; |
case 0x1 : lcd_printp_at(15, 2, PSTR("WPA "), 0); |
break; |
} |
break; |
*/ |
case PKT_Akku: switch( Value ) |
{ |
case true : pStr = PSTR("Lipo"); break; |
case false: pStr = PSTR("LiIon"); break; |
} |
break; |
} // end: switch( what ) |
//lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
if( pStr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16S"), pStr ); // PGM |
if( pstr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16s"), pstr ); // RAM |
} |
//-------------------------------------------------------------- |
// int16_t Edit_generic( Value, min, max, what, Addfactor, strHelp1, strHelp2) |
// |
// generische Funktion zum aendern von Setup-Werten |
// |
// PARAMETER: |
// Value: Ausgangswert der zu aendern ist |
// min : minimaler Wert |
// max : maximaler Wert |
// what : Typ des Wertes, abhaengig davon wird der Wert dargestellt, ENUM dafuer in setup.h |
// Addfactor : Sprungwert der beim erhöhen und erniedrigen addiert wird |
// strHelp1 : Hilefetext 1 (PGM) |
// strHelp2 : Hilefetext 2 (PGM) |
//S |
// RUECKGABE: |
// int16_t Return : Ergebnis der Veraenderung des Wertes |
//-------------------------------------------------------------- |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ) |
{ |
uint8_t redraw = true; |
uint8_t noCancel = false; |
int8_t yoffs = 0; // yoffs fuer das Edit-Label |
int8_t yoffsV = 4; // yoffs fuer den Eingabebereich |
const char *pStr; |
Pre = Value; |
do |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // Titel mit PKT-LiPo Anzeige und clearcsreen |
//------------------------ |
// Platz reservieren fuer |
// spezielle Typen |
//------------------------ |
if( what == MK_Connection ) |
{ |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 1 zeiliger Hilfstext |
//------------------------ |
if( strHelp1 != NULL && strHelp2 == NULL ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strHelp1, MNORMAL, 0,-7); // Hilfe-Text 1 |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 2 zeiliger Hilfstext |
//------------------------ |
if( strHelp2 != NULL ) |
{ |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_center( 5, strHelp1, MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_center( 6, strHelp2, MNORMAL, 0,-5); // Hilfe-Text 2 |
yoffs = -4; // yoffs fuer das Edit-Label |
yoffsV = -2; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// Label |
//------------------------ |
pStr = PSTR("???"); |
if( MenuCtrl_GetMenuIndex() >= 0 ) pStr = MenuCtrl_GetItemText(); // das Label wird vom Menueeintrag uebernommen (Menuetext muss im PGM sein!) |
else |
{ |
if( what == Language ) // Ausnahme: wird ggf. beim Start vom PKT von HAL_HW3_9.c aufgerufen |
{ |
pStr = strGetLanguage( LANGUAGE_de, LANGUAGE_en); |
noCancel = true; |
} |
} |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,yoffs, PSTR("%S:"), pStr ); // Ausgabe des Labels |
lcd_printp_at( 0, 7, strGet(KEYLINE5) , MNORMAL); // Keyline: <- -> Abbr. OK |
if( noCancel ) lcd_printp_at(11, 7, PSTR(" "), MNORMAL); // Keyline: "Abbr." loeschen wenn noCancel |
Edit_generic_printout( Value, min, max, what, yoffsV); // aktuellen Eingabewert anzeigen |
redraw = false; |
} |
//------------------------ |
// PKT CtrlHook |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Key: PLUS |
//------------------------ |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS), 2)) ) |
{ |
if( Value <= (max-Addfactor) ) Value = Value + Addfactor; |
else Value = min; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: MINUS |
//------------------------ |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS), 2)) ) |
{ |
if( Value >= (min + Addfactor) ) Value = Value - Addfactor; |
else Value = max; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: ENTER |
//------------------------ |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
return Value; |
} |
//------------------------ |
// Key: ESC |
//------------------------ |
if( !noCancel && get_key_press(1 << KEY_ESC) ) // Ende ohne speichern |
{ |
break; |
} |
} |
while( true ); |
get_key_press(KEY_ALL); |
return Pre; |
} |
//-------------------------------------------------------------- |
// bei HW 3.9x geht das nicht mehr wegen dem Platinenlayout |
// bei HW 3.9m geht es theoretisch noch - aber durch einen Bug scheinbar |
// nicht mehr. Kann nur von jemanden mit HW 3.9m repariert werden. |
// ALSO -> raus und Speicher sparen |
//-------------------------------------------------------------- |
/* |
uint8_t Edit_DisplayHelligkeit(uint8_t Value, uint8_t min, uint8_t max) |
{ |
float ValCorr = 2.55; // (Value * ValCorr) maximal 255 |
Pre = Value; |
OCR2A = Value * ValCorr; |
lcd_cls(); |
// lcd_printp_at (0, 2, Text, 0); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
write_ndigit_number_u (16, 2, Value, 3, 0,0); |
lcd_printp_at (17, 2, PSTR("%"), 0); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_2), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
write_ndigit_number_u (16, 2,Value, 3, 0,0); |
lcd_frect ((8*0), (8*4), (Value * (16*8) / 100), 6, 1); |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (Value < max)) |
{ |
Value++; |
if (Value >= max) |
{ |
Value = max; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (Value > min)) |
{ |
lcd_frect (((Value - 1) * (16*8) / 100), (8*4), (16*8), 6, 0); |
Value--; |
if (Value == min) |
{ |
Value = min; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
OCR2A = Value * ValCorr; |
Config.LCD_Helligkeit = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
OCR2A = Pre * ValCorr; |
Config.LCD_Helligkeit = Pre; |
// WriteParameter(); |
return Pre; |
} |
} |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type) |
{ |
uint8_t redraw = true; |
uint8_t x = 1; |
uint8_t I = 0; |
uint8_t lOk = false; |
uint8_t i; |
for( i = 0; i < length; i++) |
{ |
EditString[i] = data[i]; |
} |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
for( i = 0; i < length; i++) |
{ |
lcd_putc ( (i*2)+1, 3, EditString[i], MNORMAL); |
lcd_printp_at( (i*2)+2, 3, PSTR(" ") , MNORMAL); |
} |
lcd_rect( (x*6)-3, (8*3)-2, 10, 10, 1); |
lcd_printp_at(13, 6, PSTR("C"), MNORMAL); |
lcd_printp_at( 0, 7, PSTR(" \x17 \x16 \x19 OK"), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
if( (type == EDIT_BT_NAME) || (type == EDIT_SSID) || (type == EDIT_WL_PASSWORD)) // Name |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 2)) && EditString[I] < 'z') |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
if (EditString[I] > '9' && EditString[I] < 'A') |
EditString[I] = 'A'; |
if (EditString[I] > 'Z' && EditString[I] < 'a') |
EditString[I] = 'a'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 2)) && EditString[I] > ' ') |
{ |
EditString[I]--; |
if (EditString[I] < 'a' && EditString[I] > 'Z') |
EditString[I] = 'Z'; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
if (EditString[I] < '0' && EditString[I] > ' ') |
EditString[I] = ' '; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
else if( type == EDIT_BT_PIN ) // PIN |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (EditString[I] < '9')) |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (EditString[I] > '0')) |
{ |
EditString[I]--; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
if( get_key_long (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME|| type == EDIT_SSID || type == EDIT_WL_PASSWORD) EditString[I] = ' '; // Zeichen loeschen |
if( type == EDIT_BT_PIN) EditString[I] = '0'; // Zeichen setzen |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if( get_key_short (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME ) length_tmp = bt_name_length; |
if( type == EDIT_BT_PIN ) length_tmp = bt_pin_length; |
if (type == EDIT_SSID ) length_tmp = wlan_ssid_length; |
if (type == EDIT_WL_PASSWORD ) length_tmp = wlan_password_length; |
if ((x / 2) + 2 > length_tmp) |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
} |
else |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x++; |
x++; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I++; //Zeiger auf Zeichen |
} |
} |
if( get_key_press (1 << KEY_ENTER) ) |
{ |
lOk = true; |
if( type == EDIT_BT_NAME && EditString[0] == ' ' ) // BT-Name: 1. Zeichen darf nicht Space sein |
{ |
lcdx_printp_at( 0, 5, PSTR(" FEHLER! 1. Zeichen! "), MNORMAL, 0,-4); |
set_beep ( 300, 0xf00f, BeepNormal); |
_delay_ms(2000); |
lcd_frect( 0, 5*8-4, 127, 7, 0); // loesche Fehlertext |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); // setze Cursor auf Position 1 |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
get_key_press(KEY_ALL); |
lOk = false; |
} |
} |
} while( !lOk ); |
return 1; |
} |
//-------------------------------------------------------------- |
// min,max sind in Setup_PKTGeneric() festgelegt |
//-------------------------------------------------------------- |
void Edit_LipoOffset( void ) |
{ |
uint8_t redraw = true; |
uint8_t inc = 50; // in diesen Schritten hoch/runter zaehlen |
uint16_t min = 0; // min. Offset Wert |
uint16_t max = 15000; // max. Offset Wert |
uint16_t oldValue; |
if( Config.Lipo_UOffset % inc ) // ggf. wurde frueher ein anderer Wert fuer das Increment 'inc' verwendet |
Config.Lipo_UOffset = (uint16_t)(Config.Lipo_UOffset/inc)*inc; // hier wird der Wert auf das aktuelle 'inc' angepasst |
oldValue = Config.Lipo_UOffset; |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,-4, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_at( 1, 5, strGet(STR_HELP_LIPOOFFSET1), MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_at( 1, 6, strGet(STR_HELP_LIPOOFFSET2), MNORMAL, 0,-5); // Hilfe-Text 2 |
lcd_printp_at( 0, 7, strGet(KEYLINE5), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// Wert ausgeben |
//------------------------- |
lcdx_printf_at_P( 0, 3, MNORMAL, 0,-2, PSTR("> %5d => %1.2dv"), Config.Lipo_UOffset, volt_avg ); |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------- |
// Tasten |
//------------------------- |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2)) && (Config.Lipo_UOffset < max) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset + inc; |
} |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2)) && (Config.Lipo_UOffset > min) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset - inc; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return; |
} |
} while( !get_key_press(1 << KEY_ESC) ); |
clear_key_all(); |
Config.Lipo_UOffset = oldValue; // Abbruch, orginalen Wert wieder herstellen |
return; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_OLD |
uint8_t Edit_LED_Form (uint8_t Value, uint8_t min, uint8_t max ) |
{ |
Pre = Value; |
lcd_cls(); |
lcd_printp_at(0, 2, strGetOSDOLD(OSD_LED_Form), MNORMAL); |
switch (Value) |
{ |
case 0x1: |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
break; |
case 0x3 : |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
break; |
break; |
} |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
if ((get_key_press (1 << KEY_PLUS)) && (Value == 1)) |
{ |
Value = 3; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
} |
if ((get_key_press (1 << KEY_MINUS)) && (Value == 3)) |
{ |
Value = 1; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
edit = 1; |
Config.OSD_LEDform = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
edit = 0; |
Config.OSD_LEDform = Pre; |
return Pre; |
} |
} |
#endif // USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// Show_MAC( progmem, headline, mac) |
//-------------------------------------------------------------- |
void Show_MAC( uint8_t progmem, const char *headline, const char *mac ) |
{ |
uint8_t i; |
uint8_t z; |
lcd_cls(); |
PKT_TitleFromMenuItem( true ); |
if( progmem ) |
lcdx_printp_center( 2, headline, MNORMAL, 0,2); |
else |
lcdx_print_center( 2, (uint8_t *)headline, MNORMAL, 0,2); |
z = 0; |
for( i = 0; i < 13; i++) |
{ |
lcdx_putc( z+2, 5, mac[i], MNORMAL, 0,-1); |
if( (i%2==1) && (i<11) ) |
{ |
z++; |
lcdx_printp_at( z+2, 5, PSTR(":"), MNORMAL, 0,-1); |
} |
z++; |
} |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
Wait4KeyENDE( false ); |
} |
//-------------------------------------------------------------- |
// Setup_OSD() |
//-------------------------------------------------------------- |
void Setup_OSD( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Anzeige" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( OSDS_LOWBAT , MENU_ITEM, NOFUNC , OSDS_LOWBAT_de , OSDS_LOWBAT_en ); |
//MenuCtrl_PushML2_P( OSDS_MAH , MENU_ITEM, NOFUNC , OSDS_MAH_de , OSDS_MAH_en ); |
MenuCtrl_PushML2_P( OSDS_RCERROR , MENU_ITEM, NOFUNC , OSDS_RCERROR_de , OSDS_RCERROR_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_SHOWCELLU , MENU_ITEM, NOFUNC , OSDS_SHOWCELLU_de , OSDS_SHOWCELLU_en ); |
MenuCtrl_PushML2_P( OSDS_MKSETTING , MENU_ITEM, NOFUNC , OSDS_MKSETTING_de , OSDS_MKSETTING_en ); |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_FALLSPEED , MENU_ITEM, NOFUNC , OSDS_FALLSPEED_de , OSDS_FALLSPEED_en ); |
MenuCtrl_PushML2_P( OSDS_VARIOBEEP , MENU_ITEM, NOFUNC , OSDS_VARIOBEEP_de , OSDS_VARIOBEEP_en ); |
MenuCtrl_PushML2_P( OSDS_VOLTBAR , MENU_ITEM, NOFUNC , OSDS_VOLTBAR_de , OSDS_VOLTBAR_en ); |
MenuCtrl_PushML2_P( OSDS_OUTFORMAT , MENU_ITEM, NOFUNC , OSDS_OUTFORMAT_de , OSDS_OUTFORMAT_en ); |
MenuCtrl_PushML2_P( OSDS_OUTPOLARITY , MENU_ITEM, NOFUNC , OSDS_OUTPOLARITY_de , OSDS_OUTPOLARITY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_DATASV2 , MENU_ITEM, NOFUNC , OSDS_DATASV2_de , OSDS_DATASV2_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == OSDS_LOWBAT ) { Config.MK_LowBat = Edit_generic( Config.MK_LowBat ,32,255, Show_uint10th,1 ,strGet(STR_HELP_LOWBAT1),strGet(STR_HELP_LOWBAT2)); } |
if( itemid == OSDS_SHOWCELLU ) { Config.OSD_ShowCellU = Edit_generic( Config.OSD_ShowCellU , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_MAH ) { Config.OSD_mAh_Warning = Edit_generic( Config.OSD_mAh_Warning , 0,30000, Show_uint5,100 ,NULL,NULL); } |
if( itemid == OSDS_RCERROR ) { Config.OSD_RCErrorbeep = Edit_generic( Config.OSD_RCErrorbeep , 0, 1, OnOff,1 ,NULL,NULL); } |
if( itemid == OSDS_MKSETTING ) { Config.OSD_ShowMKSetting = Edit_generic( Config.OSD_ShowMKSetting, 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_DATASV2 ) { Config.OSD_SendOSD = Edit_generic( Config.OSD_SendOSD , 0, 1, YesNo,1 ,NULL,NULL); } |
#ifdef USE_OSD_SCREEN_OLD |
if( itemid == OSDS_FALLSPEED ) { Config.OSD_Fallspeed = Edit_generic( Config.OSD_Fallspeed , 0,247, Show_uint10th,1 ,NULL,NULL); } |
if( itemid == OSDS_VARIOBEEP ) { Config.OSD_VarioBeep = Edit_generic( Config.OSD_VarioBeep , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_OUTFORMAT ) { Config.OSD_LEDform = Edit_LED_Form(Config.OSD_LEDform , 1, 3 ); } |
if( itemid == OSDS_OUTPOLARITY ) { Config.OSD_InvertOut = Edit_generic( Config.OSD_InvertOut , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_VOLTBAR ) { Config.OSD_LipoBar = Edit_generic( Config.OSD_LipoBar , 0, 1, YesNo,1 ,NULL,NULL); } |
#endif |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Mark_OSDScreen( uint8_t osdscreenid ) |
{ |
MenuCtrl_ItemMarkR( osdscreenid, (Config.OSD_UseScreen & (1 << osdscreenid)) != 0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Edit_OSDScreen( uint8_t osdscreenid ) |
{ |
if( (Config.OSD_UseScreen & (1 << osdscreenid)) == 0 ) |
Config.OSD_UseScreen = (Config.OSD_UseScreen | (1 << osdscreenid)); |
else |
Config.OSD_UseScreen = (Config.OSD_UseScreen ^ (1 << osdscreenid)); |
Mark_OSDScreen( osdscreenid ); |
} |
//-------------------------------------------------------------- |
// Setup_OSDScreens() |
//-------------------------------------------------------------- |
void Setup_OSDScreens( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Screens" |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
MenuCtrl_Push_P( OSDSCREEN_NAVIGATION, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_NAVIGATION) ); Mark_OSDScreen( OSDSCREEN_NAVIGATION ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
MenuCtrl_Push_P( OSDSCREEN_WAYPOINTS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_WAYPOINTS) ); Mark_OSDScreen( OSDSCREEN_WAYPOINTS ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC_N |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
MenuCtrl_Push_P( OSDSCREEN_MKSTATUS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_MKSTATUS) ); Mark_OSDScreen( OSDSCREEN_MKSTATUS ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
MenuCtrl_Push_P( OSDSCREEN_USERGPS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_USERGPS) ); Mark_OSDScreen( OSDSCREEN_USERGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
MenuCtrl_Push_P( OSDSCREEN_3DLAGE , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_3DLAGE) ); Mark_OSDScreen( OSDSCREEN_3DLAGE ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
MenuCtrl_Push_P( OSDSCREEN_STATISTICS, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_STATISTIC) ); Mark_OSDScreen( OSDSCREEN_STATISTICS ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_Push_P( OSDSCREEN_OSD0, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD0) ); Mark_OSDScreen( OSDSCREEN_OSD0 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD1, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD1) ); Mark_OSDScreen( OSDSCREEN_OSD1 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD2, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD2) ); Mark_OSDScreen( OSDSCREEN_OSD2 ); |
#endif |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
Edit_OSDScreen( itemid ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_FollowMe() |
//-------------------------------------------------------------- |
#ifdef USE_FOLLOWME |
void Setup_FollowMe( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( PSTR("FollowMe Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nicht verwendet. FollowMe wird aktuell ueber die Daten-Updaterate |
// der BT GPS-Maus getriggert! |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( FME_REFRESH , MENU_ITEM, NOFUNC , FME_REFRESH_de , FME_REFRESH_en ); |
#ifdef USE_FOLLOWME_STEP2 |
MenuCtrl_PushML2_P( FME_DISTANCE, MENU_ITEM, NOFUNC, FME_DISTANCE_de, FME_DISTANCE_en ); |
MenuCtrl_PushML2_P( FME_AZIMUTH , MENU_ITEM, NOFUNC, FME_AZIMUTH_de , FME_AZIMUTH_en ); |
#endif |
MenuCtrl_PushML2_P( FME_SPEED , MENU_ITEM, NOFUNC, FME_SPEED_de , FME_SPEED_en ); |
MenuCtrl_PushML2_P( FME_RADIUS , MENU_ITEM, NOFUNC, FME_RADIUS_de , FME_RADIUS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC, GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//if( itemid == FME_REFRESH ) { Config.FM_Refresh = Edit_generic( Config.FM_Refresh ,250,60000, Show_uint3,1 ,NULL,NULL); } |
// FollowMeStep2: |
if( itemid == FME_DISTANCE) { Config.FM_Distance= Edit_generic( Config.FM_Distance , 0, 100, Show_uint3,1 ,strGet(STR_METERS),NULL); } |
if( itemid == FME_AZIMUTH ) { Config.FM_Azimuth = Edit_generic( Config.FM_Azimuth , 0, 360, Show_uint3,1 ,PSTR("Grad"),NULL); } |
// FollowMe: |
if( itemid == FME_SPEED ) { Config.FM_Speed = Edit_generic( Config.FM_Speed , 0, 100, Show_uint3,1 ,PSTR("0.1 m/s") ,NULL); } |
if( itemid == FME_RADIUS ) { Config.FM_Radius = Edit_generic( Config.FM_Radius , 1, 20, Show_uint3,1 ,strGet(STR_METERS),NULL); } |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif // USE_FOLLOWME |
//-------------------------------------------------------------- |
// Setup_GPSMouse() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_GPSMouse( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "GPS Maus" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( GPS_SEARCH , MENU_ITEM, &BT_SearchDevices , GPS_SEARCH_de , GPS_SEARCH_en ); |
MenuCtrl_PushML2_P( GPS_DEVICES , MENU_ITEM, &BT_SelectDevice , GPS_DEVICES_de , GPS_DEVICES_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil erstmal nicht weiter benoetigt. Wurde zwar verwendet in tracking.c |
// aber auch dort wurde betroffener Code deaktiviert - siehe Anmerkung tracking.c / PKT_tracking() |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_TYPE , MENU_ITEM, NOFUNC , GPS_TYPE_de , GPS_TYPE_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nirgendwo verwendet |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_ACTIVE , MENU_ITEM, NOFUNC , GPS_ACTIVE_de , GPS_ACTIVE_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_SHOWDEV , MENU_ITEM, NOFUNC , GPS_SHOWDEV_de , GPS_SHOWDEV_en ); |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC , GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// GPS_TYPE |
//-------------------- |
/* |
if( itemid == GPS_TYPE ) |
{ |
Config.gps_UsedGPSMouse = Edit_generic( Config.gps_UsedGPSMouse, 0,1,GPSMOUSE,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_ACTIVE |
//-------------------- |
/* |
if( itemid == GPS_ACTIVE ) |
{ |
Config.gps_UseGPS = Edit_generic( Config.gps_UseGPS, 0,1,YesNo,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_SHOWDEV |
//-------------------- |
if( itemid == GPS_SHOWDEV ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( false, Config.gps_UsedDevName, Config.gps_UsedMac); |
} |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// zeigt die Konfig des BTM-222 |
//-------------------------------------------------------------- |
void BTM222_Info( void ) |
{ |
lcd_cls(); |
set_Modul_On( Bluetooth ); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MNORMAL, PSTR("BTM-222 Config") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
bt_showsettings(); |
ScrollBox_Push_P( MNORMAL, PSTR("End of Config") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Setup_BTM222_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob BTM222 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_BTM222_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( BT_INSTALLED , MENU_ITEM, NOFUNC , BT_INSTALLED_de , BT_INSTALLED_en ); |
if( Config.UseBT ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_NAME , MENU_ITEM, NOFUNC , BT_NAME_de , BT_NAME_en ); |
MenuCtrl_PushML2_P( BT_PIN , MENU_ITEM, NOFUNC , BT_PIN_de , BT_PIN_en ); |
MenuCtrl_PushML2_P( BT_REID , MENU_ITEM, NOFUNC , BT_REID_de , BT_REID_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_INIT , MENU_ITEM, NOFUNC , BT_INIT_de , BT_INIT_en ); |
MenuCtrl_PushML2_P( BT_PCCONFIG , MENU_ITEM, &Port_FC2CFG_BT , BT_PCCONFIG_de , BT_PCCONFIG_en ); |
MenuCtrl_PushML2_P( BT_SHOWCONFIG, MENU_ITEM, &BTM222_Info , BT_SHOWCONFIG_de, BT_SHOWCONFIG_en); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_MAC , MENU_ITEM, NOFUNC , BT_MAC_de , BT_MAC_en ); |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BTM222() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_BTM222( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT; |
uint8_t i; |
char string[11]; |
Setup_BTM222_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
UseBT = Config.UseBT; |
Config.UseBT = Edit_generic( Config.UseBT ,0,1, YesNo,1 ,NULL,NULL); |
if( UseBT != Config.UseBT ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseBT ) // BTM222 wurde aktiviert - initialisieren |
{ |
BTM222_Initalize(); |
//if( bt_init() ) |
// Config.BTIsSet = true; |
//else |
// Config.BTIsSet = false; |
} |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_BTM222_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// BT_NAME |
//-------------------- |
if( itemid == BT_NAME ) |
{ |
bt_fixname(); |
strncpyfill( string, Config.bt_name, bt_name_length+1 ); // bt_name_length=10 + 1 Terminierende 0 |
Edit_String( string, bt_name_length, EDIT_BT_NAME ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
//lcd_print_at( 0, 5, EditString, 0); // DEBUG |
//lcdx_printf_at( 17, 5, MNORMAL, 0,0, "%3d", strlen(EditString) ); |
//_delay_ms(8000); |
strncpy( Config.bt_name, EditString, bt_name_length+1 ); |
} |
} |
//-------------------- |
// BT_PIN |
//-------------------- |
if( itemid == BT_PIN ) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
string[i] = Config.bt_pin[i]; |
} |
string[bt_pin_length] = 0; |
Edit_String( string, bt_pin_length, EDIT_BT_PIN ); |
if (edit == 1) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
Config.bt_pin[i] = EditString[i]; |
} |
} |
} |
//-------------------- |
// BT_INIT |
//-------------------- |
if( itemid == BT_INIT ) |
{ |
//lcd_cls (); |
//Old_Baudrate = Config.PKT_Baudrate; |
BTM222_Initalize(); |
/* |
if( bt_init() == true ) |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init ok"), MNORMAL); |
WriteBTInitFlag(); |
} |
else |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init Error"), MNORMAL); |
Config.BTIsSet = false; |
set_beep( 1000, 0x0040, BeepNormal); |
} |
_delay_ms(2500); // 2.5 Sekunden anzeigen fuer Benutzer |
*/ |
} |
//-------------------- |
// BT_MAC |
//-------------------- |
if( itemid == BT_MAC ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( true, PSTR("BTM-222 MAC"), Config.bt_Mac); |
} |
//-------------------- |
// BT_REID |
//-------------------- |
if( itemid == BT_REID ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
string[i] = Config.RE_ID[i]; |
} |
string[RE_ID_length] = 0; |
Edit_String( string, RE_ID_length, EDIT_BT_PIN ); |
if( edit == 1 ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
Config.RE_ID[i] = EditString[i]; |
} |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BLE() |
//-------------------------------------------------------------- |
void Setup_BLE( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( BLE_INSTALLED, MENU_ITEM, NOFUNC, strGet(MODULE_EXIST) ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
Config.UseBLE = Edit_generic( Config.UseBLE, 0, 1, YesNo, 1 ,strGet(STR_EXTSV2MODULE),NULL); |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_MKConnection() |
//-------------------------------------------------------------- |
void Setup_MKConnection( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MKCONN_CONNECTION , MENU_ITEM, NOFUNC , MKCONN_CONNECTION_de , MKCONN_CONNECTION_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( MKCONN_BAUDRATE , MENU_ITEM, NOFUNC , MKCONN_BAUDRATE_de , MKCONN_BAUDRATE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// MKCONN_CONNECTION |
//-------------------- |
if( itemid == MKCONN_CONNECTION ) |
{ |
old = Config.U02SV2; |
Config.U02SV2 = Edit_generic( Config.U02SV2, 0, 1, MK_Connection,1, NULL,NULL); |
if( Config.U02SV2 != old ) // Verbindung geaendert? |
{ |
if( Config.U02SV2 == 1 ) Change_Output( Uart02FC ); // 1 = Kabel |
else Change_Output( Uart02Wi ); // 0 = Wi.232 |
} |
if( (Config.U02SV2 == 0) && (Config.UseWi == false) ) // FEHLER: Wi.232 nicht aktiviert (Config.U02SV2 = 0 -> entspricht Wi.232) |
{ |
//PKT_Message_P( text , error, timeout, beep, clear) |
PKT_Message_P( strGet(STR_WI232_ACTIVATE), true , 3000 , true, true); |
} |
} |
//-------------------- |
// MKCONN_BAUDRATE |
//-------------------- |
if( itemid == MKCONN_BAUDRATE ) |
{ |
old = Config.PKT_Baudrate; |
Config.PKT_Baudrate = Edit_generic( Config.PKT_Baudrate ,1,6,Baudrate,1 ,PSTR("Standard: ! 57600 !"),NULL); |
if( Config.PKT_Baudrate != old ) |
{ |
// Wi.232 umkonfigurieren |
if( Config.UseWi ) |
{ |
Wi232_Initalize(); |
} |
// BTM222 umkonfigurieren |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
{ |
//set_BTOn(); |
BTM222_Initalize(); |
//set_BTOff(); |
} |
#endif // end: #ifdef USE_BLUETOOTH |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_WI232_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob Wi.232 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WI232_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Wi.232" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( WI_INSTALLED , MENU_ITEM, NOFUNC , strGet(CONNECT13) ); |
if( Config.UseWi ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_TXRX , MENU_ITEM, NOFUNC , WI_TXRX_de , WI_TXRX_en ); |
MenuCtrl_PushML2_P( WI_GROUP , MENU_ITEM, NOFUNC , WI_GROUP_de , WI_GROUP_en ); |
// 06.06.2014 OG |
//MenuCtrl_PushML2_P( WI_MODE , MENU_ITEM, NOFUNC , WI_MODE_de , WI_MODE_en ); |
//MenuCtrl_PushML2_P( WI_TIMEOUT , MENU_ITEM, NOFUNC , WI_TIMEOUT_de , WI_TIMEOUT_en ); |
//MenuCtrl_PushML2_P( WI_MTU , MENU_ITEM, NOFUNC , WI_MTU_de , WI_MTU_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_INIT , MENU_ITEM, NOFUNC , WI_INIT_de , WI_INIT_en ); |
//MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Port_USB2CFG_Wi , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Wi232_ConfigPC , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
} |
} |
//-------------------------------------------------------------- |
// Setup_WI232() |
//-------------------------------------------------------------- |
void Setup_WI232( void ) |
{ |
uint8_t itemid; |
uint8_t UseWi; |
Setup_WI232_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
if( itemid == WI_TXRX ) { Config.WiTXRXChannel = Edit_generic( Config.WiTXRXChannel , 0, 5, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_GROUP ) { Config.WiNetworkGroup = Edit_generic( Config.WiNetworkGroup , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MODE ) { Config.WiNetworkMode = Edit_generic( Config.WiNetworkMode , 0, 1, Wi_Netmode,1 ,NULL,NULL); } |
//if( itemid == WI_TIMEOUT ) { Config.WiTXTO = Edit_generic( Config.WiTXTO , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MTU ) { Config.WiUartMTU = Edit_generic( Config.WiUartMTU , 0, 127, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_INIT ) { Wi232_Initalize(); } |
//-------------------- |
// WI_INSTALLED |
//-------------------- |
if( itemid == WI_INSTALLED ) |
{ |
UseWi = Config.UseWi; |
Config.UseWi = Edit_generic( Config.UseWi, 0,1, YesNo,1, NULL,NULL); |
if( UseWi != Config.UseWi ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseWi ) |
Wi232_Initalize(); // Wi232 wurde aktiviert: init |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_WI232_MenuCreate(); |
continue; |
} |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_PKTGeneric() |
// |
// entspricht "PKT allgemein" |
//-------------------------------------------------------------- |
void Setup_PKTGeneric( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT allgemein" |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( PKT_LANGUAGE , MENU_ITEM, NOFUNC , LANGUAGE_de , LANGUAGE_en ); |
MenuCtrl_PushML2_P( TIME_ZONE , MENU_ITEM, NOFUNC , TIME_ZONE_de , TIME_ZONE_en ); |
MenuCtrl_PushML2_P( TIME_SUMMER , MENU_ITEM, NOFUNC , TIME_SUMMER_de , TIME_SUMMER_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( PKT_PKTOFF , MENU_ITEM, NOFUNC , PKT_PKTOFF_de , PKT_PKTOFF_en ); |
MenuCtrl_PushML2_P( PKT_LIGHTOFF , MENU_ITEM, NOFUNC , LIGHTOFF_de , LIGHTOFF_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
// 18.04.2014 OG: rausgenommen da es aktuell auch nicht mehr mit PKT 3.9m Hardware geht |
//if( PCB_Version == PKT39m ) // Helligkeit nur bei PKT39m - bei der PKT39x geht das nicht mehr |
// MenuCtrl_PushML2_P( PKT_BRIGHTNESS , MENU_ITEM, NOFUNC , BRIGHTNESS_de , BRIGHTNESS_en ); |
MenuCtrl_PushML2_P( PKT_CONTRAST , MENU_ITEM, NOFUNC , CONTRAST_de , CONTRAST_en ); |
MenuCtrl_PushML2_P( PKT_BEEPER , MENU_ITEM, NOFUNC , BEEPER_de , BEEPER_en ); |
#ifdef USE_SOUND |
MenuCtrl_PushML2_P( PKT_SOUNDMODUL , MENU_ITEM, NOFUNC , SOUNDMODUL_de , SOUNDMODUL_en ); |
MenuCtrl_PushML2_P( PKT_VOLUME , MENU_ITEM, NOFUNC , VOLUME_de , VOLUME_en ); |
#endif |
MenuCtrl_PushML2_P( PKT_ACCUTYPE , MENU_ITEM, NOFUNC , ACCUTYPE_de , ACCUTYPE_en ); |
MenuCtrl_PushML2_P( PKT_ACCUMEASURE , MENU_ITEM, NOFUNC , ACCUMEASURE_de , ACCUMEASURE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == PKT_LANGUAGE ) |
{ |
old = Config.DisplayLanguage; |
Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage , 0, 1, Language,1, NULL,NULL); |
if( old != Config.DisplayLanguage ) |
{ |
PKT_Ask_Restart( strGetLanguage(LANGUAGE_de,LANGUAGE_en) ); |
} |
} |
if( itemid == TIME_ZONE ) Config.timezone = Edit_generic( Config.timezone ,-12, 12, Show_uint3,1, PSTR("+1 = Berlin "),PSTR("-5 = New York")); |
if( itemid == TIME_SUMMER ) Config.summertime = Edit_generic( Config.summertime , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_LIGHTOFF ) Config.DisplayTimeout = Edit_generic( Config.DisplayTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_PKTOFF ) Config.PKTOffTimeout = Edit_generic( Config.PKTOffTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_BEEPER ) Config.HWBeeper = Edit_generic( Config.HWBeeper , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_SOUNDMODUL ) Config.HWSound = Edit_generic( Config.HWSound , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_VOLUME ) Config.Volume = Edit_generic( Config.Volume , 0, 50, Show_uint3,1, NULL,NULL); |
if( itemid == PKT_CONTRAST ) { Config.LCD_Kontrast = Edit_generic( Config.LCD_Kontrast , 10, 40, Kontrast,1, NULL,NULL); |
lcd_set_contrast( Config.LCD_Kontrast ); |
} |
//if( itemid == PKT_BRIGHTNESS ) Config.LCD_Helligkeit = Edit_DisplayHelligkeit(Config.LCD_Helligkeit,0,100); |
if( itemid == PKT_ACCUTYPE ) Config.PKT_Accutyp = Edit_generic( Config.PKT_Accutyp , 0, 1, PKT_Akku,1, NULL,NULL); |
if( itemid == PKT_ACCUMEASURE ) Edit_LipoOffset(); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Main() |
// |
// Das Hauptmenue fuer das gesamte Setup |
//-------------------------------------------------------------- |
void Setup_MAIN( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Setup" |
//MenuCtrl_SetTitle_P( PSTR("PKT Setup") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( SETUP_PKTCONFIG , MENU_SUB , &Setup_PKTGeneric , SETUP_PKTCONFIG_de , SETUP_PKTCONFIG_en ); |
MenuCtrl_PushML2_P( SETUP_OSDVIEW , MENU_SUB , &Setup_OSD , SETUP_OSDVIEW_de , SETUP_OSDVIEW_en ); |
MenuCtrl_PushML2_P( SETUP_OSDSCREENS , MENU_SUB , &Setup_OSDScreens , SETUP_OSDSCREENS_de , SETUP_OSDSCREENS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_MKCONNECTION , MENU_SUB , &Setup_MKConnection , SETUP_MKCONNECTION_de , SETUP_MKCONNECTION_en ); |
MenuCtrl_PushML2_P( SETUP_WI232 , MENU_SUB , &Setup_WI232 , SETUP_WI232_de , SETUP_WI232_en ); |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_BTM222 , MENU_SUB , &Setup_BTM222 , SETUP_BTM222_de , SETUP_BTM222_en ); |
#endif |
#if (defined(USE_SV2MODULE_BLE) || defined(USE_WLAN)) |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_BLE , MENU_SUB , &Setup_BLE , SETUP_BLE_de , SETUP_BLE_en ); |
#endif |
#ifdef USE_WLAN |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_WIFLY , MENU_SUB , &Setup_WiFly , SETUP_WIFLY_de , SETUP_WIFLY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_GPSMAUS , MENU_SUB , &Setup_GPSMouse , SETUP_GPSMAUS_de , SETUP_GPSMAUS_en ); |
#endif |
#ifdef USE_FOLLOWME |
MenuCtrl_PushML2_P( SETUP_FOLLOWME , MENU_SUB , &Setup_FollowMe , SETUP_FOLLOWME_de , SETUP_FOLLOWME_en ); |
#endif |
#ifdef USE_TRACKING |
MenuCtrl_PushML2_P( SETUP_SERVOCONFIG , MENU_SUB , &Setup_ServoTracking, SETUP_SERVOCONFIG_de , SETUP_SERVOCONFIG_en ); // tracking/servo_setup.c |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( SETUP_JOYSTICK , MENU_SUB , &Setup_Joysticks , SETUP_JOYSTICK_de , SETUP_JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_PKTUPDATE , MENU_ITEM, &PKT_Update , SETUP_PKTUPDATE_de , SETUP_PKTUPDATE_en ); |
//MenuCtrl_PushML2_P( SETUP_PKTDEBUG , MENU_ITEM, NOFUNC , SETUP_PKTDEBUG_de , SETUP_PKTDEBUG_en ); |
MenuCtrl_PushML2_P( SETUP_EEPROMRESET , MENU_ITEM, &PKT_Reset_EEprom , SETUP_EEPROMRESET_de , SETUP_EEPROMRESET_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// SETUP_PKTDEBUG |
//-------------------- |
if( itemid == SETUP_PKTDEBUG ) |
{ |
Config.Debug = Edit_generic( Config.Debug, 0,1,Show_uint3,1 ,NULL,NULL); |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/setup/setup.h |
---|
0,0 → 1,111 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.h |
//# |
//# 26.06.2014 OG |
//# - add: Setup_FollowMe() |
//# |
//# 22.06.2014 OG |
//# - del: Variable CheckGPS |
//# |
//# 13.06.2014 OG |
//# - del: ChangeWi_SV2() |
//# |
//# 11.06.2014 OG |
//# - del: Edit_Language() |
//# |
//# 31.04.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt |
//# |
//# 28.05.2014 OG |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# - Parameter 'Text' enfernt da vom Menue geerbt |
//# - Parameter fuer Hilfetexte |
//# |
//# 04.04.2014 OG |
//# - del: Edit_ShowTitle_P() |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: WlanMode |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um WlanSecurity erweitert |
//# |
//# 22.05.2013 OG |
//# - del: verschiedene Cleanups |
//############################################################################ |
#ifndef _setup_H |
#define _setup_H |
#define EDIT_BT_NAME 1 |
#define EDIT_BT_PIN 2 |
#define EDIT_SSID 3 |
#define EDIT_WL_PASSWORD 4 |
typedef enum // Varianten fuer die Anzeige der Werte im Setupmenüe |
{ |
Show_uint3, Show_int3, Show_uint5, Show_uint10th, GPSMOUSE, Wi_Netmode, |
OnOff, YesNo, Orientation, NormRev, Kontrast, Baudrate, Language, Sticktype, WlanMode, WlanSecurity, PKT_Akku, MK_Connection |
} EditMode; |
void Setup_MAIN(void); |
void Setup_FollowMe( void ); |
void Display_Setup (void); |
void Wi_Use (void); |
void BT_Use (void); |
void BT_SelectDevice (void); |
uint8_t BT_CheckBTM222( void ); |
void Wait4KeyOK( void ); |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ); |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type); |
uint8_t BT_ShowGPSData( uint8_t modus ); |
void BTM222_Info( void ); |
extern uint8_t bt_name_len; |
extern uint8_t length_tmp; |
extern uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
extern uint8_t edit; |
extern char EditString[21]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/setup |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/sound/pwmsine8bit.c |
---|
0,0 → 1,280 |
//file pwmsine8bit.c |
//generate 8 bit sinewave using wavetable lookup |
//12khz sample rate, 62500 hz pwm on pd5 oc1a |
//for ere co embmega32 16Mhz 38400 |
//May 3 07 Bob G initial edit 8 bit |
//May 5 07 Bob G add traffic tones |
//Jan 18 08 Bob G compile with 7.15 |
//#include <stdio.h> |
//#include <stdlib.h> |
#include <ctype.h> |
#include <math.h> //sin |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdint.h> |
#include "../timer/timer.h" |
#include "../main.h" |
#ifdef USE_SOUND |
#define INTR_OFF() asm("CLI") |
#define INTR_ON() asm("SEI") |
//------vars in bss------- |
volatile unsigned char sindat[256]; //8 bit sine table |
//unsigned int addat[8]; //internal 10 bit |
unsigned int idesfreq; |
unsigned char iattenfac; //0-255 |
unsigned char dispon; |
unsigned char freqincipart,freqincfpart; |
unsigned char waveptipart,waveptfpart; |
unsigned char tics; |
unsigned char generate; |
float basefreq; |
float freqratio; |
float sampfreq; //12khz |
float samppd; //83.33usec |
float fdesfreq; |
float attendb; //0-50 |
float attenfac; //.999-0 |
const float pi=3.141592654; |
#define MSB(w) ((char*) &w)[1] |
#define LSB(w) ((char*) &w)[0] |
#define TONE_BUFFER_SIZE 64 |
#define TONE_BUFFER_MASK ( TONE_BUFFER_SIZE - 1) |
static volatile unsigned char ToneBuf[TONE_BUFFER_SIZE]; |
static volatile unsigned char ToneBufHead; |
static volatile unsigned char ToneBufTail; |
//------------------------------- |
void cvtfreq2frac(void){ |
//calc freq ratio, separate into integer and fractional parts |
fdesfreq=idesfreq; |
freqratio=fdesfreq/basefreq; //calc freq ratio from basefreq |
freqincipart=(int)freqratio; //calc integer part |
freqincfpart=(freqratio-freqincipart)*256; //calc fractional part |
// cprintf("ipart %02x fpart %02x\n",freqincipart,freqincfpart); |
} |
//------------------------------- |
void cvtdb2fac(void){ |
//cvt db to attenuation factor |
attenfac=pow(10.0,-attendb/20.0); //10^0=1 |
iattenfac=(unsigned char)(attenfac*255.0); |
// cprintf("attenfac %#7.5f\n",attenfac); |
// cprintf("iattenfac %02x\n",iattenfac); |
} |
//------------------------- |
void initsindat(void){ |
//init 8 bit signed sin table 0-ff in ram 36dB s/n 46Hz basefreq |
unsigned int n; |
for(n=0; n < 256; n++){ |
sindat[n]=(signed char)127.0*sin(2.0*pi*n/256.0); //256 sin points |
} |
} |
//------------------------- |
void initvars(void){ |
//init vars |
// sampfreq=12000.0; //hz |
sampfreq=19530.0; //hz |
// printf("samp freq %#7.1f hz\n\r",sampfreq); |
// printf("samp freq %#7.1f hz\n\r",1234567.1); |
samppd=1.0/sampfreq; //83.33usec |
// printf("samppd %#7.1f usec \n\r",samppd*1e6); |
basefreq=sampfreq/256.0; //46.875hz |
// printf("basefreq %#7.5f hz\n\r",basefreq); |
idesfreq=400; |
cvtfreq2frac(); |
cvtdb2fac(); //0db =.9999= 0xff |
} |
//--------------------- |
void tone1(int hz) |
//tone at hz for ms |
{ |
uint8_t volume = 0; |
// TCNT2=0; |
// sound_timer = ms/5; |
// soundpause_timer = soundpause/5; |
idesfreq=hz; |
cvtfreq2frac(); |
attendb=volume; |
cvtdb2fac(); |
if (hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
} |
//------------------------------- |
void tone_handler(void) // wird aus Timerinterrupt aufgerufen |
{ |
uint16_t f_Hz=0; |
uint16_t dur_ms=0; |
char volume=0; |
char tmp_high = 0; |
char tmp_low = 0; |
unsigned char tmptail; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* f_Hz low */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_high = ToneBuf[tmptail]; /* f_Hz high */ |
}else{ |
return; |
} |
f_Hz = (((uint16_t) tmp_high) << 8) | tmp_low; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
volume = ToneBuf[tmptail]; /* volume */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* low Duratuion */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
tmp_high = ToneBuf[tmptail]; /* high Duration */ |
}else{ |
return; |
} |
dur_ms = (((uint16_t) tmp_high) << 8) | tmp_low; |
sound_timer = dur_ms/10; |
// TCNT2=0; |
idesfreq= f_Hz; |
cvtfreq2frac(); |
attendb= volume; |
cvtdb2fac(); |
if (f_Hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
// printf("tone_handler f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
} |
void tone_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (ToneBufHead + 1) & TONE_BUFFER_MASK; |
while ( tmphead == ToneBufTail ){ |
;/* wait for free space in buffer */ |
} |
ToneBuf[tmphead] = data; |
ToneBufHead = tmphead; |
}/* tone_putc */ |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume) |
{ |
// printf("Playtone f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
tone_putc(LSB(f_Hz)); |
tone_putc( MSB(f_Hz)); |
tone_putc( volume); |
tone_putc(LSB(dur_ms)); |
tone_putc(MSB(dur_ms)); |
} |
//----------------- |
void InitSound(void){//main program |
ToneBufHead = 0; |
ToneBufTail = 0; |
initvars(); |
initsindat(); |
Timer2_Init(); |
Timer3_Init(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/sound/pwmsine8bit.h |
---|
0,0 → 1,22 |
/* |
* pwmsine8bit.h |
* |
* Created on: 01.09.2012 |
* Author: cebra |
*/ |
#ifndef PWMSINE8BIT_H_ |
#define PWMSINE8BIT_H_ |
extern unsigned char freqincipart,freqincfpart; |
extern unsigned char waveptipart,waveptfpart; |
extern unsigned char iattenfac; //0-255 |
extern unsigned char generate; |
extern volatile unsigned char sindat[256]; //8 bit sine table |
void InitSound(void); |
void tone(int hz,uint8_t volume, uint16_t ms, uint16_t soundpause); |
void tone1(int hz); |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume); |
void tone_handler(void); |
#endif /* PWMSINE8BIT_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/stick/stick.c |
---|
0,0 → 1,507 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* Copyright (C) 2012 Martin Runkel * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
//#include "../tracking/servo.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "stick.h" |
#include "stick_setup.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#define SERVO_CORRECT 3.125 |
#include <util/delay.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
int16_t Pos_Stick[12]; // 1,5mS |
int16_t Pos_alt[5]; // |
uint8_t BalkenPos = 0; |
uint8_t Stick_Display = 0; |
//uint8_t serialChannelRichtung = 0; |
//uint8_t serialChannelNeutral = 0; |
//uint8_t serialChannelConfig = 2; |
//-------------------------------------------------------------- |
// |
void joystick (void) |
{ |
// uint8_t chg = 0; |
// uint8_t Pos_Stick = 150; // 1,5mS |
// uint8_t Pos_alt = 150; // |
//int16_t Pos_Stick[12]; // 1,5mS |
uint8_t chg = 0; |
//uint8_t BalkenPos = 0; |
uint8_t Stick_Nr = 0; |
//uint8_t Stick_Display = 0; |
uint8_t i = 0; |
memset (Pos_Stick, 150, 3); // füllt 3+1 Byte vom Pos_Stick[12] mit 150 |
//int16_t Pos_alt[5]; // |
int16_t Poti_Summe[5]; // |
memset (Poti_Summe, 0, 5); // füllt 3+1 Byte mit 0 |
int16_t Poti_Neutral[5]; // |
// ADC- init |
//if (Config.Lipomessung == true) |
//{ |
//} |
//for (uint8_t i = 0; i < 4; i++) |
if (Config.stick_typ[0] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA0); |
DDRA &= ~(1<<DDA0); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA0); |
DDRA |= (1<<DDA0); |
} |
if (Config.Lipomessung == false) |
{ |
if (Config.stick_typ[1] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA1); |
DDRA &= ~(1<<DDA1); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA1); |
DDRA |= (1<<DDA1); |
} |
} |
if (Config.stick_typ[2] == 0) |
{ |
PORTA &= ~(1<<PORTA2); |
DDRA &= ~(1<<DDA2); |
} |
else |
{ |
PORTA |= (1<<PORTA2); |
DDRA |= (1<<DDA2); |
} |
if (Config.stick_typ[3] == 0) |
{ |
PORTA &= ~(1<<PORTA3); |
DDRA &= ~(1<<DDA3); |
} |
else |
{ |
PORTA |= (1<<PORTA3); |
DDRA |= (1<<DDA3); |
} |
//if (Config.stick_dir[i] == 0) serialChannelRichtung &= ~(1<<i); else serialChannelRichtung |= (1<<i); |
//if (Config.stick_neutral[i] == 0) serialChannelNeutral &= ~(1<<i); else serialChannelNeutral |= (1<<i); |
//if (Config.Lipomessung == true) |
//serialChannelConfig |
// Ermittlung der Neutralposition |
for (uint8_t i = 0; i < 4; i++) |
{ |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
} |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
ADCSRA |= (1<<ADSC); // ADC Start |
/* |
Stick_Nr = 0; |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[Stick_Nr] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
// Stick-Neutralposition bestimmen |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 2 ; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
*/ |
//OCR1A = 150 * SERVO_CORRECT; // Servomitte |
lcd_cls (); |
// Kopfzeile und Rahmen zeichnen |
lcd_printp (PSTR(" serielle Potis 1-5 "), 2); |
//lcd_printp_at (7, 5, PSTR("%"), 0); |
//lcd_printp_at (16, 5, PSTR("mS"), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x19O\x18"), 0); |
for (i=0 ; i< 5 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_rect(3,BalkenPos, 120, 6, 1); // +-150% Rahmen |
lcd_line(23,BalkenPos,23,(BalkenPos+6),1); // -100% |
lcd_line(43,BalkenPos,43,(BalkenPos+6),1); // -50% |
lcd_frect(62,BalkenPos, 2, 6, 1); // 0% |
lcd_line(83,BalkenPos,83,(BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
} |
// Reset auf Mittelstellung |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
chg = 255; |
do |
{ |
if (!(ADCSRA & (1<<ADSC))) // wenn ADC fertig |
{ |
//Pos_Stick[Stick_Nr] = 150 + 128 - ((ADCW>>2)&0xff); |
//if (serialChannelRichtung & (1<<Stick_Nr)) |
if (Config.stick_dir[Stick_Nr] > 0) |
Pos_Stick[Stick_Nr] = Poti_Neutral[Stick_Nr] - ((ADCW>>2)&0xff); |
else |
Pos_Stick[Stick_Nr] = ((ADCW>>2)&0xff) - Poti_Neutral[Stick_Nr]; |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],-120,120); |
//if ((Stick_Nr==0) || (Stick_Nr==2)) // nur die Potis 1,2 sind nicht neutralisierend |
if (Config.stick_neutral[Stick_Nr]==0) // nicht neutralisierend |
{ |
Poti_Summe[Stick_Nr] += (Pos_Stick[Stick_Nr]/8) * abs(Pos_Stick[Stick_Nr]/8); |
LIMIT_MIN_MAX (Poti_Summe[Stick_Nr],-(120*128),(120*128)); |
Pos_Stick[Stick_Nr]= Poti_Summe[Stick_Nr] / 128; // nicht neutralisierend |
} |
Pos_Stick[Stick_Nr] += 150; |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],Config.stick_min[Stick_Nr],Config.stick_max[Stick_Nr]); // war 30, 270 |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) // nur bei Änderung |
{ |
chg |= (1<<Stick_Nr) ; |
//Pos_alt=Pos_Stick ; // verschoben |
} |
Stick_Nr ++ ; |
//if (Stick_Nr==1) Stick_Nr=2; // Lipo überspringen |
if (Stick_Nr==3) // Taster |
{ |
if (Config.stick_dir[Stick_Nr] > 0) |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
} |
else |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
} |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) |
{ |
chg |= (1<<Stick_Nr) ; |
} |
Stick_Nr=0; |
} |
/* |
#ifndef ohne_Lipo // MartinR |
Stick_Nr = 1; // MartinR AD-Kanal 1 überspringen wegen Lipo Überwachung |
#endif |
*/ |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 0= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
//serialPotis (); |
} |
if ((get_key_press (1 << KEY_PLUS) | get_key_long_rpt_sp ((1 << KEY_PLUS), 3))) |
//if (get_key_press (1 << KEY_PLUS)) |
//if (PINA & (1 << KEY_PLUS)) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] ++ ; else Pos_Stick[4] -- ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_max[4] ; else Pos_Stick[4] = Config.stick_min[4] ; |
} |
chg |= (1<<4) ; |
} |
else if ((get_key_press (1 << KEY_MINUS) | get_key_long_rpt_sp ((1 << KEY_MINUS), 3))) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] -- ; else Pos_Stick[4] ++ ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_min[4] ; else Pos_Stick[4] = Config.stick_max[4] ; |
} |
chg |= (1<<4) ; |
} |
if (Config.stick_neutral[4]!= 0 && Pos_Stick[4]!= 150 && (PINA &(1 << KEY_PLUS)) && (PINA &(1 << KEY_MINUS))) |
{ |
Pos_Stick[4] = 150 ; |
chg |= (1<<4) ; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
/* |
for (i=0 ; i< 4 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_frect (4, (BalkenPos+1), 118, 4, 0); // Balken löschen |
lcd_frect(62, BalkenPos, 2, 6, 1); // 0% |
} |
*/ |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
BeepTime = 200; |
BeepMuster = 0x0080; |
chg = 255; |
} |
if (chg) |
{ |
if (chg & (1<<0)); // Stick 1 |
{ |
BalkenPos = 12 + (0*8) ; |
Stick_Display = 0; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
// Stick 2 = Lipo |
if (chg & (1<<1)); // Stick 2 |
{ |
BalkenPos = 12 + (1*8) ; |
Stick_Display = 1; |
//if (serialChannelConfig & (0<<1)) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
if (Config.Lipomessung == false) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<2)); // Stick 3 |
{ |
BalkenPos = 12 + (2*8) ; |
Stick_Display = 2; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<3)); // Stick 4 = Taster |
{ |
BalkenPos = 12 + (3*8) ; |
Stick_Display = 3; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<4)); // Stick 5 = Taster vom PKT |
{ |
BalkenPos = 12 + (4*8) ; |
Stick_Display = 4; |
Balken_Zeichnen () ; |
//OCR1A = (((Pos_Stick[Stick_Display]-150)/1.6)+150) * SERVO_CORRECT; // Servostellung , 1.6=0.8*0.5 |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
chg = 0; |
serialPotis (); |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
//#ifdef HWVERSION3_9 |
//#ifndef ohne_Lipo // MartinR |
if (Config.Lipomessung == true) // MartinR: geändert |
{ |
//DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
ADC_Init(); // ADC für Lipomessung wieder aktivieren |
} |
//ADC_Init(); // ADC für Lipomessung wieder aktivieren |
//#endif |
//#endif |
} |
//-------------------------------------------------------------- |
// |
void serialPotis (void) |
{ |
uint8_t i = 0; |
memset (buffer, 0, 12); // füllt die 12+1 Byte vom buffer mit 0 |
for (i=0 ; i< 5 ; i++) |
{ |
buffer[i] = Pos_Stick[i]-150 ; |
} |
SendOutData('y', ADDRESS_FC, 1, buffer, 12); |
} |
//-------------------------------------------------------------- |
// |
void Balken_Zeichnen (void) |
{ |
// Balken löschen |
if ((Pos_Stick[Stick_Display] > Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] < 150)) // Balken links löschen |
lcd_frect ((63-((150 -Pos_alt[Stick_Display]) * 0.5)), (BalkenPos+1), (63-((150- Pos_Stick[Stick_Display]) * 0.5)), 4, 0); |
if ((Pos_Stick[Stick_Display] < Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] > 150)) // Balken rechts löschen |
lcd_frect ((63+((Pos_Stick[Stick_Display] - 150) * 0.5)), (BalkenPos+1), (63+((Pos_alt[Stick_Display] - 150) * 0.5)), 4, 0); |
// Balken zeichnen |
if (Pos_Stick[Stick_Display] >= 150) |
{ |
lcd_frect (63, (BalkenPos+1), ((Pos_Stick[Stick_Display] - 150) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((Pos_Stick[Stick_Display] - 150) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
else |
{ |
lcd_frect (63 - ((150 - Pos_Stick[Stick_Display]) * 0.5), (BalkenPos+1), ((150 - Pos_Stick[Stick_Display]) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((150 - Pos_Stick[Stick_Display]) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
// Raster zeichnen |
lcd_line(3, BalkenPos,3, (BalkenPos+6),1); // -150% |
lcd_line(23, BalkenPos,23, (BalkenPos+6),1); // -100% |
lcd_line(43, BalkenPos,43, (BalkenPos+6),1); // -50% |
lcd_line(83, BalkenPos,83, (BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
lcd_line(123,BalkenPos,123,(BalkenPos+6),1); // +150% |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/stick/stick.h |
---|
0,0 → 1,42 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _STICK_H |
#define _STICK_H |
void joystick (void); |
void serialPotis (void); |
void Balken_Zeichnen (void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/stick/stick_setup.c |
---|
0,0 → 1,279 |
/****************************************************************/ |
/* Setup für die Sticks */ |
/* 2013 Cebra */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY stick_setup.c |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 31.05.2013 OG |
//# - chg: einige redundante Menuetitel auf define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "stick_setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
#ifdef USE_JOYSTICK |
//############################################################################ |
//----------------------------- |
// Setup_Joysticks() |
//----------------------------- |
#define JSTICKS_OVERVIEW 1 |
#define JSTICKS_STICK1 2 |
#define JSTICKS_STICK2 3 |
#define JSTICKS_STICK3 4 |
#define JSTICKS_STICK4 5 |
#define JSTICKS_STICK5 6 |
#define JSTICKS_LIPO 7 |
static const char JSTICKS_OVERVIEW_de[] PROGMEM = "Übersicht"; |
static const char JSTICKS_OVERVIEW_en[] PROGMEM = "summary"; |
static const char JSTICKS_STICK1_de[] PROGMEM = "Stick 1"; |
#define JSTICKS_STICK1_en JSTICKS_STICK1_de |
static const char JSTICKS_STICK2_de[] PROGMEM = "Stick 2"; |
#define JSTICKS_STICK2_en JSTICKS_STICK2_de |
static const char JSTICKS_STICK3_de[] PROGMEM = "Stick 3"; |
#define JSTICKS_STICK3_en JSTICKS_STICK3_de |
static const char JSTICKS_STICK4_de[] PROGMEM = "Stick 4"; |
#define JSTICKS_STICK4_en JSTICKS_STICK4_de |
static const char JSTICKS_STICK5_de[] PROGMEM = "Stick 5"; |
#define JSTICKS_STICK5_en JSTICKS_STICK5_de |
static const char JSTICKS_LIPO_de[] PROGMEM = "PKT Lipomessung"; |
static const char JSTICKS_LIPO_en[] PROGMEM = "PKT Lipo measure."; |
//############################################################################ |
//----------------------------- |
// Setup_Stick() |
//----------------------------- |
#define MSTICK_MIN 1 |
#define MSTICK_MAX 2 |
#define MSTICK_TYPE 3 |
#define MSTICK_DIR 4 |
#define MSTICK_NEUT 5 |
static const char MSTICK_MIN_de[] PROGMEM = "Minimal Wert"; |
static const char MSTICK_MIN_en[] PROGMEM = "minimal value"; |
static const char MSTICK_MAX_de[] PROGMEM = "Maximal Wert"; |
static const char MSTICK_MAX_en[] PROGMEM = "maximal value"; |
static const char MSTICK_TYPE_de[] PROGMEM = "Type"; |
static const char MSTICK_TYPE_en[] PROGMEM = "type"; |
static const char MSTICK_DIR_de[] PROGMEM = "Richtung"; |
static const char MSTICK_DIR_en[] PROGMEM = "direction"; |
static const char MSTICK_NEUT_de[] PROGMEM = "Neutralisiered"; |
static const char MSTICK_NEUT_en[] PROGMEM = "neutralizing"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Joysticks_Uebersicht(void) |
{ |
lcd_cls (); |
lcd_printpns_at(0, 0, PSTR(" Joystick Setup "), 2); |
lcd_printpns_at(0, 1, PSTR("S Min Max Typ Dir N"), 0); |
for (uint8_t i = 0; i < 5; i++) { |
write_ndigit_number_u (0, 2+i,i+1, 1, 0,0); |
write_ndigit_number_u (2, 2+i,Config.stick_min[i], 3, 0,0); |
write_ndigit_number_u (6, 2+i,Config.stick_max[i], 3, 0,0); |
if (Config.stick_typ[i] == 0) lcd_printpns_at(10, 2+i, PSTR("Poti"), 0); else lcd_printpns_at(10, 2+i, PSTR("Tast"), 0); |
//if (Config.stick_typ[i] == 0) // MartinR: geändert |
//{ |
if (Config.stick_dir[i] == 0) lcd_printpns_at(15, 2+i, PSTR("Norm"), 0); else lcd_printpns_at(15, 2+i, PSTR("Rev"), 0); |
if (Config.stick_neutral[i] == 0) lcd_printpns_at(20, 2+i, PSTR("N"), 0); else lcd_printpns_at(20, 2+i, PSTR("Y"), 0); |
//} |
if (i == 1) if (Config.Lipomessung == true) lcd_printpns_at(3, 2+i, PSTR("PKT Lipomessung "), 0); |
} |
lcd_printp_at (18, 7, PSTR("OK"), 0); |
do{} |
while (!(get_key_press (1 << KEY_ENTER))); |
} |
//-------------------------------------------------------------- |
// Setup_Stick() |
//-------------------------------------------------------------- |
void Setup_Stick( uint8_t stick ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Stick n" |
//MenuCtrl_SetTitle_P( PSTR("Joystick") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSTICK_MIN , MENU_ITEM, NOFUNC , MSTICK_MIN_de , MSTICK_MIN_en ); |
MenuCtrl_PushML2_P( MSTICK_MAX , MENU_ITEM, NOFUNC , MSTICK_MAX_de , MSTICK_MAX_en ); |
MenuCtrl_PushML2_P( MSTICK_TYPE , MENU_ITEM, NOFUNC , MSTICK_TYPE_de , MSTICK_TYPE_en ); |
MenuCtrl_PushML2_P( MSTICK_DIR , MENU_ITEM, NOFUNC , MSTICK_DIR_de , MSTICK_DIR_en ); |
MenuCtrl_PushML2_P( MSTICK_NEUT , MENU_ITEM, NOFUNC , MSTICK_NEUT_de , MSTICK_NEUT_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSTICK_MIN ) { Config.stick_min[stick] = Edit_generic( Config.stick_min[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_MAX ) { Config.stick_max[stick] = Edit_generic( Config.stick_max[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_TYPE ) { Config.stick_typ[stick] = Edit_generic( Config.stick_typ[stick] ,0, 1,Sticktype,1 ,NULL,NULL); } |
if( itemid == MSTICK_DIR ) { Config.stick_dir[stick] = Edit_generic( Config.stick_dir[stick] ,0, 1,NormRev,1 ,NULL,NULL); } |
if( itemid == MSTICK_NEUT ) { Config.stick_neutral[stick]= Edit_generic( Config.stick_neutral[stick] ,0, 1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Joysticks() |
//-------------------------------------------------------------- |
void Setup_Joysticks( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Joystick" |
//MenuCtrl_SetTitle_P( PSTR("Joystick Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( JSTICKS_OVERVIEW , MENU_ITEM, NOFUNC , JSTICKS_OVERVIEW_de , JSTICKS_OVERVIEW_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK1 , MENU_SUB , NOFUNC , JSTICKS_STICK1_de , JSTICKS_STICK1_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK2 , MENU_SUB , NOFUNC , JSTICKS_STICK2_de , JSTICKS_STICK2_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK3 , MENU_SUB , NOFUNC , JSTICKS_STICK3_de , JSTICKS_STICK3_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK4 , MENU_SUB , NOFUNC , JSTICKS_STICK4_de , JSTICKS_STICK4_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK5 , MENU_SUB , NOFUNC , JSTICKS_STICK5_de , JSTICKS_STICK5_en ); |
MenuCtrl_PushML2_P( JSTICKS_LIPO , MENU_ITEM, NOFUNC , JSTICKS_LIPO_de , JSTICKS_LIPO_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == JSTICKS_OVERVIEW ) { Joysticks_Uebersicht(); } |
if( itemid == JSTICKS_STICK1 ) { Setup_Stick(0); } |
if( itemid == JSTICKS_STICK2 ) { if (Config.Lipomessung == true) |
{ |
lcd_cls (); |
lcd_printp_at(0, 3, PSTR("LiPo measure!"), MNORMAL); |
_delay_ms(1000); |
} |
else Setup_Stick(1); //Stick 2 |
} |
if( itemid == JSTICKS_STICK3 ) { Setup_Stick(2); } |
if( itemid == JSTICKS_STICK4 ) { Setup_Stick(3); } |
if( itemid == JSTICKS_STICK5 ) { Setup_Stick(4); } |
if( itemid == JSTICKS_LIPO ) { Config.Lipomessung = Edit_generic( Config.Lipomessung ,0,1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################ |
#endif // USE_JOYSTICK |
//############################################################################ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/stick/stick_setup.h |
---|
0,0 → 1,13 |
//############################################################################ |
//# HISTORY stick_setup.h |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//############################################################################ |
#ifndef _STICK_SETUP_H_ |
#define _STICK_SETUP_ |
void Setup_Joysticks(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/stick |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/timer/timer.c |
---|
0,0 → 1,856 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.c |
//# |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 29.06.2014 OG |
//# - chg: ISR( TIMER0_COMPA_vect ) - LipoCheck() wieder hinzugefuegt |
//# - chg: LipoCheck() - leichte optische Anpassung und lcd-Ausgabe an ISR angepasst |
//# - add: #include "../lipo/lipo.h" |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# - chg: ISR(TIMER0_COMPA_vect) - "PKT aus nach" (via timer_pkt_off) |
//# - chg: ISR(TIMER0_COMPA_vect) - "LCD aus nach" umgestellt von Sekunden |
//# auf Minuten (via timer_lcd_off) |
//# - chg: Code-Formattierung |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() - loescht ALLE Tasten egal ob short, long, repeat... |
//# |
//# 13.06.2013 cebra |
//# - chg: Abfrage nach Hardwaresound bei Displaybeleuchtung entfällt |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//# |
//# 24.03.2013 OG |
//# - add: UTCTime - inkrementiert wird UTCTime.seconds auf der Basis |
//# von timer_pkt_uptime (modulo 100) aktuell initialisert von |
//# der MK-NC Time durch OSD_MK_UTCTime() |
//# - add: timer_get_tidata |
//# |
//# 21.03.2013 OG |
//# - add: timer_pkt_uptime (auch in timer.h) |
//# - add: timer_osd_refresh (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_bldata (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_erdata (auch in timer.h) -> verwendet in osd.c |
//# |
//# 09.03.2013 OG |
//# - add: timer2 (auch in timer.h) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../lcd/lcd.h" |
#include "../uart/uart1.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include"../tracking/tracking.h" |
#include "../sound/pwmsine8bit.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
// |
//#if defined HWVERSION1_2W || defined HWVERSION1_2 |
//#include "HAL_HW1_2.h" |
//#endif |
// |
//#if defined HWVERSION1_3W || defined HWVERSION1_3 |
//#include "HAL_HW1_3.h" |
//#endif |
//#ifdef HWVERSION3_9 |
#include "../HAL_HW3_9.h" |
//#endif |
//---------------------- |
// generische Timer |
//---------------------- |
volatile uint16_t timer; |
volatile uint16_t timer1; |
volatile uint16_t timer2; |
volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV!) |
volatile uint16_t timer_osd_refresh; // verwendet in osd.c |
volatile uint16_t timer_get_bldata; // verwendet in osd.c |
volatile uint16_t timer_get_tidata; // verwendet in osd.c |
volatile uint16_t timer_get_displaydata; // verwendet in osd.c |
volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
volatile uint16_t abo_timer; |
uint32_t timer_lcd_off = 0; // LCD aus nach... (der Zaehler muss 32Bit sein) |
uint32_t timer_pkt_off = 0; // PKT aus nach... (der Zaehler muss 32Bit sein) |
volatile uint16_t sound_timer; |
volatile uint16_t soundpause_timer; |
volatile static unsigned int tim_main; |
volatile PKTdatetime_t UTCTime; |
volatile uint32_t timer_pkt_uptime = 0; // OG: ja, 32 bit muss so sein! |
uint8_t key_state = 0; // debounced and inverted key state: |
// bit = 1: key pressed |
uint8_t key_press = 0; // key press detect |
uint8_t key_long = 0; // key long press |
uint8_t key_rpt = 0; // key repeat |
uint8_t key_lrpt = 0; // key long press and repeat |
uint8_t key_rpts = 0; // key long press and speed repeat |
uint8_t repeat_speed = 0; |
volatile uint8_t Display_on; // LCD Flag Display on/off |
volatile uint16_t WarnCount = 0; // Zähler der LIPO Warnzeit |
volatile uint16_t WarnToggle = 0; // Togglezähler zum blinken |
volatile uint16_t WarnTime = 10; // Laenge der LIPO Warnzeit 10 Sek. |
volatile uint16_t PoffTime = 15; // Laenge der Wartezeit vor abschalten 15 Sek. |
//volatile uint16_t PoffTime = 30; // Laenge der Wartezeit vor abschalten 30 Sek. |
unsigned int BeepTime = 0; |
unsigned int BeepMuster = 0xffff; |
unsigned int BeepPrio = 0; |
volatile unsigned int CountMilliseconds = 0; |
//-------------------------------------------------------------- |
// System (100Hz) |
//-------------------------------------------------------------- |
void Timer0_Init( void ) |
{ |
timer = 0; |
TCCR0A = (1 << WGM01); |
TCCR0B = (1 << CS02) | (1 << CS00); |
OCR0A = (F_CPU / (100L * 1024L)) ; |
TIMSK0 |= (1 << OCIE0A); // enable interrupt for OCR |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void Timer1_Init (void) // Timer 1-A |
//{ |
// // löschen |
// TCCR1A = 0; |
// TCCR1B = 0; |
// TIMSK1 = 0; |
// |
// // setzen |
// TCCR1A |= (1 << COM1A1) | (1 << WGM11); |
// TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << WGM13) | (1 << WGM12); |
// |
// ICR1 = (F_CPU / 64) * 20 / 1000; |
// |
// OCR1A = 470; // ca. Servomitte |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPA_vect ) |
{ |
PORTD |= (1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPB_vect ) |
{ |
PORTD &= ~(1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Timer2_Init( void ) |
{ |
if( Config.HWSound == 1 ) |
{ // Sound PWM |
TCCR2A = 0x00; //stop |
ASSR = 0x00; //set async mode |
TCNT2 = 0x00; //setup |
OCR2A = 0xff; |
//Fast PWM 0xFF BOTTOM MAX |
//Set OC2A on Compare Match |
//clkT2S/8 (From prescaler) |
TCCR2A |= (1 << WGM20) | (1 << WGM21) |(1 << COM2A1) | (1 << COM2A0); |
TCCR2B |= (1 << CS20); |
} |
else |
{ // Displayhelligkeit |
DDRD |= (1 << DDD7); // PD7 output |
TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); // non invers |
TCCR2B |= (1 << CS20); // Prescaler 1/1 |
TIMSK2 |= (1 << OCIE2A) | (1 << OCIE2B); |
OCR2A = 255; |
} |
} |
//-------------------------------------------------------------- |
// Sound, Timer CTC |
//-------------------------------------------------------------- |
void Timer3_Init( void ) |
{ |
TCCR3A = 0x00; // stop |
TCNT3H = 0xF8; // set count |
TCNT3L = 0x00; // set count |
OCR3AH = 0x07; // set compare |
OCR3AL = 0xFF; // set compare |
TCCR3A |= (1 << WGM31); |
TCCR3B |= (1 << CS30); |
TIMSK3 |= (1 << OCIE3A); // timer interrupt sources 2=t0 compare |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_SOUND |
ISR(TIMER3_COMPA_vect) // Sound |
{ |
//void timer0_comp_isr(void){ |
//was 8 KHz 125usec sampling rate |
//now 12 KHz 83usec sampling rate |
unsigned char oldfpart; |
signed char fullsamp; |
signed int tmp; |
TCNT3 = 0; |
if( generate ) |
{ |
DDRD |= (1<<DDD7); // Port aus Ausgang |
oldfpart=waveptfpart; // remember fractional part |
waveptfpart+=freqincfpart; // add frac part of freq inc to wave pointer |
if( waveptfpart < oldfpart ) // did it walk off the end? |
{ |
waveptipart++; // yes, bump integer part |
} |
waveptipart+=freqincipart; // add int part of freq increment to wave pointer |
fullsamp=sindat[waveptipart]; // get 8 bit sin sample from table (signed) |
tmp=fullsamp*iattenfac; // cvt 7 bit x 8 = 15 bit |
OCR2A=(tmp >> 8)+128; // cvt 15 bit signed to 8 bit unsigned |
} |
else |
{ |
DDRD &= ~(1 << DDD7); // Port auf Eingang, sperrt das Rauschen |
} |
} |
#endif |
//-------------------------------------------------------------- |
// Timer-Interrupt (100 Hz) |
//-------------------------------------------------------------- |
ISR( TIMER0_COMPA_vect ) |
{ |
static uint8_t ct0 = 0; |
static uint8_t ct1 = 0; |
static uint8_t k_time_l = 0; |
static uint8_t k_time_r = 0; |
static uint8_t k_time_lr = 0; |
static uint8_t k_time_rs = 0; |
uint8_t i; |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char beeper_ein = 0; |
i = key_state ^ ~KEY_PIN; // key changed ? |
ct0 = ~(ct0 & i); // reset or count ct0 |
ct1 = ct0 ^ (ct1 & i); // reset or count ct1 |
i &= (ct0 & ct1); // count until roll over ? |
key_state ^= i; // then toggle debounced state |
key_press |= (key_state & i); // 0->1: key press detect |
if( !cnt-- ) |
{ |
cnt = 9; |
CountMilliseconds++; |
cnt_1ms++; |
} |
//-------------------------------- |
// Key pressed -> Timer Reset |
//-------------------------------- |
if( i!=0 ) // eine Taste wurde gedrueckt! -> Timer Rest und weiteres... |
{ |
if( Display_on == 0 ) // ggf. Displaylicht einschalten |
set_D_LIGHT(); |
Display_on = 1; // Flag Display on |
timer_lcd_off = 0; // Timer Reset |
timer_pkt_off = 0; // Timer Reset |
} |
//-------------------------------- |
// LCD off Timeout |
// LCD ausschalten nach n Minuten |
//-------------------------------- |
if( (Config.DisplayTimeout > 0) && (Display_on == 1) ) |
{ |
timer_lcd_off++; |
if( (timer_lcd_off/(100*60)) == Config.DisplayTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
clr_D_LIGHT(); // Displaylicht ausschalten |
Display_on = 0; // Flag Display off |
} |
} |
//-------------------------------- |
// PKT off Timeout |
// PKT ausschalten nach n Minuten |
//-------------------------------- |
if( Config.PKTOffTimeout > 0 ) |
{ |
timer_pkt_off++; |
if( (timer_pkt_off/(100*60)) == Config.PKTOffTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------- |
// PKT bei Unterspannung abschalten |
//-------------------------------- |
LipoCheck(); |
//-------------------------------- |
// Beeper |
//-------------------------------- |
if( Config.HWBeeper==1 ) |
{ |
if( BeepTime ) |
{ |
if( BeepTime > 10 ) BeepTime -= 10; |
else BeepTime = 0; |
if( BeepTime & BeepMuster ) beeper_ein = 1; |
else beeper_ein = 0; |
} |
else |
{ |
beeper_ein = 0; |
BeepMuster = 0xffff; |
BeepPrio = BeepNormal; |
} |
if( beeper_ein == 1 ) set_BEEP(); |
else clr_BEEP(); |
} |
//-------------------------------- |
// Sound |
//-------------------------------- |
#ifdef USE_SOUND |
if (sound_timer > 0) // Ton spielen |
{ |
sound_timer--; |
} |
else |
{ |
//TIMSK2 &= ~_BV(TOIE2); // Interrupt sperren, verhindert Störgeräusche |
//TCCR2A = 0x00; //stop |
generate = 0; // Sound aus |
tone_handler(); |
if (soundpause_timer > 0) |
{ |
soundpause_timer --; // Ton pause |
} |
} |
#endif |
//-------------------------------- |
// Tasten |
//-------------------------------- |
if ((key_state & LONG_MASK) == 0) // check long key function |
k_time_l = REPEAT_START; // start delay |
if (--k_time_l == 0) // long countdown |
key_long |= (key_state & LONG_MASK); |
//------ |
if ((key_state & REPEAT_MASK) == 0) // check repeat function |
k_time_r = 1; // kein delay |
if (--k_time_r == 0) |
{ |
k_time_r = REPEAT_NEXT; // repeat delay |
key_rpt |= (key_state & REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_MASK) == 0) // check repeat function |
k_time_lr = REPEAT_START; // start delay |
if (--k_time_lr == 0) |
{ |
k_time_lr = REPEAT_NEXT; // repeat delay |
key_lrpt |= (key_state & LONG_REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_SP_MASK) == 0) // check repeatX function |
k_time_rs = REPEAT_START; // start delay |
if( --k_time_rs == 0 ) // repeat countdown |
{ |
if( repeat_speed == 1 ) |
{ |
k_time_rs = REPEAT_SPEED_1; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 2 ) |
{ |
k_time_rs = REPEAT_SPEED_2; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 3 ) |
{ |
k_time_rs = REPEAT_SPEED_3; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
} |
//-------------------------------- |
// generische Timer |
//-------------------------------- |
if( timer > 0 ) timer --; |
if( timer1 > 0 ) timer1 --; |
if( timer2 > 0 ) timer2 --; |
if( timer3 > 0 ) timer3 --; |
//-------------------------------- |
// spezielle Timer |
//-------------------------------- |
if( timer_osd_refresh > 0 ) timer_osd_refresh--; // Timer fuer OSD-Screenrefresh (verwendet von osd.c) |
if( timer_get_bldata > 0 ) timer_get_bldata--; // Timer um BL-Daten zu holen (verwendet von osd.c) |
if( timer_get_tidata > 0 ) timer_get_tidata--; // Timer um Datum/Zeit vom MK zu holen (verwendet von osd.c) |
if( timer_get_displaydata > 0 ) timer_get_displaydata--; // Timer um Display vom MK zu holen (verwendet von osd.c) |
if( timer_mk_timeout > 0 ) timer_mk_timeout--; // verwendet u.a. von osd.c |
if( abo_timer > 0 ) abo_timer --; // Timer zum anfordern neuer Abo-Datenpakete wie OSD oder BL-Daten (verwendet u.a. von osd.c) |
if( timer_pktupdatecheck > 0 ) timer_pktupdatecheck--; // Timer fuer pkt.c (PKT-Update-Check) - * FUER NICHTS ANDERES! * |
if( timer_nmea_timeout > 0 ) timer_nmea_timeout--; // verwendet u.a. von osd.c |
//-------------------------------- |
//-------------------------------- |
if( Gauge_active ) // Gauge_active -> pkt.c |
{ |
if( timer_gauge > 0 ) timer_gauge--; |
if( timer_gauge==0 ) PKT_Gauge_Next(); |
} |
//-------------------------------- |
// PKT Uptime Timer |
//-------------------------------- |
timer_pkt_uptime++; |
if( timer_pkt_uptime % 100 == 0 ) // theoretisch muesste noch die Tagesgrenze (0 Uhr) implementiert werden... |
UTCTime.seconds++; |
} // end: ISR(TIMER0_COMPA_vect) |
//-------------------------------------------------------------- |
// Lowbatpin des Spannungswandlers pruefen |
// LBO des LT1308 wechselt zum Ende der Batterielaufzeit haeufig seinen Zustand in der Uebergangsphase zum LowBat |
// Die Akkuspannung schwankt auch abhaengig vom momentanen Stromverbrauch |
//-------------------------------------------------------------- |
void LipoCheck( void ) |
{ |
uint8_t lcd_xpos_save; |
uint8_t lcd_ypos_save; |
if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
{ |
set_beep( 1000, 0x0020, BeepNormal); |
lcd_xpos_save = lcd_xpos; // innerhalb einer ISR -> LCD Screenpos muss gesichert werden! |
lcd_ypos_save = lcd_ypos; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 0, MINVERS ,0 ); // Zeile 0 komplett invers |
lcd_printp_at( 0, 0, PSTR(" ** PKT LiPo! ** "), MINVERS); // und Warn-Text ausgeben |
lcd_xpos = lcd_xpos_save; // lcd Screenpos wieder herstellen |
lcd_ypos = lcd_ypos_save; |
} |
if( WarnToggle == WarnTime * 100 ) |
WarnToggle = 0; // erstmal bis hier warnen |
if( WarnToggle > 0 ) |
WarnToggle++; // weiter hochzaehlen |
if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
{ |
WarnCount = 0; |
//if (WarnCount > 0) |
// WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
} |
if( !(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
{ |
WarnCount++; // solange LBO low ist Zähler hochzählen |
if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
WarnToggle = 1; // Warnhinweis starten |
} |
if( WarnCount == (PoffTime * 100) ) |
{ |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void LipoCheck_OLD( void ) |
//{ |
// if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
// { |
// set_beep ( 1000, 0x0020, BeepNormal); |
// lcd_printp_at (0, 0, PSTR(" LIPO !!Warnung!! "), 2); |
// } |
// |
// if( WarnToggle == WarnTime * 100 ) |
// WarnToggle = 0; // erstmal bis hier warnen |
// |
// if( WarnToggle > 0 ) |
// WarnToggle++; // weiter hochzaehlen |
// |
// if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
// { |
// WarnCount = 0; |
// //if (WarnCount > 0) |
// // WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
// } |
// |
// if (!(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
// { |
// WarnCount++; // solange LBO low ist Zähler hochzählen |
// if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
// WarnToggle = 1; // Warnhinweis starten |
// } |
// |
// if( WarnCount == (PoffTime * 100) ) |
// { |
// clr_V_On(); // Spannung abschalten |
// } |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned int SetDelay( unsigned int t ) |
{ |
return( CountMilliseconds + t + 1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
char CheckDelay( unsigned int t ) |
{ |
return( ((t - CountMilliseconds) & 0x8000) >> 9 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_ms( unsigned int w ) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while( !CheckDelay(akt) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_press( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_press; // read key(s) |
key_press ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_short( uint8_t key_mask ) |
{ |
uint8_t ret; |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
ret = get_key_press (~key_state & key_mask); |
SREG = sreg; // restore status register |
return ret; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_long; // read key(s) |
key_long ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_press (get_key_rpt (key_mask)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpt; // read key(s) |
key_rpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_lrpt; // read key(s) |
key_lrpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_rpt (~key_press^key_mask); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt_sp( uint8_t key_mask, uint8_t key_speed ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpts; // read key(s) |
key_rpts ^= key_mask; // clear key(s) |
repeat_speed = key_speed; |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clear_key_all( void ) |
{ |
key_rpt = 0; // clear key(s) |
key_rpts = 0; // clear key(s) |
key_lrpt = 0; // clear key(s) |
key_long = 0; // clear key(s) |
key_press = 0; // clear key(s) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_beep( uint16_t Time, uint16_t Muster, uint8_t Prio) |
{ |
if( Config.HWBeeper == 1 ) |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // BEEPER: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) // BEEPER: hohe Prio setzen |
{ |
BeepPrio = BeepSevere; |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( Prio == BeepOff ) |
{ |
BeepPrio = BeepNormal; // BEEPER: Beep hohe Prio aus |
BeepTime = 0; |
BeepMuster = 0; |
} |
} // end: if( Config.HWBeeper==1 ) |
#ifdef USE_SOUND |
else |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // SOUND: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
playTone(900,Time/10,0); |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) |
{ |
playTone(1200,Time/10,0); |
playTone(1100,Time/10,0); |
} |
if( Prio == BeepOff ) |
{ |
playTone(0,0,0); |
} |
} |
#endif // end: #ifdef USE_SOUND |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/timer/timer.h |
---|
0,0 → 1,170 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.h |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() |
//# |
//# 07.07.2013 OG |
//# - chg: ABO_TIMEOUT verschoben nach main.h |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//############################################################################ |
#ifndef _TIMER_H |
#define _TIMER_H |
#include "../cpu.h" |
#include "../main.h" |
typedef struct |
{ |
uint32_t seconds; // seconds since nnew day |
uint8_t day; |
uint8_t month; |
uint16_t year; |
} PKTdatetime_t; |
#define KEY_ALL ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_SP_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_START 70 // after 700ms |
#define REPEAT_NEXT 15 // every 150ms |
#define REPEAT_SPEED_1 20 // every 200ms |
#define REPEAT_SPEED_2 8 // every 80ms |
#define REPEAT_SPEED_3 1 // every 10ms |
#define BeepNormal 0 // Normal Beep |
#define BeepSevere 1 // schwerer Fehler, Beep nicht unterbrechbar |
#define BeepOff 2 // Beep aus |
extern volatile uint8_t Display_on; |
extern volatile PKTdatetime_t UTCTime; |
//---------------------- |
// generische Timer |
//---------------------- |
extern volatile uint16_t timer; |
extern volatile uint16_t timer1; |
extern volatile uint16_t timer2; |
extern volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
extern volatile uint32_t timer_pkt_uptime; |
extern volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV! FUER NICHTS ANDERES!) |
extern volatile uint16_t timer_osd_refresh; |
extern volatile uint16_t timer_get_bldata; |
extern volatile uint16_t timer_get_tidata; |
extern volatile uint16_t timer_get_displaydata; |
extern volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
extern volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
extern volatile uint16_t abo_timer; |
extern volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
extern volatile uint16_t sound_timer; |
extern volatile uint16_t soundpause_timer; |
//extern volatile uint16_t WarnCount; |
//extern volatile unsigned int BeepTime; |
extern unsigned int BeepTime; |
extern unsigned int BeepMuster; |
/** |
* Get the value of the system timer counter. |
* |
* @return The value of the system timer counter. |
*/ |
uint16_t timer_get_system_count(void); |
void Timer0_Init (void); // Systeminterrupt |
void Timer1_Init (void); // Servotester |
void Timer2_Init (void); // Displayhelligkeit |
void Timer3_Init (void); // Überwachung |
uint8_t get_key_press (uint8_t key_mask); // sofort beim drücken |
uint8_t get_key_short (uint8_t key_mask); // erst beim loslassen |
uint8_t get_key_long (uint8_t key_mask); // verzögert |
uint8_t get_key_rpt (uint8_t key_mask); // mit verzögerung |
uint8_t get_key_long_rpt (uint8_t key_mask); // |
uint8_t get_key_long_rpt_sp (uint8_t key_mask, uint8_t key_speed); // mit verzögerung und 3 versch. geschw. |
void clear_key_all( void ); |
void set_beep ( uint16_t Time, uint16_t Muster, uint8_t Prio); |
extern volatile unsigned int CountMilliseconds; |
void Delay_ms(unsigned int); |
unsigned int SetDelay (unsigned int t); |
char CheckDelay (unsigned int t); |
void LipoCheck (void); // Lowbatpin des Spannungswandlers prüfen |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/ng_servo.c |
---|
0,0 → 1,236 |
/********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library.*/ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 2 */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/********************************************************************/ |
//############################################################################ |
//# HISTORY ng_servo.c |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 24.04.2013 OG |
//# - chg: umbenannt servo_test(void) nach _servo_test(void) |
//# das verursachte in meinem PKT-Projekt Errors weil die Funktion |
//# servo_test() ebenfalls ein servo.c vorhanden ist |
//############################################################################ |
#include <avr/interrupt.h> |
#include <avr/io.h> |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../HAL_HW3_9.h" |
#include "../eeprom/eeprom.h" |
//#include "config.h" |
#ifdef USE_TRACKING |
// Global variables |
uint16_t ServoPosTics; |
uint16_t ServoPeriodTics; |
// servo channel registers |
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS]; |
const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER}, |
{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}}; |
// Servo limits (depend on hardware) |
const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX}, |
{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX}, |
{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}}, |
{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX}, |
{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX}, |
{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}}; |
// Servopositionen normiert für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
const steps_pw_t steps_pw[2] = {{(uint64_t)950*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*F_CPU*1e-6/SERVO_PRESCALER + 0.5,(uint64_t)1500*F_CPU*1e-6/SERVO_PRESCALER + 0.5}, |
{(uint64_t)950*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5}}; |
// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 }; |
// functions |
void _servo_test(void) |
{ |
//Dummy |
} |
//! initializes software PWM system 16-bit Timer |
// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt |
// Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!! |
void servoInit(uint8_t servo_period) |
{ uint16_t OCValue; // set initial interrupt time |
// disble Timer/Counter1, Output Compare A Match Interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
// set the prescaler for timer1 |
if (Config.sIdxSteps == STEPS_255) { |
TCCR1B &= ~((1<<CS11) | (1<<CS10)); |
TCCR1B |= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel |
} |
else { |
TCCR1B &= ~(1<<CS12); |
TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel |
} |
// attach the software PWM service routine to timer1 output compare A |
// timerAttach(TIMER1OUTCOMPAREA_INT, servoService); |
// enable channels |
for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) { |
// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5 |
ServoChannels[channel].pin = (1 << (SERVO2 + channel)); |
} |
ServoPosTics = 0; // set PosTics |
// set PeriodTics |
ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3; |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH << 8); // read high byte of OCR1A |
OCValue += ServoPeriodTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
TIMSK1 |= (1<<OCIE1A); // enable the timer1 output compare A interrupt |
coldstart = 1; |
} |
void servoSetDefaultPos(void) |
{ |
servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN |
servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT |
} |
uint16_t pw_us(uint16_t Steps) |
{ |
return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in µs |
} |
uint16_t ServoSteps(void) |
{ |
return(ServoConst[Config.sIdxSteps].steps); |
} |
void servoSet_rev(uint8_t channel, uint8_t val) |
{ |
ServoChannels[channel].rev = val & 0x01; |
} |
void servoSet_min(uint8_t channel, uint16_t min) |
{ |
ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min; |
} |
void servoSet_max(uint8_t channel, uint16_t max) |
{ |
ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max; |
} |
void servoSet_mid(uint8_t channel, uint16_t mid) |
{ |
ServoChannels[channel].mid = mid; |
// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen |
ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \ |
(16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min; |
} |
//! turns off software PWM system |
void servoOff(void) |
{ |
// disable the timer1 output compare A interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
} |
//! set servo position on channel (raw unscaled format) |
void servoSetPositionRaw(uint8_t channel, uint16_t position) |
{ |
// bind to limits |
if (position < ServoChannels[channel].min) position = ServoChannels[channel].min; |
if (position > ServoChannels[channel].max) position = ServoChannels[channel].max; |
// set position |
ServoChannels[channel].duty = position; |
} |
//! set servo position on channel |
// input should be between 0 and ServoSteps() (entspricht Maximalausschlag) |
void servoSetPosition(uint8_t channel, uint16_t position) |
{ uint16_t pos_scaled; |
// calculate scaled position |
if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position; |
if (position < ServoConst[Config.sIdxSteps].steps/2) |
//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2) |
pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \ |
ServoConst[Config.sIdxSteps].steps; |
else |
//bei Position >= Servomittelstellung |
pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \ |
* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps; |
// set position |
servoSetPositionRaw(channel, pos_scaled); |
} |
// Umrechnung Winkel in Servoschritte |
void servoSetAngle(uint8_t servo_nr, int16_t angle) |
{ |
servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180)); |
} |
//Interruptroutine |
ISR(TIMER1_COMPA_vect) |
{ static uint8_t ServoChannel; |
uint16_t nextTics; |
uint16_t OCValue; // schedule next interrupt |
if(ServoChannel < SERVO_NUM_CHANNELS) { |
PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel |
} |
ServoChannel++; // next channel |
if(ServoChannel != SERVO_NUM_CHANNELS) { |
// loop to channel 0 if needed |
if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
// turn on new channel |
PORTD |= ServoChannels[ServoChannel].pin; |
// schedule turn off time |
nextTics = ServoChannels[ServoChannel].duty; |
} |
else { |
// ***we could save time by precalculating this |
// schedule end-of-period |
nextTics = ServoPeriodTics-ServoPosTics; |
} |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH <<8); // read high byte of OCR1A |
OCValue += nextTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
ServoPosTics += nextTics; // set our new tic position |
if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/ng_servo.h |
---|
0,0 → 1,190 |
/*********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library. */ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 4 */ |
/* */ |
/*ingroup driver_sw */ |
/*defgroup servo Interrupt-driven RC Servo Function Library (servo.c)*/ |
/*code #include "servo.h" \endcode */ |
/*par Overview */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/*which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/*********************************************************************/ |
#ifndef SERVO_H |
#define SERVO_H |
/* Servo */ |
#define SERVO_PAN 0 |
#define SERVO_TILT 1 |
#define SERVO_NUM_CHANNELS 2 // Anzahl der angeschlossen Servos max. 2!!! |
/* Servokalibrierungen derzeit zu SERVO_STEPS = 255 skaliert */ |
//prescaler 256 |
#define SERVO_I0_RIGHT 45 // default Wert, ca. 0,9ms |
#define SERVO_I0_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I0_RIGHT_MAX 100 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I0_LEFT 45 // default Wert, ca. 2,1ms |
#define SERVO_I0_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I0_LEFT_MAX 100 // SERVO_MAX - SERVO_LEFT |
#define SERVO_I0_MIDDLE SERVO_STEPS/2 |
#define SERVO_I0_MIDDLE_MIN SERVO_STEPS/2 - 25 |
#define SERVO_I0_MIDDLE_MAX SERVO_STEPS/2 + 25 |
//prescaler 64 |
#define SERVO_I1_RIGHT 180 // default Wert, ca. 0,9ms |
#define SERVO_I1_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I1_RIGHT_MAX 400 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I1_LEFT 180 // default Wert, ca. 2,1ms |
#define SERVO_I1_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I1_LEFT_MAX 400 // SERVO_MAX - SERVO_LEFT |
//#define SERVO_I1_MIDDLE ((SERVO_STEPS + 1) * 4 - 1)/2 |
#define SERVO_I1_MIDDLE_MIN ((SERVO_STEPS + 1) * 4 - 1)/2 - 100 |
#define SERVO_I1_MIDDLE_MAX ((SERVO_STEPS + 1) * 4 - 1)/2 + 100 |
#define SERVO_REV 0 // kein Reverse |
/* Test Servo */ |
#define SERVO_PERIODE 20 // default Angabe in ms |
#define SERVO_PERIODE_MIN 10 // 10ms |
#define SERVO_PERIODE_MAX 30 // 30ms |
#define SINGLE_STEP 0 // Einzelschritt aus |
#define SINGLE_STEP_MIN 0 |
#define SINGLE_STEP_MAX 20 // bei prescaler 256, sonst * 4 (von links nach rechts in 9 Abschnitte) |
// zwischen den Schritten muss Pause > der Servoperiode sein, sonst keine Aktualisierung |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// The numbers below good for parallax servos at an F_CPU of 20.000MHz. |
// Da einige Servo's auch eien Winkel von 180 grd zulassen, Wertebereich |
// entgegen den sonst üblichen. Einschränkung mit default Kalibrierung |
// auf 0,9ms (45) bis 2,1ms(45) gesetzt. Je nach Servo, entspricht einen |
// Winkel von etwa 180grd |
// Periode default 20ms |
#define SERVO_MAX 211 // 2,7 ms bei prescaler 256, bei prescaler 64 SERVO_MAX * 4 |
#define SERVO_MIN 26 // 0,33ms bei prescaler 256, bei prescaler 64 SERVO_MIN * 4 |
#define SERVO_STEPS 255 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define SERVO_PRESCALER 256 // bei prescaler 256, bei prescaler 64 SERVO_PRESCALER / 4 |
#define STEPS_255 0 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define STEPS_1023 1 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
typedef struct //Servo-Konstante je nach Prescaler |
{ |
uint16_t max; |
uint16_t min; |
uint16_t steps; |
uint16_t prescaler; |
}ServoConst_t; |
typedef struct //struct_ServoChannel |
{ |
uint8_t pin; // hardware I/O port and pin for this channel |
uint16_t duty; // PWM duty setting which corresponds to servo position |
uint8_t rev; // Parameter, wie on/off; reverse; range |
uint16_t min; // SERVO_MIN + Parameter min |
uint16_t max; // SERVO_MAX - Parameter max |
uint16_t mid_scaled; // skalierte Servomitte |
int16_t mid; // Servomitte = SERVO_STEPS/2 +/- x Schritte; bei Pescaler 256 wird nur uint8_t benötigt aber bei 64 |
}ServoChannelType; |
//uint8_t sIdxSteps; // 0 für 255 und 1 für 1023 Schritte; Prescaler 256 oder 64 |
// Define servo limits (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
}servo_limit_t; |
extern const servo_limit_t servo_limit[2][3]; |
// Define servo positions (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
}steps_pw_t; |
typedef struct { |
uint8_t rev; |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
} servo_t; |
// Servopositionen für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
extern const steps_pw_t steps_pw[2]; |
#define RIGHT 0 // Servopostionen, welche vom Einbau abhängig sind |
#define LEFT 1 |
#define MIDDLE 2 |
#define POSIDX_MAX 4 |
extern const uint8_t PosIdx[POSIDX_MAX]; // anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
// functions |
void servoSetDefaultPos(void); |
// initializes servo system |
// You must run this to begin servo control |
void servoInit(uint8_t servo_period); |
// turns off servo system |
// This stops controlling the servos and |
// returns control of the SERVOPORT to your code |
void servoOff(void); |
// set servo position on a given channel |
// servoSetPosition() commands the servo on <channel> to the position you |
// desire. The position input must lie between 0 and POSITION_MAX and |
// will be automatically scaled to raw positions between SERVO_MIN and |
// SERVO_MAX |
void servoSetPosition(uint8_t channel, uint16_t position); |
// set raw servo position on a given channel |
// Works like non-raw commands but position is not scaled. Position must |
// be between SERVO_MIN and SERVO_MAX |
void servoSetPositionRaw(uint8_t channel, uint16_t position); |
// set servo to a specific angle |
void servoSetAngle(uint8_t servo_nr, int16_t angle); |
// vor servoInit(), oder vor sei() ServoWerte mit servoSet...() initialisieren, einschließlich servoSetPosition(...)! |
void servoSet_rev(uint8_t channel, uint8_t val); |
void servoSet_min(uint8_t channel, uint16_t min); |
void servoSet_max(uint8_t channel, uint16_t max); |
void servoSet_mid(uint8_t channel, uint16_t mid); |
uint16_t pw_us(uint16_t Steps); // gibt Zeit in µs bei x Servoschritte |
uint16_t ServoSteps(void); // gibt "Konstante" derzeitiger Servoschritte zürück |
#endif /* SERVO_H */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/servo_setup.c |
---|
0,0 → 1,1577 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY servo_setup.c |
//# |
//# 27.06.2014 OG |
//# - del: #include "tools.h" |
//# - del: #include "mymath.h" |
//# |
//# 30.05.2014 OG |
//# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben; |
//# einige Optimierungen an Aufruffunktionen (unnoetige Menue- |
//# Interfacefunktionen entfernt) |
//# |
//# 13.05.2014 OG |
//# - chg: Menu_Test_Start() - Variable 'range' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt |
//# wegen Warning: variable set but not used |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(), |
//# Setup_ServoTestCont(), Setup_ServoAdjust() |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Men�steuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "servo_setup.h" |
#include "tracking.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../menu.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#ifdef USE_TRACKING |
//#include "ng_usart.h" |
//#include "ng_config.h" |
//#include "servo.h" |
//#include "tools.h" |
//#include "mk.h" |
//#include "keys.h" |
//#include "mymath.h" |
// |
//GPS_Pos_t last5pos[7]; |
//uint8_t error1 = 0; |
//NaviData_t *naviData; |
//HomePos_t MK_pos; // Home position of station |
//GPS_Pos_t currentPos; // Current position of flying object |
//int8_t satsInUse; // Number of satelites currently in use |
//uint8_t tracking = TRACKING_MIN; |
//uint8_t track_hyst = TRACKING_HYSTERESE; |
//uint8_t track_tx = 0; |
//geo_t geo; |
//int16_t anglePan, angleTilt; // Servo Winkel |
//uint8_t coldstart = 1; |
uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden |
//uint8_t FCStatusFlags; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTracking() |
//----------------------------- |
#define MSERVO_STEPS 1 |
#define MSERVO_SERVO1 2 |
#define MSERVO_SERVO2 3 |
#define MSERVO_SERVOTEST 4 |
static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte"; |
static const char MSERVO_STEPS_en[] PROGMEM = "servo steps"; |
static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1"; |
#define MSERVO_SERVO1_en MSERVO_SERVO1_de |
static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2"; |
#define MSERVO_SERVO2_en MSERVO_SERVO2_de |
static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest"; |
#define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de |
//############################################################################ |
//----------------------------- |
// Setup_ServoTest() |
//----------------------------- |
#define MSERVOT_PULS 1 |
#define MSERVOT_CONT 2 |
#define MSERVOT_SERVO 3 |
#define MSERVOT_FRAME 4 |
static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulslänge"; |
static const char MSERVOT_PULS_en[] PROGMEM = "test puls width"; |
static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd."; |
static const char MSERVOT_CONT_en[] PROGMEM = "test cont."; |
static const char MSERVOT_SERVO_de[] PROGMEM = "Servo"; |
#define MSERVOT_SERVO_en MSERVOT_SERVO_de |
static const char MSERVOT_FRAME_de[] PROGMEM = "Periode"; |
static const char MSERVOT_FRAME_en[] PROGMEM = "frame"; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTestCont() |
//----------------------------- |
#define MSERVOTC_START 1 |
#define MSERVOTC_SINGLE 2 |
#define MSERVOTC_COUNT 3 |
#define MSERVOTC_PAUSEEND 4 |
#define MSERVOTC_PAUSEINC 5 |
static const char MSERVOTC_START_de[] PROGMEM = "Start Test"; |
static const char MSERVOTC_START_en[] PROGMEM = "start test"; |
static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt"; |
static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step"; |
static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test"; |
static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test"; |
static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos."; |
static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos"; |
static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc."; |
static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc."; |
//############################################################################ |
//----------------------------- |
// Setup_ServoAdjust() |
//----------------------------- |
#define MSERVOADJ_REV 1 |
#define MSERVOADJ_LEFT 2 |
#define MSERVOADJ_RIGHT 3 |
#define MSERVOADJ_MID 4 |
static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse"; |
static const char MSERVOADJ_REV_en[] PROGMEM = "reverse"; |
static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links"; |
static const char MSERVOADJ_LEFT_en[] PROGMEM = "left"; |
static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts"; |
static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right"; |
static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte"; |
static const char MSERVOADJ_MID_en[] PROGMEM = "middle"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void draw_input_screen( void ) |
{ |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); |
} |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */ |
///* :1 springt nach max_val auf min_val und umgedreht */ |
///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */ |
///* uint16_t Change_Value_plmi(...) :R�ckgabe ge�nderter Wert */ |
///* */ |
///************************************************************************************/ |
void Servo_tmp_Original(uint8_t track) |
{ |
servoSetDefaultPos(); |
// tracking = track; // urspr�nglicher Wert Tracking aus, RSSI oder GPS |
// NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet |
// Jump_Menu(pmenu); |
} |
uint8_t Servo_tmp_on(uint8_t servo_period) |
{ |
// uint8_t tmp_tracking = tracking; |
// tracking = 0; // Servopositionierung durch tracking abschalten |
// if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten |
servoInit(servo_period); |
// lcdGotoXY(0, 0); // lcd Cursor vorpositionieren |
// return(tmp_tracking); |
return (0); |
} |
void Displ_Off_On(uint16_t val) |
{ |
if (val == 0) lcd_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_at(17, 2, strGet(ON), 0); |
} |
uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct) |
{ |
// >> Menueauswahl nach oben |
if (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) |
{ |
if (val < max_val) { |
edit = 1; |
val++; |
} |
else |
{ |
val = min_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
// >> Menueauswahl nach unten |
if (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) |
{ |
if (val > min_val) { |
val--; |
edit = 1; |
} |
else |
{ |
val = max_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
return(val); |
} |
// |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t *val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */ |
///* Bit 6 zyklische Werte�nderung aus/ein */ |
///* Bit 4-5 z.Z. ohne Funktion */ |
///* Bit 0-3 Wert xPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t Change_Value(...) :R�ckgabe ge�ndert ergibt TRUE */ |
///* */ |
///************************************************************************************/ |
//// Bei Bedarf k�nnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) �bergeben werden |
uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct) |
{ |
uint16_t tmp_val; |
tmp_val = *val; |
Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Men�punkt |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) |
*val = Change_Value_plmi(*val, min_val, max_val, 16,2, Displ_Fnct); |
if (*val == tmp_val) { |
edit = 0; |
// lcd_printp_at (0, 5, PSTR("Edit=0"), 0); |
// _delay_ms(500); |
//// return (*val); |
} |
// |
else |
{ |
edit = 1; |
// lcd_printp_at (0, 5, PSTR("Edit=1"), 0); |
// _delay_ms(500); |
} |
return (tmp_val != *val); |
} |
uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid) |
{ uint16_t range; |
if (PosProzent < 0) { |
range = mid - min; |
// if (chrxs == CHRRS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
// chrxs = CHRLS; |
//// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// } |
} |
else { |
range = max - mid; |
// if (chrxs == CHRLS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
//// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRRS; |
// } |
} |
return(range); |
} |
/************************************************************************************/ |
/* zeigt einen max. 3-stelligen Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Format_Int( uint16_t val ) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Displ_PulseWidth( uint16_t val ) |
{ |
int16_t PosProzent, range; |
servoSetPositionRaw( servo_nr, val); |
PosProzent = val - steps_pw[Config.sIdxSteps].mid; |
range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid); |
PosProzent = (int32_t)1000 * PosProzent / range; |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent ); |
} |
/************************************************************************************/ |
/* zeigt Pausenl�nge der Links-, Mittel- und Rechtsposition auf Display an */ |
/* Parameter: */ |
/* uint16_t val : Zeit in 1ms * 100 */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause(uint16_t val) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val ); |
//lcdPuts("s "); |
} |
/************************************************************************************/ |
/* zeigt aus oder Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Off_Format_Int( uint16_t val ) |
{ |
if( val == 0 ) |
{ |
lcd_printp_at( 17, 2, strGet(OFF), MNORMAL); |
} |
else |
{ |
Displ_Format_Int( val ); |
} |
} |
/************************************************************************************/ |
/* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause_Step( uint16_t val ) |
{ |
Displ_Off_Format_Int( val ); |
} |
/************************************************************************************/ |
/* zeigt zu testende Servonummer zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_ServoNr( uint16_t val ) |
{ |
Displ_Format_Int( val+1 ); |
} |
/**************************/ |
/* */ |
/* Servos-Tests */ |
/* */ |
/**************************/ |
//void Menu_Servo_Test(void) |
//{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME}; |
// |
// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global |
// servo_nr = eeprom_read_byte(&ep_servo_nr); |
// Jump_Menu(pmenu); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Frame( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.servo_frame; |
if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) ) |
{ |
Config.servo_frame = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_ServoNr( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = servo_nr; |
if( Change_Value( &tmp_val, 0, 1, Displ_ServoNr) ) |
{ // pmenu global; es gibt nur 0=Servo1, 1=Servo2 |
servo_nr = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
// Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: f�r Modelleinstellungen ohne Empf�nger |
//-------------------------------------------------------------- |
void Menu_Test_PulseWidth( void ) |
{ |
uint8_t tmp_tracking; |
uint16_t tmp_val; |
draw_input_screen(); |
// 30.05.2014 OG: macht dieser Code Sinn? |
// Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen? |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
tmp_val = steps_pw[Config.sIdxSteps].mid; |
Change_Value( &tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, Displ_PulseWidth); // pmenu global |
cli(); |
servoInit( SERVO_PERIODE ); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_SingleStep(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.single_step; |
if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) ) |
{ // pmenu global |
Config.single_step = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Repeat( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.repeat; |
if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) ) |
{ |
Config.repeat = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause; |
if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) ) |
{ |
Config.pause = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause_Step( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause_step; |
if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) ) |
{ |
Config.pause_step = tmp_val; |
} |
} |
int8_t calc_dir(uint8_t idx, int16_t *Position) |
{ uint8_t nextIdx; |
int8_t nextDir = 1; |
nextIdx = idx; |
if ((idx + 1) < POSIDX_MAX) |
nextIdx++; |
else |
nextIdx = 0; |
if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1; |
return(nextDir); |
} |
void Displ_LoopCounter( uint8_t val ) |
{ |
lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val ); |
//lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0); |
} |
// Test �ber Scalierung der Servos mit Anschlagkalibrierung |
void Menu_Test_Start(void) |
{ |
uint8_t tmp_tracking, idx, rep; |
int8_t dir; |
int16_t sPos; |
int16_t Position[3]; |
//int16_t range; |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRLS; // Flag, welche K�stchensymbole geladen |
// Displ_Title(MSG_CONTINOUS); |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
Displ_LoopCounter(Config.repeat); |
Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl m�glich |
Position[1] = ServoSteps()/2; |
Position[2] = ServoSteps(); |
// init Einzelschritt |
idx = 0; |
dir = calc_dir(idx, Position); |
sPos = Position[PosIdx[idx]]; |
idx++; |
rep = Config.repeat; |
// Test bis Ende der Wiederholungen oder irgendein Enter |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) { |
calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
//range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
// draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile |
servoSetPosition(servo_nr, sPos); |
if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht? |
sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren |
if (((dir < 0) && (sPos < Position[PosIdx[idx]])) || ((dir > 0) && (sPos > Position[PosIdx[idx]])) || !(Config.single_step)) |
sPos = Position[PosIdx[idx]]; // �berlauf bei variabler Schrittweite ber�cksichtigen oder Einzelschritt |
Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 w�rden welche �bersprungen, zus�tzlich pro Servoschritt verz�gert |
} |
else { |
dir = calc_dir(idx, Position); // Richtungs�nderung |
if (idx < (POSIDX_MAX - 1)) { |
if (idx == 0) { |
rep--; // bei jeden vollen Durchlauf Wiederholz�hler verringern |
Displ_LoopCounter(rep); |
} |
idx++; // Index f�r n�chsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge |
} |
else |
idx = 0; |
_delay_ms( Config.pause*100 ); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf) |
} |
} |
// lcdClear(); |
// if (pmenu[0] == '\0') |
// Displ_Main_Disp(); |
// else |
// return_m_pkt(strlen(pmenu)); // um bei R�cksprung auf urspr�nglichen Men�punkt zeigen oder Displ_Main_Disp() |
// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char f�r Bargraph zur�ckschreiben |
cli(); |
servoInit(SERVO_PERIODE); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
/********************************************************************************/ |
/* zeigt Servo-Anschlagposition links auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/********************************************************************************/ |
void Displ_Servo_Min(uint16_t val) |
{ |
uint16_t steps = 0; |
write_ndigit_number_s( 15, 2, val, 5, 0,0); |
servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = ServoSteps(); |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition rechts auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Max(uint16_t val) |
{ |
uint16_t steps = ServoSteps(); |
write_ndigit_number_u( 15, 2, val, 5, 0,0); |
servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = 0; |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition Mitte auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Mid( uint16_t val ) |
{ |
int16_t mid_val; |
mid_val = val - ServoSteps()/2; |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val ); |
servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die |
servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_rev( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
tmp_val = Config.servo[servo_nr].rev; |
if( Change_Value(&tmp_val , 0, 1, Displ_Off_On) ) |
{ //reverse gibt es nur 0=off, 1=on |
Config.servo[servo_nr].rev = tmp_val ; |
servoSet_rev(servo_nr, tmp_val ); |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_left( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].max; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) ) |
{ |
Config.servo[servo_nr].max = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_right( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].min; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) ) |
{ |
Config.servo[servo_nr].min = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_middle( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].mid; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) ) |
{ |
Config.servo[servo_nr].mid = tmp_val; |
} |
Servo_tmp_Original(tmp_tracking); |
} |
void Servo_NewValues(uint8_t idx_presc) |
{ |
for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) { |
if (idx_presc == STEPS_255) { // Werte umrechnen f�r Prescaler = 256 |
Config.servo[i].min /= 4; |
Config.servo[i].max /= 4; |
Config.servo[i].mid /= 4; |
} |
else { // Werte umrechnen f�r Prescaler = 64 |
Config.servo[i].min *= 4; |
Config.servo[i].max *= 4; |
Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1; |
} |
servoSet_min(i, Config.servo[i].min); |
servoSet_max(i, Config.servo[i].max); |
servoSet_mid(i, Config.servo[i].mid); |
// eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t)); |
} |
// Vorberechnung von ServoChannels[channel].duty |
servoSetDefaultPos(); // Ausgangsstellung beider Servos |
} |
/************************************************************************************/ |
/* zeigt Servoschritte zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = 255 oder 1 = 1023, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Steps(uint16_t val) |
{ |
if (val==0) |
lcd_puts_at(16,2,INTERNAT_STEPS_255,0 ); |
else |
lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Servo_Steps(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.sIdxSteps; |
if( Change_Value( &tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps) ) |
{ |
cli(); |
Config.sIdxSteps = tmp_val; |
Servo_NewValues( Config.sIdxSteps ); // hier ist der neue Index anzugeben! |
servoInit(SERVO_PERIODE); |
sei(); |
} |
} |
//-------------------------------------------------------------- |
// Setup_ServoAdjust() |
//-------------------------------------------------------------- |
void Setup_ServoAdjust( uint8_t servo ) |
{ |
servo_nr = servo; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1" |
//MenuCtrl_SetTitle_P( PSTR("Servo Adjust") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTestCont() |
//-------------------------------------------------------------- |
void Setup_ServoTestCont( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd." |
//MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en ); |
MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en ); |
MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTest() |
//-------------------------------------------------------------- |
void Setup_ServoTest( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servotest" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en ); |
MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en ); |
MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en ); |
MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTracking() |
//-------------------------------------------------------------- |
void Setup_ServoTracking( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Tracking" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); } |
if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//void start_tracking(void) |
//{ |
// #define TIMEOUT 200 // 2 sec |
// |
// uint16_t old_anglePan = 0; |
// uint16_t old_angleTilt = 0; |
// |
// //uint16_t old_hh = 0; |
// uint8_t flag; |
// uint8_t tmp_dat; |
// |
// lcd_cls (); |
// //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2); |
// |
// //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0); |
// |
// lcd_ecircle(22, 35, 16, 1); |
// lcd_ecircle(88, 35, 16, 1); |
// lcd_putc (10, 1, 0x1e, 0); // degree symbol |
// lcd_putc (20, 1, 0x1e, 0); // degree symbol |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// SwitchToNC(); |
// |
// mode = 'O'; |
// |
// // disable debug... |
// // RS232_request_mk_data (0, 'd', 0); |
// tmp_dat = 0; |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// if(error1 == 1) |
// lcd_cls(); |
// } |
// |
// GPS_Pos_t currpos; |
// currpos.Latitude = naviData->CurrentPosition.Latitude; |
// currpos.Longitude = naviData->CurrentPosition.Longitude; |
// |
// flag = 0; |
// timer = TIMEOUT; |
// abo_timer = ABO_TIMEOUT; |
// |
// coldstart = 1; |
// |
// do |
// { |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// |
////CB uint8_t FCStatusFlag = naviData->FCFlags; |
// uint8_t FCStatusFlag = naviData->FCStatusFlags; |
// //write_ndigit_number_u (0, 0, FCStatusFlag); |
// |
// Tracking_GPS(); |
// |
// //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
// |
// // alte Linien l�schen |
// //lcd_ecirc_line (22, 35, 15, old_hh, 0); |
// //old_hh = heading_home; |
// lcd_ecirc_line (22, 35, 15, old_anglePan, 0); |
// old_anglePan = anglePan; |
// lcd_ecirc_line (88, 35, 15, old_angleTilt, 0); |
// old_angleTilt = angleTilt; |
// |
// lcd_ecirc_line (22, 35, 15, anglePan, 1); |
// write_ndigit_number_u (7, 1, anglePan, 3, 0,0); |
// lcd_ecirc_line (88, 35, 15, angleTilt, 1); |
// write_ndigit_number_u (17, 1, angleTilt, 3, 0,0); |
// |
// rxd_buffer_locked = FALSE; |
// |
// if (!abo_timer) |
// { // renew abo every 3 sec |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// abo_timer = ABO_TIMEOUT; |
// } |
// } |
// |
// if (!timer) |
// { |
// OSD_Timeout(flag); |
// flag = 0; |
// } |
// } |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// //lcd_cls(); |
// //return; |
//} |
//-------------------------------------------------------------- |
// |
//void conect2at_unit(void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_ser (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_bt (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
//-------------------------------------------------------------- |
//void conect2gps_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_CONECT_GPS; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// conect2gps_ser(); |
// if((val+Offset) == 2 ) |
// conect2gps_bt(); |
// } |
//} |
//-------------------------------------------------------------- |
//void tracking_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_AT; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// test_servo_menu(); |
// if((val+Offset) == 2 ) |
// adjust_servo_menu(); |
// if((val+Offset) == 3 ) |
// show_angle(); |
// if((val+Offset) == 4 ) |
////TODO: start_tracking(); |
// if((val+Offset) == 5 ) |
// conect2at_unit(); |
// if((val+Offset) == 6 ) |
// conect2gps_menu(); |
// } |
//} |
//-------------------------------------------------------------- |
// kapeschi Ant.Treking Funktionen |
//-------------------------------------------------------------- |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ int32_t lat1, lon1, lat2, lon2; |
// int32_t d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = pos->Longitude; |
// lat2 = pos->Latitude; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator |
// // es wird jedoch in dm Meter weiter gerechnet |
// // (tlon1 - tlon2)/10) sonst uint32_t-�berlauf bei cos(0) gleich 1 |
// d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
// dlat = 1113 * (lat1 - lat2) / 10000; |
// geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
// geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
//void do_tracking(void) |
//{ //static uint8_t hysteresis = 0; |
// // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
// // (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
// |
// geo = calc_geo(&MK_pos, ¤tPos); |
// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
// //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
// //geo.bearing = MK_pos.direction; |
// //angleTilt = 0; |
// //hysteresis = 1; |
// //} |
// //else { |
// //hysteresis = 0; |
// //} |
//// |
// //// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen |
// //// 360 grd negative Winkelwerte als positive |
// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
// |
// //if (angleTilt < 0) angleTilt = 0; |
// //if (angleTilt > 180) angleTilt = 180; |
//// |
// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
// //anglePan = anglePan - 180; |
// //angleTilt = 180 - angleTilt; |
// // |
// //} |
////angleTilt = 180; |
////angleTilt = 180; |
// |
//// servoSetAngle(0, anglePan); |
//// servoSetAngle(1, angleTilt); |
//} |
/****************************************************************/ |
/* */ |
/* MK GPS Tracking */ |
/* */ |
/****************************************************************/ |
// MK OSD-Daten lesen und verifizieren |
//uint8_t OSD_Data_valid(NaviData_t **navi_data) |
//{ uint8_t ret = 0; |
//char *tx_osd = {"#co?]==EH\r"}; |
//// char interval[2] = {10, '\0'}; |
// |
//if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren |
////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
//currentPos = (*navi_data)->CurrentPosition; |
//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
//ret = 1; |
//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
//// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//satsInUse = (*navi_data)->SatsInUse; |
//} |
//} |
//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval |
//return ret; |
//} |
// |
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
// Hauptprogramm von GPS Antennen-Nachf�hrung |
//void Tracking_GPS(void) |
//{ //NaviData_t *navidata; |
// static uint8_t track_running = 0; |
// |
// if (!track_running) |
// { |
// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
// //if (OSD_Data_valid(&naviData)) { |
// if (coldstart) |
// { |
// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen |
// //if (naviData->FCFlags & FC_FLAG_MOTOR_START) |
// //{ |
// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0; |
// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0; |
// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude; |
// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude; |
// MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
// MK_pos.direction = naviData->CompassHeading; |
// coldstart = 0; |
// //} |
// //} |
// //else { |
// //do_tracking(); |
// } |
// //} |
// track_running = 0; |
// } |
// do_tracking(); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/servo_setup.h |
---|
0,0 → 1,45 |
#ifndef _TRACKING_H_ |
#define _TRACKING_H_ |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define DLEFT 0 |
#define DRIGHT 1 |
#define AltFaktor 22.5 |
#define PAN_SERVO_CORRECT 3.125 |
#define TILT_SERVO_CORRECT 3.125 |
typedef void (*Displ_Fnct_t)( uint16_t ); |
//typedef struct { |
// int32_t distance; |
// int16_t bearing; |
//}geo_t; |
//typedef struct { |
// double Home_Lon; // in degrees |
// double Home_Lat; // in degrees |
// int32_t Home_Lon7; // in 1E-7 degrees |
// int32_t Home_Lat7; // in 1E-7 degrees |
// int32_t Home_Alt; // in mm |
// // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
// int16_t direction; |
//}__attribute__((packed)) HomePos_t; |
#define INTERNAT_STEPS_255 "255 " |
#define INTERNAT_STEPS_1023 "1023" |
// kapeschi |
void Setup_ServoTracking( void ); |
void Tracking_GPS(void); |
void Tracking_NMEA(void); |
void Tracking_RSSI(void); |
void setNMEAdir(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/tracking.c |
---|
0,0 → 1,648 |
/* |
* tracking.c |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.c |
//# |
//# 27.06.2014 OG |
//# - umgestellt auf gps/gps_nmea.c |
//# - Codeformattierung |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# - del: #include "tools.h" |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_tracking() - auskommentierungen um Zugriffe auf Config.gps_UsedGPSMouse |
//# zu unterbinden. Anmerkung dazu siehe dort. |
//# |
//# 22.06.2014 OG |
//# - chg: Tracking_NMEA() - do_tracking() auskommentiert (spaeter fuer |
//# Tracking wieder geradeziehen) |
//# - Variable 'CheckGPS' hinzugefuegt weil aus anderen Sourcen (setup.c) entfernt |
//# |
//# 18.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# |
//# 01.06.2014 OG |
//# - chg: PKT_trackingMK() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# |
//# 30.05.2014 OG |
//# - chg: calc_geo()- my_itoa() ersetzt durch writex_gpspos() und lcdx_printf_at_P() |
//# weil my_itoa() nicht mehr vorhanden |
//# |
//# 13.05.2014 OG |
//# - chg: PKT_trackingBT() - Variable 'BT_WhasOn' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: PKT_trackingBT() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# |
//# 28.04.2014 OG |
//# - chg: PKT_trackingBT(), PKT_trackingMK() - OSD_Timeout() entfernt (weil es |
//# das nicht mehr gibt) und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, tracking hat niedrige Prio) |
//# - add: include "../pkt/pkt.h" |
//# |
//# 12.02.2014 OG |
//# - del: mk_param_struct entfernt |
//# |
//# 29.08.2013 Cebra |
//# - chg: falsche Parameterübergabe bei getLatitude/getLongitude |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING . NMEA Routinen werden nur noch für BT_NMEA genutzt |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
//++++++++++++++++++++++++++++++++++ |
#ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
#include "../tracking/tracking.h" |
#include "../tracking/ng_servo.h" |
#include <avr/pgmspace.h> |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../lcd/lcd.h" |
#include "../mk-data-structs.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../gps/mymath.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
uint8_t CheckGPS = true; // Patch 22.06.2014 OG: hier integriert weil ais anseren Sourcen entfernt (u.a. setup.c) |
char data_decode[RXD_BUFFER_SIZE]; |
#define DLEFT 0 |
#define DRIGHT 1 |
#define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
#define AltFaktor 22.5 |
#define TIMEOUT 200 // 2 sec |
NaviData_t *naviData; |
HomePos_t MK_pos; // Home position of station |
GPS_Pos_t currentPos; // Current position of flying object |
uint8_t tracking = TRACKING_MIN; |
uint8_t track_hyst = TRACKING_HYSTERESE; |
uint8_t track_tx =0; |
uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
geo_t geo; |
int16_t anglePan, angleTilt; |
//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
//// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ double lat1, lon1, lat2, lon2, d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = (double)pos->Longitude / 10000000.0; |
// lat2 = (double)pos->Latitude / 10000000.0; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
// // es wird jedoch in Meter weiter gerechnet |
// d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
// dlat = 111300 * (double)(lat1 - lat2); |
// // returns a value in metres http://www.kompf.de/gps/distcalc.html |
// geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
// if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
// geo.distance = sqrt(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//-------------------------------------------------------------- |
// bei Gelegenheit den Code auf gps/gps.c/gps_Deviation() aendern |
//-------------------------------------------------------------- |
geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
geo_t geo; |
lon1 = home->Home_Lon; |
lat1 = home->Home_Lat; |
lon2 = pos->Longitude; |
lat2 = pos->Latitude; |
if( !CheckGPS ) |
{ |
writex_gpspos( 0, 3, home->Home_Lat , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 3, home->Home_Lon , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
writex_gpspos( 0, 4, pos->Latitude , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 4, pos->Longitude , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
//lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
} |
// Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator |
// es wird jedoch in dm Meter weiter gerechnet |
// (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1 |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = 1113 * (lat1 - lat2) / 10000; |
geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
if( !CheckGPS ) |
{ |
lcd_printp_at (0, 5, PSTR("Bear:"), 0); |
lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing ); |
//lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance ); |
//lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (20, 5, PSTR("m"), 0); |
} |
return(geo); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void do_tracking( void ) |
{ |
static uint8_t hysteresis = 0; |
geo = calc_geo(&MK_pos, ¤tPos); |
angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance); |
if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) |
{ // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
geo.bearing = MK_pos.direction; |
if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf |
hysteresis = 1; |
} |
else |
{ |
hysteresis = 0; |
} |
// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
// 360 grd negative Winkelwerte als positive |
anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
if (angleTilt < 0) |
angleTilt = 0; |
if (angleTilt > 180) |
angleTilt = 180; |
if (anglePan >= 180) |
{ // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
anglePan = anglePan - 180; |
angleTilt = 180 - angleTilt; |
} |
servoSetAngle(0, anglePan); |
servoSetAngle(1, angleTilt); |
if (!CheckGPS) |
{ |
lcd_printp_at (0, 6, PSTR("Pan :"), 0); |
write_ndigit_number_u (6, 6, anglePan, 3, 1,0); |
lcd_printp_at (11, 6, PSTR("Tilt:"), 0); |
write_ndigit_number_u (17, 6, angleTilt, 3, 1,0); |
} |
// write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0); |
// |
// write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingBT( void ) // Tracking mit NMEA-Daten von BT-Maus |
{ |
//uint8_t BT_WhasOn = 0; |
uint8_t BT_status; |
//uint8_t flag; |
uint8_t tmp_dat; |
coldstart =1; |
//lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0); |
lcd_puts_at (0, 1,Config.gps_UsedDevName, 0); |
//set_BTOn(); |
set_Modul_On(Bluetooth); |
//BT_WhasOn = true; |
if( Config.BTIsSlave==true ) |
{ |
bt_downlink_init(); |
} |
lcd_printp_at (18, 1, PSTR(" ?? "), 0); |
BT_status = bt_connect(Config.gps_UsedMac); |
if( BT_status==true ) |
{ |
lcd_printp_at (18, 1, PSTR(" OK "), 0); |
receiveNMEA = true; |
} |
else |
{ |
lcd_printp_at (17, 1, PSTR("FAIL"), 2); |
} |
if( receiveNMEA==true ) |
{ |
lcd_cls_line( 0, 1, 20); |
lcd_printp_at( 0, 2, PSTR(" Latitude Longitude"), 2); |
lcd_printp_at( 0, 3, PSTR("H"), 0); |
lcd_printp_at( 0, 4, PSTR("M"), 0); |
bt_rx_ready = 0; |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benoetigt für Navidata Ausgabe an SV2 |
//flag = 0; |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//bt_rx_ready = 0; |
// if( !NMEA_receiveBT() ) |
// break; |
if (!NMEA_isdataready()) |
break; |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
currentPos = naviData->CurrentPosition; |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//----------------- |
// MK: Lat / Long |
//----------------- |
writex_gpspos( 2, 4, currentPos.Latitude , MNORMAL, 0,0 ); // MK: Latitude |
writex_gpspos(12, 4, currentPos.Longitude, MNORMAL, 0,0 ); // MK: Longitude |
// NMEA_GetNewData(); // neue NMEA GPGGA Daten von der BT GPA-Maus holen |
//----------------- |
// GPS-Maus: Lat / Long |
//----------------- |
writex_gpspos( 2, 3, NMEA.Latitude , MNORMAL, 0,0 ); // GPS-Maus: Latitude |
writex_gpspos(12, 3, NMEA.Longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
//do_tracking(); // das geht so noch nicht mit einer BT GPS-Maus! do_tracking(); ueberarbeiten! |
if( !CheckGPS ) |
{ |
//lcd_printp_at( 0, 2, PSTR("GPS Time: "), 0); |
lcd_puts_at( 13, 0, NMEA.Time , 2); |
lcd_printp_at( 16, 1, PSTR("Sat:"), 0); |
write_ndigit_number_u( 19, 1, NMEA.SatsInUse, 2, 1,0); |
lcd_printp_at( 0, 1, PSTR("Fix:"), 0); |
write_ndigit_number_u( 4, 1, NMEA.SatFix, 1, 1,0); |
lcd_printp_at( 6, 1, PSTR("HDOP:"), 0); |
write_ndigit_number_u_10th( 11, 1, NMEA.HDOP, 3, 0,0); |
} |
rxd_buffer_locked = FALSE; |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if (rxd_buffer_locked) |
if( !timer ) |
{ |
//OSD_Timeout(flag); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
//flag = 0; |
error = 1; |
} |
} while( !get_key_press (1 << KEY_ESC) || !receiveNMEA==true || error ==1); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if( !receiveNMEA ) |
lcd_printp_at (0, 2, PSTR("lost BT data"), 0); |
lcd_printp_at (0, 3, PSTR("GPS trennen"), 0); |
} // end: if( receiveNMEA==true ) |
else |
{ |
lcd_printp_at (0, 4, PSTR("Error at connecting"), 0); |
lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0); |
while( !get_key_press (1 << KEY_ENTER) ); |
} |
receiveNMEA = false; |
if( !bt_disconnect() ) |
lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0); |
//set_BTOff(); |
set_Modul_On(USB); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingMK( void ) // Tracking mit GPS-Daten vom Mikrokopter |
{ |
//uint8_t BT_WhasOn = 0; |
//uint8_t BT_status; |
uint8_t GPSfix=0; |
uint8_t tmp_dat; |
uint8_t toggletimer=0; |
coldstart = true; |
lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
lcd_cls_line (0,1,20); |
//lcd_printp_at (0, 3, PSTR("H"), 0); |
//lcd_printp_at (0, 4, PSTR("M"), 0); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
error = 0; |
do |
{ |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
//OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1); |
//if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//OSD_Element_SatsInUse( 18, 1, 1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//if (GPSfix == true) OSD_Element_StatusFlags( 0, 1); |
if (!(naviData->NCFlags & NC_FLAG_GPS_OK)) |
{ |
toggletimer++; |
if (toggletimer == 50) toggletimer = 0; |
if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2); |
if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0); |
rxd_buffer_locked = false; |
GPSfix = false; |
} |
else GPSfix = true; |
if( GPSfix ) |
{ |
if( coldstart ) |
{ |
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) |
{ |
MK_pos.Home_Lon = naviData->HomePosition.Longitude; |
MK_pos.Home_Lat = naviData->HomePosition.Latitude; |
MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
MK_pos.direction = naviData->CompassHeading; |
coldstart = false; |
rxd_buffer_locked = false; |
lcd_printp_at(0,1, PSTR(" "), 0); |
} |
else |
{ |
lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0); |
rxd_buffer_locked = false; |
} |
} // end: if( coldstart ) |
else |
{ |
//run |
currentPos = naviData->CurrentPosition; |
currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
do_tracking(); |
//lcd_puts_at (13, 0, NMEAtime, 2); |
//lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
//write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
//lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
//write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
//lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
//write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
rxd_buffer_locked = FALSE; |
} // run |
} |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if( rxd_buffer_locked ) |
if( !timer ) |
{ |
//OSD_Timeout(1); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
error = 1; |
} |
} while( (!get_key_press (1 << KEY_ENTER)) && (error ==0) ); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if (error ==1) |
{ |
lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
_delay_ms(2000); |
} |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_tracking(void) |
{ |
clear_key_all(); |
lcd_cls (); |
// 25.06.2014 OG: auskommentiert weil erstmal kein Config.gps_UsedGPSMouse mehr unterstuetzt |
// wird. Falls tracking.c mal richtig implementiert wird sollte man sich auf eine BT-GPS-Maus |
// konzentrieren bis Tracking einwandfrei laeuft - wenn dann noch immer Bedarf nach MK-GPS ist |
// kann man das ja wieder einbauen |
// |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT(); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK(); |
lcd_printp_at( 0,0, PSTR("Tracking Bluetooth "), MINVERS); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
PKT_trackingBT(); |
clear_key_all(); |
} |
//++++++++++++++++++++++++++++++++++ |
#endif // #ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/tracking.h |
---|
0,0 → 1,77 |
/* |
* tracking.h |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.h |
//# |
//# 27.06.2014 OG |
//# - Entkernt: verschiedene Variablen und Funktionen geloescht |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# - add: Source-Historie |
//############################################################################ |
#ifndef TRACKING_H_ |
#define TRACKING_H_ |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// Antennen-Nachführung |
#define TRACKING_MIN 0 // aus, TRACKING_RSSI, TRACKING_GPS, TRACKING_MKCOCKPIT, TRACKING_NMEA |
#define TRACKING_MAX 4 |
// Antennen-Nachführung per RSSI |
#define TRACKING_HYSTERESE 40 // Hysterese bevor Tracking bei Richtungswechsel anspricht |
#define TRACKING_HYST_MIN 0 |
#define TRACKING_HYST_MAX 100 |
#define FC_FLAG_MOTOR_RUN 0x01 |
#define FC_FLAG_FLY 0x02 |
#define FC_FLAG_CALIBRATE 0x04 |
#define FC_FLAG_MOTOR_START 0x08 |
typedef struct { |
int32_t distance; |
int16_t bearing; |
} geo_t; |
typedef struct { |
int32_t Home_Lon; // in 1E-7 degrees |
int32_t Home_Lat; // in 1E-7 degrees |
int32_t Home_Alt; // in mm |
int16_t direction; // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
} __attribute__((packed)) HomePos_t; |
extern uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
#ifdef USE_TRACKING |
void PKT_tracking(void); |
#endif // #ifdef USE_TRACKING |
#endif // TRACKING_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/uart/uart1.c |
---|
0,0 → 1,638 |
/************************************************************************* |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.c,v 1.6.2.2 2009/11/29 08:56:12 Peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4.6 or higher |
Hardware: any AVR with built-in UART, |
License: GNU General Public License |
DESCRIPTION: |
An interrupt is generated when the UART has finished transmitting or |
receiving a byte. The interrupt handling routines use circular buffers |
for buffering received and transmitted data. |
The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define |
the buffer size in bytes. Note that these variables must be a |
power of 2. |
USAGE: |
Refere to the header file uart.h for a description of the routines. |
See also example test_uart.c. |
NOTES: |
Based on Atmel Application Note AVR306 |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
*************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.c |
//# |
//# 08.08.2015 CB |
//# - chg: Umbau ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - add: Erweiterung ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Funktion uart1_peek |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdbool.h> |
#include "uart1.h" |
#include "usart.h" |
#include "../main.h" |
#include "../bluetooth/bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../avr-nmea-gps-library/nmea.h" |
// |
// constants and macros |
// |
// size of RX/TX buffers |
#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1) |
#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1) |
#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK ) |
#error RX buffer size is not a power of 2 |
#endif |
#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK ) |
#error TX buffer size is not a power of 2 |
#endif |
// ATmega with two USART |
#define ATMEGA_USART1 |
#define UART1_STATUS UCSR1A |
#define UART1_CONTROL UCSR1B |
#define UART1_DATA UDR1 |
#define UART1_UDRIE UDRIE1 |
// |
// module global variables |
// |
#if defined( ATMEGA_USART1 ) |
static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE]; |
static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE]; |
static volatile unsigned char UART1_TxHead; |
static volatile unsigned char UART1_TxTail; |
static volatile unsigned char UART1_RxHead; |
static volatile unsigned char UART1_RxTmpHead; |
static volatile unsigned char UART1_RxTail; |
static volatile unsigned char UART1_LastRxError; |
static volatile unsigned char UART1_RxTmpHead;; |
volatile uint16_t UART1_RxError; |
#endif |
volatile uint8_t UART1_receiveNMEA = false; |
void SetBaudUart1(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: uart1_init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: uart1_init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
// case Baud_9600: uart1_init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_9600: uart1_init( 129); break; |
case Baud_19200: uart1_init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: uart1_init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_57600: uart1_init( 21); break; |
// case Baud_115200: uart1_init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
case Baud_115200: uart1_init( 10 ); break; |
} |
} |
// |
// these functions are only for ATmegas with two USART |
// |
#if defined( ATMEGA_USART1 ) |
#if 0 |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
// read UART status register and UART data register |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
UART1_LastRxError = lastRxError; |
} |
#else |
#warning Test neue UART ISR für NMEA |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
if (UART1_receiveNMEA) |
{ |
fusedata(data); // NMEA Daten bearbeiten |
} |
else // if (UART1_receiveNMEA) |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
} |
UART1_LastRxError = lastRxError; |
} |
////-------------------------------------------------------------- |
//// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
//// Purpose: called when the UART1 has received a character |
////-------------------------------------------------------------- |
//ISR(USART1_RX_vect) |
//{ |
// unsigned char tmphead; |
// unsigned char data; |
// unsigned char usr; |
// unsigned char lastRxError; |
// |
// static volatile uint8_t GGA; |
// static char buffer[6]; |
// static volatile uint8_t bufferindex; |
// static volatile uint8_t NMEAstart,NMEAend = false; |
// |
// // read UART status register and UART data register |
// usr = UART1_STATUS; |
// data = UART1_DATA; |
// |
// lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// |
// // calculate buffer index |
// |
// tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
// |
// if ( tmphead == UART1_RxTail ) |
// { |
// // error: receive buffer overflow |
// lastRxError = UART_BUFFER_OVERFLOW >> 8; |
// UART1_RxError++; |
// } |
// else |
// { |
// if (UART1_receiveNMEA) |
// { |
// cli(); |
// switch(data) |
// { |
// case '$': //start of a packet |
// |
// if (!NMEAstart && GGA) // falls mitten im Datensatz eine neuer kommt, warum auch immer |
// { |
// // restore index, war kein GPGGA Datensatz |
// UART1_RxHead = UART1_RxTmpHead; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// } |
// NMEAstart = true; |
// NMEAend = false; |
// GGA=false; |
// bufferindex=0; //wipe all these so they can be reused |
// |
// UART1_RxTmpHead = tmphead-1; // erstmal sichern, Start des NMEA Datensatzes |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// sei(); |
// break; |
// |
// default: //we have some of the CSV data |
// if(bufferindex<6) //dont mess up ! Dont overflow |
// { |
// buffer[bufferindex]=data; //stick the character in our buffer |
// } |
// |
// if(GGA) |
// { |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// switch ( data) |
// { |
// case '\r' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// |
// case '\n' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// } |
// |
// } |
// |
// else if(NMEAstart) //the header, erstes Komma noch nicht gekommen |
// |
// { |
// if(bufferindex<4) |
// |
// { |
// bufferindex++; //increase the position in the buffer |
// |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// } |
// else |
// |
// { |
//// cli(); |
// if(!strcmp(buffer,"GPGGA")) //the last character will be a 0(end of string) |
// |
// { |
// GGA=true; |
// NMEAstart = false; |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
//// sei(); |
// } |
// |
// else |
// |
// { |
// // restore index, war kein GPGGA Datensatz |
// |
// UART1_RxHead = UART1_RxTmpHead; |
//// commacount=0; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
//// sei(); |
// } |
// } |
// } |
// else if (!NMEAend && !GGA) UART1_RxHead = UART1_RxTmpHead; // Zeichen verwerfen |
// |
// } // end switch (data) |
// } |
// |
// else // if (UART1_receiveNMEA) |
// |
// { |
// // store new index |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
// } |
// } |
// UART1_LastRxError = lastRxError; |
//} |
#endif |
//-------------------------------------------------------------- |
// Function: UART1 Data Register Empty interrupt |
// Purpose: called when the UART1 is ready to transmit the next byte |
//-------------------------------------------------------------- |
ISR(USART1_UDRE_vect) |
{ |
unsigned char tmptail; |
if ( UART1_TxHead != UART1_TxTail) |
{ |
// calculate and store new buffer index |
tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK; |
UART1_TxTail = tmptail; |
// get one byte from buffer and write it to UART |
UART1_DATA = UART1_TxBuf[tmptail]; // start transmission |
} |
else |
{ |
// tx buffer empty, disable UDRE interrupt |
UART1_CONTROL &= ~_BV(UART1_UDRIE); |
} |
} |
//-------------------------------------------------------------- |
// Function: uart1_init() |
// Purpose: initialize UART1 and set baudrate |
// Input: baudrate using macro UART_BAUD_SELECT() |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_init(unsigned int baudrate) |
{ |
UART1_TxHead = 0; |
UART1_TxTail = 0; |
UART1_RxHead = 0; |
UART1_RxTail = 0; |
UART1_RxError = 0; |
UART1_receiveNMEA = false; //Empfang von NMEA Daten aus |
// Set baud rate |
if ( baudrate & 0x8000 ) |
{ |
UART1_STATUS = (1<<U2X1); //Enable 2x speed |
baudrate &= ~0x8000; |
} |
UBRR1H = (unsigned char)(baudrate>>8); |
UBRR1L = (unsigned char) baudrate; |
// Enable USART receiver and transmitter and receive complete interrupt |
UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1); |
// Set frame format: asynchronous, 8data, no parity, 1stop bit |
#ifdef URSEL1 |
UCSR1C = (1<<URSEL1)|(3<<UCSZ10); |
#else |
UCSR1C = (3<<UCSZ10); |
// UCSR1C = (1<<UCSZ11) | (1<<UCSZ10); // 8data Bit |
#endif |
} |
//-------------------------------------------------------------- |
// Function: uart1_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
unsigned int uart1_getc(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return UART_NO_DATA; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
UART1_RxTail = tmptail; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//-------------------------------------------------------------- |
// Function: uart1_peek() |
// Purpose: Returns the next byte (character) of incoming serial data without removing it from the internal serial buffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
int uart1_peek(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return -1; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//int HardwareSerial::peek(void) |
//{ |
// if (_rx_buffer->head == _rx_buffer->tail) { |
// return -1; |
// } else { |
// return _rx_buffer->buffer[_rx_buffer->tail]; |
// } |
//} |
//-------------------------------------------------------------- |
// Function: uart1_putc() |
// Purpose: write byte to ringbuffer for transmitting via UART |
// Input: byte to be transmitted |
// Returns: 1 on succes, 0 if remote not ready |
//-------------------------------------------------------------- |
int uart1_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK; |
while ( tmphead == UART1_TxTail ) |
{;} // wait for free space in buffer |
UART1_TxBuf[tmphead] = data; |
UART1_TxHead = tmphead; |
// enable UDRE interrupt |
UART1_CONTROL |= _BV(UART1_UDRIE); |
return (UART1_LastRxError << 8) + data; |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts() |
// Purpose: transmit string to UART1 |
// Input: string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts(const char *s ) |
{ |
while (*s) |
uart1_putc(*s++); |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts_p() |
// Purpose: transmit string from program memory to UART1 |
// Input: program memory string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts_p(const char *progmem_s ) |
{ |
register char c; |
while ( (c = pgm_read_byte(progmem_s++)) ) |
uart1_putc(c); |
} |
//-------------------------------------------------------------- |
// Function: uart1_available() |
// Purpose: Determine the number of bytes waiting in the receive buffer |
// Input: None |
// Returns: Integer number of bytes in the receive buffer |
//-------------------------------------------------------------- |
uint16_t uart1_available(void) |
{ |
return (UART_RX_BUFFER_MASK + UART1_RxHead - UART1_RxTail) % UART_RX_BUFFER_MASK; |
} |
//-------------------------------------------------------------- |
// Function: uart1_flush() |
// Purpose: Flush bytes waiting the receive buffer. Acutally ignores them. |
// Input: None |
// Returns: None |
//-------------------------------------------------------------- |
void uart1_flush(void) |
{ |
UART1_RxHead = UART1_RxTail; |
} |
/************************************************************************* |
Function: utoa() |
Purpose: convert unsigned integer to ascii |
Input: string_buffer, string_buffer_size, unsigned integer value |
Returns: first ascii character |
**************************************************************************/ |
char *utoa1(char* buffer, const unsigned int size, unsigned int value) |
{ |
char *p; |
// p points to last char |
p = buffer+size-1; |
// Set terminating Zero |
*p='\0'; |
do |
{ |
*--p = value%10 + '0'; |
value = value/10; |
} while (value!=0 && p>buffer); |
return p; |
}/* utoa */ |
#endif |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/uart/uart1.h |
---|
0,0 → 1,194 |
/************************************************************************ |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.h,v 1.8.2.1 2007/07/01 11:14:38 peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4 |
Hardware: any AVR with built-in UART, tested on AT90S8515 & ATmega8 at 4 Mhz |
License: GNU General Public License |
Usage: see Doxygen manual |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.h |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - chg: UART_RX_BUFFER_SIZE war 1024, darf aber nur max 256 sein |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 30.03.2013 CB |
//# - add: uart1_flush(void); in Headerdatei aufgenommen |
//########################################################################### |
// |
// @defgroup pfleury_uart UART Library |
// @code #include <uart.h> @endcode |
// |
// @brief Interrupt UART library using the built-in UART with transmit and receive circular buffers. |
// |
// This library can be used to transmit and receive data through the built in UART. |
// |
// An interrupt is generated when the UART has finished transmitting or |
// receiving a byte. The interrupt handling routines use circular buffers |
// for buffering received and transmitted data. |
// |
// The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE constants define |
// the size of the circular buffers in bytes. Note that these constants must be a power of 2. |
// You may need to adapt this constants to your target and your application by adding |
// CDEFS += -DUART_RX_BUFFER_SIZE=nn -DUART_RX_BUFFER_SIZE=nn to your Makefile. |
// |
// @note Based on Atmel Application Note AVR306 |
// @author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
// |
#ifndef UART_H |
#define UART_H |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
// constants and macros |
// @brief UART Baudrate Expression |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu)/((baudRate)*16l)-1) |
// @brief UART Baudrate Expression for ATmega double speed mode |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT_DOUBLE_SPEED(baudRate,xtalCpu) (((xtalCpu)/((baudRate)*8l)-1)|0x8000) |
// Size of the circular receive buffer, must be power of 2 |
#ifndef UART_RX_BUFFER_SIZE |
#define UART_RX_BUFFER_SIZE 256 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// Size of the circular transmit buffer, must be power of 2 |
#ifndef UART_TX_BUFFER_SIZE |
#define UART_TX_BUFFER_SIZE 64 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// test if the size of the circular buffers fits into SRAM |
#if ( (UART_RX_BUFFER_SIZE+UART_TX_BUFFER_SIZE) >= (RAMEND-0x60 ) ) |
#error "size of UART_RX_BUFFER_SIZE + UART_TX_BUFFER_SIZE larger than size of SRAM" |
#endif |
//global variable |
extern volatile uint16_t UART1_RxError; |
extern volatile uint8_t UART1_receiveNMEA; // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// high byte error return code of uart_getc() |
#define UART_FRAME_ERROR 0x0800 // Framing Error by UART |
#define UART_OVERRUN_ERROR 0x0400 // Overrun condition by UART |
#define UART_BUFFER_OVERFLOW 0x0200 // receive ringbuffer overflow |
#define UART_NO_DATA 0x0100 // no receive data available |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define NMEA_receive 5 // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// |
// function prototypes |
// |
// |
// @brief Initialize UART and set baudrate |
// @param baudrate Specify baudrate using macro UART_BAUD_SELECT() |
// @return none |
// |
extern void uart_init(unsigned int baudrate); |
// |
// @brief Get received byte from ringbuffer |
// |
// Returns in the lower byte the received character and in the |
// higher byte the last receive error. |
// UART_NO_DATA is returned when no data is available. |
// |
// @param void |
// @return lower byte: received byte from ringbuffer |
// @return higher byte: last receive status |
// - \b 0 successfully received data from UART |
// - \b UART_NO_DATA |
// <br>no receive data available |
// - \b UART_BUFFER_OVERFLOW |
// <br>Receive ringbuffer overflow. |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// - \b UART_OVERRUN_ERROR |
// <br>Overrun condition by UART. |
// A character already present in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped. |
// - \b UART_FRAME_ERROR |
// <br>Framing Error by UART |
// |
extern unsigned int uart_getc(void); |
// |
// @brief Put byte to ringbuffer for transmitting via UART |
// @param data byte to be transmitted |
// @return none |
// |
// @brief Set Baudrate USART1 |
extern void SetBaudUart1(uint8_t Baudrate); |
// @brief Initialize USART1 (only available on selected ATmegas) @see uart_init |
extern void uart1_init(unsigned int baudrate); |
// @brief Get received byte of USART1 from ringbuffer. (only available on selected ATmega) @see uart_getc |
extern unsigned int uart1_getc(void); |
// @brief Put byte to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_putc |
//extern void uart1_putc(unsigned char data); |
extern int uart1_putc(unsigned char data); |
// @brief Put string to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts |
extern void uart1_puts(const char *s ); |
// @brief Put string from program memory to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts_p |
extern void uart1_puts_p(const char *s ); |
// @brief Macro to automatically put a string constant into program memory |
#define uart1_puts_P(__s) uart1_puts_p(PSTR(__s)) |
extern char *utoa1(char* buffer, const unsigned int size, unsigned int value); |
extern uint16_t uart1_available(void); |
extern int uart1_peek(void); |
// @brief Flush bytes waiting the receive buffer. Acutally ignores them. |
extern void uart1_flush(void); |
#endif // UART_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/uart/usart.c |
---|
0,0 → 1,732 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.c |
//# |
//# 3.04.2015 Cebra |
//# - chg: 2 weitere /r am Ende des gesendeten Strings angefügt, damit Funktion gleich wie im MK-Tool |
//# |
//# 24.01.2014 OG |
//# - add: Debug-Code fuer die ISR-RX Funktion |
//# schaltbar via define DEBUG_FC_COMMUNICATION in main.h - der Code |
//# kann somit drin bleiben |
//# - add: Source-History |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <stdarg.h> |
#include "../main.h" |
#include "usart.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../osd/osd.h" |
uint8_t buffer[30]; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t ReceivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t RxDataLen = 0; |
volatile uint16_t stat_crc_error = 0; |
volatile uint16_t stat_overflow_error = 0; |
volatile uint8_t rx_byte; |
volatile uint8_t rxFlag = 0; |
#define UART_RXBUFSIZE 64 |
#define UART_NO_DATA 0x0100 /* no receive data available */ |
volatile static uint8_t rxbuf[UART_RXBUFSIZE]; |
volatile static uint8_t *volatile rxhead, *volatile rxtail; |
#ifdef DEBUG_FC_COMMUNICATION |
char *tmp = (char *)" "; |
#endif |
/* |
//----------------------------------------------------------------------------- |
// USART1 transmitter ISR |
ISR (USART1_TX_vect) |
{ |
static uint16_t ptr_txd1_buffer = 0; |
uint8_t tmp_tx1; |
if(!txd1_complete) // transmission not completed |
{ |
ptr_txd1_buffer++; // [0] was already sent |
tmp_tx1 = txd1_buffer[ptr_txd1_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx1 == '\r') || (ptr_txd1_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd1_buffer = 0; // reset txd pointer |
txd1_complete = TRUE; // stop transmission |
} |
UDR1 = tmp_tx1; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd1_buffer = 0; |
} |
*/ |
#ifdef USART_INT |
//----------------------------------------------------------------------------- |
// USART0 transmitter ISR |
ISR (USART_TX_vect) |
{ |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if(!txd_complete) // transmission not completed |
{ |
ptr_txd_buffer++; // [0] was already sent |
tmp_tx = txd_buffer[ptr_txd_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd_buffer = 0; // reset txd pointer |
txd_complete = TRUE; // stop transmission |
} |
UDR = tmp_tx; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd_buffer = 0; |
} |
#endif |
void SetBaudUart0(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: USART_Init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: USART_Init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
case Baud_9600: USART_Init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_19200: USART_Init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: USART_Init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: USART_Init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_115200: USART_Init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
// Macro erechnet falschen Wert (9,85 = 9) für 115200 Baud mit 20Mhz Quarz, zu grosse Abweichung |
//#warning "Baurate prüfen wenn kein 20 Mhz Quarz verwendet wird" |
case Baud_115200: USART_Init( 10 ); break; |
} |
} |
//----------------------------------------------------------------------------- |
// |
// |
//uint8_t uart_getc_nb(uint8_t *c) |
//{ |
// if (rxhead==rxtail) return 0; |
// *c = *rxtail; |
// if (++rxtail == (rxbuf + UART_RXBUFSIZE)) rxtail = rxbuf; |
// return 1; |
//} |
ISR (USART0_RX_vect) |
{ |
static uint16_t crc; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t c; |
// IdleTimer = 0; |
if (current_hardware == Wi232) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
// USART_putc (c); |
return; |
} |
if (current_hardware == MKGPS) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
return; |
} |
c = UDR; // catch the received byte |
if (OSD_active && Config.OSD_SendOSD) // Daten an SV2 senden |
uart1_putc(c); |
if (rxd_buffer_locked) |
return; // if rxd buffer is locked immediately return |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
{ |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
lcd_print( "#", MNORMAL ); // OG DEBUG |
#endif |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
} |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
{ |
if(c != '\r') // no termination character |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
else // termination character was received |
{ |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer-2]; |
crc -= rxd_buffer[ptr_rxd_buffer-1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
// compare checksum to transmitted checksum bytes |
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
{ // checksum valid |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
*tmp = rxd_buffer[2]; |
lcd_print( tmp, MNORMAL ); |
lcd_putc( 0,0, '[', MNORMAL ); |
lcd_putc( 1,0, rxd_buffer[2], MNORMAL ); |
lcd_putc( 2,0, ']', MNORMAL ); x |
if( rxd_buffer[2] != 'k' ) set_beep( 13, 0x0001, BeepNormal); |
if( rxd_buffer[2] == 'Q' ) set_beep( 150, 0x0080, BeepNormal); |
#endif |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
if (mode == rxd_buffer[2]) |
{ |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
} |
} |
else |
{ // checksum invalid |
stat_crc_error++; |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
ptr_rxd_buffer = 0; // reset rxd buffer pointer |
} |
} |
else // rxd buffer overrun |
{ |
stat_overflow_error++; |
ptr_rxd_buffer = 0; // reset rxd buffer |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
} |
//----------------------------------------------------------------------------- |
// Function: uart0_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//----------------------------------------------------------------------------- |
char USART_getc(void) |
{ |
char val; |
// while(rxhead==rxtail) ; |
if (rxhead==rxtail) |
return val=0; |
// IdleTimer = 0; |
val = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return val; |
} |
uint8_t uart_getc_nb(uint8_t *c) |
{ |
if (rxhead==rxtail) |
return 0; |
// IdleTimer = 0; |
*c = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return 1; |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void USART_Init (unsigned int baudrate) |
{ |
// set clock divider |
// #undef BAUD |
// #define BAUD baudrate |
// #include <util/setbaud.h> |
// UBRRH = UBRRH_VALUE; |
// UBRRL = UBRRL_VALUE; |
//#ifndef F_CPU |
///* In neueren Version der WinAVR/Mfile Makefile-Vorlage kann |
// F_CPU im Makefile definiert werden, eine nochmalige Definition |
// hier wuerde zu einer Compilerwarnung fuehren. Daher "Schutz" durch |
// #ifndef/#endif |
// |
// Dieser "Schutz" kann zu Debugsessions führen, wenn AVRStudio |
// verwendet wird und dort eine andere, nicht zur Hardware passende |
// Taktrate eingestellt ist: Dann wird die folgende Definition |
// nicht verwendet, sondern stattdessen der Defaultwert (8 MHz?) |
// von AVRStudio - daher Ausgabe einer Warnung falls F_CPU |
// noch nicht definiert: */ |
//#warning "F_CPU war noch nicht definiert, wird nun nachgeholt mit 4000000" |
//#define F_CPU 18432000UL // Systemtakt in Hz - Definition als unsigned long beachten |
// Ohne ergeben sich unten Fehler in der Berechnung |
//#endif |
//#define BAUD 115200UL // Baudrate |
// |
//// Berechnungen |
//#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) // clever runden |
//#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) // Reale Baudrate |
//#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) // Fehler in Promille, 1000 = kein Fehler. |
// |
// |
//#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010)) |
// #error "Systematischer Fehler der Baudrate grösser 1% und damit zu hoch!" |
//#endif |
UBRRH = (unsigned char)(baudrate>>8); |
UBRRL = (unsigned char) baudrate; |
// UBRRH = (unsigned char)(BAUD_REAL>>8); |
// UBRRL = (unsigned char) BAUD_REAL; |
#if USE_2X |
UCSRA |= (1 << U2X); // enable double speed operation |
#else |
UCSRA &= ~(1 << U2X); // disable double speed operation |
#endif |
// set 8N1 |
#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega32__) |
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0); |
#else |
UCSRC = (1 << UCSZ1) | (1 << UCSZ0); |
#endif |
UCSRB &= ~(1 << UCSZ2); |
// flush receive buffer |
while ( UCSRA & (1 << RXC) ) UDR; |
UCSRB |= (1 << RXEN) | (1 << TXEN); |
#ifdef USART_INT |
UCSRB |= (1 << RXCIE) | (1 << TXCIE); |
#else |
UCSRB |= (1 << RXCIE); |
#endif |
rxhead = rxtail = rxbuf; |
} |
//----------------------------------------------------------------------------- |
// disable the txd pin of usart |
void USART_DisableTXD (void) |
{ |
#ifdef USART_INT |
UCSRB &= ~(1 << TXCIE); // disable TX-Interrupt |
#endif |
UCSRB &= ~(1 << TXEN); // disable TX in USART |
DDRB &= ~(1 << DDB3); // set TXD pin as input |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
} |
//----------------------------------------------------------------------------- |
// enable the txd pin of usart |
void USART_EnableTXD (void) |
{ |
DDRB |= (1 << DDB3); // set TXD pin as output |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
UCSRB |= (1 << TXEN); // enable TX in USART |
#ifdef USART_INT |
UCSRB |= (1 << TXCIE); // enable TX-Interrupt |
#endif |
} |
//----------------------------------------------------------------------------- |
// short script to directly send a request thorugh usart including en- and disabling it |
// where <address> is the address of the receipient, <label> is which data set to request |
// and <ms> represents the milliseconds delay between data |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms) |
{ |
USART_EnableTXD (); // re-enable TXD pin |
unsigned char mstenth = ms/10; |
SendOutData(cmd, addr, 1, &mstenth, 1); |
// wait until command transmitted |
while (txd_complete == FALSE); |
USART_DisableTXD (); // disable TXD pin again |
} |
//----------------------------------------------------------------------------- |
// |
void USART_putc (char c) |
{ |
#ifdef USART_INT |
#else |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = c; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts (char *s) |
{ |
#ifdef USART_INT |
#else |
while (*s) |
{ |
USART_putc (*s); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts_p (const char *s) |
{ |
#ifdef USART_INT |
#else |
while (pgm_read_byte(s)) |
{ |
USART_putc (pgm_read_byte(s)); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
{ |
va_list ap; |
uint16_t pt = 0; |
uint8_t a,b,c; |
uint8_t ptr = 0; |
uint16_t tmpCRC = 0; |
uint8_t *pdata = 0; |
int len = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
va_start(ap, numofbuffers); |
if(numofbuffers) |
{ |
pdata = va_arg (ap, uint8_t*); |
len = va_arg (ap, int); |
ptr = 0; |
numofbuffers--; |
} |
while(len) |
{ |
if(len) |
{ |
a = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
a = 0; |
if(len) |
{ |
b = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
b = 0; |
if(len) |
{ |
c = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
c = 0; |
txd_buffer[pt++] = '=' + (a >> 2); |
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
txd_buffer[pt++] = '=' + ( c & 0x3f); |
} |
va_end(ap); |
for(a = 0; a < pt; a++) |
{ |
tmpCRC += txd_buffer[a]; |
} |
tmpCRC %= 4096; |
txd_buffer[pt++] = '=' + tmpCRC / 64; |
txd_buffer[pt++] = '=' + tmpCRC % 64; |
txd_buffer[pt++] = '\r'; |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_complete = FALSE; |
#ifdef USART_INT |
UDR = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
#else |
for(a = 0; a < pt; a++) |
{ |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = txd_buffer[a]; |
} |
txd_complete = TRUE; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void Decode64 (void) |
{ |
uint8_t a,b,c,d; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = ReceivedBytes - 6; |
while (len) |
{ |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
//if(ptrIn > ReceivedBytes - 3) break; |
if (len--) |
rxd_buffer[ptrOut++] = (a << 2) | (b >> 4); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((c & 0x03) << 6) | d; |
else |
break; |
} |
pRxData = &rxd_buffer[3]; |
RxDataLen = ptrOut - 3; |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToNC (void) |
{ |
if(hardware == NC) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void SwitchToWi232 (void) |
{ |
// if(hardware == NC) |
{ |
// switch to Wi232 |
current_hardware = Wi232; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToFC (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToMAG (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToGPS (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms (50); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/uart/usart.h |
---|
0,0 → 1,162 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.h |
//# |
//# 24.01.2014 OG |
//# - fix: RXD_BUFFER_LEN und TXD_BUFFER_LEN vergroessert 180 auf 230 wegen |
//# neuer FC-Version (FC v2.03) |
//# - add: Source-History |
//############################################################################ |
#ifndef _USART_H |
#define _USART_H |
//-------------------------------------------------------------- |
// |
#ifndef FALSE |
#define FALSE 0 |
#endif |
#ifndef TRUE |
#define TRUE 1 |
#endif |
// addresses |
#define ADDRESS_ANY 0 |
#define ADDRESS_FC 1 |
#define ADDRESS_NC 2 |
#define ADDRESS_MAG 3 |
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
//#define TXD_BUFFER_LEN 180 |
//#define RXD_BUFFER_LEN 180 |
#define TXD_BUFFER_LEN 230 |
#define RXD_BUFFER_LEN 230 |
// Baud rate of the USART |
#define USART_BAUD 57600 |
//#define USART_BAUD 125000 |
//-------------------------------------------------------------- |
// |
extern uint8_t buffer[30]; |
extern volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd_complete; |
extern volatile uint8_t txd1_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd1_complete; |
extern volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
extern volatile uint8_t rxd_buffer_locked; |
extern volatile uint8_t ReceivedBytes; |
extern volatile uint8_t *pRxData; |
extern volatile uint8_t RxDataLen; |
extern volatile uint16_t stat_crc_error; |
extern volatile uint16_t stat_overflow_error; |
extern volatile uint8_t rxFlag; |
extern volatile uint8_t rx_byte; |
//-------------------------------------------------------------- |
// |
void SetBaudUart0(uint8_t Baudrate); |
void USART_Init (unsigned int baudrate); |
void USART_DisableTXD (void); |
void USART_EnableTXD (void); |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms); |
void USART_putc (char c); |
void USART_puts (char *s); |
void USART_puts_p (const char *s); |
extern char USART_getc(void); |
void SendOutData (uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...); // uint8_t *pdata, uint8_t len, ... |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, uint8_t *pdata, uint8_t len); // uint8_t *pdata, uint8_t len, ... |
void Decode64 (void); |
void SwitchToNC (void); |
void SwitchToFC (void); |
void SwitchToMAG (void); |
void SwitchToGPS (void); |
void SwitchToWi232 (void); |
void debug1(void); |
uint8_t uart_getc_nb(uint8_t*); |
//-------------------------------------------------------------- |
//Anpassen der seriellen Schnittstellen Register |
#define USART_RXC_vect USART0_RX_vect |
//-------------------------------------------------------------- |
#define UCSRA UCSR0A |
#define UCSRB UCSR0B |
#define UCSRC UCSR0C |
#define UDR UDR0 |
#define UBRRL UBRR0L |
#define UBRRH UBRR0H |
// UCSRA |
#define RXC RXC0 |
#define TXC TXC0 |
#define UDRE UDRE0 |
#define FE FE0 |
#define UPE UPE0 |
#define U2X U2X0 |
#define MPCM MPCM0 |
// UCSRB |
#define RXCIE RXCIE0 |
#define TXCIE TXCIE0 |
#define UDRIE UDRIE0 |
#define TXEN TXEN0 |
#define RXEN RXEN0 |
#define UCSZ2 UCSZ02 |
#define RXB8 RXB80 |
#define TXB8 TXB80 |
// UCSRC |
#define UMSEL1 UMSEL01 |
#define UMSEL0 UMSEL00 |
#define UPM1 UPM01 |
#define UPM0 UPM00 |
#define USBS USBS0 |
#define UCSZ1 UCSZ01 |
#define UCSZ0 UCSZ00 |
#define UCPOL UCPOL0 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/i2cmaster.h |
---|
0,0 → 1,178 |
#ifndef _I2CMASTER_H |
#define _I2CMASTER_H 1 |
/************************************************************************* |
* Title: C include file for the I2C master interface |
* (i2cmaster.S or twimaster.c) |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: i2cmaster.h,v 1.10 2005/03/06 22:39:57 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device |
* Usage: see Doxygen manual |
**************************************************************************/ |
#ifdef DOXYGEN |
/** |
@defgroup pfleury_ic2master I2C Master library |
@code #include <i2cmaster.h> @endcode |
@brief I2C (TWI) Master Software Library |
Basic routines for communicating with I2C slave devices. This single master |
implementation is limited to one bus master on the I2C bus. |
This I2c library is implemented as a compact assembler software implementation of the I2C protocol |
which runs on any AVR (i2cmaster.S) and as a TWI hardware interface for all AVR with built-in TWI hardware (twimaster.c). |
Since the API for these two implementations is exactly the same, an application can be linked either against the |
software I2C implementation or the hardware I2C implementation. |
Use 4.7k pull-up resistor on the SDA and SCL pin. |
Adapt the SCL and SDA port and pin definitions and eventually the delay routine in the module |
i2cmaster.S to your target when using the software I2C implementation ! |
Adjust the CPU clock frequence F_CPU in twimaster.c or in the Makfile when using the TWI hardware implementaion. |
@note |
The module i2cmaster.S is based on the Atmel Application Note AVR300, corrected and adapted |
to GNU assembler and AVR-GCC C call interface. |
Replaced the incorrect quarter period delays found in AVR300 with |
half period delays. |
@author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
@par API Usage Example |
The following code shows typical usage of this library, see example test_i2cmaster.c |
@code |
#include <i2cmaster.h> |
#define Dev24C02 0xA2 // device address of EEPROM 24C02, see datasheet |
int main(void) |
{ |
unsigned char ret; |
i2c_init(); // initialize I2C library |
// write 0x75 to EEPROM address 5 (Byte Write) |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_write(0x75); // write value 0x75 to EEPROM |
i2c_stop(); // set stop conditon = release bus |
// read previously written value back from EEPROM address 5 |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_rep_start(Dev24C02+I2C_READ); // set device address and read mode |
ret = i2c_readNak(); // read one byte from EEPROM |
i2c_stop(); |
for(;;); |
} |
@endcode |
*/ |
#endif /* DOXYGEN */ |
/**@{*/ |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
#include <avr/io.h> |
/** defines the data direction (reading from I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_READ 1 |
/** defines the data direction (writing to I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_WRITE 0 |
/** |
@brief initialize the I2C master interace. Need to be called only once |
@param void |
@return none |
*/ |
extern void i2c_init(void); |
/** |
@brief Terminates the data transfer and releases the I2C bus |
@param void |
@return none |
*/ |
extern void i2c_stop(void); |
/** |
@brief Issues a start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_start(unsigned char addr); |
/** |
@brief Issues a repeated start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_rep_start(unsigned char addr); |
/** |
@brief Issues a start condition and sends address and transfer direction |
If device is busy, use ack polling to wait until device ready |
@param addr address and transfer direction of I2C device |
@return none |
*/ |
extern void i2c_start_wait(unsigned char addr); |
/** |
@brief Send one byte to I2C device |
@param data byte to be transfered |
@retval 0 write successful |
@retval 1 write failed |
*/ |
extern unsigned char i2c_write(unsigned char data); |
/** |
@brief read one byte from the I2C device, request more data from device |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readAck(void); |
/** |
@brief read one byte from the I2C device, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readNak(void); |
/** |
@brief read one byte from the I2C device |
Implemented as a macro, which calls either i2c_readAck or i2c_readNak |
@param ack 1 send ack, request more data from device<br> |
0 send nak, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_read(unsigned char ack); |
#define i2c_read(ack) (ack) ? i2c_readAck() : i2c_readNak(); |
/**@}*/ |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/menuctrl.c |
---|
0,0 → 1,1501 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.c |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 31.05.2014 OG |
//# - chg: MenuCtrl_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# - chg: include "../mk/mkparameters.h" geaendert auf "../mksettings/mkparameters.h" |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# - chg: _MenuCtrl_Error() - auf PKT_Popup_P() umgestellt, Layout geaendert und |
//# einen neuen Fehler (NOPARENTMENU) hinzugefuegt |
//# |
//# 09.05.2014 OG |
//# - fix: MenuCtrl_Refresh() Anzeige von deaktivierten MK-Parametern (Favoriten) |
//# Wenn MK-Paramketer-Menueeintraege deaktiviert sind werden sie vom |
//# aktuellen MK-Parameterset nicht unterstuetzt - dennoch wurde der |
//# aktuelle Wert des MK-Parameters im Menuepunkt (ganz rechts) angezeigt. |
//# Das ist jetzt behoben - es wird kein Wert angezeigt (stattdessen "***") |
//# und auch keine entsprechende editParam-Funktionen aufgerufen. |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - chg: MenuCtrl_Refresh() Keyline leicht angepasst |
//# - chg: 'not possible' Anzeige (bei deaktiviertem Item) umgestellt auf |
//# PKT_Popup_P() |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - chg: MenuCtrl_Control() |
//# -> Unterstuetzung fuer: MenuCtrl_SetMove() - Menueeintraege verschieben |
//# - chg: MenuCtrl_Refresh() - Anpassung Slider fuer MenuCtrl_SetDelete() |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# - add: _MenuCtrl_SwapItem() |
//# - add: die Datenstruktur vom Menue wurde erweitert um die Eigenschaften |
//# canMove und canDelete |
//# |
//# 17.04.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 22 auf 28 erhoeht |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 29.03.2014 OG |
//# - chg: default MENUDEFAULT_SHOWBATT auf true umgestellt (=PKT-Batterie anzeigen) |
//# - chg: MenuCtrl_Control() umgestellt auf clear_key_all() |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() - fuegt eine Trennlinie im Menue ein |
//# - chg: MenuCtrl_Refresh() Codeoptimierung bzgl. topspace/Zeile loeschen/Slider |
//# - fix: MenuCtrl_Control() es wurde zuoft ein Screen-Refresh durchgefuehrt |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - chg: MenuCtrl_Refresh() Unterstuetzung fuer MENU_PARAMEDIT |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: paramset.h und mkparameters.h |
//# |
//# 04.03.2014 OG |
//# - fix: MenuCtrl_Refresh() bei einem blauen Display (das ist wohl schneller |
//# in der Anzeige als das S/W-Display) flackerte der Menue-Slider |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# - chg: MenuCtrl_Refresh() angepasst auf MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 01.02.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 16 auf 22 fuer MK_parameters |
//# - fix: _MenuCtrl_Error() Anzeigezeit von Menu-Errors auf 8 Sekunden verlaengert |
//# und Anzeige von "menuctrl.c" |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() - Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts) |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - chg: MenuCtrl_Refresh() Anpassungen und Optimierung |
//# - add: MenuCtrl_ItemMark() |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - fix: MenuCtrl_Control() Eventcode Rueckgabe bei KEY_ENTER |
//# |
//# 22.05.2013 OG |
//# - fix: nach Aufruf einer Menuefunktion in MenuCtrl_Control() jetzt |
//# get_key_press(KEY_ALL) |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 20.05.2013 OG |
//# - chg: MenuCtrl_Control() wieder mit get_key_press(KEY_ALL) ergaenzt |
//# wenn aufgerufen mit MENUCTRL_EVENT oder erster Aufruf mit RETURN |
//# |
//# 20.05.2013 OG |
//# - chg: Space am Anfang der Titelanzeige |
//# |
//# 19.05.2013 OG |
//# - add: define MENU_SHOWLEVEL mit dem der aktuelle Menuelevel in der |
//# Titelzeile angezeigt werden kann |
//# - fix: Redraw-Logik bei MENUCTRL_RETURN |
//# - chg: MenuCtrl_Control() erweitert um PKT_Update() |
//# |
//# 18.05.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> // memset |
#include <stdarg.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../menu.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "../mksettings/mkparameters.h" |
#include "menuctrl.h" |
//############################################################################################# |
//# internal defines & structs |
//############################################################################################# |
#define MENU_SHOWLEVEL false // Anzeige der Menue-Verschachtelungstiefe in der Titelzeile |
// zeigt dem Benutzer wie weit er wieder zurueckspringen muss |
// um in das Hauptmenue zu kommen |
#define MENUDEFAULT_CYCLE false // cycle: wenn Menue am Ende dann wieder zum Anfang |
#define MENUDEFAULT_SHOWBATT true // showbatt: zeigt PKT-Batterie im Menuetitel ganz rechts |
#define MENUDEFAULT_BEEP true // beep: wenn Cursor ueber Ende/Anfang |
#define MENUCTRL_MAXMENUES 6 // Anzahl max. verschachlteter Menues |
//#define MENUCTRL_MAXITEMS 22 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
//#define MENUCTRL_MAXITEMS 28 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define MENUCTRL_MAXITEMS 34 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define ERROR_NOMENU 1 // Errorcode |
#define ERROR_MAXMENU 2 // Errorcode |
#define ERROR_MAXITEM 3 // Errorcode |
#define ERROR_NOITEM 4 // Errorcode |
#define ERROR_NOPARENTMENU 5 // Errorcode |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
const char *text; // Tastentext |
void (*func)(void); // ggf. Funktions-Pointer (0=keine Funktion) |
} menue_key_t; |
//--------------------------- |
// typedef: einzelnes Menueitem |
//--------------------------- |
typedef struct |
{ |
uint8_t id; |
uint8_t type; // MENU_ITEM, MENU_SUB, MENU_PARAMEDIT |
uint8_t disabled; // true / false (deaktiviert einen Menuepunkt) |
uint8_t mark; // true / false (markiert einen Menuepunkt Links '*') |
uint8_t markR; // true / false (markiert einen Menuepunkt Rechts) |
uint8_t textPGM; // true / false (true = text in PROGMEN; false = text im RAM) |
uint8_t textcount; // 1 .. NUM_LANG |
const char *text[NUM_LANG]; // de, en, nl (NUM_LANG aus messages.h) |
void (*func)(void); |
} menue_item_t; |
//--------------------------- |
// typedef: Menueitem Liste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; // aktives item |
uint8_t itemcount; // Anzahl Items |
uint8_t scroll_pos; // aktuelle obere Anzeigezeile |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
uint8_t topspace; // obere Leerzeilen - default=0 (Vorsicht! ggf. nur bedingt einsetzbar bei Menues die ohne scrollen auskommen!) |
uint8_t firstredraw; // true / false (flag fuer ein erstes redraw bei MENUCTRL_RETURN) |
uint8_t cycle; // true / false |
uint8_t showbatt; // true / false |
uint8_t beep; // true / false |
uint8_t canMove; // true / false |
uint8_t canDelete; // false/0 = kein entfernen; true/1 = entfernen mit Nachfrage; 2 = entfernen ohne Nachfrage |
uint8_t titlePGM; // true / false |
const char *title; // Menuetitel (oberste Zeile) (ggf. Multisprache machen?) |
uint8_t lastkey; // verwendet von GetKey() |
menue_key_t key_enter; |
menue_key_t key_enter_long; |
menue_key_t key_esc; |
menue_key_t key_esc_long; |
menue_item_t item[MENUCTRL_MAXITEMS]; |
} menue_t; |
uint8_t showlevel = MENU_SHOWLEVEL; // Anzeige Menulevel in der Titelanzeige |
int8_t midx = -1; // aktives Menue; -1 = kein Menue |
menue_t menu[MENUCTRL_MAXMENUES]; // Stack fuer n geschachltete Menues |
//############################################################################################# |
//# PRIVAT funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Error( uint8_t error ) |
{ |
const char *pStr = NULL; |
switch( error ) |
{ |
case ERROR_NOMENU: pStr = PSTR("NOMENU"); break; |
case ERROR_MAXMENU: pStr = PSTR("MAXMENU"); break; |
case ERROR_MAXITEM: pStr = PSTR("MAXITEM"); break; |
case ERROR_NOITEM: pStr = PSTR("NOITEM"); break; |
case ERROR_NOPARENTMENU: pStr = PSTR("NOPARENTMENU"); break; |
} |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error |
PKT_Popup_P( 10000, PSTR("menuctrl.c"), 0, PSTR("* ERROR *"), pStr ); // 10000 = max. 100 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
int8_t _MenuCtrl_FindItem( uint8_t itemid ) |
{ |
int8_t i; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return -1; } |
for( i=0; i<menu[midx].itemcount; i++) |
{ |
if( itemid == menu[midx].item[i].id ) |
{ |
return i; // found: return index |
} |
} |
return -1; // not found |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_CalcDisplayPos( void ) |
{ |
int8_t i; |
i = menu[midx].active - menu[midx].display_pos; |
if( i < 0 ) |
menu[midx].display_pos = menu[midx].active; |
else if( i > 5 ) |
menu[midx].display_pos = (menu[midx].active - 5); |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Beep( void ) |
{ |
if( menu[midx].beep ) |
set_beep( 25, 0xffff, BeepNormal ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// Dreieckstausch von zwei Menuepunkten. |
// Wird verwendet um Menueitems zu verschieben. |
//-------------------------------------------------------------- |
void _MenuCtrl_SwapItem( uint8_t itemindex_1, uint8_t itemindex_2 ) |
{ |
menue_item_t tmpItem; |
memcpy( &tmpItem , &menu[midx].item[itemindex_1], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_1] , &menu[midx].item[itemindex_2], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_2] , &tmpItem, sizeof(menue_item_t) ); |
} |
//############################################################################################# |
//# PUBLIC funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// MenuCtrl_Create() |
//-------------------------------------------------------------- |
void MenuCtrl_Create( void ) |
{ |
if( midx >= MENUCTRL_MAXMENUES) { _MenuCtrl_Error( ERROR_MAXMENU ); return; } |
midx++; |
memset( &menu[midx], 0, sizeof(menue_t) ); |
menu[midx].cycle = MENUDEFAULT_CYCLE; |
menu[midx].showbatt = MENUDEFAULT_SHOWBATT; |
menu[midx].beep = MENUDEFAULT_BEEP; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Destroy() |
//-------------------------------------------------------------- |
void MenuCtrl_Destroy( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
midx--; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ShowLevel( value ) |
// |
// Diese Einstellung ist Global und wirkt sich direkt auf alle |
// Menues aus! |
// Man kann das also nicht fuer einzelne Menues ein-/ausschalten. |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ShowLevel( uint8_t value ) |
{ |
showlevel = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle_P( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle_P( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = true; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String im RAM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = false; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitleFromParentItem() |
// |
// setzte anhand des uebergeordneten Menuepunktes (dem aufrufenden |
// Menuepunkt) den Titel des neuen Menues |
// |
// wird u.a. verwendet in setup.c |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitleFromParentItem( void ) |
{ |
uint8_t lang; |
const char *pStr; |
uint8_t mparent; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( midx < 1) { _MenuCtrl_Error( ERROR_NOPARENTMENU ); return; } |
mparent = midx-1; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[mparent].item[ menu[mparent].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Text vom uebergeordneten Menuepunkt |
//-------------------------- |
pStr = menu[mparent].item[ menu[mparent].active ].text[lang]; |
//-------------------------- |
// Titel setzen |
//-------------------------- |
if( menu[mparent].item[ menu[mparent].active ].textPGM ) |
{ |
MenuCtrl_SetTitle_P( pStr ); |
} |
else |
{ |
MenuCtrl_SetTitle( pStr ); |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetCycle( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetCycle( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].cycle = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetBeep( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetBeep( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].beep = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetMove( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege nach |
// oben oder nach unten zu verschieben. |
// Zum verschieden eines Menueeintrages muessen die PLUS/MINUS Tasten |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetMove( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canMove = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetDelete( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege zu loeschen. |
// Zum loeschen eines Menueeintrages muss dann die ganz rechte 'OK' Taste |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetDelete( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canDelete = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetShowBatt( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetShowBatt( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].showbatt = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTopSpace( n ) |
// |
// fuegt oben im Menue n Leerzeichen ein um Abstand vom Menuetitel |
// zu erhalten |
// |
// ACHTUNG! Das funktioniert nur mit Menues die nicht Scrollen! |
// -> also weniger als 6 Eintraege haben (je nach Anzahl von TopSpace) |
// |
// ACHTUNG! Keine Absicherung bzgl. obigem Warnpunkt! Das kann man |
// zwar machen - ist aber aktuell nicht so! |
// |
// Parameter: |
// value: 0..n Anzahl der Leerzeilen ueber dem Menue |
//-------------------------------------------------------------- |
void MenuCtrl_SetTopSpace( uint8_t n ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].topspace = n; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetKey( key, *keytext, *keyfunc ) |
// |
// Parameter: |
// key : KEY_ENTER, KEY_ENTER_LONG, KEY_ESC, KEY_ESC_LONG |
// keytext: Pointer auf Text PROGMEM oder NOTEXT (=0) |
// keyfunc: Pointer auf Funktion oder NOFUNC (=0) |
//-------------------------------------------------------------- |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein aktives Menue |
if( key == KEY_ENTER ) |
{ |
menu[midx].key_enter.active = true; |
menu[midx].key_enter.text = keytext; |
menu[midx].key_enter.func = keyfunc; |
} |
if( key == KEY_ENTER_LONG ) |
{ |
menu[midx].key_enter_long.active = true; |
menu[midx].key_enter_long.text = keytext; |
menu[midx].key_enter_long.func = keyfunc; |
} |
if( key == KEY_ESC ) |
{ |
menu[midx].key_esc.active = true; |
menu[midx].key_esc.text = keytext; |
menu[midx].key_esc.func = keyfunc; |
} |
if( key == KEY_ESC_LONG ) |
{ |
menu[midx].key_esc_long.active = true; |
menu[midx].key_esc_long.text = keytext; |
menu[midx].key_esc_long.func = keyfunc; |
} |
} |
//-------------------------------------------------------------- |
// key = MenuCtrl_GetKey() |
// |
// Rueckgabe: |
// key: z.B. KEY_ENTER oder KEY_ENTER_LONG |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetKey( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].lastkey; |
} |
//-------------------------------------------------------------- |
// menuindex = MenuCtrl_GetMenuIndex() |
// |
// gibt den aktuellen Menuindex zurueck |
// |
// Rueckgabe: |
// menuindex: <0 = keine Menue (-1) |
// =0 = Hauptmenue (Toplevel) |
// >0 = Untermenue Level |
//-------------------------------------------------------------- |
int8_t MenuCtrl_GetMenuIndex( void ) |
{ |
return midx; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemId() |
// |
// gibt die itemID des aktuell angewaehlten Menuepunktes zurueck |
// |
// Rueckgabe: |
// itemid: |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemId( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].id; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemIdByIndex( index ) |
// |
// damit koennen die aktuell vorhanden itemID's der Menuepunkte |
// ermittelt wenn im Menue via SetMove oder SetDelete Menuepunkte |
// durch den Benutzer verschoben oder geloescht wurden. |
// |
// Parameter: |
// index: von 0..itemcount-1 |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
if( (menu[midx].itemcount == 0) || (index > menu[midx].itemcount-1) ) return 0; // index ausserhalb aktiver Menuepunkte |
return menu[midx].item[ index ].id; |
} |
//-------------------------------------------------------------- |
// num = MenuCtrl_GetItemCount() |
// |
// gibt die Anzahl der Menuepunkte zurueck |
// |
// Rueckgabe: |
// num: Anzahl der Menuepunkte |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemCount( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].itemcount; |
} |
//-------------------------------------------------------------- |
// textPtr = MenuCtrl_GetItemText() |
// |
// gibt einen Pointer auf den Text des aktuellen Menuepunktes zurueck |
// |
// ACHTUNG! Der Pointer kann auf RAM oder auch auf PGM zeigen! |
// |
// Um herauszufinden auf welchen Bereich der Pointer zeigt kann |
// MenuCtrl_IsItemTextPGM() verwendet werden. |
// |
// Rueckgabe: |
// textPtr: in RAM oder PGM |
//-------------------------------------------------------------- |
const char * MenuCtrl_GetItemText( void ) |
{ |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[ menu[midx].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
return menu[midx].item[ menu[midx].active ].text[lang]; |
} |
//-------------------------------------------------------------- |
// isPGM = MenuCtrl_IsItemTextPGM() |
// |
// gibt true/false zurueck je nachdem der Text von MenuCtrl_GetItemText() |
// im Progmem (=true) oder im RAM (=false) ist |
// |
// Rueckgabe: |
// isPGM: true / false (true = text in PROGMEN; false = text im RAM) |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_IsItemTextPGM( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].textPGM; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemSelect( itemid ) |
// |
// aktiviert einen Menuepunkt / setzt den Menue-Cursor darauf |
// |
// die Funktion ist nicht durchgetestet, funktioniert aber |
// in MKSettings_Menu() / mkparameters.c |
// |
// es kann sein das es zu Problemen kommt wenn das Menue |
// scrollt (mehr als 6 Menueeintraege) |
// |
// Parameter: |
// itemid: ID des Menuitems |
//-------------------------------------------------------------- |
void MenuCtrl_ItemSelect( uint8_t itemid ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].active = i; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemActive( itemid, value ) |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].disabled = !value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMark( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem '*' am Anfang (Links). |
// Wird u.a. von BT_SelectDevice() (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].mark = value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMarkR( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts). |
// Wird u.a. von ... (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].markR = value; |
} |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// fuer: MenuCtrl_PushML_P(), MenuCtrl_Push() |
//-------------------------------------------------------------- |
void _MenuCtrl_Push( uint8_t useprogmem, uint8_t numlang, uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), ... ) |
{ |
va_list ap; |
uint8_t idx; |
uint8_t i; |
if( midx < 0 ) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( menu[midx].itemcount >= MENUCTRL_MAXITEMS ) { _MenuCtrl_Error( ERROR_MAXITEM ); return; } |
//if( numlang > NUM_LANG ) { _MenuCtrl_Error( ERROR_MAXLANG ); return; } |
idx = menu[midx].itemcount; |
menu[midx].item[idx].id = itemid; |
menu[midx].item[idx].type = itemtype; // MENU_ITEM, MENU_SUB |
menu[midx].item[idx].mark = false; |
menu[midx].item[idx].func = menufunc; |
menu[midx].item[idx].textPGM = useprogmem; |
menu[midx].item[idx].textcount= numlang; |
va_start(ap, menufunc); |
for( i=0; i<numlang; i++) |
{ |
menu[midx].item[idx].text[i] = va_arg( ap, const char *); |
} |
va_end(ap); |
menu[midx].itemcount++; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2_P( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// PROGMEN Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// RAM Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf RAM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
// Spezialfunktion fuer mkparameters.c ! |
// |
// -> siehe: mkparameters.c / Menu_EditCategory() |
// |
// ACHTUNG! |
// Die externe Variable paramEditItem muss in mkparameters.c |
// vor Aufruf dieser Funktion richtig initialisiert sein! |
// Ich habe darauf verzichtet hier nochmal ein find_paramEditItem() |
// aufzurufen um Speicherplatz zu sparen. |
//-------------------------------------------------------------- |
void MenuCtrl_PushParamEdit( uint8_t paramID ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 2, paramID, MENU_PARAMEDIT, NOFUNC, paramEditItem.title_de, paramEditItem.title_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparator() |
// |
// fuegt eine Trennlinie im Menue ein |
// (die itemID ist dabei 0) |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparator( void ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, 0, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparatorID( itemId ) |
// |
// fuegt eine Trennlinie im Menue ein mit einer eigenen itemID |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparatorID( uint8_t itemid ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, itemid, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Refresh( mode ) |
// |
// Parameter: |
// mode: MENU_REDRAW || MENU_REFRESH |
//-------------------------------------------------------------- |
void MenuCtrl_Refresh( uint8_t mode ) |
{ |
uint8_t y; |
uint8_t item; |
uint8_t sh; |
uint8_t sy; |
uint8_t actchar; |
uint8_t markchar; |
uint8_t markcharR; |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein Menue vorhanden |
if( mode == MENU_REDRAW ) |
{ |
//-------------------------- |
// Clear |
//-------------------------- |
lcd_frect( 0, 8, 6, 47, 0); // Clear: ganz linke Spalte des Sliders |
lcd_frect( 127-2, 0, 2, 63, 0); // Clear: ganz rechts 2 Pixel |
//-------------------------- |
// Titel |
//-------------------------- |
if( menu[midx].title != 0 ) |
{ |
if( showlevel ) |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19s"), menu[midx].title ); |
writex_ndigit_number_u( 0, 0, midx, 1, 0, MINVERS, 1,0); // Menuelevel zeigen |
} |
else |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20s"), menu[midx].title ); |
} |
} |
else |
lcd_frect( 0, 0, 128, 7, 1); // Titel: Leerzeile |
//-------------------------- |
// Keyline Beschriftung |
//-------------------------- |
lcdx_cls_row( 7, MNORMAL, 0); |
PKT_KeylineUpDown( 1, 7, 0,0); |
lcd_printp_at( 12, 7, strGet(KEYLINE4), MNORMAL); |
// Taste: KEY_ENTER |
if( menu[midx].key_enter.active && menu[midx].key_enter.text != 0 ) |
{ |
lcdx_printf_at_P( 17, 7, MNORMAL, 0,0 , PSTR("%4S"), menu[midx].key_enter.text ); |
} |
// Taste: KEY_ESC |
if( menu[midx].key_esc.active && menu[midx].key_esc.text != 0 ) |
{ |
lcdx_printf_at_P( 12, 7, MNORMAL, 0,0 , PSTR("%5S"), menu[midx].key_esc.text ); |
} |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Zeilen |
//-------------------------- |
for( y=0; y<6; y++) |
{ |
item = y + menu[midx].display_pos; |
if( item < menu[midx].itemcount ) |
{ |
//-------------------------- |
// char: aktuelle Auswahl |
//-------------------------- |
actchar = ' '; |
if( item == menu[midx].active ) |
{ |
if( menu[midx].item[item].disabled ) actchar = 0x15; // aktuelle Auswahl: Pfeil disabled |
else actchar = 0x1d; // aktuelle Auswahl: Pfeil |
} |
//-------------------------- |
// char: markiert |
//-------------------------- |
markchar = ' '; |
if( menu[midx].item[item].mark ) markchar = '*'; |
//-------------------------- |
// char: markiert RECHTS |
//-------------------------- |
markcharR = ' '; |
if( menu[midx].item[item].type == MENU_SUB ) markcharR = 0x1d; |
if( menu[midx].item[item].markR ) markcharR = SYMBOL_CHECK; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[item].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Ausgabe: RAM oder PROGMEM |
//-------------------------- |
if( MENU_SEPARATOR == menu[midx].item[item].type ) |
{ |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%18S") , actchar, markchar, PSTR("") ); |
lcd_frect( 6*3, ((y+1)*8)+3, 124-(6*3), 0, 1); // Trennlinie |
} |
else if( MENU_PARAMEDIT == menu[midx].item[item].type ) |
{ |
// SPEZIELL fuer mkparameters.c - zeigt die paramID-Bezeichnung inkl. dem aktuellen WERT an! |
strcpy_P( mkparam_strValueBuffer, PSTR("***")); // Vorbelegung von mkparam_strValueBuffer |
if( !menu[midx].item[item].disabled ) // nur wenn Menuepunkt nicht deaktiviert (disabled = paramID nicht vorhanden) |
{ // -> ermittle den aktuellen Wert der paramID fuer Anzeige |
find_paramEditItem( menu[midx].item[item].id ); // paramID suchen (Ergebnis in Variable 'paramEditItem') |
paramEditItem.editfunc( paramEditItem.paramID, MKPARAM_SHORTVALUE ); // Wert von paramID in mkparam_strValueBuffer darstellen |
} |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%14S %3s") , actchar, markchar, menu[midx].item[item].text[lang], mkparam_strValueBuffer ); |
} |
else |
{ |
// Anmerkung zu menu[midx].topspace: NUR fuer Menues OHNE scrollen!! |
if( menu[midx].item[item].textPGM ) |
{ |
// String im PGM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16S%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17S ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
else |
{ |
// String im RAM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16s%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17s ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
} |
} // end: if( item < menu[midx].itemcount ) |
//-------------------------- |
// evtl. Leerzeile loeschen |
//-------------------------- |
if( (y < menu[midx].topspace) || (item >= menu[midx].itemcount+menu[midx].topspace) ) |
{ |
lcd_frect( 6, (y+1)*8, 128-6, 7, 0); // Zeile loeschen |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 45 // Finetuning der Slider-Hoehe |
lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear |
if( menu[midx].itemcount > 6 ) // Slider nur bei mehr als 6 Menueitems |
{ |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 6) / menu[midx].itemcount; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = 8+(menu[midx].display_pos * (SLIDERH-sh)) / (menu[midx].itemcount-6); // Slider: Position |
//lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear (flackert auf blauen Display!) |
//lcd_frect( 0, 8+1, 1, sy-(8), 0); // Slider: Clear / oben (nicht mehr notwendig da Aenderung am Refresh) |
//lcd_frect( 0, sy+1+sh+1, 1, SLIDERH-(sh-sy+1), 0); // Slider: Clear / unten (nicht mehr notwendig da Aenderung am Refresh) |
lcd_frect( 0, sy+1, 1, sh, 1); // Slider: Draw |
} |
} |
//-------------------------------------------------------------- |
// event = MenuCtrl_Control( ctrlmode ) |
// |
// Parameter: |
// ctrlmode: MENUCTRL_EVENT || MENUCTRL_RETURN |
// |
// Rueckgabe: |
// event: MENUEVENT_NONE || MENUEVENT_ITEM || MENUEVENT_KEY |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ) |
{ |
uint8_t menu_event; |
uint8_t refreshmode; |
uint8_t item; |
uint8_t wahl; |
uint8_t i; |
uint8_t goUp = false; |
uint8_t goDown = false; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein Menue vorhanden |
if( menu[midx].itemcount == 0) { _MenuCtrl_Error( ERROR_NOITEM ); return 0; } // kein Menueitem vorhanden |
// bei MENUCTRL_RETURN muss man ggf. selber fuer ein Redraw sorgen |
// (beim ersten Aufruf wird es jedoch dennoch erzwungen) |
if( ctrlmode == MENUCTRL_EVENT || !menu[midx].firstredraw ) |
{ |
MenuCtrl_Refresh( MENU_REDRAW ); |
menu[midx].firstredraw = true; |
} |
clear_key_all(); |
do |
{ |
menu_event = MENUEVENT_NONE; |
menu[midx].lastkey = KEY_NONE; |
refreshmode = false; |
//-------------------------- |
// Pruefe PKT Update oder |
// andere PKT-Aktion |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
refreshmode = MENU_REDRAW; |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Cursor: nach oben (UP) |
//-------------------------- |
//+++++ |
// UP - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_MINUS) ) |
{ |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active-1 ); |
goUp = true; |
} |
else _MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
if( get_key_short(1 << KEY_MINUS) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus AKTIV |
//+++++ |
// UP - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus NICHT aktiv |
//+++++ |
// UP - goUp |
//+++++ |
if( goUp ) // up |
{ |
refreshmode = MENU_REFRESH; |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
menu[midx].active--; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = menu[midx].itemcount-1; |
} |
else |
{ |
_MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
// Cursor: nach unten (DOWN) |
//-------------------------- |
//+++++ |
// DOWN - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active+1 ); |
goDown = true; |
} |
else _MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
if( get_key_short(1 << KEY_PLUS) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus AKTIV |
//+++++ |
// DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
// DOWN - goDown |
//+++++ |
if( goDown ) // down |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
menu[midx].active++; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = 0; |
} |
else |
{ |
_MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
//-------------------------- |
item = menu[midx].active; |
goUp = false; |
goDown = false; |
//-------------------------- |
// KEY_ENTER |
//-------------------------- |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
// todo: keyfunc |
if( menu[midx].item[item].type == MENU_SEPARATOR ) |
{ |
// do nothing |
} |
else if( menu[midx].item[item].disabled ) |
{ |
set_beep( 200, 0x00ff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_NOTPOSSIBLE),0,0,0); // "nicht möglich!" |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].item[item].func != NOFUNC ) |
{ |
menu[midx].item[item].func(); |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].key_enter.func != NOFUNC ) |
{ |
menu[midx].key_enter.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu[midx].lastkey = KEY_ENTER; |
menu_event = MENUEVENT_ITEM; |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (Deletemodus AKTIV) |
// |
// loescht einen Menueeintrag |
//-------------------------- |
if( menu[midx].canDelete && get_key_long(1 << KEY_ENTER) && (menu[midx].itemcount > 0) ) |
{ |
wahl = 1; |
if( menu[midx].canDelete == 1 ) // ==1 -> mit Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 0, strGet(STR_MENUCTRL_DELASK),PSTR(""),0,0); // "Eintrag entfernen?" |
lcd_printp_at( 12, 7, strGet(YESNO), MINVERS); |
wahl = 0; |
while( !wahl ) |
{ |
if( get_key_short(1<<KEY_ESC) ) wahl = 1; // "Ja" |
if( get_key_short(1<<KEY_ENTER) ) wahl = 2; // "Nein" |
} |
refreshmode = MENU_REDRAW; |
} |
if( wahl==1 ) |
{ |
for( i=menu[midx].active; i<menu[midx].itemcount-1; i++) |
{ |
_MenuCtrl_SwapItem( i, i+1 ); |
} |
menu[midx].itemcount--; |
if( (menu[midx].display_pos > 0) && (menu[midx].display_pos+6 > menu[midx].itemcount) ) |
{ |
menu[midx].display_pos--; |
menu[midx].active--; |
} |
if( menu[midx].active >= menu[midx].itemcount ) |
menu[midx].active = menu[midx].itemcount-1; |
_MenuCtrl_CalcDisplayPos(); |
refreshmode = MENU_REDRAW; |
if( menu[midx].canDelete == 2 ) // ==2 -> direkt loeschen, ohne Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_DELITEM),0,0,0); // "Eintrag entfernt!" |
} |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (NICHT Deletemodus) |
//-------------------------- |
if( !menu[midx].canDelete && menu[midx].key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_enter_long.func != NOFUNC ) |
{ |
menu[midx].key_enter_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ENTER_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC_LONG |
//-------------------------- |
if( menu[midx].key_esc_long.active && get_key_long(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc_long.func != NOFUNC ) |
{ |
menu[midx].key_esc_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC |
//-------------------------- |
if( get_key_short(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc.func != NOFUNC ) |
{ |
menu[midx].key_esc.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC; |
} |
} |
if( refreshmode ) |
{ |
MenuCtrl_Refresh( refreshmode ); |
clear_key_all(); |
} |
} |
while ( menu_event == MENUEVENT_NONE && ctrlmode == MENUCTRL_EVENT ); |
return menu_event; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/menuctrl.h |
---|
0,0 → 1,138 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.h |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() |
//# - add: MENU_SEPARATOR |
//# - chg: Wert von MENU_REFRESH und MENU_REDRAW |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: define MENU_PARAMEDIT |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - add: MenuCtrl_ItemMark() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 17.05.2013 OG - NEU |
//############################################################################ |
#ifndef MENUCTRL_H |
#define MENUCTRL_H |
#define MENU_ITEM 1 // Typ des Menueitems: normaler Eintrag ohne ">" am Ende |
#define MENU_SUB 2 // Typ des Menueitems: Submenue - es wird noch ein ">" angezeigt |
#define MENU_SEPARATOR 3 // nur fuer MenuCtrl_PushSeparator() fuer (verwendet von mkparameters.c) |
#define MENU_PARAMEDIT 4 // nur fuer MenuCtrl_PushParamEdit() fuer (verwendet von mkparameters.c) |
#define NOFUNC 0 // keine Funktion uebergeben |
#define NOTEXT 0 // kein Text uebergeben |
#define MENUEVENT_NONE 0 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_ITEM 1 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_KEY 2 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUCTRL_EVENT 0 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung nur bei einem Event |
#define MENUCTRL_RETURN 1 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung immer |
#define MENU_REFRESH 1 // MenuCtrl_Refresh() |
#define MENU_REDRAW 2 // MenuCtrl_Refresh() |
void MenuCtrl_Create( void ); |
void MenuCtrl_Destroy( void ); |
void MenuCtrl_ShowLevel( uint8_t value ); |
void MenuCtrl_SetTitle( const char *title ); |
void MenuCtrl_SetTitle_P( const char *title ); |
void MenuCtrl_SetTitleFromParentItem( void ); |
void MenuCtrl_SetCycle( uint8_t value ); |
void MenuCtrl_SetShowBatt( uint8_t value ); |
void MenuCtrl_SetBeep( uint8_t value ); |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ); |
void MenuCtrl_SetTopSpace( uint8_t value ); |
void MenuCtrl_SetMove( uint8_t value ); |
void MenuCtrl_SetDelete( uint8_t value ); |
int8_t MenuCtrl_GetMenuIndex( void ); |
uint8_t MenuCtrl_GetItemId( void ); |
uint8_t MenuCtrl_GetKey( void ); |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ); |
uint8_t MenuCtrl_GetItemCount( void ); |
const char * MenuCtrl_GetItemText( void ); |
uint8_t MenuCtrl_IsItemTextPGM( void ); |
void MenuCtrl_ItemSelect( uint8_t itemid ); |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ); |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_PushParamEdit( uint8_t itemid ); |
void MenuCtrl_PushSeparator( void ); |
void MenuCtrl_PushSeparatorID( uint8_t itemid ); |
void MenuCtrl_Refresh( uint8_t mode ); |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ); |
#endif // MENUCTRL_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/scrollbox.c |
---|
0,0 → 1,570 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.c |
//# |
//# 31.05.2014 OG |
//# - chg: ScrollBox_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 29.03.2014 OG |
//# - chg: ScrollBox_Show() umgestellt auf clear_key_all() |
//# |
//# 22.05.2013 OG |
//# - fix: include pkt/pkt.h |
//# |
//# 19.05.2013 OG |
//# - chg: ScrollBox_Show() erweitert um PKT_CtrlHook() um u.a. PKT-Updates |
//# zu ermoeglichen |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() - Variante fuer 'format' im RAM |
//# - chg: ScrollBox_Push_P() -> keine Rueckgabe mehr (void) |
//# - chg: auf xprintf umgestellt (siehe utils/xstring.c) |
//# - add: verschiedene Beschreibungen ueber Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
//#define USE_SCROLLBOX_DEBUG // Debug-Funktionen einkompilieren/aktivieren (gesendet wird an uart1) |
#include "../cpu.h" |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdarg.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" // xprintf |
#include "scrollbox.h" |
// Debug |
#ifdef USE_SCROLLBOX_DEBUG |
#include "../uart/uart1.h" |
#include <util/delay.h> |
#endif |
#define SCROLLBOX_ALLOC_MEM // dynamische RAM-Belegen (malloc) verwenden? (Standardeinstellung: ja) |
#define SCROLLBOX_LINESIZE 20+1 // Ausgabezeile der ScrollBox (20 Chars + 0x00) |
#define SCROLLBOX_KEYSIZE 4+1 // Textlabel einer Taste (4 Chars + 0x00) |
#define SCROLLBOX_W 20 // Textbreite |
#define SCROLLBOX_H 7 // Textzeilen |
#define MSBLINE 10 // drawmode-code fuer: Linie |
//----------------------------------------------------------- |
// statischer Speicher wenn nicht SCROLLBOX_ALLOC_MEM |
// verwendet wird |
//----------------------------------------------------------- |
#ifndef SCROLLBOX_ALLOC_MEM |
#define SCROLLBOX_BUFFER_SIZE 2048 // 2 KByte - ram-size in bytes |
char scrollbox_buffer[SCROLLBOX_BUFFER_SIZE]; |
#endif |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
char text[SCROLLBOX_KEYSIZE]; // Tastentext |
} scrollbox_key_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_line_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t mode; // drawmode: MNORMAL, MINVERSE oder MSBLINE |
char line[SCROLLBOX_LINESIZE]; // malloc: lines * 21 bytes (20 chars + 0) |
} scrollbox_line_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_t |
//----------------------------------------------------------- |
typedef struct |
{ |
scrollbox_line_t *buffer; // malloc: lines * 22 bytes (21 chars + 0) |
uint8_t maxlines; // max. reservierte Lines (malloc buffer) |
uint8_t lines; // Anzahl gepushte lines |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
scrollbox_key_t key_enter; |
scrollbox_key_t key_enter_long; |
} scrollbox_t; |
//----------------------------------------------------------- |
// Buffer & Co. |
//----------------------------------------------------------- |
scrollbox_t scrollbox; |
char buffer_sbline[SCROLLBOX_LINESIZE]; |
//############################################################################################# |
//############################################################################################# |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#ifdef USE_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// nur wenn define: USE_SCROLLBOX_DEBUG |
//-------------------------------------------------------------- |
void ScrollBox_Debug( void ) |
{ |
char s[33]; |
uint8_t i,j; |
char *p; |
scrollbox_line_t *sboxp; |
uint32_t h; |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: BEGIN\r\n\r\n") ); |
ltoa( scrollbox.buffer, s, 16); |
uart1_puts_p( PSTR("address: buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
ltoa( scrollbox_buffer, s, 16); |
uart1_puts_p( PSTR("address: scrollbox_buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
itoa( sizeof(scrollbox_line_t), s, 10); |
uart1_puts_p( PSTR("sizeof(scrollbox_line_t) = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.lines, s, 10); |
uart1_puts_p( PSTR("scrollbox.lines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.maxlines, s, 10); |
uart1_puts_p( PSTR("scrollbox.maxlines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.display_pos, s, 10); |
uart1_puts_p( PSTR("scrollbox.display_pos = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
for(i=0; i<scrollbox.lines; i++) |
{ |
itoa( i, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
//sboxp = scrollbox.buffer + sizeof(scrollbox_line_t)*i; |
sboxp = scrollbox.buffer + i; |
ltoa( sboxp, s, 16); // hex |
uart1_puts( "[0x" ); |
uart1_puts( s ); |
uart1_puts( "]" ); |
ltoa( sboxp, s, 10); // dec |
uart1_puts( "[" ); |
uart1_puts( s ); |
uart1_puts( "] " ); |
itoa( sboxp->mode, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
p = sboxp->line; |
if( sboxp->mode != 10 ) |
{ |
uart1_puts( p ); |
} |
uart1_puts( "|\r\n" ); |
} |
//-------------------------------------------------------------- |
// Hier wird fuer eine gewisse Zeit (insgesamt 2500 mal) permanent die Zeile 23 ausgegeben. |
// Wenn ich das richtig sehe schreibe ich hierbei nicht ;-) |
// Aber was sagt das Empfangsterminal.... |
//-------------------------------------------------------------- |
sboxp = scrollbox.buffer + 23; |
for( h=0; h<10; h++) |
{ |
itoa( h, s, 10); |
uart1_puts( s ); |
uart1_puts( "# " ); |
uart1_puts( sboxp->line ); |
uart1_puts( "|\r\n" ); |
_delay_ms(10); |
} |
//-------------------------------------------------------------- |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: END\r\n") ); |
uart1_puts( "##########################################\r\n\r\n" ); |
} |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#endif // USER_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// ok = ScrollBox_Create( uint8_t maxlines ) |
// |
// Rueckgabe: |
// true = ok |
// false = kein Speicher |
// |
// - reserviert speicher fuer max. maxlines |
// - initialisiert scrollbox-Datenstruktur |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Create( uint8_t maxlines ) |
{ |
scrollbox.buffer = 0; |
#ifdef SCROLLBOX_ALLOC_MEM |
scrollbox.buffer = malloc( maxlines * sizeof(scrollbox_line_t)); |
//scrollbox.buffer = calloc( maxlines, sizeof(scrollbox_line_t) ); |
#else |
scrollbox.buffer = (scrollbox_line_t *) scrollbox_buffer; |
#endif |
if( !scrollbox.buffer ) // kein RAM mehr |
{ |
lcd_cls(); |
lcd_printp_at( 1, 3, PSTR("Error: ScrollBox"), MNORMAL); |
lcd_printp_at( 1, 4, PSTR("No more RAM"), MNORMAL); |
set_beep ( 500, 0x3333, BeepNormal); |
while (!get_key_short (1 << KEY_ESC)); |
return false; |
} |
scrollbox.maxlines = maxlines; |
scrollbox.lines = 0; |
scrollbox.display_pos = 0; |
scrollbox.key_enter.active = false; |
scrollbox.key_enter_long.active = false; |
return true; |
} |
//-------------------------------------------------------------- |
// ScrollBox_Destroy() |
// |
// gibt reservierten Speicher wieder frei |
// Wichtig! Da malloc verwendet wird! |
//-------------------------------------------------------------- |
void ScrollBox_Destroy( void ) |
{ |
if( scrollbox.buffer ) |
{ |
#ifdef SCROLLBOX_ALLOC_MEM |
free( scrollbox.buffer ); |
#endif |
} |
scrollbox.buffer = 0; |
} |
//-------------------------------------------------------------- |
// ScrollBox_PushLine() |
// |
// fuegt eine Trennlinie hinzu |
//-------------------------------------------------------------- |
void ScrollBox_PushLine( void ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
sboxp->mode = MSBLINE; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// _scrollbox_push(...) |
// |
// intern fuer: ScrollBox_Push_P(), ScrollBox_Push() |
// Beschreibung siehe dort |
//-------------------------------------------------------------- |
void _scrollbox_push( uint8_t useprogmem, uint8_t mode, const char *format, va_list ap ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
_xvsnprintf( useprogmem, buffer_sbline, SCROLLBOX_LINESIZE, format, ap); |
strncpyfill( sboxp->line, buffer_sbline, SCROLLBOX_LINESIZE); // copy: buffer_sbline zur scrollbox |
sboxp->mode = mode; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push_P( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im PROGMEM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( true, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im RAM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( false, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// key: KEY_ENTER, KEY_ENTER_LONG (keine weiteren bisher!) |
//-------------------------------------------------------------- |
void ScrollBox_SetKey( uint8_t key, const char *keytext ) |
{ |
if( key == KEY_ENTER ) |
{ |
scrollbox.key_enter.active = true; |
strncpyfill( scrollbox.key_enter.text, keytext, SCROLLBOX_KEYSIZE); |
} |
if( key == KEY_ENTER_LONG ) |
{ |
scrollbox.key_enter_long.active = true; |
strncpyfill( scrollbox.key_enter_long.text, keytext, SCROLLBOX_KEYSIZE); |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Refresh() |
// |
// zeigt die ScrollBox - wird normalerweise von ScrollBox_Show() |
// automatisch durchgefuehrt |
//-------------------------------------------------------------- |
void ScrollBox_Refresh( void ) |
{ |
uint8_t y; |
uint8_t sh; |
uint8_t sy; |
scrollbox_line_t *sboxp; |
//-------------------------- |
// Text |
//-------------------------- |
for( y=0; y<7; y++) |
{ |
sboxp = scrollbox.buffer + (scrollbox.display_pos + y); |
if( y + scrollbox.display_pos < scrollbox.lines ) |
{ |
if( sboxp->mode == MSBLINE ) |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
lcd_line ( (21-SCROLLBOX_W)*6, (y*8)+3, 125, (y*8)+3, 1); // line |
} |
else |
{ |
lcd_puts_at( 21-SCROLLBOX_W, y, sboxp->line, sboxp->mode); |
} |
//p += sizeof(scrollbox_line_t); |
} |
else // clear |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 55 // Finetuning der Slider-Hoehe |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 7) / scrollbox.lines; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = (scrollbox.display_pos * (SLIDERH-sh)) / (scrollbox.lines-7); // Slider: Position |
lcd_frect( 0, 0, 1, SLIDERH, 0); // Slider: Clear |
lcd_frect( 0, sy, 1, sh, 1); // Slider: draw // Slider: Draw |
//-------------------------- |
// Keyline |
//-------------------------- |
PKT_KeylineUpDown( 1, 7, 0,0); // Keyline: Up / Down |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline: Ende |
if( scrollbox.key_enter.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter.text , MNORMAL); |
else if ( scrollbox.key_enter_long.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter_long.text, MNORMAL); |
else lcd_printp_at(17, 7, PSTR(" ") , MNORMAL); |
//ScrollBox_Debug(); |
} |
//-------------------------------------------------------------- |
// key = ScrollBox_Show() |
// |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Show( void ) |
{ |
uint8_t keyexit = 0; |
clear_key_all(); |
if( scrollbox.buffer ) |
{ |
ScrollBox_Refresh(); |
do |
{ |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
//-------------------------- |
// scrollen: nach unten |
//-------------------------- |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2 )) // down |
{ |
if( scrollbox.display_pos < ( scrollbox.lines - SCROLLBOX_H) ) |
{ |
scrollbox.display_pos++; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am unteren Ende angelangt |
} |
} |
//-------------------------- |
// scrollen: nach oben |
//-------------------------- |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2 )) // up |
{ |
if( scrollbox.display_pos > 0 ) |
{ |
scrollbox.display_pos--; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am oberen Ende angelangt |
} |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter.active && get_key_short(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER; |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER_LONG; |
} |
} while( !get_key_press(1 << KEY_ESC) && (keyexit == 0) ); |
} |
//get_key_press(KEY_ALL); // ersetzt durch obiges clear_key_all() |
return keyexit; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/scrollbox.h |
---|
0,0 → 1,49 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.h |
//# |
//# 17.05.2013 OG |
//# - del: defines KEY_xyz_LONG verschoben nach HAL_HW3_9.h |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() |
//# - chg: ScrollBox_Push_P() (keine Rueckgabe mehr) |
//# - add: include <stdarg.h> |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _SCROLLBOX_H |
#define _SCROLLBOX_H |
#include <stdarg.h> // Notwendig! (OG) |
uint8_t ScrollBox_Create( uint8_t maxlines ); |
void ScrollBox_Destroy( void ); |
void ScrollBox_PushLine( void ); |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ); |
void ScrollBox_Push( uint8_t mode, const char *format, ... ); |
void ScrollBox_SetKey( uint8_t key, const char *keytext ); |
void ScrollBox_Refresh( void ); |
uint8_t ScrollBox_Show( void ); |
//void ScrollBox_Debug( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/twi_slave.c |
---|
0,0 → 1,373 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.c |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : This is sample driver to AVRs TWI module. |
* It is interupt driveren. All functionality is controlled through |
* passing information to and from functions. Se main.c for samples |
* of how to use the driver. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
#if defined(__ICCAVR__) |
#include "ioavr.h" |
#include "inavr.h" |
#else |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#endif |
#include "twi_slave.h" |
// Emulate GCC ISR() statement in IAR |
#if defined(__ICCAVR__) |
#define PRAGMA(x) _Pragma( #x ) |
#define ISR(vec) PRAGMA( vector=vec ) __interrupt void handler_##vec(void) |
#endif |
static unsigned char TWI_buf[TWI_BUFFER_SIZE]; // Transceiver buffer. Set the size in the header file |
static unsigned char TWI_msgSize = 0; // Number of bytes to be transmitted. |
static unsigned char TWI_state = TWI_NO_STATE; // State byte. Default set to TWI_NO_STATE. |
// This is true when the TWI is in the middle of a transfer |
// and set to false when all bytes have been transmitted/received |
// Also used to determine how deep we can sleep. |
static volatile unsigned char TWI_busy = 0; |
union TWI_statusReg_t TWI_statusReg = {0}; // TWI_statusReg is defined in TWI_Slave.h |
/**************************************************************************** |
Call this function to set up the TWI slave to its initial standby state. |
Remember to enable interrupts from the main application after initializing the TWI. |
Pass both the slave address and the requrements for triggering on a general call in the |
same byte. Use e.g. this notation when calling this function: |
TWI_Slave_Initialise( (TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) ); |
The TWI module is configured to NACK on any requests. Use a TWI_Start_Transceiver function to |
start the TWI. |
****************************************************************************/ |
void twi_slave_init( unsigned char TWI_ownAddress ) |
{ |
TWAR = TWI_ownAddress; // Set own TWI slave address. Accept TWI General Calls. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins. |
(0<<TWIE)|(0<<TWINT)| // Disable TWI Interupt. |
(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Do not ACK on any requests, yet. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to test if the TWI_ISR is busy transmitting. |
****************************************************************************/ |
unsigned char twi_slave_busy( void ) |
{ |
return TWI_busy; |
} |
/**************************************************************************** |
Call this function to fetch the state information of the previous operation. The function will hold execution (loop) |
until the TWI_ISR has completed with the previous operation. If there was an error, then the function |
will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_status( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI has completed the transmission. |
return ( TWI_state ); // Return error state. |
} |
/**************************************************************************** |
Call this function to send a prepared message, or start the Transceiver for reception. Include |
a pointer to the data to be sent if a SLA+W is received. The data will be copied to the TWI buffer. |
Also include how many bytes that should be sent. Note that unlike the similar Master function, the |
Address byte is not included in the message buffers. |
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation, |
then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start_with_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char temp; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_msgSize = msgSize; // Number of data to transmit. |
for ( temp = 0; temp < msgSize; temp++ ) // Copy data that may be transmitted if the TWI Master requests data. |
{ |
TWI_buf[ temp ] = msg[ temp ]; |
} |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 1; |
} |
/**************************************************************************** |
Call this function to start the Transceiver without specifing new transmission data. Useful for restarting |
a transmission, or just starting the transceiver for reception. The driver will reuse the data previously put |
in the transceiver buffers. The function will hold execution (loop) until the TWI_ISR has completed with the |
previous operation, then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to read out the received data from the TWI transceiver buffer. I.e. first call |
TWI_Start_Transceiver to get the TWI Transceiver to fetch data. Then Run this function to collect the |
data when they have arrived. Include a pointer to where to place the data and the number of bytes |
to fetch in the function call. The function will hold execution (loop) until the TWI_ISR has completed |
with the previous operation, before reading out the data and returning. |
If there was an error in the previous transmission the function will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char i; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
if( TWI_statusReg.lastTransOK ) // Last transmission completed successfully. |
{ |
for ( i=0; i<msgSize; i++ ) // Copy data from Transceiver buffer. |
{ |
msg[ i ] = TWI_buf[ i ]; |
} |
TWI_statusReg.RxDataInBuf = FALSE; // Slave Receive data has been read from buffer. |
} |
return( TWI_statusReg.lastTransOK ); |
} |
// ********** Interrupt Handlers ********** // |
/**************************************************************************** |
This function is the Interrupt Service Routine (ISR), and called when the TWI interrupt is triggered; |
that is whenever a TWI event has occurred. This function should not be called directly from the main |
application. |
****************************************************************************/ |
ISR(TWI_vect) |
{ |
static unsigned char TWI_bufPtr; |
switch (TWSR) |
{ |
case TWI_STX_ADR_ACK: // Own SLA+R has been received; ACK has been returned |
// case TWI_STX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
case TWI_STX_DATA_ACK: // Data byte in TWDR has been transmitted; ACK has been received |
TWDR = TWI_buf[TWI_bufPtr++]; |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_STX_DATA_NACK: // Data byte in TWDR has been transmitted; NACK has been received. |
// I.e. this could be the end of the transmission. |
if (TWI_bufPtr == TWI_msgSize) // Have we transceived all expected data? |
{ |
TWI_statusReg.lastTransOK = TRUE; // Set status bits to completed successfully. |
} |
else // Master has sent a NACK before all data where sent. |
{ |
TWI_state = TWSR; // Store TWI State as errormessage. |
} |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Answer on next address match |
(0<<TWWC); // |
TWI_busy = 0; // Transmit is finished, we are not busy anymore |
break; |
case TWI_SRX_GEN_ACK: // General call address has been received; ACK has been returned |
// case TWI_SRX_GEN_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
TWI_statusReg.genAddressCall = TRUE; |
case TWI_SRX_ADR_ACK: // Own SLA+W has been received ACK has been returned |
// case TWI_SRX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
// Dont need to clear TWI_S_statusRegister.generalAddressCall due to that it is the default state. |
TWI_statusReg.RxDataInBuf = TRUE; |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Expect ACK on this transmission |
(0<<TWWC); |
TWI_busy = 1; |
break; |
case TWI_SRX_ADR_DATA_ACK: // Previously addressed with own SLA+W; data has been received; ACK has been returned |
case TWI_SRX_GEN_DATA_ACK: // Previously addressed with general call; data has been received; ACK has been returned |
// TODO: What is this? Seems to be no bounds checking! |
TWI_buf[TWI_bufPtr++] = TWDR; |
TWI_statusReg.lastTransOK = TRUE; // Set flag transmission successfull. |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Send ACK after next reception |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_SRX_STOP_RESTART: // A STOP condition or repeated START condition has been received while still addressed as Slave |
// Enter not addressed mode and listen to address match |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Enable interrupt and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Wait for new address match |
(0<<TWWC); // |
TWI_busy = 0; // We are waiting for a new address match, so we are not busy |
break; |
case TWI_SRX_ADR_DATA_NACK: // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
case TWI_SRX_GEN_DATA_NACK: // Previously addressed with general call; data has been received; NOT ACK has been returned |
case TWI_STX_DATA_ACK_LAST_BYTE: // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// case TWI_NO_STATE // No relevant state information available; TWINT = �0� |
case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition |
TWI_state = TWSR; //Store TWI State as errormessage, operation also clears noErrors bit |
TWCR = (1<<TWSTO)|(1<<TWINT); //Recover from TWI_BUS_ERROR, this will release the SDA and SCL pins thus enabling other devices to use the bus |
break; |
default: |
TWI_state = TWSR; // Store TWI State as errormessage, operation also clears the Success bit. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Acknowledge on any new requests. |
(0<<TWWC); // |
TWI_busy = 0; // Unknown status, so we wait for a new address match that might be something we can handle |
} |
} |
/* |
void example(){ |
unsigned char messageBuf[TWI_BUFFER_SIZE]; |
unsigned char TWI_slaveAddress; |
// LED feedback port - connect port B to the STK500 LEDS |
DDRB = 0xFF; // Set to ouput |
PORTB = 0x55; // Startup pattern |
// Own TWI slave address |
TWI_slaveAddress = 0x10; |
// Initialise TWI module for slave operation. Include address and/or enable General Call. |
TWI_Slave_Initialise( (unsigned char)((TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) )); |
SEI(); |
// Start the TWI transceiver to enable reseption of the first command from the TWI Master. |
TWI_Start_Transceiver(); |
// This example is made to work together with the AVR315 TWI Master application note. In adition to connecting the TWI |
// pins, also connect PORTB to the LEDS. The code reads a message as a TWI slave and acts according to if it is a |
// general call, or an address call. If it is an address call, then the first byte is considered a command byte and |
// it then responds differently according to the commands. |
// This loop runs forever. If the TWI is busy the execution will just continue doing other operations. |
for(;;) |
{ |
#ifdef POWER_MANAGEMENT_ENABLED |
// Sleep while waiting for TWI transceiver to complete or waiting for new commands. |
// If we have data in the buffer, we can't enter sleep because we have to take care |
// of it first. |
// If the transceiver is busy, we enter idle mode because it will wake up by all TWI |
// interrupts. |
// If the transceiver not is busy, we can enter power-down mode because next receive |
// should be a TWI address match and it wakes the device up from all sleep modes. |
if( ! TWI_statusReg.RxDataInBuf ) { |
if(TWI_Transceiver_Busy()) { |
MCUCR = (1<<SE)|(0<<SM2)|(0<<SM1)|(0<<SM0); // Enable sleep with idle mode |
} else { |
MCUCR = (1<<SE)|(0<<SM2)|(1<<SM1)|(0<<SM0); // Enable sleep with power-down mode |
} |
SLEEP(); |
} else { |
NOP(); // There is data in the buffer, code below takes care of it. |
} |
#else // No power management |
// Here you can add your own code that should be run while waiting for the TWI to finish |
NOP(); // Put own code here. |
#endif |
// Check if the TWI Transceiver has completed an operation. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
// Check if the last operation was successful |
if ( TWI_statusReg.lastTransOK ) |
{ |
// Check if the last operation was a reception |
if ( TWI_statusReg.RxDataInBuf ) |
{ |
TWI_Get_Data_From_Transceiver(messageBuf, 2); |
// Check if the last operation was a reception as General Call |
if ( TWI_statusReg.genAddressCall ) |
{ |
// Put data received out to PORTB as an example. |
PORTB = messageBuf[0]; |
} |
else // Ends up here if the last operation was a reception as Slave Address Match |
{ |
// Example of how to interpret a command and respond. |
// TWI_CMD_MASTER_WRITE stores the data to PORTB |
if (messageBuf[0] == TWI_CMD_MASTER_WRITE) |
{ |
PORTB = messageBuf[1]; |
} |
// TWI_CMD_MASTER_READ prepares the data from PINB in the transceiver buffer for the TWI master to fetch. |
if (messageBuf[0] == TWI_CMD_MASTER_READ) |
{ |
messageBuf[0] = PINB; |
TWI_Start_Transceiver_With_Data( messageBuf, 1 ); |
} |
} |
} |
else // Ends up here if the last operation was a transmission |
{ |
NOP(); // Put own code here. |
} |
// Check if the TWI Transceiver has already been started. |
// If not then restart it to prepare it for new receptions. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
TWI_Start_Transceiver(); |
} |
} |
else // Ends up here if the last operation completed unsuccessfully |
{ |
TWI_Act_On_Failure_In_Last_Transmission( TWI_Get_State_Info() ); |
} |
} |
} |
}*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/twi_slave.h |
---|
0,0 → 1,121 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.h |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : Header file for TWI_slave.c |
* Include this file in the application. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
/**************************************************************************** |
TWI Status/Control register definitions |
****************************************************************************/ |
#define TWI_BUFFER_SIZE 4 // Reserves memory for the drivers transceiver buffer. |
// Set this to the largest message size that will be sent including address byte. |
/**************************************************************************** |
Global definitions |
****************************************************************************/ |
union TWI_statusReg_t // Status byte holding flags. |
{ |
unsigned char all; |
struct |
{ |
unsigned char lastTransOK:1; |
unsigned char RxDataInBuf:1; |
unsigned char genAddressCall:1; // TRUE = General call, FALSE = TWI Address; |
unsigned char unusedBits:5; |
}; |
}; |
extern union TWI_statusReg_t TWI_statusReg; |
/**************************************************************************** |
Function definitions |
****************************************************************************/ |
void twi_slave_init( unsigned char ); |
unsigned char twi_slave_busy( void ); |
unsigned char twi_slave_get_status( void ); |
void twi_slave_start_with_data( unsigned char * , unsigned char ); |
void twi_slave_start( void ); |
unsigned char twi_slave_get_data( unsigned char *, unsigned char ); |
/**************************************************************************** |
Bit and byte definitions |
****************************************************************************/ |
#define TWI_READ_BIT 0 // Bit position for R/W bit in "address byte". |
#define TWI_ADR_BITS 1 // Bit position for LSB of the slave address bits in the init byte. |
#define TWI_GEN_BIT 0 // Bit position for LSB of the general call bit in the init byte. |
#define TRUE 1 |
#define FALSE 0 |
/**************************************************************************** |
TWI State codes |
****************************************************************************/ |
// General TWI Master staus codes |
#define TWI_START 0x08 // START has been transmitted |
#define TWI_REP_START 0x10 // Repeated START has been transmitted |
#define TWI_ARB_LOST 0x38 // Arbitration lost |
// TWI Master Transmitter staus codes |
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received |
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received |
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received |
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received |
// TWI Master Receiver staus codes |
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received |
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received |
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted |
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted |
// TWI Slave Transmitter staus codes |
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned |
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received |
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received |
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// TWI Slave Receiver staus codes |
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned |
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned |
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned |
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave |
// TWI Miscellaneous status codes |
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = �0� |
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/twimaster.c |
---|
0,0 → 1,202 |
/************************************************************************* |
* Title: I2C master library using hardware TWI interface |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: twimaster.c,v 1.3 2005/07/02 11:14:21 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device with hardware TWI |
* Usage: API compatible with I2C Software Library i2cmaster.h |
**************************************************************************/ |
#include <inttypes.h> |
#include <compat/twi.h> |
#include <i2cmaster.h> |
/* define CPU frequency in Mhz here if not defined in Makefile */ |
#ifndef F_CPU |
#define F_CPU 16000000UL |
#endif |
/* I2C clock in Hz */ |
#define SCL_CLOCK 400000L |
/************************************************************************* |
Initialization of the I2C bus interface. Need to be called only once |
*************************************************************************/ |
void i2c_init(void) |
{ |
/* initialize TWI clock: 100 kHz clock, TWPS = 0 => prescaler = 1 */ |
TWSR = 0; /* no prescaler */ |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ |
}/* i2c_init */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
return 0 = device accessible, 1= failed to access device |
*************************************************************************/ |
unsigned char i2c_start(unsigned char address) |
{ |
uint8_t twst; |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) return 1; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed and ACK/NACK has been received |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1; |
return 0; |
}/* i2c_start */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
If device is busy, use ack polling to wait until device is ready |
Input: address and transfer direction of I2C device |
*************************************************************************/ |
void i2c_start_wait(unsigned char address) |
{ |
uint8_t twst; |
while ( 1 ) |
{ |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) continue; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) |
{ |
/* device busy, send stop condition to terminate write operation */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
continue; |
} |
//if( twst != TW_MT_SLA_ACK) return 1; |
break; |
} |
}/* i2c_start_wait */ |
/************************************************************************* |
Issues a repeated start condition and sends address and transfer direction |
Input: address and transfer direction of I2C device |
Return: 0 device accessible |
1 failed to access device |
*************************************************************************/ |
unsigned char i2c_rep_start(unsigned char address) |
{ |
return i2c_start( address ); |
}/* i2c_rep_start */ |
/************************************************************************* |
Terminates the data transfer and releases the I2C bus |
*************************************************************************/ |
void i2c_stop(void) |
{ |
/* send stop condition */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
}/* i2c_stop */ |
/************************************************************************* |
Send one byte to I2C device |
Input: byte to be transfered |
Return: 0 write successful |
1 write failed |
*************************************************************************/ |
unsigned char i2c_write( unsigned char data ) |
{ |
uint8_t twst; |
// send data to the previously addressed device |
TWDR = data; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits |
twst = TW_STATUS & 0xF8; |
if( twst != TW_MT_DATA_ACK) return 1; |
return 0; |
}/* i2c_write */ |
/************************************************************************* |
Read one byte from the I2C device, request more data from device |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readAck(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readAck */ |
/************************************************************************* |
Read one byte from the I2C device, read is followed by a stop condition |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readNak(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readNak */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/xutils.c |
---|
0,0 → 1,765 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ xutils.c - erweiterte String-Funktionen, xprintf (Info nach History) |
//+ und weitere Hilfsfunktionen wie z.B. UTCdatetime2local() |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY xutils.c |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P(), _strncpyat() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# - add: buffer sprintf_buffer[] |
//# - chg: PGMBUFF_SIZE und ARGBUFF_SIZE von 80 auf 40 geaendert |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() entfernt Leerzeichen auf der rechten Seite |
//# |
//# 14.05.2013 OG |
//# - chg: Kommentarkopf von UTCdatetime2local() aktualisiert |
//# |
//# 05.05.2013 OG |
//# - chg: UTCdatetime2local() auf Config.timezone/summertime umgestellt |
//# - add: include eeprom.h |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.c |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# - fix: _xvsnprintf() Darstellung kleiner negativer Nachkommazahlen (-1<z<0) |
//# |
//# 29.04.2013 OG |
//# - chg: Doku zu xprintf Ergaenzt bei 'bekannte Einschraenkungen' |
//# - chg: includes reduziert auf das Notwendige |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
//############################################################################ |
//# xprintf |
//# |
//# Diese Variante von printf ist angepasst auf das PKT: |
//# |
//# - Unterstuetzung von Festkomma-Integer Anzeige |
//# - Overflow-Anzeige durch '*' wenn eine Zahl die Format-Maske sprengt |
//# - progmen wird optional unterstuetz fuer den format-String als auch fuer |
//# String Argumente |
//# - strikte Einhaltung von Laengen einer Format-Maske |
//# (ggf. wird die Ausgabe gekuerzt) |
//# |
//# In diesem Source sind nur die Basis-xprintf zum Erzeugen von formatierten |
//# Strings. Die direkten Ausgabefunktionen auf den Screen sind in lcd.c |
//# als lcdx_printf_at() und lcdx_printf_at_P(). |
//# |
//# FORMAT ANGABEN: |
//# |
//# %d: dezimal int signed (Rechtsbuendige Ausgabe) (Wandlung via itoa) |
//# "%d" arg: 1234 -> "1234" |
//# "%5d" arg: 1234 -> " 1234" |
//# "%5.2d" arg: 1234 -> " 12.34" |
//# "%05d" arg: 123 -> "00123" |
//# "%3.2d" arg: -13 -> " -0.13" |
//# |
//# %u: dezimal int unsigned (Rechtsbuendige Ausgabe) (Wandlung via utoa) |
//# wie %d jedoch mittels utoa |
//# |
//# %h: hex int unsigned -> Hex-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%4h" |
//# %o: octal int unsigned -> Octal-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%2o" |
//# %b: binary int unsigned -> Binaer-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%8b" |
//# |
//# %ld, %lu, %lh, %lo, %lb: |
//# wie die obigen jedoch fuer long-zahlen (via ltoa, ultoa) |
//# nur wenn define USE_XPRINTF_LONG gesetzt ist! |
//# Bespiele: "%ld", "%5.6ld" usw. |
//# |
//# %s: String aus RAM -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %S: String aus progmem -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# |
//# %c: einzelnes char -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %%: Ausgabe von "%" |
//# |
//# |
//# BEISPIELE: |
//# |
//# vorhanden in: osd/osdata.c, lcd/lcd.c |
//# |
//# |
//# BEKANNTE EINSCHRAENKUNGEN: |
//# |
//# 1. padding '0' mit negativen Zahlen wenn padding aktiv ist ergibt z.B. "00-1.5" |
//# 2. der Entwickler muss auf eine korrekte Format-Maske achten. Andernfalls |
//# kommt es zu nicht vorhersehbaren Ausgaben. |
//# 3. nicht in ISR's verwenden - dazu muss ggf. eine Anpassung durchgefuert werden |
//# |
//# KOMPILIER OPTIONEN |
//# |
//# define: USE_XPRINTF_LONG |
//# Unterstuetzung von long int / unsigned long int ('l' Modifier in Maske) |
// Wird in der main.h gesetzt |
//############################################################################ |
#define USE_XPRINTF_LONG // Unterstuetzung von long integer - Achtung! Muss fuer PKT gesetzt sein |
// da in Sourcen verwendet! |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#define PGMBUFF_SIZE 40 // max. 40 chars bei format-String aus progmem |
#define ARGBUFF_SIZE 40 // buffer fuer xprintf Parameter (strings, itoa, utoa) (fuer ltoa ggf. anpassen) |
#define SPRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer xsnprintf(), xsnprintf_P() |
char sprintf_buffer[SPRINTF_BUFFER_SIZE]; |
//---------------------- |
// xprintf parser data |
//---------------------- |
typedef struct |
{ |
uint8_t parse; // true / false |
char cmd; // char: d u h o b s S c % |
uint8_t prelen; // Vorkomma |
uint8_t declen; // decimal (Nachkomma) |
uint8_t point; // true / false |
uint8_t uselong; // true / false |
uint8_t mask; // true / false |
char pad; // ' ' oder '0' |
} xprintf_t; |
//---------------------- |
// Buffers |
//---------------------- |
char cmddigit[7]; |
char argbuff[ARGBUFF_SIZE]; |
char pgmbuff[PGMBUFF_SIZE]; |
//#################################################################################### |
//--------------------------------------------------------------------- |
// Basisfunktion von xprintf |
// Doku: siehe oben |
//--------------------------------------------------------------------- |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ) |
{ |
const char *p_fmt; // pointer auf den format-String |
char *p_dst; // pointer auf den destination buffer |
const char *p_str; // pointer auf einen arg-String wenn ein String ausgegeben werden soll |
char *p_cmddigit = 0; // pointer auf den Digit-Buffer fuer eine Maske (Laenge/Nachkommastellen) |
const char *p_fmtbuff; // pointer auf den format-Buffer (fuer progmem) |
const char *p_argbuff; // pointer auf den argbuffer |
uint8_t fmtlen, arglen, overflow; |
uint8_t i,j,dec; |
uint8_t dstcnt; |
uint8_t minus; |
xprintf_t cmd; // parser Daten |
p_fmtbuff = format; |
if( useprogmem ) // format von progmem in's RAM kopieren |
{ |
strncpy_P( pgmbuff, format, PGMBUFF_SIZE); |
pgmbuff[PGMBUFF_SIZE-1] = 0; |
p_fmtbuff = pgmbuff; |
} |
cmd.parse = false; |
p_fmt = p_fmtbuff-1; // -1 -> wird am Anfang der Schleife korrigiert |
p_dst = buffer; |
dstcnt = 0; |
do |
{ |
if( dstcnt >= n ) // max. Anzahl von Zeichen fuer Ziel 'buffer' erreicht? |
break; |
p_fmt++; // naechstes Zeichen von format |
//######################## |
//# 1. PARSE |
//######################## |
//------------------------ |
// START: parse cmd |
//------------------------ |
if( cmd.parse == false && *p_fmt == '%' ) |
{ |
memset( &cmd, 0, sizeof(xprintf_t) ); // init |
cmd.parse = true; |
cmd.pad = ' '; |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// NO parse: copy char |
//------------------------ |
if( cmd.parse == false ) |
{ |
*p_dst = *p_fmt; |
p_dst++; |
dstcnt++; |
continue; |
} |
//------------------------ |
// set: pad (eine '0' ganz am Anfang der Formatmaske) |
//------------------------ |
if( cmd.parse == true && *p_fmt == '0' && p_cmddigit == cmddigit ) |
{ |
cmd.pad = '0'; |
continue; |
} |
//------------------------ |
// set: vor/nach-kommastellen |
//------------------------ |
if( cmd.parse == true && *p_fmt >= '0' && *p_fmt <= '9' ) |
{ |
*p_cmddigit = *p_fmt; |
p_cmddigit++; |
continue; |
} |
//------------------------ |
// set: point |
//------------------------ |
if( cmd.parse == true && *p_fmt == '.' && cmd.point == false ) |
{ |
cmd.point = true; |
*p_cmddigit = 0; |
cmd.prelen = atoi( cmddigit ); |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// set: uselong |
//------------------------ |
#ifdef USE_XPRINTF_LONG |
if( cmd.parse == true && *p_fmt == 'l' ) |
{ |
cmd.uselong = true; |
continue; |
} |
#endif |
//------------------------ |
// END: parse cmd |
//------------------------ |
if( cmd.parse == true && (*p_fmt == 'd' || *p_fmt == 'u' || *p_fmt == 'x' || *p_fmt == 'b' || *p_fmt == 'o' || |
*p_fmt == 's' || *p_fmt == 'S' || *p_fmt == 'c' || *p_fmt == '%') ) |
{ |
cmd.cmd = *p_fmt; |
cmd.parse = false; |
*p_cmddigit = 0; |
if( cmd.point == false ) cmd.prelen = atoi(cmddigit); |
else cmd.declen = atoi(cmddigit); |
if( cmd.point || cmd.prelen>0 ) |
cmd.mask = true; |
} |
//######################## |
//# 2. EXECUTE |
//######################## |
//------------------------ |
// exec cmd: "d,u,x,b,o" |
//------------------------ |
if( cmd.cmd == 'd' || cmd.cmd == 'u' || cmd.cmd == 'x' || cmd.cmd == 'b' || cmd.cmd == 'o' ) |
{ |
if( cmd.uselong ) |
{ |
#ifdef USE_XPRINTF_LONG |
switch(cmd.cmd) |
{ |
case 'd': ltoa ( va_arg(ap, long) , argbuff, 10); break; // LONG dezimal int signed |
case 'u': ultoa( va_arg(ap, unsigned long), argbuff, 10); break; // LONG dezimal int unsigned |
case 'x': ultoa( va_arg(ap, unsigned long), argbuff, 16); break; // LONG hex int unsigned |
case 'b': ultoa( va_arg(ap, unsigned long), argbuff, 2); break; // LONG binary int unsigned |
case 'o': ultoa( va_arg(ap, unsigned long), argbuff, 8); break; // LONG octal int unsigned |
} |
#endif |
} |
else |
{ |
switch(cmd.cmd) |
{ |
case 'd': itoa( va_arg(ap, int) , argbuff, 10); break; // dezimal int signed |
case 'u': utoa( va_arg(ap, unsigned int), argbuff, 10); break; // dezimal int unsigned |
case 'x': utoa( va_arg(ap, unsigned int), argbuff, 16); break; // hex int unsigned |
case 'b': utoa( va_arg(ap, unsigned int), argbuff, 2); break; // binary int unsigned |
case 'o': utoa( va_arg(ap, unsigned int), argbuff, 8); break; // octal int unsigned |
} |
} |
minus = (argbuff[0] == '-'); |
arglen = strlen(argbuff); |
fmtlen = cmd.prelen + cmd.declen + (cmd.point ? 1 : 0); |
arglen = strlen(argbuff); |
overflow = cmd.mask && // Zahl zu gross -> "*" anzeigen statt der Zahl |
(arglen > cmd.prelen + cmd.declen || cmd.prelen < 1+minus); |
if( overflow ) // overflow: Zahl passt nicht in Maske |
{ // -> zeige '*.*' |
for( i=0; (i < fmtlen) && (dstcnt < n); i++) |
{ |
if( cmd.point && i==cmd.prelen ) *p_dst = '.'; |
else *p_dst = '*'; |
p_dst++; |
dstcnt++; |
} |
} |
else // else: if( overflow ) |
{ |
if( !cmd.mask ) // keine Maske: alles von der Zahl ausgeben |
fmtlen = arglen; |
p_argbuff = argbuff; |
if( minus ) // wenn Zahl negativ: merken und auf dem argbuff nehmen |
{ |
p_argbuff++; |
arglen--; |
} |
//----------------- |
// die Zahl wird 'Rueckwaerts' uebertragen |
//----------------- |
dec = -1; // wird am Anfang der Schleife auf 0 gesetzt |
j = 1; // zaehler fuer argbuff |
for( i=1; i<=fmtlen; i++ ) |
{ |
dec++; // Zaehler Dizmalstellen |
if( dstcnt+fmtlen-i <= n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( cmd.point && (dec == cmd.declen) ) // Dezimalpunkt setzen |
{ |
p_dst[fmtlen-i] = '.'; |
continue; |
} |
if( j <= arglen ) // Ziffer uebertragen aus argbuff |
{ |
p_dst[fmtlen-i] = p_argbuff[arglen-j]; |
j++; |
continue; |
} |
if( cmd.declen > 0 && // Nachkomma und 1. Vorkommastelle ggf. auf '0' |
(dec < cmd.declen || dec == cmd.declen+1) ) // setzen wenn die Zahl zu klein ist |
{ |
p_dst[fmtlen-i] = '0'; |
continue; |
} |
if( minus && ( (cmd.pad == ' ') || // ggf. Minuszeichen setzen |
(cmd.pad != ' ' && i == fmtlen) ) ) // Minuszeichen bei '0'-padding an erster Stelle setzen |
{ |
minus = false; |
p_dst[fmtlen-i] = '-'; |
continue; |
} |
p_dst[fmtlen-i] = cmd.pad; // padding ' ' oder '0' |
} // end: if( dstcnt+fmtlen-i <= n ) |
} |
p_dst += fmtlen; |
dstcnt += fmtlen; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "s", "S", "c" |
//------------------------ |
if( cmd.cmd == 's' || cmd.cmd == 'S' || cmd.cmd == 'c' ) |
{ |
switch(cmd.cmd) |
{ |
case 's': p_str = va_arg( ap, char *); // String aus dem RAM |
break; |
case 'S': strncpy_P( argbuff, va_arg( ap, char *), ARGBUFF_SIZE); // String liegt im progmem -> in's RAM kopieren |
argbuff[ARGBUFF_SIZE-1] = 0; |
p_str = argbuff; |
break; |
case 'c': argbuff[0] = va_arg( ap, int); // einzelnes char |
argbuff[1] = 0; |
p_str = argbuff; |
break; |
} |
fmtlen = cmd.prelen; |
arglen = strlen(p_str); |
if( !cmd.mask ) // keine Maske: alles vom String ausgeben |
fmtlen = arglen; |
for( i=0; i<fmtlen; i++) |
{ |
if( dstcnt < n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( *p_str ) // char uebertragen |
{ |
*p_dst = *p_str; |
p_str++; |
} |
else // padding |
{ |
*p_dst = ' '; |
} |
p_dst++; |
} |
dstcnt++; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "%" |
//------------------------ |
if( cmd.cmd == '%' ) |
{ |
*p_dst = '%'; |
p_dst++; |
dstcnt++; |
continue; |
} |
} while( (dstcnt < n) && *p_fmt ); |
*(p_dst + (dstcnt < n ? 0 : -1)) = 0; // terminierende 0 im Ausgabebuffer setzen |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( false, buffer, n, format, ap); |
va_end(ap); |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( true, buffer, n, format, ap); |
va_end(ap); |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf_P( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//-------------------------------------------------------------- |
// kopiert einen String von src auf dst mit fester Laenge und |
// ggf. Space paddings rechts |
// |
// - fuellt ggf. den dst-String auf size Laenge mit Spaces |
// - setzt Terminierung's 0 bei dst auf Position size |
//-------------------------------------------------------------- |
void strncpyfill( char *dst, const char *src, size_t size) |
{ |
uint8_t i; |
uint8_t pad = false; |
for( i=0; i<size; i++) |
{ |
if(*src == 0) pad = true; |
if( pad ) *dst = ' '; |
else *dst = *src; |
src++; |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// entfernt rechte Leerzeichen aus einem String |
//-------------------------------------------------------------- |
void strrtrim( char *dst) |
{ |
char *p; |
p = dst + strlen(dst) - 1; |
while( (p != dst) && (*p == ' ') ) p--; |
if( (*p != ' ') && (*p != 0) ) p++; |
*p = 0; |
} |
//-------------------------------------------------------------- |
// INTERN - fuer strncpyat(), strncpyat_P() |
//-------------------------------------------------------------- |
void _strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount, uint8_t progmem) |
{ |
uint8_t i; |
if( progmem ) |
strncpy_P( dst, src, size); |
else |
strncpy( dst, src, size); |
for( i=0; i<size; i++) |
{ |
if( *dst == 0) return; |
if( *dst == sepchar) |
{ |
sepcharcount--; |
if( sepcharcount==0 ) |
{ |
*dst = 0; |
return; |
} |
} |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// strncpyat( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in PROGMEM |
//-------------------------------------------------------------- |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, false); |
} |
//-------------------------------------------------------------- |
// strncpyat_P( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in RAM |
//-------------------------------------------------------------- |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, true); |
} |
//-------------------------------------------------------------- |
// UTCdatetime2local( PKTdatetime_t *dtbuffer, PKTdatetime_t dt ) |
// |
// konvertiert die UTC-Time 'dt' in die lokale Zeit und speichert |
// dieses in 'dtbuffer' ab. |
// |
// Parameter: |
// |
// dtdst: Pointer Destination (PKTdatetime_t) (Speicher muss alloziiert sein!) |
// dtsrc: Pointer Source (PKTdatetime_t) |
// |
// Hinweise: |
// |
// Schaltjahre (bzw. der 29.02.) werden nicht unterstuetzt |
//-------------------------------------------------------------- |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ) |
{ |
int8_t timeoffset; |
int32_t v; |
int8_t diff; |
// 01 02 03 04 05 06 07 08 09 10 11 12 Monat |
int8_t daymonth[] = {31,28,31,30,31,30,31,31,30,31,30,31}; |
//-------------------------- |
// timezone: Einstellbereich -12 .. 0 .. 12 (+1 = Berlin) |
// summertime: Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) |
//-------------------------- |
timeoffset = Config.timezone + Config.summertime; |
//timeoffset = 2; // solange noch nicht in PKT-Config: Berlin, Sommerzeit |
memcpy( dtdst, dtsrc, sizeof(PKTdatetime_t) ); // copy datetime to destination |
//-------------------------- |
// Zeitzonenanpassung |
//-------------------------- |
if( dtdst->year != 0 && dtdst->month >= 1 && dtdst->month <= 12 ) // nur wenn gueltiges Datum vorhanden |
{ |
//-------------------------- |
// 1. Sekunden |
//-------------------------- |
v = (int32_t)dtdst->seconds; |
v += timeoffset*3600; // Stunden korrigieren |
diff = 0; |
if( v > 86400 ) // Tagesueberschreitung? (86400 = 24*60*60 bzw. 24 Stunden) |
{ |
v -= 86400; |
diff++; // inc: Tag |
} |
else if( v < 0 ) // Tagesunterschreitung? |
{ |
v += 86400; |
diff--; // dec: Tag |
} |
dtdst->seconds = (uint32_t)v; // SET: seconds |
//-------------------------- |
// 2. Tag |
//-------------------------- |
v = (int32_t)dtdst->day; |
v += diff; |
diff = 0; |
if( v > daymonth[dtdst->month-1] ) // Monatsueberschreitung? |
{ |
v = 1; // erster Tag des Monats |
diff++; // inc: Monat |
} |
else if( v < 1 ) // Monatsunterschreitung? |
{ |
if( dtdst->month > 1 ) |
v = daymonth[dtdst->month-1-1]; // letzter Tag des vorherigen Monats |
else |
v = 31; // letzter Tag im Dezember des vorherigen Jahres |
diff--; // dec: Monat |
} |
dtdst->day = (uint8_t)v; // SET: day |
//-------------------------- |
// 3. Monat |
//-------------------------- |
v = (int32_t)dtdst->month; |
v += diff; |
diff = 0; |
if( v > 12 ) // Jahresueberschreitung? |
{ |
v = 1; |
diff++; // inc: Jahr |
} |
else if( v < 1 ) // Jahresunterschreitung? |
{ |
v = 12; |
diff--; // dec: Jahr |
} |
dtdst->month = (uint8_t)v; // SET: month |
//-------------------------- |
// 4. Jahr |
//-------------------------- |
dtdst->year += diff; // SET: year |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils/xutils.h |
---|
0,0 → 1,64 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY xutils.h |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.h |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
#ifndef _XUTILS_H |
#define _XUTILS_H |
#include <stdarg.h> |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ); |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ); |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ); |
char *buffered_sprintf( const char *format, ... ); |
char *buffered_sprintf_P( const char *format, ... ); |
void strncpyfill( char *dst, const char *src, size_t size); |
void strrtrim( char *dst); |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/utils |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+maniacbug-mighty-1284p-68ed99c |
+ |
+Arduino |
+ |
+_doc |
+ |
+maniacbug-mighty-1284p-68ed99c.zip |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/waypoints/waypoints.c |
---|
0,0 → 1,362 |
/*#######################################################################################*/ |
/* !!! THIS IS NOT FREE SOFTWARE !!! */ |
/*#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 2008 Ingo Busker, Holger Buss |
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY |
// + FOR NON COMMERCIAL USE ONLY |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdint.h> |
#include <string.h> |
#include <stdbool.h> |
//#include "91x_lib.h" |
#include "waypoints.h" |
#include "../mk-data-structs.h" |
#include "../uart/uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../main.h" |
#include <util/delay.h> |
#ifdef USE_WAYPOINTS |
// the waypoints list |
NaviData_t *NaviData; |
//Point_t PointList[MAX_LIST_LEN]; |
uint8_t WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount |
uint8_t POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount |
uint8_t WPCount = 0; // number of waypoints |
uint8_t PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN |
uint8_t POICount = 0; |
uint8_t WPActive = false; |
uint8_t PointList_Init(void) |
{ |
return PointList_Clear(); |
} |
uint8_t PointList_Clear(void) |
{ |
uint8_t i; |
WPIndex = 0; // real list position are 1 ,2, 3 ... |
POIIndex = 0; // real list position are 1 ,2, 3 ... |
WPCount = 0; // no waypoints |
POICount = 0; |
PointCount = 0; // no contents |
WPActive = false; |
NaviData->WaypointNumber = WPCount; |
NaviData->WaypointIndex = 0; |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
Config.PointList[i].Position.Status = INVALID; |
Config.PointList[i].Position.Latitude = 0; |
Config.PointList[i].Position.Longitude = 0; |
Config.PointList[i].Position.Altitude = 0; |
Config.PointList[i].Heading = 361; // invalid value |
Config.PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered |
Config.PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
Config.PointList[i].Type = POINT_TYPE_INVALID; |
Config.PointList[i].Event_Flag = 0; // future implementation |
Config.PointList[i].AltitudeRate = 0; // no change of setpoint |
} |
return true; |
} |
uint8_t PointList_GetCount(void) |
{ |
return PointCount; // number of points in the list |
} |
Point_t* PointList_GetAt(uint8_t index) |
{ |
if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1])); // return pointer to this waypoint |
else return(NULL); |
} |
uint8_t PointList_SetAt(Point_t* pPoint) |
{ |
// if index is in range |
if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN)) |
{ |
// check list entry before update |
switch(Config.PointList[pPoint->Index-1].Type) |
{ |
case POINT_TYPE_INVALID: // was invalid |
switch(pPoint->Type) |
{ |
default: |
case POINT_TYPE_INVALID: |
// nothing to do |
break; |
case POINT_TYPE_WP: |
WPCount++; |
PointCount++; |
break; |
case POINT_TYPE_POI: |
POICount++; |
PointCount++; |
break; |
} |
break; |
case POINT_TYPE_WP: // was a waypoint |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
WPCount--; |
PointCount--; |
break; |
default: |
case POINT_TYPE_WP: |
//nothing to do |
break; |
case POINT_TYPE_POI: |
POICount++; |
WPCount--; |
break; |
} |
break; |
case POINT_TYPE_POI: // was a poi |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
POICount--; |
PointCount--; |
break; |
case POINT_TYPE_WP: |
WPCount++; |
POICount--; |
break; |
case POINT_TYPE_POI: |
default: |
// nothing to do |
break; |
} |
break; |
} |
memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry |
NaviData->WaypointNumber = WPCount; |
return pPoint->Index; |
} |
else return(0); |
} |
// returns the pointer to the first waypoint within the list |
Point_t* PointList_WPBegin(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
if(WPActive == false) return(NULL); |
POIIndex = 0; // set invalid POI |
if(PointCount > 0) |
{ |
// search for first wp in list |
for(i = 0; i <MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
WPIndex = i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP in the list |
{ |
NaviData->WaypointIndex = 1; |
// update index to POI |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // some points in the list but no WP found |
{ |
NaviData->WaypointIndex = 0; |
//Check for an existing POI |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
POIIndex = i + 1; |
break; |
} |
} |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns the last waypoint |
Point_t* PointList_WPEnd(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
POIIndex = 0; // set invalid |
if(WPActive == false) return(NULL); |
if(PointCount > 0) |
{ |
// search backward! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
WPIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP within the list |
{ |
NaviData->WaypointIndex = WPCount; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // list contains some points but no WP in the list |
{ |
// search backward for a POI! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
POIIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
NaviData->WaypointIndex = 0; |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns a pointer to the next waypoint or NULL if the end of the list has been reached |
Point_t* PointList_WPNext(void) |
{ |
uint8_t wp_found = 0; |
if(WPActive == false) return(NULL); |
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list |
{ |
uint8_t i; |
for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs |
{ |
wp_found = i+1; |
break; |
} |
} |
} |
if(wp_found) |
{ |
WPIndex = wp_found; // update list position |
NaviData->WaypointIndex++; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint |
} |
else |
{ // no next wp found |
NaviData->WaypointIndex = 0; |
POIIndex = 0; |
return(NULL); |
} |
} |
void PointList_WPActive(uint8_t set) |
{ |
if(set) |
{ |
WPActive = true; |
PointList_WPBegin(); // uopdates POI index |
} |
else |
{ |
WPActive = false; |
POIIndex = 0; // disable POI also |
} |
} |
Point_t* PointList_GetPOI(void) |
{ |
return PointList_GetAt(POIIndex); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/waypoints/waypoints.h |
---|
0,0 → 1,56 |
#ifndef _WAYPOINTS_H |
#define _WAYPOINTS_H |
//#include "ubx.h" |
#include "../mk-data-structs.h" |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
//typedef struct |
//{ |
// int32_t Longitude; // in 1E-7 deg |
// int32_t Latitude; // in 1E-7 deg |
// int32_t Altitude; // in mm |
// uint8_t Status;// validity of data |
//} __attribute__((packed)) GPS_Pos_t; |
//typedef struct |
//{ |
// GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
// int16_t Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
// uint8_t ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
// uint8_t HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// uint8_t Event_Flag; // future implementation |
// uint8_t Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
// uint8_t Type; // typeof Waypoint |
// uint8_t WP_EventChannelValue; // |
// uint8_t AltitudeRate; // rate to change the setpoint |
// uint8_t reserve[8]; // reserve |
//} __attribute__((packed)) Point_t; |
// Init List, return TRUE on success |
uint8_t PointList_Init(void); |
// Clear List, return TRUE on success |
uint8_t PointList_Clear(void); |
// Returns number of points in the list |
uint8_t PointList_GetCount(void); |
// return pointer to point at position |
Point_t* PointList_GetAt(uint8_t index); |
// set a point in the list at index, returns its index on success, else 0 |
uint8_t PointList_SetAt(Point_t* pPoint); |
// goto the first WP in the list and return pointer to it |
Point_t* PointList_WPBegin(void); |
// goto the last WP in the list and return pointer to it |
Point_t* PointList_WPEnd(void); |
// goto next WP in the list and return pointer to it |
Point_t* PointList_WPNext(void); |
// enables/disables waypoint function |
void PointList_WPActive(uint8_t set); |
// returns pointer to actual POI |
Point_t* PointList_GetPOI(void); |
#endif // _WAYPOINTS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wi232/Wi232.c |
---|
0,0 → 1,451 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.c |
//# |
//# 15.06.2014 OG |
//# -chg: umgestellt auf PKT_Progress_Init() / PKT_Progress_Next() |
//# |
//# 10.06.2014 OG |
//# KOMPLETTE SANIERUNG ab v3.80cX5 |
//# (ehemaliger Code in v3.80cX4 oder frueher) |
//# - add: Wi232_ConfigPC() - ehemals in connect.c als Port_USB2CFG_Wi() |
//# |
//# 06.06.2014 OG |
//# - add: Wi232_Write_Progress() |
//# - chg: Codeformattierung |
//# |
//# 05.06.2014 OG |
//# -chg: Codeformattierung |
//# |
//# 14.05.2014 OG |
//# - chg: InitErrorWi232(), InitWi232() - umgestellt auf dezimale Anzeige des |
//# Fehlercodes damit der einfacher im Source zu identifizieren ist |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../main.h" |
#include "../wi232/Wi232.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
//############################################################################ |
static const char STR_WI232INIT[] PROGMEM = "Wi.232 Init"; |
//############################################################################ |
//############################################################################ |
//-------------------------------------------------------------- |
// lError = WriteWi232( Wi232Register, RegisterValue) |
// |
// set Register to Wi232, Register, Value |
// |
// Returns: |
// 0 = ACK, FF = NAK |
//-------------------------------------------------------------- |
uint8_t Wi232_Write( uint8_t Wi232Register, uint8_t RegisterValue ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
unsigned int v; |
if( RegisterValue < 0x80 ) |
{ |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(Wi232Register); |
USART_putc(RegisterValue); |
} |
else // RegisterValue >= 0x80 |
{ |
USART_putc(0xff); |
USART_putc(0x03); |
USART_putc(Wi232Register); |
USART_putc(0xfe); |
USART_putc(RegisterValue - 0x80); |
} |
do |
{ |
v = USART_getc(); // ACK erwartet |
_delay_ms(100); |
tc++; |
} while( (v == 0) && (tc != timeout) ); |
if( v != 0x06 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
//PKT_Message_P( PSTR("Wi.232 NAK"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen (wird nicht mehr angezeigt da uebernommen von hoeheren Funktionen) |
return 0xFF; // return: ERROR |
} |
return 0; // return: OK (v==0x06) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Wi232_Write_Progress( uint8_t Wi232Register, uint8_t RegisterValue, uint8_t errorcode ) |
{ |
uint8_t nError; |
// nError = 0: ok |
// nError > 0: Fehler |
nError = Wi232_Write( Wi232Register, RegisterValue ); |
PKT_Progress_Next(); // Progressbar |
if( nError ) |
{ |
nError = errorcode; |
} |
return nError; |
} |
//-------------------------------------------------------------- |
// Registervalue = Wi232_Read( Wi232Register ) |
// |
// send Readcommand to Wi232, |
// |
// Returns: |
// Registervalue: 0 = timeout 0xFF = Syntaxerror |
//-------------------------------------------------------------- |
uint8_t Wi232_Read( uint8_t Wi232Register ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
uint8_t v; |
v = USART_getc(); // Zeichen loeschen |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(0xfe); |
USART_putc(Wi232Register); |
_delay_ms(50); |
//lcd_printpns_at (0, 2, PSTR("read Wi232"),0); |
do |
{ |
v = USART_getc(); //ACK erwartet |
_delay_ms(100); |
tc++; |
} while( v==0 && tc!=timeout ); |
if( tc == timeout ) |
return 0; // Timeout |
if( v != 0x06 ) |
return 0xFF; // Syntaxerror |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Register*/ |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Value*/ |
// lcd_print_hex(v,0); |
return v; |
} |
//-------------------------------------------------------------- |
// foundbaud = Wi232_CheckBaud( Baudrate ) |
// |
// RUECKGABE: |
// =0: Error (Wi.232 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint16_t Wi232_CheckBaud( uint8_t Baudrate ) // Suche Wi232 mit entsprechender Baudrate |
{ |
uint8_t RegisterWi232; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
SetBaudUart0( Baudrate ); // UART auf angegebene Baudrate setzen |
RegisterWi232 = Wi232_Read( regDiscover ); |
//--------------------------------------------------- |
// Hinweis zu RegisterWi232 nach regDiscover |
// == 0 : "no Wi.232 found" |
// == 0xFF: "Wi.232 Syntaxerror" |
// |
// >0 && < 0xff: Wi.232 Version (Wi.232 gefunden) |
//--------------------------------------------------- |
if( RegisterWi232 > 0 && RegisterWi232 < 0xFF ) |
{ |
return Baudrate; // Wi.232 gefunden |
} |
return 0; // Wi.232 nicht gefunden bei gegebener Baudrate |
} |
//-------------------------------------------------------------- |
// InitWI232( InitBaudrate ) |
// |
// set Wi232Register for Mikrokopter |
// |
// Returns: |
// 0 = ACK, FF = NAK ????????? |
//-------------------------------------------------------------- |
void Wi232_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
uint8_t InitBaudrate = Config.PKT_Baudrate; |
//---------------------------- |
// 1. Kopter ausgeschaltet? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, strGet(STR_SWITCHOFFMK), NULL, strGet(STR_ATTENTION), STR_WI232INIT) ) |
{ |
return; |
} |
//---------------------------- |
// 2. Wi.232 suchen |
//---------------------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
Change_Output( Uart02Wi ); // Verbindung zu Wi232 herstellen |
SwitchToWi232(); // Serielle Kanaele Wi232 mit USB verbinden |
set_WI232CMD(); |
_delay_ms(100); |
//_delay_ms(200); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_115200 ); |
if( !foundbaud ) // gefunden? |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 not found"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
} |
//---------------------------- |
// 3. Wi.232 initialisieren |
//---------------------------- |
if( foundbaud > 0 ) |
{ |
//-------------- |
// Screen |
//-------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_WI232INIT ); |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 18, 0, 0,0); // 18 Progress Steps |
// DEBUG - hier kann ggf. die _gefundene_ Baudrate vom Wi.232 angezeigt werden |
//lcdx_printf_center_P( 6, MNORMAL, 0,-4, PSTR("(found %lu Baud)"), Baud_to_uint32(foundbaud) ); // zeige Baudrate |
// DEBUG - und hier auf welche Baudrate das Wi.232 gesetzt werden soll |
//lcdx_printf_center_P( 7, MNORMAL, 0,-2, PSTR("(set %lu Baud)"), Baud_to_uint32(Config.PKT_Baudrate) ); // zeige Baudrate |
SetBaudUart0( foundbaud ); |
// wenn sich ein EEPROM-Wert ändert wird auch das Ram beschrieben damit die Änderung sofort wirksam wird |
//-------------------------------------------------------------------------------------------------------------------- |
// + Errorcode |
// | |
// | |
if( !nError ) nError = Wi232_Write_Progress( regNVTXCHANNEL,Config.WiTXRXChannel, 1); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regTXCHANNEL,Config.WiTXRXChannel , 2); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVRXCHANNEL,Config.WiTXRXChannel, 3); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regRXCHANNEL,Config.WiTXRXChannel , 4); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVSLPMODE ,Sleep_Awake , 5); // Sleepmode |
if( !nError ) nError = Wi232_Write_Progress( regNVPWRMODE,WbModeP15 , 6); // Transceiver Mode/Powermode |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: TWaitTime16 (=0x10 // 16 ms Delay (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVTXTO,Config.WiTXTO , 7); // UART Timeout |
if( !nError ) nError = Wi232_Write_Progress( regTXTO,Config.WiTXTO , 8); // UART Timeout |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: UartMTU64 (=64 // default=64, valid 1-144) |
if( !nError ) nError = Wi232_Write_Progress( regNVUARTMTU,Config.WiUartMTU , 9); // UART Buffer |
if( !nError ) nError = Wi232_Write_Progress( regUARTMTU,Config.WiUartMTU ,10); // UART Buffer |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: NetMode_Normal (=0x01 // Normalmode (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVNETMODE,Config.WiNetworkMode ,11); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNETMODE,Config.WiNetworkMode ,12); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNVUSECRC ,CRC_Enable ,13); // CRC |
if( !nError ) nError = Wi232_Write_Progress( regNVCSMAMODE,CSMA_En ,14); // CSMA |
if( !nError ) nError = Wi232_Write_Progress( regNVNETGRP,Config.WiNetworkGroup ,15); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNETGRP,Config.WiNetworkGroup ,16); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNVDATARATE,InitBaudrate ,17); // Baudrate |
_delay_ms(200); |
if( !nError ) nError = Wi232_Write_Progress( regDATARATE,InitBaudrate ,18); // Baudrate |
_delay_ms(250); |
//_delay_ms(6000); // DEBUG |
if( nError ) |
{ |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("Wi.232 Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
} |
else |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
WriteWiInitFlag(); // erfolgreiches Init merken |
} |
} |
clr_WI232CMD(); |
SetBaudUart0( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// Wi232_ConfigPC() |
// |
// Connect USB direct to Wi.232 in Progmode |
//-------------------------------------------------------------- |
void Wi232_ConfigPC( void ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titelzeile Invers |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, strGet(STR_WI232CONFIG) ); // Titeltext "Wi.232 Konfig." |
lcdx_printp_center( 2, strGet(STR_USBCONNECTED), MNORMAL, 0,-2); // "mit USB verbunden" |
lcdx_printp_center( 4, strGet(STR_SEEMKWIKI) , MNORMAL, 0,0); // "siehe MK-Wiki:" |
lcdx_printp_center( 5, PSTR("\"RadioTronix\"") , MNORMAL, 0,2); // "RadioTronix" |
lcd_rect_round( 0, 28, 127, 24, 1, R2); // Rahmen um Text |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline "Ende" |
Change_Output(USB2Wi); |
set_WI232CMD(); // Port D6 = CMD |
while(!get_key_press(1 << KEY_ESC)) |
{ |
show_Lipo(); |
} |
clr_WI232CMD(); // Port D6 = CMD |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
// 10.06.2014 OG: das Folgende war im orginalen Port_USB2CFG_Wi() nicht |
// vorhanden - das hatte den Effekt, dass wenn man diese |
// Funktion aufgerufen und wieder beendet hatte das |
// PKT-Updatetool kein PKT-Update mehr via PKT_CtrlHook() |
// durchfuehren konnte. Die USB Umschaltung war wohl nicht sauber. |
// |
uart1_init( UART_BAUD_SELECT(USART_BAUD,F_CPU) ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
SetBaudUart1( Config.PKT_Baudrate ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wi232/Wi232.h |
---|
0,0 → 1,192 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.h |
//# |
//# 10.06.2014 OG |
//# - add: Wi232_Initalize() |
//# - add: Wi232_ConfigPC() |
//# - del: InitWi232() |
//# |
//# 08.06.2014 OG |
//# - chg: InitWi232() - Parameteraenderung |
//# - del: Wi232_New_Baudrate |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef WI232_H_ |
#define WI232_H_ |
// Non-volatile Registers |
// Name Address Description Default |
#define regNVTXCHANNEL 0x00 // Transmit channel setting ## 0 ## |
#define regNVRXCHANNEL 0x01 // Receive channel setting ## 0 ## |
#define regNVPWRMODE 0x02 // Operating mode settings ## +13 dBm widebandmode ## |
#define regNVDATARATE 0x03 // UART data rate ## 2400bps ## |
#define regNVNETMODE 0x04 // Network mode (Normal/Slave) ## Normal ## |
#define regNVTXTO 0x05 // Transmit wait timeout ## ~16ms ## |
#define regNVNETGRP 0x06 // Network group ID ## 0x00 ## |
#define regNVUSECRC 0x08 // Enable/Disable CRC ## Enabled ## |
#define regNVUARTMTU 0x09 // Minimum transmission unit. ## 64 bytes ## |
#define regNVSHOWVER 0x0A // Enable/Disable start-up message ## Enabled ## |
#define regNVCSMAMODE 0x0B // Enable/Disable CSMA ## Enabled ## |
#define regNVSLPMODE 0x0D // Power state of module ## Awake ## |
#define regNVACKONWAKE 0x0E // Send ACK character to host on wake |
// Non-volatile Read Only Registers |
// Name Address Description |
#define regMAC0 0x22 // These registers form the unique 48-bit MAC address. |
#define regMAC1 0x23 // MAC |
#define regMAC2 0x24 // MAC |
#define regOUI0 0x25 // MAC |
#define regOUI1 0x26 // MAC |
#define regOUI2 0x27 // MAC |
#define regDiscover 0x78 // Versionsregister |
// Volatile Read/Write Registers |
// Name Address Description |
#define regTXCHANNEL 0x4B // Transmit channel setting |
#define regRXCHANNEL 0x4C // Receive channel setting |
#define regPWRMODE 0x4D // Operating mode settings |
#define regDATARATE 0x4E // UART data rate |
#define regNETMODE 0x4F // Network mode (Normal or Slave) |
#define regTXTO 0x50 // Transmit wait timeout |
#define regNETGRP 0x51 // Network group ID |
#define regUSECRC 0x53 // Enable/Disable CRC |
#define regUARTMTU 0x54 // Minimum transmission unit. |
#define regSHOWVER 0x55 // Enable/Disable start-up message |
#define regCSMAMODE 0x56 // Enable/disable CSMA |
#define regSLPMODE 0x58 // Power state of module |
#define regACKONWAKE 0x59 // Send ACK character to host on wake |
// Wideband Channels |
// regNVTXCHAN (0x00) regTXCHAN (0x4B) |
// Channel Number Frequency |
#define wChan0 0x00 // 868.300 MHz |
#define wChan1 0x01 // 868.95 MHz ## MK ## |
// Narrowband Channels |
// regNVRXCHAN (0x01) regRXCHAN (0x4C) |
// Channel Number Frequency |
#define nChan0 0x00 // 868.225 MHz |
#define nChan1 0x01 // 868.375 MHz ## MK ## |
#define nChan2 0x02 // 868.850 MHz |
#define nChan3 0x03 // 869.050 MHz |
#define nChan4 0x04 // 869.525 MHz |
#define nChan5 0x05 // 869.850 MHz |
// Power Mode |
// regNVPWRMODE (0x02) regPWRMODE (0x4D) |
// PM1 PM1 PM0 Mode |
#define NbModeN0 0x00 // 0 0 0 Narrowband Mode 0dBm power setting (typical) |
#define WbModeP5 0x01 // 0 0 1 Wideband Mode +5dBm power setting (typical) |
#define WbModeP10 0x02 // 0 1 0 Wideband Mode +10dBm power setting (typical) |
#define WbModeP15 0x03 // 0 1 1 Wideband Mode +15dBm power setting (typical) ## MK ## |
#define WbModeN0 0x04 // 1 0 0 Wideband Mode 0dBm power setting (typical) |
#define NbModeP5 0x05 // 1 0 1 Narrowband Mode +5dBm power setting (typical) |
#define NbModeP10 0x06 // 1 1 0 Narrowband Mode +10dBm power setting (typical) |
#define NbModeP15 0x07 // 1 1 1 Narrowband Mode +15dBm power setting (typical) |
// Wi232 UART Baudrate |
// regNVDATARATE (0x03) regDATARATE (0x4E) |
// Baud Rate BR2 BR1 BR0 |
#define Wi232_2400 Baud_2400 // 0 0 0* (default 2400) |
#define Wi232_9600 Baud_9600 // 0 0 1 |
#define Wi232_19200 Baud_19200 // 0 1 0 |
#define Wi232_38400 Baud_38400 // 0 1 1 |
#define Wi232_57600 Baud_57600 // 1 0 0 ## MK ## |
#define Wi232_115200 Baud_115200 // 1 0 1 |
#define Wi232_10400 0x06 // 1 1 0 |
#define Wi232_31250 0x07 // 1 1 1 |
// NetworkMode |
// regNVNETMODE (0x04) regNETMODE (0x4F) |
#define NetMode_Slave 0x00 // Slavemode |
#define NetMode_Normal 0x01 // Normalmode (default) |
// Transmit Wait Timeout |
// regNVTXTO (0x05) regTXTO (0x50) |
#define TWaitTimeFull 0x00 // full Buffer required |
#define TWaitTime16 0x10 // 16 ms Delay (default) |
// Network Group |
// regNVNETGRP (0x06) regNETGRP (0x51) |
#define NetWorkGroup 66 // default = 0, valid 0-127 ## MK = 66 ## |
// CRC Control |
// regNVUSECRC (0x08) regUSECRC (0x53) |
#define CRC_Disable 0x00 // no CRC check |
#define CRC_Enable 0x01 // CRC check (default) |
// UART minimum transmission unit |
// regNVUARTMTU (0x09) regUARTMTU (0x54) |
#define UartMTU64 64 // default=64, valid 1-144 |
// Verbose mode |
// regNVSHOWVER (0x0A) |
#define ShowVers_Dis 0x00 // do not show Startupmessage ## MK = 66 ## |
#define ShowVers_En 0x01 // show Startupmessage (default) |
// CSMA enable |
// regNVCSMAMODE (0x0B) regCSMAMODE (0x56) |
#define CSMA_Dis 0x00 // disable CSMA Carrier-sense multiple access |
#define CSMA_En 0x01 // enable CSMA (default) |
// Sleep control |
// regNVSLPMODE (0x0D) regSLPMODE (0x58) |
#define Sleep_Awake 0x00 // Sleepmode = Awake (default) |
#define Sleep 0x01 // Sleepmode = Sleep |
#define Sleep_Stby 0x02 // Sleepmode = Standby |
// ACK on Wake |
// regNVACKONWAKE (0x0D) regACKONWAKE (0x59) |
#define ACKwake_Dis 0x00 // disable ACK on Wake |
#define ACKwake_En 0x01 // enable ACK on Wake (default) |
//---------------------------------- |
// EXPORT |
//---------------------------------- |
void Wi232_Initalize( void ); |
void Wi232_ConfigPC( void ); |
#endif // WI232_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wifly/PKT_WiFlyHQ.c |
---|
0,0 → 1,1406 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/** |
* @file WiFly RN-XV Library |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.c |
//# |
//# 03.06.2014 OG |
//# - fix: find_WiFly() - Benutzer kann Suche nach WiFly jetzt abbrechen |
//# (z.b. weil er kein WiFly angesteckt hat) |
//# von dieser Aenderungen betroffen sind auch noch einige Codechanges |
//# an anderen Funktionen. |
//# |
//# 15.04.2014 OG |
//# - chg: wifly_init() - Logik bei der Versionsverifikation von WIFLY_ADHOC |
//# geaendert da v2.38 ansonsten ggf. den Bach runter gegangen waere. |
//# AKTUELL UNGETESTET DA KEIN WIFLY ADHOC VORHANDEN! |
//# - add: Wifly_update() ein paar Kommentare im Code |
//# - add: KOMMENTAR zu define DEBUG_WIFLY ! (lesenswert!) |
//# |
//# 14.04.2014 Cebra |
//# - add: Defines für getestete WiFly Softwareversionen |
//# - add: wifly_init bei der Versionsabfrage erweitert |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: define DELAY_BEFORE_SAVE |
//# - add: define DELAY_BEFORE_REBOOT |
//# |
//# 02.04.2014 OG |
//# WIP: etliche weitere Aenderungen.... |
//# - chg: getVersion(): Rueckgabe/Logik geaendert |
//# - add: define DELAY_AFTER_MODUL_ON |
//# |
//# 01.04.2014 SH |
//# - chg: Nicht benötigte Progmem Strings auskommentiert |
//# |
//# 11.03.2014 SH |
//# - add: neue Funktionen wl_reset() und wl_update() |
//# - chg: wl_init() wurde komplett auf den neuen AP Modus geändert, die Init Routine vom Ad Hoc Modus befindet sich ausgeklammert darunter |
//# - chg: getVersion wurde angepasst + neuer Wert für resp_Version[] PROGMEM = "RN-171\r\n" anstatt WiFly |
//# - chg: setSSID() und setPassphrase() wurden um weiteren Parameter ergänzt |
//# - chg: createAdhocNetwork angepasst da Funktion setSSID() enthalten |
//# - del: ausgeklammerte Funktionen getConnection, getDHCPMode, getSpaceReplace, getopt_Asc und join |
//# |
//# 12.02.2014 OG |
//# - chg: readTimeout() Auskommentiert: "unused variable 'ind'" |
//# - chg: wl_init() Auskommentiert: "unused variable 'dhcpMode'" |
//# |
//# 02.07.2013 cebra |
//# - new: Routinen für WiFly WLAN-Modul |
//# |
//########################################################################### |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "PKT_WiFlyHQ.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../timer/timer.h" |
#include "../lcd/lcd.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//---------------------------------------------------- |
// DEBUG |
// |
// Wenn 'DEBUG_WIFLY' eingeschalte wird dann werden die Ausgaben |
// des WiFly auf dem PKT-Screen angezeigt - fast wie bei einem |
// Terminal Programm! |
// |
// Man kann dadurch genaueres erkennen wie die Kommunikation |
// ablaeuft und mit welchen Strings beispielsweise das WiFly |
// ein Ok quittiert. |
//---------------------------------------------------- |
//#define DEBUG_WIFLY |
//---------------------------------------------------- |
// unterstützte WiFly Softwareversionen |
//---------------------------------------------------- |
#define Adhoc_Version1 232 // getestet Version 2.32 |
#define Adhoc_Version2 238 // getestet Version 2.38 |
#define Apmode_Version1 441 // getestet Version 4.41 |
//---------------------------------------------------- |
// Einstellungen |
//---------------------------------------------------- |
static const char WIFLY_IP_ADDRESS[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_PORT[] PROGMEM = "2000"; |
static const char WIFLY_IP_GATEWAY[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_NETMASK[] PROGMEM = "255.255.0.0"; |
static const char WIFLY_VERSION_ADHOC[] PROGMEM = "2.38.3"; |
//static const char WIFLY_VERSION_ADHOC[] PROGMEM = "v2.32"; |
static const char WIFLY_VERSION_APMODE[] PROGMEM = "4.41"; |
//---------------------------------------------------- |
// Einsstellungen fuer Delay's |
//---------------------------------------------------- |
//#define WIFLY_DEFAULT_TIMEOUT 500 /* 500 milliseconds */ |
#define WIFLY_DEFAULT_TIMEOUT 100 // n milliseconds |
//---------------------------------------------------- |
//---------------------------------------------------- |
uint8_t progress_max = 0; |
uint8_t progress_act = 0; |
char progress_buffer[22]; |
char buffer[20]; |
char wifly_version_string[12]; |
uint16_t wifly_version; |
//---------------------------------------------------- |
// Texte |
//---------------------------------------------------- |
static const char STR_WIFLY_NOT_FOUND[] PROGMEM = "WiFly not found"; |
static const char RECEIVE_AOK[] PROGMEM = "AOK"; |
#define REPLACE_SPACE_CHAR '$' // ersetzt Spaces in einem String-Parameter des WiFly - '$' ist WiFly default (0x24) fuer SSID und Pwd |
//static const char [] PROGMEM = ""; |
//---------------------------------------------------- |
//############################################################################# |
//# |
//############################################################################# |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_begin( void ) |
{ |
lcd_cls(); |
lcd_setpos(0,0); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_wait( void ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) && !get_key_press(1 << KEY_ENTER) ); |
lcd_print_LF(); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_message( uint8_t *text ) |
{ |
lcd_print_LF(); |
lcd_print( text, MINVERS ); |
debug_wait(); |
lcd_print_LF(); |
} |
//############################################################################# |
//# |
//############################################################################# |
/** Read the next character from the WiFly serial interface. |
* Waits up to timeout milliseconds to receive the character. |
* @param chp pointer to store the read character in |
* @param timeout the number of milliseconds to wait for a character |
* @retval true - character read |
* @retval false - timeout reached, character not read |
*/ |
bool readTimeout( char *chp, uint16_t timeout) |
{ |
char ch; |
timer2 = timeout; |
while( timer2>0 ) |
{ |
if( uart1_available() > 0 ) |
{ |
ch = uart1_getc(); |
*chp = ch; |
#ifdef DEBUG_WIFLY |
lcd_print_char( ch, MNORMAL); |
#endif |
return (true); |
} |
} |
return (false); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void flushRx( int timeout ) |
{ |
char ch; |
while( readTimeout( &ch, timeout) ); |
} |
/** |
* Read characters from the WiFly and match them against the |
* progmem string. Ignore any leading characters that don't match. Keep |
* reading, discarding the input, until the string is matched |
* or until no characters are received for the timeout duration. |
* @param str The string to match, in progmem. |
* @param timeout fail if no data received for this period (in milliseconds). |
* @retval true - a match was found |
* @retval false - no match found, timeout reached |
*/ |
bool match_P( const char *str, uint16_t timeout) |
{ |
const char *match = str; |
char ch, ch_P; |
ch_P = pgm_read_byte(match); |
if( ch_P == '\0' ) return true; /* Null string always matches */ |
while( readTimeout( &ch, timeout) ) |
{ |
// DEBUG |
//lcd_print_char( ch, MNORMAL ); |
if( ch == ch_P ) match++; |
else |
{ |
match = str; // Restart match |
if( ch == pgm_read_byte(match) ) match++; |
} |
ch_P = pgm_read_byte( match ); |
if( ch_P == '\0' ) return (true); |
} |
return (false); |
} |
//############################################################################# |
//# progress |
//############################################################################# |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_begin( uint8_t maxsteps ) |
{ |
progress_max = maxsteps; |
progress_act = 0; |
} |
//-------------------------------------------------------------- |
// wait4key: true / false |
//-------------------------------------------------------------- |
void progress_end( uint8_t wait4key ) |
{ |
progress_max = 0; |
progress_act = 0; |
if( wait4key ) |
{ |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
} |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_show( const char *msg ) |
{ |
int8_t yoffs = 3; |
#ifdef DEBUG_WIFLY |
return; |
#endif |
if( progress_max ) |
{ |
progress_act++; |
lcdx_printf_center_P( 4, MNORMAL, 0,yoffs, PSTR("%d/%d"), progress_act, progress_max ); |
if( msg ) |
{ |
strncpyat_P( progress_buffer, msg, 20, ' ', 2); |
lcd_frect( 2, (5*8)+yoffs, 124, 7, 0); // Zeile 5 loeschen |
lcdx_print_center( 5, (uint8_t *)progress_buffer, MNORMAL, 0,yoffs); // Text zentriert; String im RAM |
} |
lcd_rect_round( 0, 28+yoffs, 127, 22, 1, R2); // Rahmen |
_delay_ms(700); // fuer Anzeige (Entschleunigung) |
} |
} |
//-------------------------------------------------------------- |
// gibt zentriert einen Text und loescht vorher die Zeile |
//-------------------------------------------------------------- |
void MsgCenter_P( uint8_t y, const char *text, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_row( y, MNORMAL, yoffs ); |
lcdx_printp_center( y, text, MNORMAL, 0, yoffs ); |
} |
//############################################################################# |
//# sendcmd |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void cmd_showerror( const char *cmdstr) |
{ |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( cmdstr, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t find_Prompt() |
{ |
char c; |
uint8_t state = 0; |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( state == 0 && (c == '>') ) { state = 1; continue; } |
if( state == 1 && (c == ' ') ) return true; |
if( state == 1 ) break; // Fehler: nicht vorgesehen |
} |
return false; |
} |
//-------------------------------------------------------------- |
// nicht verwendet... |
//-------------------------------------------------------------- |
uint8_t find_Prompt2() |
{ |
char c; |
timer2 = 800; // 8 Sekunden max. suchen |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) && timer2 ) |
{ |
if( c == '>' ) return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Das ist erstmal so uebernommen - ob das alles sein muss?? |
// |
// Put the WiFly into command mode |
// |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t cmd_EnterCommandMode( void ) |
{ |
uint8_t retry = 2; |
int8_t result = 0; // false |
while( retry && !result ) |
{ |
//_delay_ms(250); |
uart1_puts_p( PSTR("$$$") ); |
//_delay_ms(250); |
_delay_ms(100); |
uart1_putc('\r'); |
if( get_key_short(1 << KEY_ESC) ) // Abbruch durch Benutzer? |
{ |
return -1; // -1 = Abbruch Benutzer |
} |
result = find_Prompt(); // ggf. result = true |
if( !result ) |
{ |
retry--; |
_delay_ms(250); |
} |
} |
return result; |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// cmdstr : PROGMEM |
// strvalue : RAM |
// match_receive: PROGMEM |
// findprompt : true/false |
//-------------------------------------------------------------- |
uint8_t cmdsend( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
uint8_t result = true; |
const char *p; |
char c; |
progress_show( cmdstr ); |
//-------------------- |
// Senden... |
//-------------------- |
uart1_puts_p( cmdstr ); |
if( strvalue != NULL ) |
{ |
uart1_putc(' '); |
p = strvalue; |
while( *p ) |
{ |
c = *p; |
if( c == ' ' ) c = REPLACE_SPACE_CHAR; |
uart1_putc( c ); |
p++; |
} |
} |
uart1_putc('\r'); |
//-------------------- |
// Antwort... |
//-------------------- |
if( match_receive ) |
{ |
//if( !match_P( p, WIFLY_DEFAULT_TIMEOUT) ) |
if( !match_P( match_receive, 2000) ) |
{ |
cmd_showerror( cmdstr ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
result = false; |
} |
// DEBUG |
//lcdx_printf_at_P( 2, 7, MINVERS, 0,0, PSTR("%d"), result ); |
} |
#ifdef DEBUG_WIFLY |
debug_wait(); |
#endif |
// TODO: Kommandos die keinen Prompt zurueckliefern? (evtl. 'reset'?) |
//uart1_putc('\r'); // fuer cmd_Join ein extra Return... (bei den anderen Kommandos stoert es nicht) |
if( result && findprompt) |
{ |
result = find_Prompt(); |
} |
return( result ); |
} |
//-------------------------------------------------------------- |
// cmdsend_P() |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t cmdsend_P( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
strncpy_P( buffer, strvalue, 20); |
return cmdsend( cmdstr, buffer, match_receive, findprompt ); |
} |
//-------------------------------------------------------------- |
// Version wird anhand des Prompt's ermittelt |
// Implementiert als State-Machine |
//-------------------------------------------------------------- |
uint8_t cmd_GetVersion( void ) |
{ |
char c; |
uint8_t state = 0; |
char *dst = wifly_version_string; |
uint8_t n = 0; |
wifly_version = 0; |
uart1_putc('\r'); // erzwinge Prompt |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( (state == 0) && (c == '<') ) |
{ |
state = 1; |
continue; |
} |
if( (state == 1) && (c != '>') ) |
{ |
// TODO: Ende vom Prompt im Fehlerfall catchen... |
if( n+1 >= sizeof(wifly_version_string) ) |
break; |
*dst = c; |
dst++; |
n++; |
continue; |
} |
if( (state == 1) && (c == '>') ) // Versionsstring-Ende erreicht |
{ |
*dst = 0; |
state = 2; |
break; |
} |
} |
if( state == 2 ) |
{ |
//uint16_t wifly_version; |
wifly_version = (wifly_version_string[0]-'0') * 100; |
wifly_version += (wifly_version_string[2]-'0') * 10; |
wifly_version += (wifly_version_string[3]-'0') * 1; |
} |
else |
{ |
cmd_showerror( PSTR("get version") ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
} |
return (state == 2); |
} |
//-------------------------------------------------------------- |
// cmd_Set() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_Set( const char *cmdstr ) |
{ |
return cmdsend( cmdstr, 0, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : RAM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr( const char *cmdstr, const char *str ) |
{ |
return cmdsend( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr_P( const char *cmdstr, const char *str ) |
{ |
return cmdsend_P( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetNum() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// value : Dezimal |
//-------------------------------------------------------------- |
uint8_t cmd_SetNum( const char *cmdstr, const uint16_t value ) |
{ |
utoa( value, buffer, 10 ); |
return cmd_SetStr( cmdstr, buffer); |
} |
//-------------------------------------------------------------- |
// cmd_Reset() |
//-------------------------------------------------------------- |
uint8_t cmd_Reset( void ) |
{ |
uint8_t result = true; |
//_delay_ms( 500 ); |
result = cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), false ); |
_delay_ms( 1000 ); |
return result; |
// return cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Save() |
//-------------------------------------------------------------- |
uint8_t cmd_Save( void ) |
{ |
//_delay_ms( DELAY_BEFORE_SAVE ); |
_delay_ms( 200 ); |
return cmdsend( PSTR("save"), 0, PSTR("Storing"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Join() |
//-------------------------------------------------------------- |
uint8_t cmd_Join( const char *ssid ) |
{ |
uint8_t result = true; |
result = cmdsend( PSTR("join"), ssid, PSTR("Associated"), false ); |
if( result ) uart1_putc('\r');; |
if( result ) result = find_Prompt(); |
return result; |
} |
//-------------------------------------------------------------- |
// cmd_Reboot() |
//-------------------------------------------------------------- |
uint8_t cmd_Reboot( void ) |
{ |
//_delay_ms( DELAY_BEFORE_REBOOT ); |
_delay_ms( 300 ); |
cmdsend( PSTR("reboot"), 0, 0, false ); |
_delay_ms( 2000 ); |
return true; |
} |
//############################################################################# |
//# WiFly suchen |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void find_WiFly_searchmsg( const char *text, uint8_t baud ) |
{ |
lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,0, PSTR("%S %lu Baud"), text, Baud_to_uint32(baud) ); // Text zentriert |
} |
//-------------------------------------------------------------- |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t find_WiFly_baud( uint8_t baud ) |
{ |
int8_t result = 1; |
find_WiFly_searchmsg( strGet(STR_SEARCH), baud ); |
// Anmerkung 06.04.2014 OG: |
// SetBaudUart1() (in uart/uart1.c) setzt nur die Baudrate des Uart. |
// Config.PKT_Baudrate wird durch SetBaudUart1() NICHT geaendert! |
SetBaudUart1( baud ); |
result = cmd_EnterCommandMode(); |
if( result > 0 ) |
{ |
return baud; // Baud gefunden, ok! |
} |
return result; // nicht gefunden oder Abbruch durch Benutzer |
} |
//-------------------------------------------------------------- |
// ok = find_WiFly() |
// |
// Sucht das WiFly mit verschiedenen Baudraten. |
// |
// Wenn das WiFly mit von Config.PKT_Baudrate (= 57600) abweichender |
// Baudrate gefunden wird (z.B. 9600 Baud) dann wird das WiFly |
// automatisch auf 57600 Baud umkonfiguriert damit nachfolgende |
// Funktionen das nicht mehr beachten muessen. |
// |
// RUECKGABE: |
// ok = true : WiFly gefunden und ist auf 57600 Baud |
// ok = false: kein WiFly gefunden |
// |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
// |
// Anmerkung: |
// #define Baud_9600 1 |
// #define Baud_19200 2 |
// #define Baud_38400 3 |
// #define Baud_57600 4 |
// #define Baud_115200 5 |
// #define Baud_4800 6 |
// #define Baud_2400 7 |
//-------------------------------------------------------------- |
int8_t find_WiFly( void ) // search Wifly with all Baudrates |
{ |
int8_t result = 1; // 1 == true |
int8_t baud = 0; |
//inCommandMode = false; |
//exitCommand = 0; |
set_Modul_On( Wlan ); // WiFly einschalten |
timer2 = 60; // Delay damit das WiFly booten kann |
while( timer2 ); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline: "Ende" |
//------------------------ |
// suche WiFly mit versch. Baudraten |
//------------------------ |
if( baud==0 ) baud = find_WiFly_baud( Baud_57600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_9600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_115200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_38400 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_19200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_4800 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_2400 ); |
lcdx_cls_row( 7, MNORMAL, 0); // Keyline loeschen |
//------------------------ |
// Abbruch durch Benutzer? |
//------------------------ |
if( baud < 0 ) |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
return -1; // -1 = Abbruch Benutzer |
} |
//------------------------ |
// kein WiFly gefunden... |
//------------------------ |
if( baud==0 ) // WiFly nicht gefunden :-( |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( STR_WIFLY_NOT_FOUND, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
return false; // 0 = nicht gefunden |
} |
//------------------------ |
// WiFly gefunden! |
//------------------------ |
find_WiFly_searchmsg( strGet(STR_FOUND), baud ); // Anzeige: "gefunden..." |
//------------------------ |
// WiFly Baud ok? |
// -> ggf. WiFly Baud umkonfigurieren |
//------------------------ |
if( Config.PKT_Baudrate != baud ) |
{ |
find_WiFly_searchmsg( strGet(STR_SET), Config.PKT_Baudrate ); // Anzeige: "setze..." |
if( result ) result = cmd_Set( PSTR("set u b 57600") ); // -> etwas unsauber hier einfach 57600 zu setzen und nicht Config.PKT_Baudrate |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); // true = uart-baud setzen |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
if( result ) result = cmd_EnterCommandMode(); |
} |
return result; |
} |
//############################################################################# |
//# |
//############################################################################# |
//-------------------------------------------------------------- |
// zeigt die WiFly-Version an |
//-------------------------------------------------------------- |
uint8_t wifly_version_show( void ) |
{ |
int8_t result = 1; // 1 = true |
ShowTitle_P( PSTR("WiFly Version"), true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); // Ergebnis in 'wifly_version_string' |
set_Modul_On( USB ); |
if( result ) |
{ |
// TODO: |
// Anzeige ob WiFly-Version kompatibel zum gewaehlten |
// Mode (AP-Mode, AdHoc) ist |
//lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcd_rect_round( 0, 8*4+3, 127, 8*2, 1, R2); // Anzeige-Rahmen |
lcdx_printf_center_P( 5, MNORMAL, 0,0, PSTR("Version: v%s"), wifly_version_string ); |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
else |
{ |
PKT_Message_P( PSTR("Version FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t _ask( uint8_t keyline ) |
{ |
uint8_t ask = 0; |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
lcd_printp_at( 12, 7, strGet(keyline), MNORMAL); // Keyline |
if( keyline == ENDE ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !ask ) // warte auf Ja/Nein |
{ |
if( get_key_press(1 << KEY_ENTER) ) ask = 1; // "Ja" |
if( get_key_press(1 << KEY_ESC) ) ask = 2; // "Nein" |
}; |
return( ask==1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_init_perform( uint8_t wiflymode ) //AP Mode |
{ |
uint8_t result = true; |
#ifndef DEBUG_WIFLY |
MsgCenter_P( 2, PSTR("INIT"), MNORMAL, 0 ); |
#endif |
//--------------- |
// WIFLY_ADHOC |
//--------------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
progress_begin( 9 ); |
//if( result ) result = cmd_Set( PSTR("set u m 1") ); // ist das notwendig? - (turn off echo) |
//if( result ) result = cmd_Set( PSTR("set sys printlvl 0") ); // ist das notwendig? |
//if( result ) result = cmd_Set( PSTR("set comm remote 0") ); // ist das notwendig? |
if( result ) result = cmd_Set( PSTR("set wlan join 4") ); // 4 = AdHoc |
if( result ) result = cmd_Set( PSTR("set ip dhcp 0") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_SetStr( PSTR("set wlan ssid"), Config.Wlan_SSID ); |
if( result ) result = cmd_SetNum( PSTR("set wlan chan"), Config.Wlan_Channel ); |
} |
//--------------- |
// WIFLY_APMODE |
//--------------- |
if( wiflymode == WIFLY_APMODE ) |
{ |
progress_begin( 13 ); |
if( result ) result = cmd_Set( PSTR("set wlan join 7") ); |
if( result ) result = cmd_Set( PSTR("set ip dhcp 4") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_Set( PSTR("set wlan rate 3") ); |
if( result ) result = cmd_SetStr( PSTR("set apmode ssid") , Config.Wlan_SSID ); |
if( result ) result = cmd_SetStr( PSTR("set apmode pass") , Config.Wlan_Password ); |
if( result ) result = cmd_SetNum( PSTR("set wlan channel"), Config.Wlan_Channel ); |
if( result ) result = cmd_Set( PSTR("set ip protocol 0x3") ); |
if( result ) result = cmd_Set( PSTR("set apmode link_monitor 0") ); |
} |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
return( result ); |
} |
//-------------------------------------------------------------- |
// ok = wifly_init( wiflymode ) |
// |
// initialisiert das WiFly fuer AP-Mode oder AdHoc |
// |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_init( uint8_t wiflymode ) //AP Mode |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
uint8_t versionerror = false; |
char *title = (char *)PSTR("Init WiFly"); |
char *strMode; |
if( wiflymode == WIFLY_ADHOC ) strMode = (char *)PSTR("AdHoc"); |
else strMode = (char *)PSTR("AP-Mode"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Version pruefen |
//----------------------------- |
if( result ) |
{ |
//-------- |
// ADHOC |
//-------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
if( (wifly_version != Adhoc_Version1) // 2.32 |
&& (wifly_version != Adhoc_Version2) // 2.38 |
) |
{ |
versionerror = true; |
} |
} |
//-------- |
// APMODE |
//-------- |
if( wiflymode == WIFLY_APMODE && wifly_version != Apmode_Version1 ) versionerror = true; |
if( versionerror ) |
{ |
set_Modul_On( USB ); |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -1, PSTR("ERROR") ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("Version no %S!"), strMode ); // "Version no AP-Mode" / "AdHoc" |
ask = _ask( ENDE ); |
return false; |
} |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); // WiFly Versionsanzeige |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: OK") , strMode ); // "AP-Mode" oder "AdHoc" |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?") , title ); // "Init WiFly?" |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Init durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
result = wifly_init_perform( wiflymode ); |
} |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Init FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Init OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return( result ); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_update( uint8_t wiflymode ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Update WiFly"); |
const char *pStr; |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
if( wiflymode == WIFLY_ADHOC ) pStr = WIFLY_VERSION_ADHOC; |
else pStr = WIFLY_VERSION_APMODE; |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("%S: v%s"), strGet(STR_VON), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: v%S"), strGet(STR_NACH), pStr ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Update durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
progress_begin( 16 ); |
} |
//---------------------------- |
// 1. Reset |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("Reset"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
if( result ) result = find_WiFly(); |
//---------------------------- |
// 2. mit Heimnetz verbinden |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("verbinde Heimnetz"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ip dhcp 1") ); |
if( result ) result = cmd_SetStr( PSTR("set wlan phrase"), Config.Wlan_HomePassword ); |
if( result ) result = cmd_Set( PSTR("set wlan channel 0") ); |
if( result ) result = cmd_Join( Config.Wlan_HomeSSID ); |
//---------------------------- |
// 3. FTP Download & Update |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("FTP Update"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ftp address 198.175.253.161") ); |
if( result ) result = cmd_Set( PSTR("set ftp dir public") ); |
if( result ) result = cmd_Set( PSTR("set ftp user roving") ); |
if( result ) result = cmd_Set( PSTR("set ftp pass Pass123") ); |
if( result ) result = cmd_Set( PSTR("set ftp timeout 800") ); |
//if( result ) result = cmd_Set( PSTR("del config") ); |
if( result ) |
{ |
//----- |
// das kann einiges gekuerzt werden wenn das denn zuverlaessig laufen wuerde... |
// |
// hier wird auf v4.40 geprueft da laut Refrenz-Manual erst seit dieser Version |
// "cupdate" unterstuetzt wird. Bei einem Downgraden von v4.4 auf eine aeltere |
// Version ist ein clean des Dateisystems ('c') erforderlich da sonst das WiFly |
// nur via Hardware-Taster an GPIO9 wiedre zum Leben erweckt werden kann. |
// |
// Anmerkung: evtl. wuer auch ein "del config" reichen - das ist aber nicht |
// hinreichend getestet |
//----- |
if( wifly_version >= 440 ) |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp cupdate wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp cupdate wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
else |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp update wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp update wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
} |
if( result ) result = cmd_Reset(); |
if( result ) { timer2 = 50; while(timer2); }; |
/* |
//----- |
// Baustelle / Experimente bzgl. FTP-Update |
//----- |
//debug_message("FTP END"); |
//debug_message("RESET"); |
//if( result ) result = cmd_Reboot(); |
if( result ) { timer2 = 100; while(timer2); }; |
if( result ) { timer2 = 200; while(timer2); }; |
debug_message("FINDPR"); |
uart1_putc('\r'); |
if( result ) result = find_Prompt(); |
if( result ) debug_message("FINDPR END: 1"); |
else debug_message("FINDPR END: 0"); |
*/ |
//progress_end( true ); // Progress abschalten & warten |
progress_end( false ); // Progress abschalten |
//---------------------------- |
// 4. Init |
//---------------------------- |
/* |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) set_Modul_On( USB ); |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) ShowTitle_P( title, true ); |
if( result ) result = result = find_WiFly(); |
if( result ) result = wifly_init_perform( wiflymode ); |
*/ |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Update FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Update OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_reset( void ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Reset WiFly"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
ShowTitle_P( title, true ); |
if( result ) progress_begin( 2 ); |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
set_Modul_On( USB ); |
if( result ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("Reset OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
else |
{ |
PKT_Message_P( PSTR("Reset FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//############################################################################# |
//# TEST / DEBUG |
//############################################################################# |
//-------------------------------------------------------------- |
// nur zum testen! |
//-------------------------------------------------------------- |
uint8_t wifly_test( void ) |
{ |
uint8_t result = true; |
return result; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wifly/PKT_WiFlyHQ.h |
---|
0,0 → 1,89 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/* Release history |
* |
* Version Date Description |
* 0.1 25-Mar-2012 First release. |
* 0.2 09-Apr-2012 Added features to support http servers. |
* - added an httpserver.ino example. |
* - added sendChunk() and sendChunkln() to send chunked HTTP bodies. |
* - added terminal() method for simple terminal access via debug stream |
* - replaced getFreeMemory() with simpler version that works with 0 bytes |
* - turned peek buffer into a circular buffer to fix bug with detecting |
* *CLOS* and *OPEN* after a partial match. |
* - Added new TCP connection detection via *OPEN* match from available(). |
* isConnected() can now be polled until a client connects. |
* - made the match() function public, handy for matching text in a stream. |
* - Added a getProtocol() function to get current set of protocols. |
* 0.3 21-Apr-2012 Added createAdhocNetwork() to create an Ad Hoc WiFi network. |
* Optimised the setopt() and getopt() function so they handle |
* integer conversions and refactored all of the set and get functions. |
* Added a multMatch_P() function to match serial data against multiple |
* progmem strings. |
* Added failure detection to the join() function to quickly detect |
* a failure rather than relying on a timeout. |
* Added setJoin() and getJoin() function for access to the wlan join parameter. |
* Refactored getres() to use the new multiMatch_P() function. |
* |
*/ |
/** |
* @mainpage WiFlyHQ WiFly RN-XV Arduino library |
* |
* This library provides functions for setting up and managing the WiFly module, |
* sending UDP packets, opening TCP connections and sending and receiving data |
* over the TCP connection. |
* |
* @author Harlequin-Tech |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.h |
//# |
//# 13.04.2014 OG |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 02.04.2014 OG |
//# - add: Versionshistorie ergaenzt |
//########################################################################### |
#ifndef _WIFLYHQ_H_ |
#define _WIFLYHQ_H_ |
// Todo: Strings verfuegbar machen für connect.c |
//extern const char WIFLY_IP_ADDRESS[]; |
//extern const char WIFLY_IP_PORT[]; |
uint8_t wifly_update( uint8_t wiflymode ); |
uint8_t wifly_reset(void); |
uint8_t wifly_version_show( void ); |
uint8_t wifly_init( uint8_t wiflymode ); |
uint8_t wifly_test( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wifly/wifly_setup.c |
---|
0,0 → 1,424 |
/* |
* wifly_setup.c |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY wifly_setup.c |
//# |
//# 08.06.2014 OG |
//# - chg: Setup_WiFly() - Tipptext fuer Module-Installed ergaenzt |
//# |
//# 04.06.2014 OG |
//# - chg: Menuetext angepasst; Menue-Separator hinzugefuegt |
//# |
//# 31.05.2014 OG |
//# - chg: Setup's auf geaendertes Edit_String() umgestellt (Param 'Text' entfernt) |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 30.03.2014 OG |
//# - chg: Text von WL_INSTALLED geaendert von "Modul eingebaut?" auf "Modul vorhanden?" |
//# -> das Modul wird aufgesteckt und nicht eingebaut... |
//# - chg: ein paar englische Texte geaendert |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 11.03.2014 SH |
//# - add: neue Menüpunkte Reset, Version und Update |
//# - chg: Menüpunkt Passwort wurde wieder aktiviert |
//# - chg: beim Menüpunkt Kanal fängt die Auswahl jetzt bei 0 an (Modul sucht sich automatisch den besten Kanal) |
//# |
//# 12.02.2014 OG |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'z'" |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'i'" |
//# |
//# 02.07.2013 cebra |
//# - new: wifly_setup(): Setup für WiFly WLAN-Modul |
//########################################################################### |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
//#include <avr/pgmspace.h> |
//#include <string.h> |
//#include <util/delay.h> |
//#include <string.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../setup/setup.h" |
#include "../wifly/PKT_WiFlyHQ.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//char buffer[20]; // fuer Versionsabfrage |
//----------------------------- |
// Setup_WiFly() (WLAN) |
//----------------------------- |
#define WL_INSTALLED 1 |
#define WL_SSID 2 |
#define WL_PASSWORD 3 |
#define WL_CHANNEL 4 |
#define WL_INIT 5 |
#define WL_RESET 6 |
#define WL_VERSION 7 |
#define WL_UPDATE 8 |
#define WL_PCCONFIG 9 |
#define WL_WPA 10 |
#define WL_HOMESSID 11 |
#define WL_HOMEPASSWORD 12 |
#define WL_TEST 99 |
static const char WL_INSTALLED_de[] PROGMEM = "WiFly Modus"; |
static const char WL_INSTALLED_en[] PROGMEM = "WiFly mode"; |
static const char WL_SSID_de[] PROGMEM = "SSID"; |
#define WL_SSID_en WL_SSID_de |
static const char WL_PASSWORD_de[] PROGMEM = "Passwort"; |
static const char WL_PASSWORD_en[] PROGMEM = "Password"; |
static const char WL_HOMESSID_de[] PROGMEM = "Home SSID"; |
#define WL_HOMESSID_en WL_HOMESSID_de |
static const char WL_HOMEPASSWORD_de[] PROGMEM = "Home Passwort"; |
static const char WL_HOMEPASSWORD_en[] PROGMEM = "Home Password"; |
static const char WL_CHANNEL_de[] PROGMEM = "Kanal"; |
static const char WL_CHANNEL_en[] PROGMEM = "Channel"; |
static const char WL_VERSION_de[] PROGMEM = "zeige Version"; |
static const char WL_VERSION_en[] PROGMEM = "show version"; |
static const char WL_UPDATE_de[] PROGMEM = "Update"; |
#define WL_UPDATE_en WL_UPDATE_de |
static const char WL_PCCONFIG_de[] PROGMEM = "WiFly einschalten"; |
static const char WL_PCCONFIG_en[] PROGMEM = "WiFly power on"; |
static const char WL_WPA_de[] PROGMEM = "Sicherheit"; |
static const char WL_WPA_en[] PROGMEM = "Security"; |
//############################################################################ |
//-------------------------------------------------------------- |
// Setup_WiFly_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob WiFly ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WiFly_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
if( Config.UseWL == WIFLY_APMODE ) MenuCtrl_SetTitle_P( PSTR("WiFly AP-Mode")); |
else if( Config.UseWL == WIFLY_ADHOC ) MenuCtrl_SetTitle_P( PSTR("WiFly AdHoc")); |
else MenuCtrl_SetTitle_P( PSTR("WiFly")); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( WL_INSTALLED , MENU_ITEM, NOFUNC , WL_INSTALLED_de , WL_INSTALLED_en ); |
if( Config.UseWL ) |
{ |
//MenuCtrl_Push_P( WL_TEST , MENU_ITEM, NOFUNC , PSTR("TEST") ); |
MenuCtrl_PushML2_P( WL_VERSION , MENU_ITEM, NOFUNC , WL_VERSION_de , WL_VERSION_en ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_SSID , MENU_ITEM, NOFUNC , WL_SSID_de , WL_SSID_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_PushML2_P( WL_PASSWORD, MENU_ITEM, NOFUNC , WL_PASSWORD_de , WL_PASSWORD_en ); |
MenuCtrl_PushML2_P( WL_CHANNEL , MENU_ITEM, NOFUNC , WL_CHANNEL_de , WL_CHANNEL_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AP-Mode") ); |
else |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AdHoc") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_HOMESSID , MENU_ITEM, NOFUNC , WL_HOMESSID_de , WL_HOMESSID_en ); |
MenuCtrl_PushML2_P( WL_HOMEPASSWORD, MENU_ITEM, NOFUNC , WL_HOMEPASSWORD_de , WL_HOMEPASSWORD_en ); |
MenuCtrl_Push_P( WL_UPDATE , MENU_ITEM, NOFUNC , PSTR("UPDATE: WiFly") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_Push_P( WL_RESET , MENU_ITEM, NOFUNC , PSTR("RESET: WiFly") ); |
MenuCtrl_PushML2_P( WL_PCCONFIG , MENU_ITEM, &Port_WiFly2Wi , WL_PCCONFIG_de , WL_PCCONFIG_en ); |
//MenuCtrl_PushML2_P( WL_WPA , MENU_ITEM, NOFUNC , WL_WPA_de , WL_WPA_en ); |
if( Config.UseWL == WIFLY_ADHOC ) |
{ |
// aktuell kann es zu Problemen kommen wenn das WiFly |
// auf v2.38 gedowngraded wird (WiFly nicht mehr ansprechbar) |
// -> deshalb: deaktiviert |
MenuCtrl_ItemActive( WL_UPDATE, false ); |
} |
} |
} |
//-------------------------------------------------------------- |
// Setup_WiFly() |
//-------------------------------------------------------------- |
void Setup_WiFly( void ) |
{ |
uint8_t itemid; |
uint8_t UseWL; |
char string[20]; |
Setup_WiFly_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// TEST |
//-------------------- |
/* |
if( itemid == WL_TEST ) |
{ |
lcd_cls(); |
ShowTitle_P( PSTR("TEST"), true ); |
wifly_test(); |
} |
*/ |
//-------------------- |
// Wlan_INSTALLED |
//-------------------- |
if( itemid == WL_INSTALLED ) |
{ |
UseWL = Config.UseWL; |
Config.UseWL = Edit_generic( Config.UseWL, 0, 2, WlanMode, 1, strGet(STR_EXTSV2MODULE),NULL); |
if( UseWL != Config.UseWL ) // hat Benutzer Einstellung geaendert? |
{ |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wlan vorhanden / nicht vorhanden |
Setup_WiFly_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// WL_VERSION |
//-------------------- |
if( itemid == WL_VERSION ) |
{ |
wifly_version_show(); |
} |
//-------------------- |
// WL_SSID |
//-------------------- |
if( itemid == WL_SSID ) |
{ |
strncpyfill( string, Config.Wlan_SSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_SSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_PASSWORD |
//-------------------- |
if( itemid == WL_PASSWORD) |
{ |
strncpyfill( string, Config.Wlan_Password, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_Password, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMESSID |
//-------------------- |
if( itemid == WL_HOMESSID ) |
{ |
strncpyfill( string, Config.Wlan_HomeSSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomeSSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMEPASSWORD |
//-------------------- |
if( itemid == WL_HOMEPASSWORD) |
{ |
strncpyfill( string, Config.Wlan_HomePassword, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomePassword, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_CHANNEL |
//-------------------- |
if( itemid == WL_CHANNEL ) |
{ |
Config.Wlan_Channel = Edit_generic( Config.Wlan_Channel, 0,13,Show_uint3,1 ,NULL,NULL); |
} |
//-------------------- |
// WL_INIT |
//-------------------- |
if( itemid == WL_INIT ) |
{ |
wifly_init( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_RESET |
//-------------------- |
if( itemid == WL_RESET ) |
{ |
wifly_reset(); |
} |
//-------------------- |
// Wlan_UPDATE |
//-------------------- |
if( itemid == WL_UPDATE ) |
{ |
wifly_update( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_WPA |
//-------------------- |
//if( itemid == WL_WPA ) |
//{ |
// Config.Wlan_WPA = Edit_generic(Config.Wlan_WPA,0,1,WL3,WlanSecurity,1); |
//} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_WiFly() |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wifly/wifly_setup.h |
---|
0,0 → 1,18 |
/* |
* wifly_setup.h |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
#ifndef WIFLY_SETUP_H_ |
#define WIFLY_SETUP_H_ |
#define WIFLY_APMODE 1 |
#define WIFLY_ADHOC 2 |
void Setup_WiFly( void ); |
#endif /* WIFLY_SETUP_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/wifly |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
+ |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2 |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Koptertool3.9 |
+ |
+nmealib |
+ |
+Resourcen |
+ |
+TinyGPS |
+ |
+LICENSE.TXT |
+ |
+Source_PKT384e_TEST!.zip |
+ |
+Source_PKT384a.zip |
+ |
+mkparameters.c |
+ |
+DIFF_PKT384a.txt |
+ |
+DIFF_PKT384e.txt |
+ |
+DIFF_PKT384d.txt |
+ |
+.refactorings |
+ |
+.settings |
+ |
+.directory |
+ |
+.cproject |
+ |
+.project |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/bmp085.c |
---|
0,0 → 1,224 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY bmp085.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für BMP085/BMP180 Luftdrucksensor, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "bmp085.h" |
//path to i2c fleury lib |
#include BMP085_I2CFLEURYPATH |
/* |
* i2c write |
*/ |
void bmp085_writemem(uint8_t reg, uint8_t value) { |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_write(value); |
i2c_stop(); |
} |
/* |
* i2c read |
*/ |
void bmp085_readmem(uint8_t reg, uint8_t buff[], uint8_t bytes) { |
uint8_t i =0; |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_rep_start(BMP085_ADDR | I2C_READ); |
for(i=0; i<bytes; i++) { |
if(i==bytes-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
#if BMP085_FILTERPRESSURE == 1 |
#define BMP085_AVARAGECOEF 21 |
static long k[BMP085_AVARAGECOEF]; |
long bmp085_avaragefilter(long input) { |
uint8_t i=0; |
long sum=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
k[i] = k[i+1]; |
} |
k[BMP085_AVARAGECOEF-1] = input; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
sum += k[i]; |
} |
return (sum /BMP085_AVARAGECOEF) ; |
} |
#endif |
/* |
* read calibration registers |
*/ |
void bmp085_getcalibration() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
bmp085_readmem(BMP085_REGAC1, buff, 2); |
bmp085_regac1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC2, buff, 2); |
bmp085_regac2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC3, buff, 2); |
bmp085_regac3 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC4, buff, 2); |
bmp085_regac4 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC5, buff, 2); |
bmp085_regac5 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC6, buff, 2); |
bmp085_regac6 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGB1, buff, 2); |
bmp085_regb1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGB2, buff, 2); |
bmp085_regb2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMB, buff, 2); |
bmp085_regmb = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMC, buff, 2); |
bmp085_regmc = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMD, buff, 2); |
bmp085_regmd = ((int)buff[0] <<8 | ((int)buff[1])); |
} |
/* |
* get raw temperature as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawtemperature() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
long ut,x1,x2; |
//read raw temperature |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADTEMPERATURE); |
_delay_ms(5); // min. 4.5ms read Temp delay |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 2); |
ut = ((long)buff[0] << 8 | ((long)buff[1])); //uncompensated temperature value |
//calculate raw temperature |
x1 = ((long)ut - bmp085_regac6) * bmp085_regac5 >> 15; |
x2 = ((long)bmp085_regmc << 11) / (x1 + bmp085_regmd); |
bmp085_rawtemperature = x1 + x2; |
} |
/* |
* get raw pressure as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawpressure() { |
uint8_t buff[3]; |
memset(buff, 0, sizeof(buff)); |
long up,x1,x2,x3,b3,b6,p; |
unsigned long b4,b7; |
#if BMP085_AUTOUPDATETEMP == 1 |
bmp085_getrawtemperature(); |
#endif |
//read raw pressure |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADPRESSURE+(BMP085_MODE << 6)); |
_delay_ms(2 + (3<<BMP085_MODE)); |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 3); |
up = ((((long)buff[0] <<16) | ((long)buff[1] <<8) | ((long)buff[2])) >> (8-BMP085_MODE)); // uncompensated pressure value |
//calculate raw pressure |
b6 = bmp085_rawtemperature - 4000; |
x1 = (bmp085_regb2* (b6 * b6) >> 12) >> 11; |
x2 = (bmp085_regac2 * b6) >> 11; |
x3 = x1 + x2; |
b3 = (((((long)bmp085_regac1) * 4 + x3) << BMP085_MODE) + 2) >> 2; |
x1 = (bmp085_regac3 * b6) >> 13; |
x2 = (bmp085_regb1 * ((b6 * b6) >> 12)) >> 16; |
x3 = ((x1 + x2) + 2) >> 2; |
b4 = (bmp085_regac4 * (uint32_t)(x3 + 32768)) >> 15; |
b7 = ((uint32_t)up - b3) * (50000 >> BMP085_MODE); |
p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; |
x1 = (p >> 8) * (p >> 8); |
x1 = (x1 * 3038) >> 16; |
x2 = (-7357 * p) >> 16; |
bmp085_rawpressure = p + ((x1 + x2 + 3791) >> 4); |
#if BMP085_FILTERPRESSURE == 1 |
bmp085_rawpressure = bmp085_avaragefilter(bmp085_rawpressure); |
#endif |
} |
/* |
* get celsius temperature |
*/ |
double bmp085_gettemperature() { |
bmp085_getrawtemperature(); |
double temperature = ((bmp085_rawtemperature + 8)>>4); |
temperature = temperature /10; |
return temperature; |
} |
/* |
* get pressure |
*/ |
int32_t bmp085_getpressure() { |
bmp085_getrawpressure(); |
return bmp085_rawpressure + BMP085_UNITPAOFFSET; |
} |
/* |
* get altitude |
*/ |
double bmp085_getaltitude() { |
bmp085_getrawpressure(); |
return ((1 - pow(bmp085_rawpressure/(double)101325, 0.1903 )) / 0.0000225577) + BMP085_UNITMOFFSET; |
} |
/* |
* init bmp085 |
*/ |
void bmp085_init() { |
#if BMP085_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
bmp085_getcalibration(); //get calibration data |
bmp085_getrawtemperature(); //update raw temperature, at least the first time |
#if BMP085_FILTERPRESSURE == 1 |
//initialize the avarage filter |
uint8_t i=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
bmp085_getrawpressure(); |
} |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/bmp085.h |
---|
0,0 → 1,74 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- this library is a porting of the bmp085driver 0.4 ardunio library |
http://code.google.com/p/bmp085driver/ |
Notes: |
- to compile with avrgcc you may define -lm flag for some math functions |
*/ |
#ifndef BMP085_H_ |
#define BMP085_H_ |
#include <stdio.h> |
#include <avr/io.h> |
#define BMP085_ADDR (0x77<<1) //0x77 default I2C address |
#define BMP085_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define BMP085_I2CINIT 1 //init i2c |
//registers |
#define BMP085_REGAC1 0xAA |
#define BMP085_REGAC2 0xAC |
#define BMP085_REGAC3 0xAE |
#define BMP085_REGAC4 0xB0 |
#define BMP085_REGAC5 0xB2 |
#define BMP085_REGAC6 0xB4 |
#define BMP085_REGB1 0xB6 |
#define BMP085_REGB2 0xB8 |
#define BMP085_REGMB 0xBA |
#define BMP085_REGMC 0xBC |
#define BMP085_REGMD 0xBE |
#define BMP085_REGCONTROL 0xF4 //control |
#define BMP085_REGCONTROLOUTPUT 0xF6 //output 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB |
#define BMP085_REGREADTEMPERATURE 0x2E //read temperature |
#define BMP085_REGREADPRESSURE 0x34 //read pressure |
//modes |
#define BMP085_MODEULTRALOWPOWER 0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5 |
#define BMP085_MODESTANDARD 1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4 |
#define BMP085_MODEHIGHRES 2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3 |
#define BMP085_MODEULTRAHIGHRES 3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25 |
//autoupdate temperature enabled |
#define BMP085_AUTOUPDATETEMP 1 //autoupdate temperature every read |
//setup parameters |
#define BMP085_MODE BMP085_MODEULTRAHIGHRES //define a mode |
#define BMP085_UNITPAOFFSET 0 //define a unit offset (pa) |
#define BMP085_UNITMOFFSET 0 //define a unit offset (m) |
//avarage filter |
#define BMP085_FILTERPRESSURE 1 //avarage filter for pressure |
//variables |
int bmp085_regac1, bmp085_regac2, bmp085_regac3, bmp085_regb1, bmp085_regb2, bmp085_regmb, bmp085_regmc, bmp085_regmd; |
unsigned int bmp085_regac4, bmp085_regac5, bmp085_regac6; |
long bmp085_rawtemperature, bmp085_rawpressure; |
//functions |
extern void bmp085_init(); |
extern int32_t bmp085_getpressure(); |
extern double bmp085_getaltitude(); |
extern double bmp085_gettemperature(); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/hmc5883l.c |
---|
0,0 → 1,131 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY hmc5883l.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für HMC5883L Compass, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "hmc5883l.h" |
//path to i2c fleury lib |
#include HMC5883L_I2CFLEURYPATH |
static double hmc5883l_scale = 0; |
/* |
* init the hmc5883l |
*/ |
void hmc5883l_init() { |
#if HMC5883L_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
//set scale |
hmc5883l_scale = 0; |
uint8_t regValue = 0x00; |
#if HMC5883L_SCALE == HMC5883L_SCALE088 |
regValue = 0x00; |
hmc5883l_scale = 0.73; |
#elif HMC5883L_SCALE == HMC5883L_SCALE13 |
regValue = 0x01; |
hmc5883l_scale = 0.92; |
#elif HMC5883L_SCALE == HMC5883L_SCALE19 |
regValue = 0x02; |
hmc5883l_scale = 1.22; |
#elif HMC5883L_SCALE == HMC5883L_SCALE25 |
regValue = 0x03; |
hmc5883l_scale = 1.52; |
#elif HMC5883L_SCALE == HMC5883L_SCALE40 |
regValue = 0x04; |
hmc5883l_scale = 2.27; |
#elif HMC5883L_SCALE == HMC5883L_SCALE47 |
regValue = 0x05; |
hmc5883l_scale = 2.56; |
#elif HMC5883L_SCALE == HMC5883L_SCALE56 |
regValue = 0x06; |
hmc5883l_scale = 3.03; |
#elif HMC5883L_SCALE == HMC5883L_SCALE81 |
regValue = 0x07; |
hmc5883l_scale = 4.35; |
#endif |
//setting is in the top 3 bits of the register. |
regValue = regValue << 5; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_CONFREGB); |
i2c_write(regValue); |
i2c_stop(); |
//set measurement mode |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_MODEREG); |
i2c_write(HMC5883L_MEASUREMODE); |
i2c_stop(); |
} |
/* |
* get raw data |
*/ |
void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) { |
uint8_t i = 0; |
uint8_t buff[6]; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_DATAREGBEGIN); |
i2c_stop(); |
i2c_start_wait(HMC5883L_ADDR | I2C_READ); |
for(i=0; i<6; i++) { |
if(i==6-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
*mxraw = (int16_t)((buff[0] << 8) | buff[1]); |
*mzraw = (int16_t)((buff[2] << 8) | buff[3]); |
*myraw = (int16_t)((buff[4] << 8) | buff[5]); |
} |
/* |
* get scaled data |
*/ |
void hmc5883l_getdata(double *mx, double *my, double *mz) { |
int16_t mxraw = 0; |
int16_t myraw = 0; |
int16_t mzraw = 0; |
hmc5883l_getrawdata(&mxraw, &myraw, &mzraw); |
#if HMC5883L_CALIBRATED == 1 |
float mxt = mxraw - HMC5883L_OFFSETX; |
float myt = myraw - HMC5883L_OFFSETY; |
float mzt = mzraw - HMC5883L_OFFSETZ; |
*mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt; |
*my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt; |
*mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt; |
#else |
*mx = mxraw * hmc5883l_scale; |
*my = myraw * hmc5883l_scale; |
*mz = mzraw * hmc5883l_scale; |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/hmc5883l.h |
---|
0,0 → 1,71 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- HMC5883L Triple Axis Magnetometer Arduino Library |
http://bildr.org/2012/02/hmc5883l_arduino/ |
*/ |
#ifndef HMC5883L_H_ |
#define HMC5883L_H_ |
//definitions |
#define HMC5883L_ADDR (0x1E<<1) //device address |
//i2c settings |
#define HMC5883L_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define HMC5883L_I2CINIT 1 //init i2c |
//registers |
#define HMC5883L_CONFREGA 0x00 |
#define HMC5883L_CONFREGB 0x01 |
#define HMC5883L_MODEREG 0x02 |
#define HMC5883L_DATAREGBEGIN 0x03 |
//setup measurement mode |
#define HMC5883L_MEASURECONTINOUS 0x00 |
#define HMC5883L_MEASURESINGLESHOT 0x01 |
#define HMC5883L_MEASUREIDLE 0x03 |
#define HMC5883L_MEASUREMODE HMC5883L_MEASURECONTINOUS |
//setup scale |
#define HMC5883L_SCALE088 1 //0.88 |
#define HMC5883L_SCALE13 2 //1.3 |
#define HMC5883L_SCALE19 3 //1.9 |
#define HMC5883L_SCALE25 4 //2.5 |
#define HMC5883L_SCALE40 5 //4.0 |
#define HMC5883L_SCALE47 6 //4.7 |
#define HMC5883L_SCALE56 7 //5.6 |
#define HMC5883L_SCALE81 8 //8.1 |
#define HMC5883L_SCALE HMC5883L_SCALE13 |
#define HMC5883L_CALIBRATED 1 //enable this if this magn is calibrated |
//calibration values |
#if HMC5883L_CALIBRATED == 1 |
#define HMC5883L_OFFSETX -99.7 |
#define HMC5883L_OFFSETY -154.0 |
#define HMC5883L_OFFSETZ -22.7 |
#define HMC5883L_GAINX1 0.952017 |
#define HMC5883L_GAINX2 0.00195895 |
#define HMC5883L_GAINX3 0.0139661 |
#define HMC5883L_GAINY1 0.00195895 |
#define HMC5883L_GAINY2 0.882824 |
#define HMC5883L_GAINY3 0.00760243 |
#define HMC5883L_GAINZ1 0.0139661 |
#define HMC5883L_GAINZ2 0.00760243 |
#define HMC5883L_GAINZ3 0.995365 |
#endif |
//functions |
extern void hmc5883l_init(); |
extern void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw); |
extern void hmc5883l_getdata(double *mx, double *my, double *mz); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/mpu6050.c |
---|
0,0 → 1,694 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 |
#include <math.h> //include libm |
#endif |
//path to i2c fleury lib |
#include "i2cmaster.h" |
volatile uint8_t buffer[14]; |
/* |
* read bytes from chip register |
*/ |
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data) { |
uint8_t i = 0; |
int8_t count = 0; |
if(length > 0) { |
//request register |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); |
_delay_us(10); |
//read data |
i2c_start(MPU6050_ADDR | I2C_READ); |
for(i=0; i<length; i++) { |
count++; |
if(i==length-1) |
data[i] = i2c_readNak(); |
else |
data[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
return count; |
} |
/* |
* read 1 byte from chip register |
*/ |
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) { |
return mpu6050_readBytes(regAddr, 1, data); |
} |
/* |
* write bytes to chip register |
*/ |
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data) { |
if(length > 0) { |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (uint8_t i = 0; i < length; i++) { |
i2c_write((uint8_t) data[i]); |
} |
i2c_stop(); |
} |
} |
/* |
* write 1 byte to chip register |
*/ |
void mpu6050_writeByte(uint8_t regAddr, uint8_t data) { |
return mpu6050_writeBytes(regAddr, 1, &data); |
} |
/* |
* read bits from chip register |
*/ |
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { |
// 01101001 read byte |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 010 masked |
// -> 010 shifted |
int8_t count = 0; |
if(length > 0) { |
uint8_t b; |
if ((count = mpu6050_readByte(regAddr, &b)) != 0) { |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
b &= mask; |
b >>= (bitStart - length + 1); |
*data = b; |
} |
} |
return count; |
} |
/* |
* read 1 bit from chip register |
*/ |
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data) { |
uint8_t b; |
uint8_t count = mpu6050_readByte(regAddr, &b); |
*data = b & (1 << bitNum); |
return count; |
} |
/* |
* write bit/bits to chip register |
*/ |
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { |
// 010 value to write |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 00011100 mask byte |
// 10101111 original value (sample) |
// 10100011 original & ~mask |
// 10101011 masked | value |
if(length > 0) { |
uint8_t b = 0; |
if (mpu6050_readByte(regAddr, &b) != 0) { //get current data |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
data <<= (bitStart - length + 1); // shift data into correct position |
data &= mask; // zero all non-important bits in data |
b &= ~(mask); // zero all important bits in existing byte |
b |= data; // combine data with existing byte |
mpu6050_writeByte(regAddr, b); |
} |
} |
} |
/* |
* write one bit to chip register |
*/ |
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data) { |
uint8_t b; |
mpu6050_readByte(regAddr, &b); |
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); |
mpu6050_writeByte(regAddr, b); |
} |
#if MPU6050_GETATTITUDE == 2 |
/* |
* write word/words to chip register |
*/ |
void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data) { |
if(length > 0) { |
uint8_t i = 0; |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (i = 0; i < length * 2; i++) { |
i2c_write((uint8_t)(data[i++] >> 8)); // send MSB |
i2c_write((uint8_t)data[i]); // send LSB |
} |
i2c_stop(); |
} |
} |
/* |
* set a chip memory bank |
*/ |
void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank) { |
bank &= 0x1F; |
if (userBank) bank |= 0x20; |
if (prefetchEnabled) bank |= 0x40; |
mpu6050_writeByte(MPU6050_RA_BANK_SEL, bank); |
} |
/* |
* set memory start address |
*/ |
void mpu6050_setMemoryStartAddress(uint8_t address) { |
mpu6050_writeByte(MPU6050_RA_MEM_START_ADDR, address); |
} |
/* |
* read a memory block |
*/ |
void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
for (uint16_t i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
// read the chunk of data as specified |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i); |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
} |
/* |
* write a memory block |
*/ |
uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
uint8_t *verifyBuffer = 0; |
uint8_t *progBuffer = 0; |
uint16_t i; |
uint8_t j; |
if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
for (i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
if (useProgMem) { |
// write the chunk of data as specified |
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
// write the chunk of data as specified |
progBuffer = (uint8_t *)data + i; |
} |
mpu6050_writeBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer); |
// verify data if needed |
if (verify && verifyBuffer) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); |
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { |
free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 0; // uh oh. |
} |
} |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
if (verify) free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* write a dmp configuration set |
*/ |
uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem) { |
uint8_t *progBuffer = 0; |
uint8_t success, special; |
uint16_t i, j; |
if (useProgMem) { |
progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary |
} |
// config set data is a long string of blocks with the following structure: |
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] |
uint8_t bank, offset, length; |
for (i = 0; i < dataSize;) { |
if (useProgMem) { |
bank = pgm_read_byte(data + i++); |
offset = pgm_read_byte(data + i++); |
length = pgm_read_byte(data + i++); |
} else { |
bank = data[i++]; |
offset = data[i++]; |
length = data[i++]; |
} |
// write data or perform special action |
if (length > 0) { |
// regular block of data to write |
if (useProgMem) { |
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); |
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
progBuffer = (uint8_t *)data + i; |
} |
success = mpu6050_writeMemoryBlock(progBuffer, length, bank, offset, 1, 0); |
i += length; |
} else { |
// special instruction |
// NOTE: this kind of behavior (what and when to do certain things) |
// is totally undocumented. This code is in here based on observed |
// behavior only, and exactly why (or even whether) it has to be here |
// is anybody's guess for now. |
if (useProgMem) { |
special = pgm_read_byte(data + i++); |
} else { |
special = data[i++]; |
} |
if (special == 0x01) { |
// enable DMP-related interrupts |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, 1); //setIntZeroMotionEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, 1); //setIntFIFOBufferOverflowEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, 1); //setIntDMPEnabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x32); // single operation |
success = 1; |
} else { |
// unknown special command |
success = 0; |
} |
} |
if (!success) { |
if (useProgMem) free(progBuffer); |
return 0; // uh oh |
} |
} |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* get the fifo count |
*/ |
uint16_t mpu6050_getFIFOCount(void) { |
mpu6050_readBytes(MPU6050_RA_FIFO_COUNTH, 2, (uint8_t *)buffer); |
return (((uint16_t)buffer[0]) << 8) | buffer[1]; |
} |
/* |
* read fifo bytes |
*/ |
void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length) { |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, length, data); |
} |
/* |
* get the interrupt status |
*/ |
uint8_t mpu6050_getIntStatus(void) { |
mpu6050_readByte(MPU6050_RA_INT_STATUS, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* reset fifo |
*/ |
void mpu6050_resetFIFO(void) { |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} |
/* |
* get gyro offset X |
*/ |
int8_t mpu6050_getXGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset X |
*/ |
void mpu6050_setXGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Y |
*/ |
int8_t mpu6050_getYGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Y |
*/ |
void mpu6050_setYGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Z |
*/ |
int8_t mpu6050_getZGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Z |
*/ |
void mpu6050_setZGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
#endif |
/* |
* set sleep disabled |
*/ |
void mpu6050_setSleepDisabled() { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0); |
} |
/* |
* set sleep enabled |
*/ |
void mpu6050_setSleepEnabled(void) { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1); |
} |
/* |
* test connectino to chip |
*/ |
uint8_t mpu6050_testConnection(void) { |
mpu6050_readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, (uint8_t *)buffer); |
if(buffer[0] == 0x34) |
return 1; |
else |
return 0; |
} |
/* |
* initialize the accel and gyro |
*/ |
void mpu6050_init(void) { |
//allow mpu6050 chip clocks to start up |
_delay_ms(100); |
//set sleep disabled |
mpu6050_setSleepDisabled(); |
//wake up delay needed sleep disabled |
_delay_ms(10); |
//set clock source |
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) |
// as the clock reference for improved stability |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set sampe rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz |
//set gyro range |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS); |
//set accel range |
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS); |
#if MPU6050_GETATTITUDE == 1 |
#error "Do not enable timer 0 it is in use elsewhere!" |
//MPU6050_TIMER0INIT |
#endif |
} |
//can not accept many request if we alreay have getattitude requests |
/* |
* get raw data |
*/ |
void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
*ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
*ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
*az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
*gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
*gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
*gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
} |
/* |
* get raw data converted to g and deg/sec values |
*/ |
void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
*axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
*ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
*azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
*gxds = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN; |
*gyds = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN; |
*gzds = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN; |
#else |
*axg = (double)(ax)/MPU6050_AGAIN; |
*ayg = (double)(ay)/MPU6050_AGAIN; |
*azg = (double)(az)/MPU6050_AGAIN; |
*gxds = (double)(gx)/MPU6050_GGAIN; |
*gyds = (double)(gy)/MPU6050_GGAIN; |
*gzds = (double)(gz)/MPU6050_GGAIN; |
#endif |
} |
#if MPU6050_GETATTITUDE == 1 |
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; |
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
/* |
* Mahony update function (for 6DOF) |
*/ |
void mpu6050_mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) { |
float norm; |
float halfvx, halfvy, halfvz; |
float halfex, halfey, halfez; |
float qa, qb, qc; |
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
// Normalise accelerometer measurement |
norm = sqrt(ax * ax + ay * ay + az * az); |
ax /= norm; |
ay /= norm; |
az /= norm; |
// Estimated direction of gravity and vector perpendicular to magnetic flux |
halfvx = q1 * q3 - q0 * q2; |
halfvy = q0 * q1 + q2 * q3; |
halfvz = q0 * q0 - 0.5f + q3 * q3; |
// Error is sum of cross product between estimated and measured direction of gravity |
halfex = (ay * halfvz - az * halfvy); |
halfey = (az * halfvx - ax * halfvz); |
halfez = (ax * halfvy - ay * halfvx); |
// Compute and apply integral feedback if enabled |
if(mpu6050_mahonytwoKiDef > 0.0f) { |
integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq); // integral error scaled by Ki |
integralFBy += mpu6050_mahonytwoKiDef * halfey * (1.0f / mpu6050_mahonysampleFreq); |
integralFBz += mpu6050_mahonytwoKiDef * halfez * (1.0f / mpu6050_mahonysampleFreq); |
gx += integralFBx; // apply integral feedback |
gy += integralFBy; |
gz += integralFBz; |
} else { |
integralFBx = 0.0f; // prevent integral windup |
integralFBy = 0.0f; |
integralFBz = 0.0f; |
} |
// Apply proportional feedback |
gx += mpu6050_mahonytwoKpDef * halfex; |
gy += mpu6050_mahonytwoKpDef * halfey; |
gz += mpu6050_mahonytwoKpDef * halfez; |
} |
// Integrate rate of change of quaternion |
gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); // pre-multiply common factors |
gy *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
gz *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
qa = q0; |
qb = q1; |
qc = q2; |
q0 += (-qb * gx - qc * gy - q3 * gz); |
q1 += (qa * gx + qc * gz - q3 * gy); |
q2 += (qa * gy - qb * gz + q3 * gx); |
q3 += (qa * gz + qb * gy - qc * gx); |
// Normalise quaternion |
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
q0 /= norm; |
q1 /= norm; |
q2 /= norm; |
q3 /= norm; |
} |
/* |
* update quaternion |
*/ |
void mpu6050_updateQuaternion(void) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
double axg = 0; |
double ayg = 0; |
double azg = 0; |
double gxrs = 0; |
double gyrs = 0; |
double gzrs = 0; |
//get raw data |
while(1) { |
mpu6050_readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, (uint8_t *)buffer); |
if(buffer[0]) |
break; |
_delay_us(10); |
} |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
gxrs = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN*0.01745329; //degree to radians |
#else |
axg = (double)(ax)/MPU6050_AGAIN; |
ayg = (double)(ay)/MPU6050_AGAIN; |
azg = (double)(az)/MPU6050_AGAIN; |
gxrs = (double)(gx)/MPU6050_GGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy)/MPU6050_GGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz)/MPU6050_GGAIN*0.01745329; //degree to radians |
#endif |
//compute data |
mpu6050_mahonyUpdate(gxrs, gyrs, gzrs, axg, ayg, azg); |
} |
/* |
* update timer for attitude |
*/ |
ISR(TIMER0_OVF_vect) { |
mpu6050_updateQuaternion(); |
} |
/* |
* get quaternion |
*/ |
void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz) { |
*qw = q0; |
*qx = q1; |
*qy = q2; |
*qz = q3; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1); |
*pitch = -asin(2*q1*q3 + 2*q0*q2); |
*roll = atan2(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 - 1); |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/mpu6050.h |
---|
0,0 → 1,174 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- most of the code is a port of the arduino mpu6050 library by Jeff Rowberg |
https://github.com/jrowberg/i2cdevlib |
- Mahony complementary filter for attitude estimation |
http://www.x-io.co.uk |
*/ |
#ifndef MPU6050_H_ |
#define MPU6050_H_ |
#include <avr/io.h> |
#include "mpu6050registers.h" |
#ifdef __cplusplus |
extern "C" { |
#endif |
//i2c settings |
#define MPU6050_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define MPU6050_I2CINIT 0 //init i2c |
//definitions |
#define MPU6050_ADDR (0x68 <<1) //device address - 0x68 pin low (GND), 0x69 pin high (VCC) |
//enable the getattitude functions |
//because we do not have a magnetometer, we have to start the chip always in the same position |
//then to obtain your object attitude you have to apply the aerospace sequence |
//0 disabled |
//1 mahony filter |
//2 dmp chip processor |
#define MPU6050_GETATTITUDE 2 |
//definitions for raw data |
//gyro and acc scale |
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 |
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 |
#define MPU6050_GYRO_LSB_250 131.0 |
#define MPU6050_GYRO_LSB_500 65.5 |
#define MPU6050_GYRO_LSB_1000 32.8 |
#define MPU6050_GYRO_LSB_2000 16.4 |
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000 |
#endif |
#define MPU6050_ACCEL_LSB_2 16384.0 |
#define MPU6050_ACCEL_LSB_4 8192.0 |
#define MPU6050_ACCEL_LSB_8 4096.0 |
#define MPU6050_ACCEL_LSB_16 2048.0 |
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16 |
#endif |
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
#define MPU6050_AXOFFSET 0 |
#define MPU6050_AYOFFSET 0 |
#define MPU6050_AZOFFSET 0 |
#define MPU6050_AXGAIN 16384.0 |
#define MPU6050_AYGAIN 16384.0 |
#define MPU6050_AZGAIN 16384.0 |
#define MPU6050_GXOFFSET -42 |
#define MPU6050_GYOFFSET 9 |
#define MPU6050_GZOFFSET -29 |
#define MPU6050_GXGAIN 16.4 |
#define MPU6050_GYGAIN 16.4 |
#define MPU6050_GZGAIN 16.4 |
#endif |
//definitions for attitude 1 function estimation |
#if MPU6050_GETATTITUDE == 1 |
#error "GETATTITUDE == 1 is not supported!" |
//setup timer0 overflow event and define madgwickAHRSsampleFreq equal to timer0 frequency |
//timerfreq = (FCPU / prescaler) / timerscale |
// timerscale 8-bit = 256 |
// es. 61 = (16000000 / 1024) / 256 |
#define MPU6050_TIMER0INIT TCCR0B |=(1<<CS02)|(1<<CS00); TIMSK0 |=(1<<TOIE0); |
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz |
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain |
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain |
#endif |
#if MPU6050_GETATTITUDE == 2 |
//dmp definitions |
//packet size |
#define MPU6050_DMP_dmpPacketSize 42 |
//define INT0 rise edge interrupt |
#define MPU6050_DMP_INT0SETUP EICRA |= (1<<ISC01) | (1<<ISC00) |
//define enable and disable INT0 rise edge interrupt |
#define MPU6050_DMP_INT0DISABLE EIMSK &= ~(1<<INT0) |
#define MPU6050_DMP_INT0ENABLE EIMSK |= (1<<INT0) |
extern volatile uint8_t mpu6050_mpuInterrupt; |
#endif |
//functions |
extern void mpu6050_init(void); |
extern uint8_t mpu6050_testConnection(void); |
#if MPU6050_GETATTITUDE == 0 |
extern void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); |
extern void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds); |
#endif |
extern void mpu6050_setSleepDisabled(void); |
extern void mpu6050_setSleepEnabled(void); |
extern int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data); |
extern void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data); |
extern void mpu6050_writeByte(uint8_t regAddr, uint8_t data); |
extern int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data); |
extern void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); |
extern void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data); |
#if MPU6050_GETATTITUDE == 1 |
extern void mpu6050_updateQuaternion(void); |
extern void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double *pitch, double *roll, double *yaw); |
#endif |
#if MPU6050_GETATTITUDE == 2 |
extern void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data); |
extern void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank); |
extern void mpu6050_setMemoryStartAddress(uint8_t address); |
extern void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address); |
extern uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem); |
extern uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem); |
extern uint16_t mpu6050_getFIFOCount(void); |
extern void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length); |
extern uint8_t mpu6050_getIntStatus(void); |
extern void mpu6050_resetFIFO(); |
extern int8_t mpu6050_getXGyroOffset(void); |
extern void mpu6050_setXGyroOffset(int8_t offset); |
extern int8_t mpu6050_getYGyroOffset(void); |
extern void mpu6050_setYGyroOffset(int8_t offset); |
extern int8_t mpu6050_getZGyroOffset(void); |
extern void mpu6050_setZGyroOffset(int8_t offset); |
//base dmp |
extern uint8_t mpu6050_dmpInitialize(void); |
extern void mpu6050_dmpEnable(void); |
extern void mpu6050_dmpDisable(void); |
extern void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw); |
extern uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz); |
#endif |
#ifdef __cplusplus |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/mpu6050dmp6.c |
---|
0,0 → 1,495 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050dmp6.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, DMP-Routinen für IMU, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
/* |
* This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
*/ |
//to enable get roll, pitch and yaw function we include the math function |
//if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
//${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 2 |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include <math.h> //include libm |
#define MPU6050_DMP_CODE_SIZE 1929 |
#define MPU6050_DMP_CONFIG_SIZE 192 |
#define MPU6050_DMP_UPDATES_SIZE 47 |
volatile uint8_t mpu6050_mpuInterrupt = 0; |
uint8_t *dmpPacketBuffer; |
uint16_t mpu6050_fifoCount = 0; |
uint8_t mpu6050_mpuIntStatus = 0; |
uint8_t mpu6050_fifoBuffer[64]; |
/* ================================================================================================ * |
| Default MotionApps v2.0 42-byte FIFO packet structure: | |
| | |
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| | |
| [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
* ================================================================================================ */ |
// this block of memory gets written to the MPU on start-up, and it seems |
// to be volatile memory, so it has to be done each time (it only takes ~1 |
// second though) |
const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
// bank 0, 256 bytes |
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
// bank 1, 256 bytes |
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
// bank 2, 256 bytes |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
// bank 3, 256 bytes |
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
// bank 4, 256 bytes |
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
// bank 5, 256 bytes |
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
// bank 6, 256 bytes |
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
// bank 7, 138 bytes (remainder) |
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
}; |
const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
// BANK OFFSET LENGTH [DATA] |
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
// SPECIAL 0x01 = enable interrupts |
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate |
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
// It is important to make sure the host processor can keep up with reading and processing |
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
}; |
const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
0x01, 0xB2, 0x02, 0xFF, 0xFF, |
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
0x01, 0x6A, 0x02, 0x06, 0x00, |
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
0x01, 0x62, 0x02, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
}; |
/* |
* initialize mpu6050 dmp |
*/ |
uint8_t mpu6050_dmpInitialize() { |
//setup interrupt |
MPU6050_DMP_INT0SETUP; |
//reset |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
_delay_ms(30);//wait after reset |
//disable sleep mode |
mpu6050_setSleepDisabled(); |
//set memorybank to 0 |
mpu6050_setMemoryBank(0, 0, 0); |
//get X/Y/Z gyro offsets |
int8_t xgOffset = mpu6050_getXGyroOffset(); |
int8_t ygOffset = mpu6050_getYGyroOffset(); |
int8_t zgOffset = mpu6050_getZGyroOffset(); |
//setting slave 0 address to 0x7F |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
//disabling I2C Master mode |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
//setting slave 0 address to 0x68 (self) |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
//resetting I2C Master control |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
_delay_ms(20); |
//load DMP code into memory banks |
if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
//set clock source |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
//set DMP and FIFO_OFLOW interrupts enabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
//set sample rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
//set external frame sync to TEMP_OUT_L[0] |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set gyro sensitivity to +/- 2000 deg/sec |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
//set DMP configuration bytes (function unknown) |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
//clear OTP Bank flag |
mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
//set X/Y/Z gyro offsets to previous values |
//xgOffset = 0; |
//ygOffset = 0; |
zgOffset = 90; |
mpu6050_setXGyroOffset(xgOffset); |
mpu6050_setYGyroOffset(ygOffset); |
mpu6050_setZGyroOffset(zgOffset); |
//set X/Y/Z gyro user offsets to zero |
mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
//writing final memory update 1/7 (function unknown) |
uint8_t dmpUpdate[16], j; |
uint16_t pos = 0; |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 2/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reset FIFO |
mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
//reading FIFO count |
uint8_t fifoCount = mpu6050_getFIFOCount(); |
uint8_t fifoBuffer[128]; |
//current FIFO count |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
//setting motion detection threshold to 2 |
mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
//setting zero-motion detection threshold to 156 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
//setting motion detection duration to 80 |
mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
//setting zero-motion detection duration to 0 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
//reset FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
//enabling FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
//enabling DMP |
mpu6050_dmpEnable(); |
//resetting DMP |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//writing final memory update 3/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 4/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 5/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reading FIFO data...")); |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//reading final memory update 6/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//reading FIFO data |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//writing final memory update 7/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//disabling DMP (you turn it on later) |
mpu6050_dmpDisable(); |
//resetting FIFO and clearing INT status one last time |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} else { |
return 2; // configuration block loading failed |
} |
} else { |
return 1; // main binary block loading failed |
} |
return 0; // success |
} |
/* |
* enable dmp |
*/ |
void mpu6050_dmpEnable() { |
MPU6050_DMP_INT0ENABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
} |
/* |
* disable dmp |
*/ |
void mpu6050_dmpDisable() { |
MPU6050_DMP_INT0DISABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
} |
/* |
* get quaternion from packet |
*/ |
void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
if (packet == 0) packet = dmpPacketBuffer; |
*qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
*qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
*qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
*qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
*pitch = -asin(2*qx*qz + 2*qw*qy); |
*roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
} |
/* |
* get quaternion and wait |
*/ |
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
//reset interrupt |
mpu6050_mpuInterrupt = 0; |
//check for overflow |
mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
//reset |
mpu6050_resetFIFO(); |
} else if (mpu6050_mpuIntStatus & 0x02) { |
//wait for correct available data length, should be a VERY short wait |
while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
//read a packet from FIFO |
mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
//get quaternion |
mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
return 1; |
} |
return 0; |
} |
/* |
* on interrupt set data availabe |
*/ |
ISR (INT0_vect) { |
mpu6050_mpuInterrupt = 1; |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/mpu6050registers.h |
---|
0,0 → 1,365 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#ifndef MPU6050REGISTERS_H_ |
#define MPU6050REGISTERS_H_ |
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN |
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN |
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN |
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS |
#define MPU6050_RA_XA_OFFS_L_TC 0x07 |
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS |
#define MPU6050_RA_YA_OFFS_L_TC 0x09 |
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS |
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B |
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR |
#define MPU6050_RA_XG_OFFS_USRL 0x14 |
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR |
#define MPU6050_RA_YG_OFFS_USRL 0x16 |
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR |
#define MPU6050_RA_ZG_OFFS_USRL 0x18 |
#define MPU6050_RA_SMPLRT_DIV 0x19 |
#define MPU6050_RA_CONFIG 0x1A |
#define MPU6050_RA_GYRO_CONFIG 0x1B |
#define MPU6050_RA_ACCEL_CONFIG 0x1C |
#define MPU6050_RA_FF_THR 0x1D |
#define MPU6050_RA_FF_DUR 0x1E |
#define MPU6050_RA_MOT_THR 0x1F |
#define MPU6050_RA_MOT_DUR 0x20 |
#define MPU6050_RA_ZRMOT_THR 0x21 |
#define MPU6050_RA_ZRMOT_DUR 0x22 |
#define MPU6050_RA_FIFO_EN 0x23 |
#define MPU6050_RA_I2C_MST_CTRL 0x24 |
#define MPU6050_RA_I2C_SLV0_ADDR 0x25 |
#define MPU6050_RA_I2C_SLV0_REG 0x26 |
#define MPU6050_RA_I2C_SLV0_CTRL 0x27 |
#define MPU6050_RA_I2C_SLV1_ADDR 0x28 |
#define MPU6050_RA_I2C_SLV1_REG 0x29 |
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A |
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B |
#define MPU6050_RA_I2C_SLV2_REG 0x2C |
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D |
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E |
#define MPU6050_RA_I2C_SLV3_REG 0x2F |
#define MPU6050_RA_I2C_SLV3_CTRL 0x30 |
#define MPU6050_RA_I2C_SLV4_ADDR 0x31 |
#define MPU6050_RA_I2C_SLV4_REG 0x32 |
#define MPU6050_RA_I2C_SLV4_DO 0x33 |
#define MPU6050_RA_I2C_SLV4_CTRL 0x34 |
#define MPU6050_RA_I2C_SLV4_DI 0x35 |
#define MPU6050_RA_I2C_MST_STATUS 0x36 |
#define MPU6050_RA_INT_PIN_CFG 0x37 |
#define MPU6050_RA_INT_ENABLE 0x38 |
#define MPU6050_RA_DMP_INT_STATUS 0x39 |
#define MPU6050_RA_INT_STATUS 0x3A |
#define MPU6050_RA_ACCEL_XOUT_H 0x3B |
#define MPU6050_RA_ACCEL_XOUT_L 0x3C |
#define MPU6050_RA_ACCEL_YOUT_H 0x3D |
#define MPU6050_RA_ACCEL_YOUT_L 0x3E |
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F |
#define MPU6050_RA_ACCEL_ZOUT_L 0x40 |
#define MPU6050_RA_TEMP_OUT_H 0x41 |
#define MPU6050_RA_TEMP_OUT_L 0x42 |
#define MPU6050_RA_GYRO_XOUT_H 0x43 |
#define MPU6050_RA_GYRO_XOUT_L 0x44 |
#define MPU6050_RA_GYRO_YOUT_H 0x45 |
#define MPU6050_RA_GYRO_YOUT_L 0x46 |
#define MPU6050_RA_GYRO_ZOUT_H 0x47 |
#define MPU6050_RA_GYRO_ZOUT_L 0x48 |
#define MPU6050_RA_EXT_SENS_DATA_00 0x49 |
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A |
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B |
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C |
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D |
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E |
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F |
#define MPU6050_RA_EXT_SENS_DATA_07 0x50 |
#define MPU6050_RA_EXT_SENS_DATA_08 0x51 |
#define MPU6050_RA_EXT_SENS_DATA_09 0x52 |
#define MPU6050_RA_EXT_SENS_DATA_10 0x53 |
#define MPU6050_RA_EXT_SENS_DATA_11 0x54 |
#define MPU6050_RA_EXT_SENS_DATA_12 0x55 |
#define MPU6050_RA_EXT_SENS_DATA_13 0x56 |
#define MPU6050_RA_EXT_SENS_DATA_14 0x57 |
#define MPU6050_RA_EXT_SENS_DATA_15 0x58 |
#define MPU6050_RA_EXT_SENS_DATA_16 0x59 |
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A |
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B |
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C |
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D |
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E |
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F |
#define MPU6050_RA_EXT_SENS_DATA_23 0x60 |
#define MPU6050_RA_MOT_DETECT_STATUS 0x61 |
#define MPU6050_RA_I2C_SLV0_DO 0x63 |
#define MPU6050_RA_I2C_SLV1_DO 0x64 |
#define MPU6050_RA_I2C_SLV2_DO 0x65 |
#define MPU6050_RA_I2C_SLV3_DO 0x66 |
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 |
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 |
#define MPU6050_RA_MOT_DETECT_CTRL 0x69 |
#define MPU6050_RA_USER_CTRL 0x6A |
#define MPU6050_RA_PWR_MGMT_1 0x6B |
#define MPU6050_RA_PWR_MGMT_2 0x6C |
#define MPU6050_RA_BANK_SEL 0x6D |
#define MPU6050_RA_MEM_START_ADDR 0x6E |
#define MPU6050_RA_MEM_R_W 0x6F |
#define MPU6050_RA_DMP_CFG_1 0x70 |
#define MPU6050_RA_DMP_CFG_2 0x71 |
#define MPU6050_RA_FIFO_COUNTH 0x72 |
#define MPU6050_RA_FIFO_COUNTL 0x73 |
#define MPU6050_RA_FIFO_R_W 0x74 |
#define MPU6050_RA_WHO_AM_I 0x75 |
#define MPU6050_TC_PWR_MODE_BIT 7 |
#define MPU6050_TC_OFFSET_BIT 6 |
#define MPU6050_TC_OFFSET_LENGTH 6 |
#define MPU6050_TC_OTP_BNK_VLD_BIT 0 |
#define MPU6050_VDDIO_LEVEL_VLOGIC 0 |
#define MPU6050_VDDIO_LEVEL_VDD 1 |
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 |
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 |
#define MPU6050_CFG_DLPF_CFG_BIT 2 |
#define MPU6050_CFG_DLPF_CFG_LENGTH 3 |
#define MPU6050_EXT_SYNC_DISABLED 0x0 |
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 |
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 |
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 |
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 |
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 |
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 |
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 |
#define MPU6050_DLPF_BW_256 0x00 |
#define MPU6050_DLPF_BW_188 0x01 |
#define MPU6050_DLPF_BW_98 0x02 |
#define MPU6050_DLPF_BW_42 0x03 |
#define MPU6050_DLPF_BW_20 0x04 |
#define MPU6050_DLPF_BW_10 0x05 |
#define MPU6050_DLPF_BW_5 0x06 |
#define MPU6050_GCONFIG_FS_SEL_BIT 4 |
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 |
#define MPU6050_GYRO_FS_250 0x00 |
#define MPU6050_GYRO_FS_500 0x01 |
#define MPU6050_GYRO_FS_1000 0x02 |
#define MPU6050_GYRO_FS_2000 0x03 |
#define MPU6050_ACONFIG_XA_ST_BIT 7 |
#define MPU6050_ACONFIG_YA_ST_BIT 6 |
#define MPU6050_ACONFIG_ZA_ST_BIT 5 |
#define MPU6050_ACONFIG_AFS_SEL_BIT 4 |
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 |
#define MPU6050_ACCEL_FS_2 0x00 |
#define MPU6050_ACCEL_FS_4 0x01 |
#define MPU6050_ACCEL_FS_8 0x02 |
#define MPU6050_ACCEL_FS_16 0x03 |
#define MPU6050_DHPF_RESET 0x00 |
#define MPU6050_DHPF_5 0x01 |
#define MPU6050_DHPF_2P5 0x02 |
#define MPU6050_DHPF_1P25 0x03 |
#define MPU6050_DHPF_0P63 0x04 |
#define MPU6050_DHPF_HOLD 0x07 |
#define MPU6050_TEMP_FIFO_EN_BIT 7 |
#define MPU6050_XG_FIFO_EN_BIT 6 |
#define MPU6050_YG_FIFO_EN_BIT 5 |
#define MPU6050_ZG_FIFO_EN_BIT 4 |
#define MPU6050_ACCEL_FIFO_EN_BIT 3 |
#define MPU6050_SLV2_FIFO_EN_BIT 2 |
#define MPU6050_SLV1_FIFO_EN_BIT 1 |
#define MPU6050_SLV0_FIFO_EN_BIT 0 |
#define MPU6050_MULT_MST_EN_BIT 7 |
#define MPU6050_WAIT_FOR_ES_BIT 6 |
#define MPU6050_SLV_3_FIFO_EN_BIT 5 |
#define MPU6050_I2C_MST_P_NSR_BIT 4 |
#define MPU6050_I2C_MST_CLK_BIT 3 |
#define MPU6050_I2C_MST_CLK_LENGTH 4 |
#define MPU6050_CLOCK_DIV_348 0x0 |
#define MPU6050_CLOCK_DIV_333 0x1 |
#define MPU6050_CLOCK_DIV_320 0x2 |
#define MPU6050_CLOCK_DIV_308 0x3 |
#define MPU6050_CLOCK_DIV_296 0x4 |
#define MPU6050_CLOCK_DIV_286 0x5 |
#define MPU6050_CLOCK_DIV_276 0x6 |
#define MPU6050_CLOCK_DIV_267 0x7 |
#define MPU6050_CLOCK_DIV_258 0x8 |
#define MPU6050_CLOCK_DIV_500 0x9 |
#define MPU6050_CLOCK_DIV_471 0xA |
#define MPU6050_CLOCK_DIV_444 0xB |
#define MPU6050_CLOCK_DIV_421 0xC |
#define MPU6050_CLOCK_DIV_400 0xD |
#define MPU6050_CLOCK_DIV_381 0xE |
#define MPU6050_CLOCK_DIV_364 0xF |
#define MPU6050_I2C_SLV_RW_BIT 7 |
#define MPU6050_I2C_SLV_ADDR_BIT 6 |
#define MPU6050_I2C_SLV_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV_EN_BIT 7 |
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 |
#define MPU6050_I2C_SLV_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV_GRP_BIT 4 |
#define MPU6050_I2C_SLV_LEN_BIT 3 |
#define MPU6050_I2C_SLV_LEN_LENGTH 4 |
#define MPU6050_I2C_SLV4_RW_BIT 7 |
#define MPU6050_I2C_SLV4_ADDR_BIT 6 |
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV4_EN_BIT 7 |
#define MPU6050_I2C_SLV4_INT_EN_BIT 6 |
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 |
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 |
#define MPU6050_MST_PASS_THROUGH_BIT 7 |
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 |
#define MPU6050_MST_I2C_LOST_ARB_BIT 5 |
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 |
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 |
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 |
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 |
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 |
#define MPU6050_INTCFG_INT_LEVEL_BIT 7 |
#define MPU6050_INTCFG_INT_OPEN_BIT 6 |
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 |
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 |
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 |
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 |
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 |
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 |
#define MPU6050_INTMODE_ACTIVEHIGH 0x00 |
#define MPU6050_INTMODE_ACTIVELOW 0x01 |
#define MPU6050_INTDRV_PUSHPULL 0x00 |
#define MPU6050_INTDRV_OPENDRAIN 0x01 |
#define MPU6050_INTLATCH_50USPULSE 0x00 |
#define MPU6050_INTLATCH_WAITCLEAR 0x01 |
#define MPU6050_INTCLEAR_STATUSREAD 0x00 |
#define MPU6050_INTCLEAR_ANYREAD 0x01 |
#define MPU6050_INTERRUPT_FF_BIT 7 |
#define MPU6050_INTERRUPT_MOT_BIT 6 |
#define MPU6050_INTERRUPT_ZMOT_BIT 5 |
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 |
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 |
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 |
#define MPU6050_INTERRUPT_DMP_INT_BIT 1 |
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 |
// TODO: figure out what these actually do |
// UMPL source code is not very obivous |
#define MPU6050_DMPINT_5_BIT 5 |
#define MPU6050_DMPINT_4_BIT 4 |
#define MPU6050_DMPINT_3_BIT 3 |
#define MPU6050_DMPINT_2_BIT 2 |
#define MPU6050_DMPINT_1_BIT 1 |
#define MPU6050_DMPINT_0_BIT 0 |
#define MPU6050_MOTION_MOT_XNEG_BIT 7 |
#define MPU6050_MOTION_MOT_XPOS_BIT 6 |
#define MPU6050_MOTION_MOT_YNEG_BIT 5 |
#define MPU6050_MOTION_MOT_YPOS_BIT 4 |
#define MPU6050_MOTION_MOT_ZNEG_BIT 3 |
#define MPU6050_MOTION_MOT_ZPOS_BIT 2 |
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 |
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 |
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 |
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 |
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 |
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 |
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 |
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 |
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 |
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 |
#define MPU6050_DETECT_FF_COUNT_BIT 3 |
#define MPU6050_DETECT_FF_COUNT_LENGTH 2 |
#define MPU6050_DETECT_MOT_COUNT_BIT 1 |
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 |
#define MPU6050_DETECT_DECREMENT_RESET 0x0 |
#define MPU6050_DETECT_DECREMENT_1 0x1 |
#define MPU6050_DETECT_DECREMENT_2 0x2 |
#define MPU6050_DETECT_DECREMENT_4 0x3 |
#define MPU6050_USERCTRL_DMP_EN_BIT 7 |
#define MPU6050_USERCTRL_FIFO_EN_BIT 6 |
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 |
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 |
#define MPU6050_USERCTRL_DMP_RESET_BIT 3 |
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 |
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 |
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 |
#define MPU6050_PWR1_DEVICE_RESET_BIT 7 |
#define MPU6050_PWR1_SLEEP_BIT 6 |
#define MPU6050_PWR1_CYCLE_BIT 5 |
#define MPU6050_PWR1_TEMP_DIS_BIT 3 |
#define MPU6050_PWR1_CLKSEL_BIT 2 |
#define MPU6050_PWR1_CLKSEL_LENGTH 3 |
#define MPU6050_CLOCK_INTERNAL 0x00 |
#define MPU6050_CLOCK_PLL_XGYRO 0x01 |
#define MPU6050_CLOCK_PLL_YGYRO 0x02 |
#define MPU6050_CLOCK_PLL_ZGYRO 0x03 |
#define MPU6050_CLOCK_PLL_EXT32K 0x04 |
#define MPU6050_CLOCK_PLL_EXT19M 0x05 |
#define MPU6050_CLOCK_KEEP_RESET 0x07 |
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 |
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 |
#define MPU6050_PWR2_STBY_XA_BIT 5 |
#define MPU6050_PWR2_STBY_YA_BIT 4 |
#define MPU6050_PWR2_STBY_ZA_BIT 3 |
#define MPU6050_PWR2_STBY_XG_BIT 2 |
#define MPU6050_PWR2_STBY_YG_BIT 1 |
#define MPU6050_PWR2_STBY_ZG_BIT 0 |
#define MPU6050_WAKE_FREQ_1P25 0x0 |
#define MPU6050_WAKE_FREQ_2P5 0x1 |
#define MPU6050_WAKE_FREQ_5 0x2 |
#define MPU6050_WAKE_FREQ_10 0x3 |
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 |
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 |
#define MPU6050_BANKSEL_MEM_SEL_BIT 4 |
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 |
#define MPU6050_WHO_AM_I_BIT 6 |
#define MPU6050_WHO_AM_I_LENGTH 6 |
#define MPU6050_DMP_MEMORY_BANKS 8 |
#define MPU6050_DMP_MEMORY_BANK_SIZE 256 |
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/sensor.c |
---|
0,0 → 1,30 |
/* |
* sensor.c |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY sensor.c |
//# |
//# 19.09.2015 cebra |
//# - add: Routinen für das GY-87 Sensorboard zur Berechnung der Kompasswerte etc. |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include "mpu6050.h" |
#include "sensor.h" |
void Init_Sensors(void) |
{ |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF/sensor.h |
---|
0,0 → 1,15 |
/* |
* sensor.h |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
#ifndef SENSOR_H_ |
#define SENSOR_H_ |
#endif /* 10DOF_SENSOR_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/10DOF |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/HAL_HW3_9.c |
---|
0,0 → 1,562 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.c |
//# |
//# |
//# 10.11.2014 cebra |
//# - chg: Displaybeleuchtung aktiv einschalten beim Start |
//# |
//# 14.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf PKT_TitlePKTlipo(false) da eine |
//# richtige LiPo-Anzeige im Startbildschirm zu diesem Zeitpunkt noch |
//# nicht moeglich ist |
//# - chg: InitHWPorts() - bt_init() umgestellt auf BTM222_Initalize() |
//# |
//# 11.06.2014 OG |
//# - fix: InitHWPorts() einstellen der Sprache geht jetzt wieder |
//# |
//# 10.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - chg: InitHWPorts() - Aufruf von InitWi232() geaendert |
//# |
//# 28.05.2014 OG |
//# - chg: InitHWPorts() - Bearbeiten der Sprache auskommentiert weil sich |
//# Edit_generic() geaendert hat und notwendige Texte aus einem Menue |
//# bezieht welches hier in der HAL nicht vorhanden ist |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 24.03.2014 OG |
//# - chg: InitHWPorts(): den Begriff "WiFly" bzw. "WiFly Patch" geaendert |
//# zu "SV2 Patch" |
//# |
//# 21.02.2014 OG |
//# - chg: InitHWPorts() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: InitHWPorts() Anzeige vom Startscreen ueberarbeitet |
//# - chg: Code-Formatierung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING vor Servo_init |
//# |
//# 07.07.2013 OG |
//# - add: DEBUG_SV2_EXTENSION in InitHWPorts() bzgl. Anzeige von "A:.. B:.." usw. |
//# |
//# 26.06.2013 OG |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 26.06.2013 Cebra |
//# - add: Modulumschaltung für WiFlypatch umgebaut |
//# - chg: Wartezeit nach Einschalten von BT auf 3 Sekunden verlängert |
//# |
//# 22.06.2013 cebra |
//# - chg: Fehler in CheckPCB führte zu fehlerhafter Lipomessung, alle Pins von Port A waren auf Eingang |
//# |
//# 13.06.2013 cebra |
//# - chg: Displaybeleuchtung ohne Helligkeitssteuerung |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 28.04.2013 OG |
//# - add: Unterstuetzung von define USE_FONT_BIG (Layoutanpassung) |
//# |
//# 12.04.2013 Cebra |
//# - chg: Fehler bei der Umschaltung Wi232/Kabelverbindung beim Start des PKT |
//########################################################################### |
#ifndef HAL_HW3_9_C_ |
#define HAL_HW3_9_C_ |
#include "cpu.h" |
#include <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include "main.h" |
//#if defined HWVERSION3_9 |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "uart/uart1.h" |
#include "timer/timer.h" |
#include "wi232/Wi232.h" |
//#include "i2cmaster.h" |
#include "bluetooth/bluetooth.h" |
#include "bluetooth/error.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "pkt/pkt.h" |
#include "setup/setup.h" |
#include "osd/osd.h" |
#include "sound/pwmsine8bit.h" |
#include "tracking/ng_servo.h" |
volatile uint8_t USBBT; |
volatile uint8_t U02SV2; |
volatile uint8_t PCB_Version; |
volatile uint8_t PCB_SV2RxTxPatch; |
uint8_t PortA; |
uint8_t PortB; |
uint8_t PortC; |
uint8_t PortD; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void CheckPCB(void) |
{ |
// |
DDRA = 0x00; |
DDRB = 0x00; |
DDRC = 0x00; |
DDRD = 0x00; |
PortA = PINA; |
PortB = PINB; |
PortC = PINC; |
PortD = PIND; |
DDRC = 0x00; |
PORTC &= ~(1<<PORTC7); // Pullup setzen |
if (PINC & (1<<PINC7) ) PCB_Version = PKT39m; else PCB_Version = PKT39x; |
PORTC &= ~(1<<PORTC0); // Pullup setzen |
if (PINC & (1<<PINC0) ) PCB_SV2RxTxPatch = false; else PCB_SV2RxTxPatch = true; |
PORTC |= (1<<PORTC0); // Pullup aus |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void InitHWPorts( void ) // Initialisierung der Hardware für die jeweilige Leiterplattenversion |
{ |
int8_t yoffs; |
CheckPCB(); // Version der Leiterplatte prüfen |
if( PCB_SV2RxTxPatch ) |
{ |
DDRC |= (1<<DDC0)|(1<<DDC1); // Ausgang: C0 Reset BT, C1 Reset WiFly |
PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
} |
PORTA |= (1<<PORTA4)|(1<<PORTA5)|(1<<PORTA6)|(1<<PORTA7); // Enable Pull Up for the 4 keys |
DDRA &= ~(1<<DDA4); // Eingang: A4 auf Low setzen (Power On) |
if( Config.Lipomessung == true ) |
{ |
DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
} |
DDRB = 0xFF; // alles Ausgaenge |
PORTC |= (1<<PORTC4)|(1<<PORTC7); // Enable Pull Up for LBO + Summer |
DDRC |= (1<<DDC2)|(1<<DDC3)|(1<<DDC5)|(1<<DDC6|(1<<DDC7)); // Ausgang: Led2,Rs232Switch,Summer |
DDRC &= ~(1<<DDC4); // Eingang: LowBat LTC1308 |
if( PCB_SV2RxTxPatch ) clr_USBOn(); |
else _BTOn(); // Erstmal USB dektivieren, damit beim versehentlichen Einschalten USB im PC ruhig bleibt |
DDRD |= (1<<DDD4)|(1<<DDD5)|(1<<DDD6)|(1<<DDD7); // Ausgang: PiepserTest, Servo, Wi232-CMD und Beleuchtung |
PORTD |= (1<<PORTD6); // Wi232-CMD auf High schalten |
set_D_LIGHT(); // Displaybeleuchtung an |
set_V_On(); // Spannung mit T3 halten |
Timer0_Init(); // system |
Display_on = 1; |
USART_Init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
uart1_init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); // USB |
// I2C_Init(1); |
// i2c_init(); |
LCD_Init(0); // muss vor "ReadParameter" stehen |
ReadParameter(); // aktuelle Werte aus EEProm auslesen |
#ifdef USE_TRACKING |
servoInit(SERVO_PERIODE); |
#endif |
//servoSetDefaultPos(); |
sei(); |
Old_Baudrate = Config.PKT_Baudrate; |
SetBaudUart1( Config.PKT_Baudrate ); |
SetBaudUart0( Config.PKT_Baudrate ); |
// 10.11.2015 cebra |
// TODO cebra: diese Stelle muss bei HW_Sound überarbeitet werden |
// if( (Config.HWSound==0) && (PCB_Version == PKT39m) ) |
// { |
// Timer2_Init(); // Displaybeleuchtung oder Soundmodul |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
// } |
#ifdef USE_SOUND |
if (Config.HWSound==1) InitSound(); |
#endif |
LCD_Init(1); |
set_beep ( 400, 0x0080, BeepNormal); |
OSD_active = false; // keine OSD Ausgabe |
ADC_Init(); // ADC für Lipomessung// MartinR: verschoben |
//------------------------- |
// beim ersten Start (oder nach Reset) Sprache abfragen |
//------------------------- |
if( Config.DisplayLanguage > NUM_LANG ) |
{ |
Config.DisplayLanguage = 1; |
//Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage, 0, 1, Language,1, NULL,NULL); // OG: ?? hierbei wird die falsche Sprache angezeigt - warum? |
Config.DisplayLanguage = Edit_generic( 2, 0, 1, Language,1, NULL,NULL); |
WriteParameter(); |
} |
//------------------------- |
// Anzeige Start-Screen |
//------------------------- |
PKT_TitlePKTlipo( false ); // Titel mit PKT-Version anzeigen (und clearscreen) |
yoffs = 11; |
if( PCB_SV2RxTxPatch ) |
{ |
lcdx_printp_center( 2, PSTR("SV2 Patch"), MNORMAL, 0,7); // "SV2 Patch" (ehemals "Wifly Patch") |
yoffs = 5; |
} |
if( PCB_Version==PKT39m ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9m (09.2011)"), MNORMAL, 0,yoffs); |
} |
if( PCB_Version==PKT39x ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9x (11.2012)"), MNORMAL, 0,yoffs); |
} |
lcdx_printp_center( 5, strGet(BOOT1), MNORMAL, 0,2); // "Taste 1 Sekunde" |
lcdx_printp_center( 6, strGet(BOOT2), MNORMAL, 0,4); // "festhalten!" |
lcd_rect_round( 0, 5*8-2, 127, 2*8+8, 1, R2); // Rahmen |
//------------------------- |
//------------------------- |
#ifdef DEBUG_SV2_EXTENSION |
lcd_printpns_at (0, 7, PSTR("A:"),0); |
lcd_print_hex(PortA,0); |
lcd_printpns_at (5, 7, PSTR("B:"),0); |
lcd_print_hex(PortB,0); |
lcd_printpns_at (10, 7, PSTR("C:"),0); |
lcd_print_hex(PortC,0); |
lcd_printpns_at (15, 7, PSTR("D:"),0); |
lcd_print_hex(PortD,0); |
#endif |
//_delay_ms(800); |
_delay_ms(900); |
if (PINA & (1<<PINA7)) // Spannung eingeschaltet lassen |
clr_V_On(); |
_delay_ms(100); |
set_beep ( 400, 0x0080, BeepNormal); |
get_key_press(KEY_ALL); |
#ifdef USE_SOUND |
if (Config.HWSound==1); |
{ |
playTone(505,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(535,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(505,100,Config.Volume); |
} |
#endif |
//lcd_cls(); // 20.02.2014 OG |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
// _delay_ms(250); |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
set_BTOff(); |
//set_Modul_On(USB); |
if( (Config.UseWi == true) && (Config.WiIsSet == false) ) |
{ |
Wi232_Initalize(); // wenn Wi232 nicht initialisiert ist, dann jetzt tun |
} |
#ifdef USE_BLUETOOTH |
if ((Config.UseBT == true) && (Config.BTIsSet == false)) // BT ausschalten |
{ |
//lcd_cls(); |
BTM222_Initalize(); |
// set_USBOn(); |
} |
bt_start(); |
#endif // end: #ifdef USE_BLUETOOTH |
if( (Config.UseWi == true) && (Config.U02SV2 == 0) ) |
{ |
Change_Output(Uart02Wi); // Verbindung zu Wi232 herstellen |
} |
else |
{ |
Change_Output(Uart02FC); // Verbindung zu SV" (Kabel) herstellen |
} |
} // InitHWPorts() |
//#define set_BEEP() (PORTC &= ~(1 << Summer)) // Summer |
//#define clr_BEEP() (PORTC |= (1 << Summer)) |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC &= ~(1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD &= ~(1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC |= (1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD |= (1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_D_LIGHT(void) /* Displaybeleuchtung ein*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHTV2(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_D_LIGHT(void) /* Displaybeleuchtung aus*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHTV2(); |
} |
} |
uint8_t BTIsOn=0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_Modul_On(uint8_t Modul) // Umschaltfunktionen für Leiterplatte 3.9x mit WiFlypatch |
{ |
if (PCB_SV2RxTxPatch) PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
switch (Modul) |
{ |
case USB: if (PCB_SV2RxTxPatch) set_USBOn(); |
else set_USBOn(); |
BTIsOn = 0; |
break; |
case Bluetooth: if (PCB_SV2RxTxPatch) set_BTM222On(); |
else _BTOn(); |
if (BTIsOn == 0) |
{ |
BTIsOn = 1; |
_delay_ms(3000); // wait for BTM222 initialisation |
} |
break; |
case Wlan: set_WiFlyOn(); |
BTIsOn = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOn(void) |
{ |
if (BTIsOn == 0) |
{ |
if (PCB_SV2RxTxPatch) set_Modul_On(Bluetooth); |
else |
{ |
_BTOn(); |
BTIsOn = 1; |
_delay_ms(3000); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOff(void) |
{ |
if( PCB_SV2RxTxPatch ) |
set_Modul_On(USB); |
else |
{ |
set_USBOn(); |
BTIsOn = 0; |
} |
} |
#endif |
//#endif // HAL_HW3_9_C_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/HAL_HW3_9.h |
---|
0,0 → 1,194 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.h |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 26.06.2013 Cebra |
//# - add: defines für WyFlypatch Port C0 + C1 |
//# |
//# 17.05.2013 OG |
//# - add: defines: KEY_ENTER_LONG, KEY_ESC_LONG, KEY_PLUS_LONG, KEY_MINUS_LONG |
//############################################################################ |
#ifndef HAL_HW3_9_H_ |
#define HAL_HW3_9_H_ |
//#define PIEPSER_NERVT // Summer zu Testzwecken ganz Ausschalten |
#define PKT39m 1 |
#define PKT39x 2 |
#define USB 1 |
#define Bluetooth 2 |
#define Wlan 3 |
// Hardware 3.9 Portbelegung |
#define KEY_PIN PINA // Port A als Input |
#define Touch0 PA0 // Pin 37 |
#define Touch1 PA1 // Pin 36 |
#define Touch2 PA2 // Pin 35 |
#define Touch3 PA3 // Pin 34 |
#define KEY_EXT PINA3 // MartinR |
#define Key1 PA4 // Pin 33 |
#define Key2 PA5 // Pin 32 |
#define Key3 PA6 // Pin 31 |
#define Key4 PA7 // Pin 30 |
#define USB2Wi PB0 // Pin 40 aktiv low > IC5 |
#define VoltageHold PB1 // Pin 41 High = Spannung T3 halten |
#define Display_Reset PB2 // Pin 42 |
#define Display_A0 PB3 // Pin 43 |
#define Display_CS PB4 // Pin 44 |
#define Display_SI PB5 // Pin 1 |
#define LED1 PB6 // Pin 2 Low = LED1 (nicht benutzbar wegen SCL |
#define Display_SCL PB7 // Pin 3 |
#define I2C_SCL PC0 // Pin 19 SCL |
#define I2C_CDA PC1 // Pin 20 SDA |
#define BT_Reset PC0 // Pin 19 Reseteingang BTM222 / WiyFlypatch |
#define WiFly_Reset PC1 // Pin 20 Reseteingang WyFly / WiyFlypatch |
#define USB2FC PC2 // Pin 21 aktiv low > IC5 |
#define USB_BT PC3 // Pin 22 high = USB, Low = Bluetooth, LED2 |
#define USB_Reset PC3 // Pin 22 Reseteingang USB FT232 / WiFlypatch |
#define LowBat PC4 // Pin 23 Low Bat Warning Lipo PKT,Input |
#define Uart02Wi PC5 // Pin 24 aktiv Low > IC4 |
#define Uart02FC PC6 // Pin 25 aktiv Low > IC4 |
#define Summer PC7 // Pin 26 Low = Summer |
#define DisplaybeleuchtungV2 PC7 // Pin 26 High = Display-LED PCB 3.9x |
#define Uart0RxD PD0 // Pin 9 über IC4 =Wi | SV2 |
#define Uart0TxD PD1 // Pin 10 über IC4 =Wi | SV2 |
#define Uart1RxD PD2 // Pin 11 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define Uart1TxD PD3 // Pin 12 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define SERVO2 PD4 // Pin 13 PWM Servo 2 |
#define SERVO1 PD5 // Pin 14 PWM Servo 1 |
#define Wi232_CMD PD6 // Pin 15 aktiv Low = Wi232 CMD |
#define Displaybeleuchtung PD7 // Pin 16 High = Display-LED PCB 3.9m |
#define SummerV2 PD7 // Pin 16 Low = Summer, PCB 3.9x |
#define KEY_ENTER Key1 |
#define KEY_ESC Key2 |
#define KEY_PLUS Key3 |
#define KEY_MINUS Key4 |
#define KEY_NONE 0 // keine Taste (menuctrl) |
#define KEY_ENTER_LONG KEY_ENTER+100 // for long press keys used in scrollbox & menuctrl |
#define KEY_ESC_LONG KEY_ESC+100 |
#define KEY_PLUS_LONG KEY_PLUS+100 |
#define KEY_MINUS_LONG KEY_MINUS+100 |
// |= schaltet Ausgang auf HIGH |
// &= ~ schaltet Ausgang auf LOW |
#define set_reset() (PORTB |= (1 << Display_Reset)) |
#define clr_reset() (PORTB &= ~(1 << Display_Reset)) |
#define set_A0() (PORTB |= (1 << Display_A0)) |
#define clr_A0() (PORTB &= ~(1 << Display_A0)) |
#define set_cs() (PORTB |= (1 << Display_CS)) |
#define clr_cs() (PORTB &= ~(1 << Display_CS)) |
#define set_si() (PORTB |= (1 << Display_SI)) |
#define clr_si() (PORTB &= ~(1 << Display_SI)) |
#define set_scl() (PORTB |= (1 << Display_SCL)) |
#define clr_scl() (PORTB &= ~(1 << Display_SCL)) |
#define _BTOn() (PORTC &= ~(1 << USB_BT)) // Bluetooth ein |
#define set_USBOn() (PORTC |= (1 << USB_Reset)) // USB ein |
#define clr_USBOn() (PORTC &= ~(1 << USB_Reset)) // USB aus |
#define set_BTM222On() (PORTC |= (1 << BT_Reset)) // BT ein |
#define clr_BTM222On() (PORTC &= ~(1 << BT_Reset)) // BT aus |
#define set_WiFlyOn() (PORTC |= (1 << WiFly_Reset)) // WiFly ein |
#define clr_WiFlyOn() (PORTC &= ~(1 << WiFly_Reset)) // WiFly aus |
#define clr_V_On() (PORTB &= ~(1 << VoltageHold)) // Spannung mit T3 halten |
#define set_V_On() (PORTB |= (1 << VoltageHold)) |
#define set_USB2FC() (PORTC &= ~(1 << USB2FC)) // USB mit FC-Kabel verbinden |
#define clr_USB2FC() (PORTC |= (1 << USB2FC)) |
#define set_USB2Wi() (PORTB &= ~(1 << USB2Wi)) // USB mit Wi232 verbinden |
#define clr_USB2Wi() (PORTB |= (1 << USB2Wi)) |
#define set_Uart02FC() (PORTC &= ~(1 << Uart02FC)) // Uart0 mit FC-Kabel verbinden |
#define clr_Uart02FC() (PORTC |= (1 << Uart02FC)) |
#define set_Uart02Wi() (PORTC &= ~(1 << Uart02Wi)) // Uart0 mit Wi232 verbinden |
#define clr_Uart02Wi() (PORTC |= (1 << Uart02Wi)) |
#define set_DISPLAYLIGHT() (PORTD &= ~(1 << Displaybeleuchtung)) // Displaybeleuchtung PCB3.9m |
#define clr_DISPLAYLIGHT() (PORTD |= (1 << Displaybeleuchtung)) |
#define set_DISPLAYLIGHTV2() (PORTC &= ~(1 << DisplaybeleuchtungV2)) // Displaybeleuchtung PCB3.9x |
#define clr_DISPLAYLIGHTV2() (PORTC |= (1 << DisplaybeleuchtungV2)) |
#define set_WI232CMD() (PORTD &= ~(1 << Wi232_CMD)) // Wi232 Programmierpin |
#define clr_WI232CMD() (PORTD |= (1 << Wi232_CMD)) |
void set_D_LIGHT(void); /* Displaybeleuchtung ein*/ |
void clr_D_LIGHT(void); /* Displaybeleuchtung aus */ |
void set_BEEP(void); /* Beeper ein*/ |
void clr_BEEP(void); /* Beeper aus*/ |
void InitHWPorts(void); |
void set_BTOn(void); /* Bluetooth einschalten*/ |
void set_BTOff(void); /* Bluetooth einschalten*/ |
void set_Modul_On(uint8_t Modul); |
extern volatile uint8_t PCB_Version; |
extern volatile uint8_t PCB_SV2RxTxPatch; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/avr-nmea-gps-library/nmea.c |
---|
0,0 → 1,525 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY nmea.c |
//# |
//# 08.08.2015 CB |
//# - add: Einführung eines neuen Interruptbasierten NMEA Parsers |
//# cpp-Code auf c geändert und an PKT angepasst |
//# |
//############################################################################ |
/* URSPRUNG: |
File: nmea.cpp |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include "nmea.h" |
#include "../timer/timer.h" |
//#include <stdio.h> |
//#include <stdlib.h> |
//#include <util/delay.h> |
#include <stdbool.h> |
#define MAX_POWER 10 |
#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
static const int32_t powers[MAX_POWER] PROGMEM = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
uint8_t m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
m_bFlagDataReady = false; // valid GPS fix and data available, user can call reader functions |
volatile uint32_t res_nNMEAcounter = 0; // NMEA Datensatzzähler |
volatile uint32_t res_nCRCerror = 0; // zählt die CRC Errors beim dekodieren |
uint8_t receiveNMEA = false; |
char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// will be set to true for characters between $ and * only |
bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
int m_nChecksum ; // numeric checksum, computed for a given sentence |
bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
int index_received_checksum ; // used to parse received checksum |
// word cutting variables |
int m_nWordIdx , // the current word in a sentence |
m_nPrevIdx, // last character index where we did a cut |
m_nNowIdx ; // current character index |
// globals to store parser results |
int32_t res_fLongitude; // GPRMC and GPGGA |
int32_t res_fLatitude; // GPRMC and GPGGA |
unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
char Time[9]; // GPRMC and GPGGA |
uint8_t res_nSatellitesUsed; // GPGGA |
uint8_t res_nGPSfix; // GPGGA |
int16_t res_nHDOP; // GPGGA |
int16_t res_fAltitude; // GPGGA |
float res_fSpeed; // GPRMC |
int16_t res_fBearing; // GPRMC |
NMEA_GPGGA_t NMEA; // Variable mit der Struktur für die NMEA Daten für PKT |
// the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// aux functions |
int digit2dec(char hexdigit); |
float string2float(char* s); |
int mstrcmp(const char *s1, const char *s2); |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c) |
{ |
if (c == '$') { |
m_bFlagRead = true; |
// init parser vars |
m_bFlagComputedCks = false; |
m_nChecksum = 0; |
// after getting * we start cuttings the received m_nChecksum |
m_bFlagReceivedCks = false; |
index_received_checksum = 0; |
// word cutting variables |
m_nWordIdx = 0; m_nPrevIdx = 0; m_nNowIdx = 0; |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
} |
if (m_bFlagRead) { |
// check ending |
if (c == '\r' || c== '\n') { |
// catch last ending item too |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
// cut received m_nChecksum |
tmp_szChecksum[index_received_checksum] = 0; |
// sentence complete, read done |
m_bFlagRead = false; |
// parse |
parsedata(); |
} else { |
// computed m_nChecksum logic: count all chars between $ and * exclusively |
if (m_bFlagComputedCks && c == '*') m_bFlagComputedCks = false; |
if (m_bFlagComputedCks) m_nChecksum ^= c; |
if (c == '$') m_bFlagComputedCks = true; |
// received m_nChecksum |
if (m_bFlagReceivedCks) { |
tmp_szChecksum[index_received_checksum] = c; |
index_received_checksum++; |
} |
if (c == '*') m_bFlagReceivedCks = true; |
// build a word |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = c; |
if (c == ',') { |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
m_nPrevIdx = m_nNowIdx; |
} |
else m_nNowIdx++; |
} |
} |
return m_nWordIdx; |
} |
/* |
* parse internal tmp_ structures, fused by pushdata, and set the data flag when done |
*/ |
void parsedata(void) { |
int received_cks = 16*digit2dec(tmp_szChecksum[0]) + digit2dec(tmp_szChecksum[1]); |
//uart1.Send("seq: [cc:%X][words:%d][rc:%s:%d]\r\n", m_nChecksum,m_nWordIdx, tmp_szChecksum, received_cks); |
// check checksum, and return if invalid! |
if (m_nChecksum != received_cks) { |
m_bFlagDataReady = false; |
res_nCRCerror = res_nCRCerror+1; |
return; |
} |
/* $GPGGA |
* $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh |
* ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A, |
* |
* WORDS: |
* 1 = UTC of Position |
* 2 = Latitude |
* 3 = N or S |
* 4 = Longitude |
* 5 = E or W |
* 6 = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix) |
* 7 = Number of satellites in use [not those in view] |
* 8 = Horizontal dilution of position |
* 9 = Antenna altitude above/below mean sea level (geoid) |
* 10 = Meters (Antenna height unit) |
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. |
* -geoid is below WGS-84 ellipsoid) |
* 12 = Meters (Units of geoidal separation) |
* 13 = Age in seconds since last update from diff. reference station |
* 14 = Diff. reference station ID# |
* 15 = Checksum |
*/ |
if (mstrcmp(tmp_words[0], "$GPGGA") == 0) { |
// Check GPS Fix: 0=no fix, 1=GPS fix, 2=Dif. GPS fix |
res_nGPSfix = atoi(&tmp_words[6][0]); |
NMEA.SatFix = res_nGPSfix; |
if (tmp_words[6][0] == '0') { |
// clear data |
res_fLatitude = 0; |
res_fLongitude = 0; |
// m_bFlagDataReady = false; |
// return; |
} |
// parse time |
res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
NMEA_getTime (tmp_words[1]); |
// parse latitude and longitude in NMEA format |
// res_fLatitude = string2float(tmp_words[2]); |
// res_fLongitude = string2float(tmp_words[4]); |
// get decimal format |
// if (tmp_words[3][0] == 'S') res_fLatitude *= -1.0; |
// if (tmp_words[5][0] == 'W') res_fLongitude *= -1.0; |
// float degrees = trunc(res_fLatitude / 100.0f); |
// float minutes = res_fLatitude - (degrees * 100.0f); |
// res_fLatitude = degrees + minutes / 60.0f; |
// degrees = trunc(res_fLongitude / 100.0f); |
// minutes = res_fLongitude - (degrees * 100.0f); |
// res_fLongitude = degrees + minutes / 60.0f; |
res_fLatitude = NMEA_calcLatitude(tmp_words[2],tmp_words[3]); |
res_fLongitude = NMEA_calcLongitude(tmp_words[4],tmp_words[5]); |
NMEA.Latitude = res_fLatitude; |
NMEA.Longitude = res_fLongitude; |
// parse number of satellites |
res_nSatellitesUsed = (int)string2float(tmp_words[7]); |
NMEA.SatsInUse = atoi(tmp_words[7]); |
// parse HDOP |
res_nHDOP = NMEA_floatStrToInt(tmp_words[8],1); |
NMEA.HDOP = res_nHDOP; |
// parse altitude |
// res_fAltitude = string2float(tmp_words[9]); |
res_fAltitude = NMEA_floatStrToInt( tmp_words[9], 1); |
NMEA.Altitude = res_fAltitude; |
// data ready |
m_bFlagDataReady = true; |
res_nNMEAcounter = res_nNMEAcounter +1; |
NMEA.Counter = res_nNMEAcounter; |
} |
// 8.8.2015 CB wird zur Zeit nicht benötigt |
/* $GPRMC |
* note: a siRF chipset will not support magnetic headers. |
* $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh |
* ex: $GPRMC,230558.501,A,4543.8901,N,02112.7219,E,1.50,181.47,230213,,,A*66, |
* |
* WORDS: |
* 1 = UTC of position fix |
* 2 = Data status (V=navigation receiver warning) |
* 3 = Latitude of fix |
* 4 = N or S |
* 5 = Longitude of fix |
* 6 = E or W |
* 7 = Speed over ground in knots |
* 8 = Track made good in degrees True, Bearing This indicates the direction that the device is currently moving in, |
* from 0 to 360, measured in azimuth. |
* 9 = UT date |
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course) |
* 11 = E or W |
* 12 = Checksum |
*/ |
// if (mstrcmp(tmp_words[0], "$GPRMC") == 0) { |
// // Check data status: A-ok, V-invalid |
// if (tmp_words[2][0] == 'V') { |
// // clear data |
// res_fLatitude = 0; |
// res_fLongitude = 0; |
//// m_bFlagDataReady = false; |
// return; |
// } |
//// // parse time |
//// res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
//// res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
//// res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
//// // parse latitude and longitude in NMEA format |
//// res_fLatitude = string2float(tmp_words[3]); |
//// res_fLongitude = string2float(tmp_words[5]); |
//// // get decimal format |
//// if (tmp_words[4][0] == 'S') res_fLatitude *= -1.0; |
//// if (tmp_words[6][0] == 'W') res_fLongitude *= -1.0; |
//// float degrees = trunc(res_fLatitude / 100.0f); |
//// float minutes = res_fLatitude - (degrees * 100.0f); |
//// res_fLatitude = degrees + minutes / 60.0f; |
//// degrees = trunc(res_fLongitude / 100.0f); |
//// minutes = res_fLongitude - (degrees * 100.0f); |
//// res_fLongitude = degrees + minutes / 60.0f; |
// //parse speed |
// // The knot (pronounced not) is a unit of speed equal to one nautical mile (1.852 km) per hour |
// res_fSpeed = NMEA_floatStrToInt(tmp_words[7],1); |
// res_fSpeed /= 1.852; // convert to km/h |
// // parse bearing |
// res_fBearing = NMEA_floatStrToInt(tmp_words[8],1); |
//// // parse UTC date |
//// res_nUTCDay = digit2dec(tmp_words[9][0]) * 10 + digit2dec(tmp_words[9][1]); |
//// res_nUTCMonth = digit2dec(tmp_words[9][2]) * 10 + digit2dec(tmp_words[9][3]); |
//// res_nUTCYear = digit2dec(tmp_words[9][4]) * 10 + digit2dec(tmp_words[9][5]); |
// |
// // data ready |
// res_nNMEAcounter = res_nNMEAcounter +1; |
// NMEA.Counter = res_nNMEAcounter; |
// m_bFlagDataReady = true; |
// } |
} |
//-------------------------------------------------------------- |
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLatitude( const char *s, const char *NS) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(0,5,NS,0,0,0); |
deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
min = NMEA_floatStrToInt( &s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *NS == 'S' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLongitude( const char *s, const char *WE) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(10,5,WE,0,0,0); |
deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
min = NMEA_floatStrToInt( &s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *WE == 'W' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
void NMEA_getTime( const char *s) |
{ |
uint8_t sem = 0; |
uint8_t i; |
for( i=0; i<6; i++ ) |
{ |
NMEA.Time[sem++] = s[i]; |
if( i==1 || i==3) |
NMEA.Time[sem++] = ':'; |
} |
NMEA.Time[sem] = '\0'; |
} |
//-------------------------------------------------------------- |
/* |
* returns base-16 value of chars '0'-'9' and 'A'-'F'; |
* does not trap invalid chars! |
*/ |
int digit2dec(char digit) |
{ |
if ((int)digit >= 65) |
return ((int)digit - 55); |
else |
return ((int)digit - 48); |
} |
//-------------------------------------------------------------- |
/* returns base-10 value of zero-terminated string |
* that contains only chars '+','-','0'-'9','.'; |
* does not trap invalid strings! |
*/ |
float string2float(char* s) { |
long integer_part = 0; |
float decimal_part = 0.0; |
float decimal_pivot = 0.1; |
bool isdecimal = false, isnegative = false; |
char c; |
while ( ( c = *s++) ) { |
// skip special/sign chars |
if (c == '-') { isnegative = true; continue; } |
if (c == '+') continue; |
if (c == '.') { isdecimal = true; continue; } |
if (!isdecimal) { |
integer_part = (10 * integer_part) + (c - 48); |
} |
else { |
decimal_part += decimal_pivot * (float)(c - 48); |
decimal_pivot /= 10.0; |
} |
} |
// add integer part |
decimal_part += (float)integer_part; |
// check negative |
if (isnegative) decimal_part = - decimal_part; |
return decimal_part; |
} |
//-------------------------------------------------------------- |
// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
//-------------------------------------------------------------- |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ) |
{ |
char *endPtr; |
int32_t v; |
v = strtol(s, &endPtr, 10); |
if( *endPtr == '.' ) |
{ |
for (s = endPtr + 1; *s && power1; s++) |
{ |
v = v * 10 + (*s - '0'); |
--power1; |
} |
} |
if( power1 ) |
{ |
// Table to avoid multiple multiplications |
v = v * getPower(power1); |
} |
return v; |
} |
//-------------------------------------------------------------- |
int mstrcmp(const char *s1, const char *s2) |
{ |
while((*s1 && *s2) && (*s1 == *s2)) |
s1++,s2++; |
return *s1 - *s2; |
} |
//-------------------------------------------------------------- |
bool NMEA_isdataready() { |
return m_bFlagDataReady; |
} |
int NMEA_getHour() { |
return res_nUTCHour; |
} |
int NMEA_getMinute() { |
return res_nUTCMin; |
} |
int NMEA_getSecond() { |
return res_nUTCSec; |
} |
int NMEA_getDay() { |
return res_nUTCDay; |
} |
int NMEA_getMonth() { |
return res_nUTCMonth; |
} |
int NMEA_getYear() { |
return res_nUTCYear; |
} |
int32_t NMEA_getLatitude() { |
return res_fLatitude; |
} |
int32_t NMEA_getLongitude() { |
return res_fLongitude; |
} |
uint8_t NMEA_getSatellites() { |
return res_nSatellitesUsed; |
} |
uint8_t NMEA_getGPSfix() { |
return res_nGPSfix; |
} |
int16_t NMEA_getHDOP() { |
return res_nHDOP; |
} |
int16_t NMEA_getAltitude() { |
return res_fAltitude; |
} |
float NMEA_getSpeed() { |
return res_fSpeed; |
} |
int16_t NMEA_getBearing() { |
return res_fBearing; |
} |
uint32_t NMEA_getNMEAcounter() { |
return res_nNMEAcounter; |
} |
uint32_t NMEA_getCRCerror() { |
return res_nCRCerror; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/avr-nmea-gps-library/nmea.h |
---|
0,0 → 1,135 |
/* |
File: nmea.h |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include <string.h> |
//#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <math.h> |
#include <stdio.h> |
//--------------------------------- |
#define NMEA_TIMEOUT 500 // Timeout 5 sek. |
//--------------------------------- |
typedef struct |
{ |
uint32_t Counter; // Zaehler empfangene GPGGA Pakete |
int32_t Latitude; // in 1E-7 deg |
int32_t Longitude; // in 1E-7 deg |
int16_t Altitude; // Hoehe in Meter |
uint8_t SatFix; // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix, 6=Estimated) |
uint8_t SatsInUse; // Number of satelites currently in use |
int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser |
char Time[9]; // "hh:mm:ss" |
} NMEA_GPGGA_t; |
//--------------------------------- |
// export vars |
//--------------------------------- |
extern NMEA_GPGGA_t NMEA; |
extern uint8_t receiveNMEA; |
extern volatile uint32_t res_nNMEAcounter; |
// /*class NMEA { |
// |
// private: |
// bool m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
// m_bFlagDataReady; // valid GPS fix and data available, user can call reader functions |
// char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
// tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// |
// // will be set to true for characters between $ and * only |
// bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
// int m_nChecksum ; // numeric checksum, computed for a given sentence |
// bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
// int index_received_checksum ; // used to parse received checksum |
// |
// // word cutting variables |
// int m_nWordIdx , // the current word in a sentence |
// m_nPrevIdx, // last character index where we did a cut |
// m_nNowIdx ; // current character index |
// |
// // globals to store parser results |
// float res_fLongitude; // GPRMC and GPGGA |
// float res_fLatitude; // GPRMC and GPGGA |
// unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
// res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
// int res_nSatellitesUsed; // GPGGA |
// float res_fAltitude; // GPGGA |
// float res_fSpeed; // GPRMC |
// float res_fBearing; // GPRMC |
// |
// // the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// // aux functions |
// int digit2dec(char hexdigit); |
// float string2float(char* s); |
// int mstrcmp(const char *s1, const char *s2); |
// |
// public: |
// // constructor, initing a few variables |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c); |
void parsedata(void); |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ); |
int32_t NMEA_calcLatitude( const char *s, const char *NS); |
int32_t NMEA_calcLongitude( const char *s, const char *WE); |
void NMEA_getTime( const char *s); |
// READER functions: retrieving results, call isdataready() first |
bool NMEA_isdataready(); |
int NMEA_getHour(); |
int NMEA_getMinute(); |
int NMEA_getSecond(); |
int NMEA_getDay(); |
int NMEA_getMonth(); |
int NMEA_getYear(); |
int32_t NMEA_getLatitude(); |
int32_t NMEA_getLongitude(); |
uint8_t NMEA_getSatellites(); |
uint8_t NMEA_getGPSfix(); |
int16_t NMEA_getHDOP(); |
int16_t NMEA_getAltitude(); |
float NMEA_getSpeed(); |
int16_t NMEA_getBearing(); |
uint32_t NMEA_getNMEAcounter(); |
uint32_t NMEA_getCRCerror(); |
// }; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/bluetooth.c |
---|
0,0 → 1,1760 |
/** |
* source for the Bluetooth driver |
* @file bluetooth.c |
* @author Linus Lotz<lotz@in.tum.de> |
* @author Salomon Sickert |
*/ |
//############################################################################ |
//# HISTORY bluetooth.c |
//# |
//# |
//# 02.08.2015 CB |
//# - chg: bt_showsettings(), angepasst und vollendet, zur Zeit nur im Mastermodus des BTM222 nutzbar |
//# |
//# 30.06.2014 OG |
//# - chg: bt_discover() wieder auf (berechnete) Progressbar umgestellt |
//# |
//# 26.06.2014 OG |
//# - del: bt_receiveNMEA() - jetzt in gps/nmea.c |
//# - chg: bt_discover(), bt_searchDevice() - stark geaendert; Timeout geht jetzt |
//# nach Zeit und Abbruch durch Benutzer moeglich |
//# - fix: BT-Devicesuche - wenn bei einer Suche Devices gefunden wurden und bei |
//# einer nachfolgenden Suche weniger (z.B. weil ein Device ausgeschaltet wurde) |
//# dann wurden zuviele Devices angezeigt; fixed |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover(), bt_searchDevice() |
//# |
//# 22.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 21.06.2014 CB |
//# - fix: Fehler in "copy_DevName", zwei Leerzeichen am Anfang beendeten sofort den Namen, Namen war dann leer |
//# - fix: Umbau der Timeoutfunktion bei der Devicesuche, wenn kein BT Device in Reichweite wurde die Suche nicht beendet |
//# - add: Progressbar bei BT Device Suche |
//# |
//# 19.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - etliche Aenderungen und Funktionsumbenennungen - vorallem rund um den |
//# Bereich BT-Init |
//# - viel Code-Layout (noch nicht abgeschlossen) |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 2013 Cebra |
//# - Erweiterung um BT Local-ID Abfrage |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung fuer WiFlypatch geaendert |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() - korrigiert ggf. fehlehaften BT-Namen in Config.bt_name |
//# |
//# 24.06.2013 OG |
//# - chg: bt_init(): Code-Layout, Code-Struktur, ggf. Debug-Ausgaben |
//# - chg: bt_searchmodul(): Code-Layout und Ausgabe |
//# - chg: send_cmd(): Handling von BT_SET_NAME & Code-Layout |
//# - add: define DEBUG_BT |
//# |
//# 23.05.2013 OG |
//# - chg: Timeout reduziert in bt_disconnect() (trennen der BT-Verbindung) |
//# - fix: copy_DevName() Range angepasst und 0 Terminierung |
//# |
//# 24.04.2013 OG |
//# - chg: Auskommentiert: #define __DELAY_BACKWARD_COMPATIBLE__ |
//# das wird bereits vom Compiler-Symbol dieses PKT-Projektes definiert |
//# und erzeugt nur zusaetzliche Warnings |
//# |
//# 03.04.2013 Cebra |
//# - chg: Init Routinen BT überarbeitet, es kam zu falschen Baudrateneinstellungen |
//# beim BTM-222 ohne Hinweis, mehr Displayinformationen beim Init |
//############################################################################ |
#include <string.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdlib.h> |
#include "bluetooth.h" |
#include "../main.h" |
//---------------------------------------------------------------------------- |
#ifdef USE_BLUETOOTH // 30.01.2014 OG |
// brutal hier den Source ausklammern, damit man an anderen Stellen des PKT |
// den BT-Code durch den Compiler entdeckt |
// ansonsten kann es passieren, dass einiges an BT-Code unenddeckt einkompiliert wird |
// -> bei den ersten Test's waren das immerhin ca. 10 KByte durch unenddeckten Code... |
//---------------------------------------------------------------------------- |
//#define DEBUG_BT |
//#ifdef HWVERSION3_9 |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "fifo.h" |
#include "error.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../utils/xutils.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "../utils/scrollbox.h" |
//############################################################################ |
//############################################################################ |
//#define BT_CMD_TIMEOUT_MS 2000 |
#define BT_CMD_TIMEOUT_MS 500 // 15.06.2014 OG: Timeout reduziert von 2000 auf 500 |
static const char STR_BTM222INIT[] PROGMEM = "BTM222 Init"; |
//#define SaveMem |
// |
// Baudrate for the UART-connection to the BTM-222 on SQUIRREL |
// |
#define SQUIRREL |
#ifdef SQUIRREL |
#define UART_BAUD_RATE 19200 |
#endif |
#ifdef NUT |
#define UART_BAUD_RATE 19200 |
#endif |
typedef enum { |
BT_RAW, |
BT_DATA, |
BT_CMD, |
BT_NOECHO, |
BT_NOANSWER |
} communication_mode_t; |
typedef enum { |
BT_TEST, // AT |
BT_CONNECT, // ATA |
BT_DISCONNECT, // ATH |
BT_CLEAR_ADDRESS, // ATD0 |
BT_SET_ADDRESS, // ATD=_____ |
BT_GET_LOCALID, // ATB? Inquire the Local BD address |
BT_FIND_DEVICES, // ATF? |
BT_DISABLE_AUTOCONNECT, // ATO1 |
BT_ENABLE_AUTOCONNECT, // ATO0 |
BT_SET_MASTER, // ATR0 |
BT_SET_SLAVE, // ATR1 |
BT_SET_PIN, // ATP=1234 |
BT_SET_2400, // ATL* Baudrate 2400 |
BT_SET_4800, // ATL0 Baudrate 4800 |
BT_SET_9600, // ATL1 Baudrate 9600 |
BT_SET_19200, // ATL2 Baudrate 19200 |
BT_SET_38400, // ATL3 Baudrate 38400 |
BT_SET_57600, // ATL4 Baudrate 57600 |
BT_SET_115200, // ATL5 Baudrate 115200 |
BT_SET_NOANSWER, // ATQ1 Rückmeldungen aus |
BT_SET_NOECHO, // ATE0 ECHO deaktivieren |
BT_SET_ANSWER, // ATQ0 Rückmeldungen |
BT_SET_ECHO, // ATE1 ECHO aktivieren |
BT_SET_DEFAULT, // Defaultwerte setzen |
BT_SET_NAME, // Devicename |
BT_SET_DISPWRDOWN, // disable auto Powerdown |
BT_SHOW_SETTINGS // ATI1 Listing all setting value |
} bt_cmd_t; |
//TODO: FIFO Groesse |
#define IN_FIFO_SIZE 512 |
char localID[15]="12345678901234"; |
static uint8_t bt_buffer[IN_FIFO_SIZE]; |
fifo_t in_fifo; |
char bt_rx_buffer[RXD_BUFFER_SIZE]; |
volatile uint8_t bt_rx_len; |
volatile uint8_t bt_rx_ready = 0; |
uint8_t EchoAnswerOn=false; //Merkzelle |
static bt_mode_t bt_mode = BLUETOOTH_SLAVE; |
static communication_mode_t comm_mode = BT_CMD; |
uint8_t i = 0; |
uint8_t bt_devicecount = 0; |
uint8_t bt_rxerror = 0; |
uint8_t bt_frameerror = 0; |
uint8_t bt_overrun = 0; |
uint8_t bt_bufferoverflow = 0; |
device_info device_list[NUTS_LIST]; |
// Set a timeout of Y ms and a Conditon X, which have to be true while timeout |
#define while_timeout(X, Y) for(uint16_t __timeout = 0; __timeout++ <= Y && (X); Delay_MS(Y ? 1 : 0)) |
//-------------------------------------------------------------- |
// entfernt ungueltige Zeichen und Leerzeichen (rechts) aus dem |
// BT-Namen die evtl. durch Default's reingekommen sind. |
// Falls ungueltige Zeichen ('.') wird der BT-Name auf "PKT" |
// gesetzt. |
//-------------------------------------------------------------- |
void bt_fixname(void) |
{ |
char *p; |
p = Config.bt_name; |
while( *p!=0 && *p!='.' ) p++; // suche '.' |
if( *p=='.' || Config.bt_name[0]==' ' || Config.bt_name[0]==0) // wenn '.' gefunden oder Anfang mit ' ' oder kein String |
strcpy_P(Config.bt_name, PSTR("PKT")); // -> dann BT-Name auf "PKT" setzen |
strrtrim( Config.bt_name ); // ggf. Leerzeichen rechts loeschen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_MS(int count) |
{ |
for (int i = 0; i < count; i++) |
_delay_ms(1); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_start(void) |
{ |
if( Config.BTIsSlave == true ) EchoAnswerOn = false; |
else EchoAnswerOn = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void uart_receive(void) |
{ |
unsigned int uart_data; |
while( !fifo_is_full(&in_fifo) ) |
{ |
uart_data = uart1_getc(); |
//USART_puts("."); |
if (!(uart_data==UART_NO_DATA)) USART_putc(uart_data); |
switch( uart_data & 0xFF00 ) |
{ |
// Framing Error detected, i.e no stop bit detected |
case UART_FRAME_ERROR: |
bt_rxerror++; |
bt_frameerror++; |
return; |
// Overrun, a character already presend in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped |
// |
case UART_OVERRUN_ERROR: |
bt_rxerror++; |
bt_overrun++; |
return; |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// |
case UART_BUFFER_OVERFLOW: |
bt_rxerror++; |
bt_bufferoverflow++; |
return; |
// UART Inputbuffer empty, nothing to do |
case UART_NO_DATA: |
return; |
default: |
fifo_write( &in_fifo, uart_data); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void uart_send(const char *data, const uint8_t length) |
{ |
#ifdef SND_DEBUG |
debug_pgm(PSTR("bt_uart_send")); |
if (comm_mode == BT_CMD) debug_pgm(PSTR("bt_uart_send:BT_CMD")); else debug_pgm(PSTR("bt_uart_send:Wrong comm-mode")); |
if (EchoAnswerOn == true) debug_pgm(PSTR("bt_uart_send:EchoAnswer ON")); else debug_pgm(PSTR("bt_uart_send:EchoAnswer OFF")); |
#endif |
char echo; |
// lcd_printp_at (i++, 1, PSTR("."), 0); |
for (uint8_t i = 0; i < length; i++) |
{ |
#ifdef SND_DEBUG |
USART_putc((data[i])); //test |
#endif |
// debug_pgm(PSTR("bt_init_S")); |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR("UART: Remote not ready")); |
#endif |
return; |
} |
if (comm_mode == BT_RAW) |
_delay_ms(50); |
if (comm_mode == BT_DATA) |
_delay_ms(1); |
if ((comm_mode == BT_CMD) && (EchoAnswerOn == true)) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: get Echo")); |
#endif |
uint8_t x = 0; |
for (; x < 3; x++) |
{ |
while_timeout(fifo_is_empty(&in_fifo), 200) |
// for(uint16_t __timeout = 0; __timeout++ <= 200 && (fifo_is_empty(&in_fifo)); _delay_ms(200 ? 1 : 0)) |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
if (echo != data[i]) { |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UART: Remote not ready")); |
#endif |
return; |
} |
} |
else |
break; |
} |
#ifdef DEBUG |
if (x == 3) |
{ |
error_putc(data[i]); |
error_pgm(PSTR("BT: WRONG ECHO")); |
//_delay_ms(2000); |
} |
#endif |
} |
else |
{ |
{ |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
} |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: skip Echo")); |
#endif |
} |
} |
} |
//-------------------------------------------------------------- |
// send_cmd( command, *data) |
//-------------------------------------------------------------- |
static uint16_t send_cmd( const bt_cmd_t command, const char *data) |
{ |
uint16_t CommandDelay; // nach BTM222 Kommandos verschiedene Verzögerungszeiten bevor es weitergehen kann |
uint8_t i; |
char full_command[20]; // Maximum command size |
CommandDelay = 100; // Default = 100ms |
switch( command ) |
{ |
case BT_SET_PIN: strcpy_P( full_command, PSTR("ATP=")); |
for( i = 0; i < bt_pin_length; i++) |
{ |
full_command[i+4] = Config.bt_pin[i]; |
} |
full_command[(bt_pin_length+4)] = 0; |
break; |
case BT_SET_DEFAULT: strcpy_P( full_command, PSTR("ATZ0")); |
CommandDelay = 3000; |
break; |
case BT_SET_2400: strcpy_P( full_command, PSTR("ATL*")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL*")); |
#endif |
break; |
case BT_SET_4800: strcpy_P( full_command, PSTR("ATL0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL0")); |
#endif |
break; |
case BT_SET_9600: strcpy_P( full_command, PSTR("ATL1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL1")); |
#endif |
break; |
case BT_SET_19200: strcpy_P( full_command, PSTR("ATL2")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL2")); |
#endif |
break; |
case BT_SET_38400: strcpy_P( full_command, PSTR("ATL3")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL3")); |
#endif |
break; |
case BT_SET_57600: strcpy_P( full_command, PSTR("ATL4")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL4")); |
#endif |
break; |
case BT_SET_115200: strcpy_P( full_command, PSTR("ATL5")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL5")); |
#endif |
break; |
case BT_SET_NOANSWER: strcpy_P( full_command, PSTR("ATQ1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ1")); |
#endif |
break; |
case BT_SET_NOECHO: strcpy_P( full_command, PSTR("ATE0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE0")); |
#endif |
break; |
case BT_SET_ANSWER: strcpy_P( full_command, PSTR("ATQ0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ0")); |
#endif |
break; |
case BT_SET_ECHO: strcpy_P( full_command, PSTR("ATE1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE1")); |
#endif |
break; |
case BT_TEST: strcpy_P( full_command, PSTR("AT")); |
#ifdef DEBUG |
debug_pgm(PSTR("AT")); |
#endif |
break; |
case BT_CONNECT: strcpy_P( full_command, PSTR("ATA")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATA")); |
#endif |
break; |
case BT_DISCONNECT: strcpy_P( full_command, PSTR("ATH")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATH")); |
#endif |
break; |
case BT_CLEAR_ADDRESS: strcpy_P( full_command, PSTR("ATD0")); |
break; |
case BT_SET_ADDRESS: strcpy_P( full_command, PSTR("ATD=")); |
memcpy( (full_command + strlen(full_command)), data, 12); |
full_command[16] = 0; |
#ifdef DEBUG |
debug_pgm(PSTR("ATLD=")); |
#endif |
break; |
case BT_FIND_DEVICES: strcpy_P( full_command, PSTR("ATF?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATF?")); |
#endif |
break; |
case BT_DISABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO1")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO1")); |
#endif |
break; |
case BT_ENABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO0")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO0")); |
#endif |
break; |
case BT_SET_MASTER: strcpy_P( full_command, PSTR("ATR0")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR0")); |
#endif |
break; |
case BT_SET_SLAVE: strcpy_P( full_command, PSTR("ATR1")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR1")); |
#endif |
break; |
case BT_SET_NAME: strcpy_P( full_command, PSTR("ATN=") ); |
strcat( full_command, Config.bt_name); |
//lcd_print_at( 0, 7, full_command, 0); // DEBUG |
//lcdx_printf_at( 17, 7, MNORMAL, 0,0, "%3d", strlen(full_command) ); |
//_delay_ms(6000); |
#ifdef DEBUG |
debug_pgm(PSTR("ATN=")); |
#endif |
break; |
case BT_SET_DISPWRDOWN: strcpy_P(full_command, PSTR("ATS1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATS1")); |
#endif |
break; |
case BT_GET_LOCALID: strcpy_P(full_command, PSTR("ATB?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATB?")); |
#endif |
break; |
case BT_SHOW_SETTINGS: strcpy_P(full_command, PSTR("ATI1")); |
break; |
default: warn_pgm(PSTR("CMD UNK")); |
CommandDelay = 0; |
return false; |
} |
strcat_P( full_command, PSTR("\r") ); |
// throw away your television |
uart_receive(); |
fifo_clear( &in_fifo ); |
// send command |
uart_send( full_command, strlen(full_command)); |
// get response |
#ifdef DEBUG |
debug_pgm(PSTR("send_cmd:get Response")); |
#endif |
if( !EchoAnswerOn ) |
{ |
_delay_ms(CommandDelay); |
uart1_flush(); |
fifo_clear(&in_fifo); |
return true; |
} |
else |
{ |
while_timeout(true, BT_CMD_TIMEOUT_MS) |
{ |
uart_receive(); |
if( command == BT_GET_LOCALID ) |
{ |
_delay_ms(CommandDelay); |
return bt_getID(); |
} |
if( fifo_strstr_pgm( &in_fifo, PSTR("OK\r\n")) ) |
{ |
_delay_ms(CommandDelay); |
return true; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("ERROR\r\n")) ) |
{ |
return false; |
} |
} // end: while_timeout() |
} |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void clean_line(void) |
{ |
while_timeout(true, 50) |
uart_receive(); |
fifo_strstr_pgm( &in_fifo, PSTR("\r\n") ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static communication_mode_t update_comm_mode( uint16_t timeout_ms ) |
{ |
while_timeout(true, timeout_ms) |
{ |
uart_receive(); |
if( fifo_strstr_pgm(&in_fifo, PSTR("DISCONNECT")) ) |
{ |
clean_line(); |
comm_mode = BT_CMD; |
send_cmd(BT_TEST, NULL); |
return comm_mode; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("CONNECT")) ) |
{ |
_delay_ms(100); // don't delete this, else there will be no success!!!!!!!!! |
comm_mode = BT_DATA; |
return comm_mode; |
} |
if( fifo_strstr_pgm (&in_fifo, PSTR("Time out,Fail to connect!")) ) |
{ |
clean_line(); |
#ifdef DEBUG |
debug_pgm(PSTR("CONNECT FAILED\n")); |
#endif |
send_cmd(BT_TEST, NULL); |
comm_mode = BT_CMD; |
return comm_mode; |
} |
} |
return comm_mode; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_set_EchoAnswer( uint8_t on ) |
{ |
comm_mode = BT_CMD; // Set comm_mode to CMD |
EchoAnswerOn = false; |
if( on ) |
{ |
send_cmd( BT_SET_ECHO , NULL); |
send_cmd( BT_SET_ANSWER , NULL); |
send_cmd( BT_TEST , NULL); |
// EchoAnswerOn = true; |
/* |
if( !send_cmd( BT_TEST , NULL) ) |
{ |
//lcd_printp_at (0, 6, PSTR("Error set Echo"), MNORMAL); |
//_delay_ms(3000); |
} |
*/ |
} |
else |
{ |
send_cmd( BT_SET_NOECHO , NULL); |
send_cmd( BT_SET_NOANSWER, NULL); |
// EchoAnswerOn = false; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t BTM222_SetBaud( uint8_t baudrate ) |
{ |
bt_cmd_t btcommand = BT_SET_57600; |
uint8_t lOk = true; |
comm_mode = BT_CMD; // Set comm_mode to CMD |
uart1_flush(); |
fifo_clear( &in_fifo ); |
bt_set_EchoAnswer(true); |
if( !send_cmd(BT_TEST, NULL) ) // Test mit PKT_Baudrate |
{ |
return false; |
} |
else |
{ |
switch( baudrate ) |
{ |
case Baud_2400: btcommand = BT_SET_2400; break; |
case Baud_4800: btcommand = BT_SET_4800; break; |
case Baud_9600: btcommand = BT_SET_9600; break; |
case Baud_19200: btcommand = BT_SET_19200; break; |
case Baud_38400: btcommand = BT_SET_38400; break; |
case Baud_57600: btcommand = BT_SET_57600; break; |
case Baud_115200: btcommand = BT_SET_115200; break; |
} |
if( !(send_cmd( btcommand, NULL)) ) lOk = false; // Fehler |
SetBaudUart1( baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
send_cmd( BT_TEST, NULL); |
if( !(send_cmd(BT_TEST, NULL)) ) lOk = false; // Fehler |
bt_set_EchoAnswer(false); |
} |
return lOk; |
} |
//-------------------------------------------------------------- |
// foundbaud = BTM222_CheckBaud( Baudrate ) |
// |
// suche BTM222 mit entsprechender Baudrate |
// |
// RUECKGABE: |
// =0: Error (BTM222 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint8_t BTM222_CheckBaud( uint8_t Baudrate ) |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
comm_mode = BT_CMD; // Set comm_mode to CMD |
SetBaudUart1( Baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
EchoAnswerOn = false; |
send_cmd( BT_TEST , NULL); |
send_cmd( BT_SET_ANSWER, NULL); |
send_cmd( BT_SET_ECHO , NULL); |
EchoAnswerOn = true; |
if( send_cmd(BT_TEST, NULL) ) |
{ |
return Baudrate; // BTM222 Baudrate gefunden |
} |
return 0; // 0 = nicht gefunden |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void BTM222_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
// ggf. rechte Leerzeichen oder ungueltige Zeichen |
// von bt_name entfernen |
bt_fixname(); |
//---------------------------- |
// 1. Frage: initialisieren? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, PSTR("initialisieren?"), NULL, STR_BTM222INIT, STR_BTM222INIT) ) |
{ |
return; |
} |
//------------------------------------ |
// 2. BTM222 suchen |
//------------------------------------ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Old_Baudrate) ); // "suche nnn Baud" |
set_Modul_On( Bluetooth ); |
EchoAnswerOn = false; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_115200 ); |
if( !foundbaud ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 not found!"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
Config.UseBT = false; // Modul in Konfig sperren, verhindert Init beim Start |
} |
//------------------------------------ |
// 2. Initialisieren |
//------------------------------------ |
if( foundbaud > 0 ) |
{ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel: mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_BTM222INIT ); // Headline |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 7, 0, 0,0); // 7 Progress Steps |
comm_mode = BT_CMD; // Set comm_mode to CMD |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_CLEAR_ADDRESS, NULL) ) nError = 1; // Clear remote address |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_SLAVE, NULL) ) nError = 2; // Set BTM to SLAVE |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_PIN, NULL) ) nError = 3; // Set BTM PIN |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_NAME, NULL) ) nError = 4; // Set BTM Name |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_DISPWRDOWN, NULL) ) nError = 5; |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_GET_LOCALID, NULL) ) nError = 6; // MAC ermitteln |
//if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_ANSWER, NULL) ) nError = 7; |
if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Config.PKT_Baudrate)) nError = 8; // Set BTM Baudrate |
//if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Baud_38400)) nError = 8; // * TEST/DEBUG * Set BTM Baudrate |
SetBaudUart1( Config.PKT_Baudrate ); // PKT wieder auf Original Baudrate setzen |
if( !nError ) |
{ |
bt_set_EchoAnswer( false ); |
bt_mode = BLUETOOTH_SLAVE; |
WriteBTInitFlag(); // Init merken |
WriteBTSlaveFlag(); |
bt_start(); |
set_Modul_On( USB ); |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
return; |
} |
else |
{ |
set_Modul_On(USB); |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("BTM Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
return; |
} |
} |
SetBaudUart1( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
set_Modul_On( USB ); |
clear_key_all(); |
return; |
} |
//###################################################################################### |
//###################################################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_set_mode( const bt_mode_t mode ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: set Mode")); |
#endif |
//if (update_comm_mode(0) == BT_DATA) // 30.1.2012 CB |
// return false; |
if( mode == bt_mode ) |
return true; |
if( mode == BLUETOOTH_MASTER ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_SET_MASTER, NULL) ) |
{ |
bt_mode = BLUETOOTH_MASTER; |
send_cmd(BT_DISABLE_AUTOCONNECT, NULL); |
WriteBTMasterFlag(); |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Master is set")); |
#endif |
} |
} |
if( mode == BLUETOOTH_SLAVE ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_ENABLE_AUTOCONNECT, NULL) ) |
{ |
bt_mode = BLUETOOTH_SLAVE; |
send_cmd(BT_SET_SLAVE, NULL); |
WriteBTSlaveFlag(); |
bt_set_EchoAnswer(false); |
comm_mode = BT_CMD; |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Slave is set")); |
#endif |
} |
} |
// if (bt_mode == BLUETOOTH_MASTER) debug_pgm(PSTR("bt_mode:BLUETOOTH_MASTER ")); |
// if (bt_mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("bt_mode:BLUETOOTH_SLAVE")); |
// if (mode == BLUETOOTH_MASTER) debug_pgm(PSTR("mode:BLUETOOTH_MASTER ")); |
// if (mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("mode:BLUETOOTH_SLAVE")); |
return (mode == bt_mode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_receive( void *data, uint8_t length, uint16_t timeout_ms ) |
{ |
uint8_t rec_length = 0; |
uint8_t i = 0; |
//while_timeout(true, timeout_ms); |
for( uint16_t __timeout = 0; __timeout++ <= true && (timeout_ms); _delay_ms(true ? 1 : 0)) |
{ |
if( i == length ) |
return true; |
uart_receive(); |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( update_comm_mode(0) != BT_DATA) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("not connected")); |
#endif |
return false; |
} |
if( timeout_ms == 0 ) // We have a connection |
timeout_ms += 2000; |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( !rec_length ) // Find starting point of packet |
{ |
fifo_read( &in_fifo, (char *)&rec_length); |
if( rec_length != length ) |
{ |
rec_length = 0; |
} |
else |
{ |
timeout_ms += 2000; |
} |
} |
else |
{ |
fifo_read( &in_fifo, (char *)data + i); |
i++; |
} |
} |
return false; |
} |
#ifndef SaveMem |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_send( void *data, const uint8_t length ) |
{ |
if (update_comm_mode(0) == BT_CMD) |
return false; |
uart_send((const char *)&length, 1); |
uart_send((char *)data, length); |
return (update_comm_mode(0) == BT_DATA); |
} |
#ifdef SQUIRREL |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_connect( const char *address ) |
{ |
fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart_receive(); |
fifo_clear(&in_fifo); |
// Maybe we already disconnected??? |
if (BT_DATA == update_comm_mode(0)) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("We are still connected...")); |
#endif |
return false; |
} |
// send_cmd(BT_TEST, NULL); |
/* |
if (!send_cmd(BT_DISABLE_AUTOCONNECT, address)) |
return false; |
*/ |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("SET_ADD")); |
#endif |
if (!send_cmd(BT_SET_ADDRESS, address)) |
return false; |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("CONNECT")); |
#endif |
if (!send_cmd(BT_CONNECT, NULL)) |
return false; |
#ifdef DEBUG |
debug_pgm (PSTR ("WAIT FOR COMM")); |
#endif |
return (BT_DATA == update_comm_mode(20000)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_disconnect(void) |
{ |
uint8_t RetVal = false; |
for( uint8_t i = 0; i < 4; i++) // Switch to online cmd mode |
{ |
const char plus = '+'; |
uart_send(&plus, 1); |
_delay_ms(1000); |
} |
comm_mode = BT_CMD; |
EchoAnswerOn = true; |
uart1_flush(); // noch nicht verarbeitete Daten löschen |
fifo_clear(&in_fifo); |
if( !send_cmd(BT_DISCONNECT, NULL) ) |
RetVal = false; |
if( BT_CMD == update_comm_mode(2000) ) // neue Position um Disconnect-Antwort zu finden |
{ |
fifo_clear(&in_fifo); |
RetVal = true; |
} |
if( !send_cmd(BT_CLEAR_ADDRESS, NULL) ) |
RetVal = false; |
// if( BT_CMD == update_comm_mode(2000) ) // 31.7.2015 CB ursprüngliche Position, wurde verschoben |
// { |
// fifo_clear(&in_fifo); |
// return true; |
// } |
#ifdef DEBUG |
debug_pgm(PSTR("Still in DATA??")); |
#endif |
return RetVal; |
} |
/* |
BTM-222 Softwareversion 4.35 |
Inquiry Results: |
111111111222222222233333333334 |
01234567890123456789012345678901234567890 |
1 LE091452 0024-2C-BEB0CA |
2 E71 c 0024-7C-3EC9B9 |
BTM-222 Softwareversion 6.26 |
Inquiry Results: |
1 E71 c 0024-7C-3EC9B9 N.A. |
2 LE091452 0024-2C-BEB0CA N.A. |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_mac(const char *src, char *dst) |
{ |
uint8_t off = 0; |
for (uint8_t i = 0; i < 40; i++) |
{ |
if (src[i] == '-') |
if (src[i+3] == '-')// MAC Adresse suchen |
{ |
off = i-4; |
break; |
} |
} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_localID(const char *src, char *dst) |
{ |
uint8_t off = 0; |
//for (uint8_t i = 0; i < 40; i++) |
//{ |
// if (src[i] == '-') if (src[i+3] == '-')// MAC Adresse suchen |
// { |
// off = i-4; |
// break; |
// } |
//} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_DevName(const char *src, char *dst) |
{ |
uint8_t off = 0; |
uint8_t i = 0; |
for (i = 0; i < 18; i++) |
{ |
if (src[i] == ' ' && src[i+1] == ' ') |
{ |
dst[i] = 0; //Stringende |
break; // nach zwei Leerzeichen ist der Name zuende |
} |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_discover( void ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t devcount = 0; |
uint16_t Timeout = 10000; |
uint8_t progress = 0; |
uint8_t keyesc = false; |
#define BT_SEARCHTIME 7500 // = 75 Sekunden BT-Geraete suchen (60 werden vom BTM222 min. dafuer veranschlagt + 15 Sekunden Sicherheit) |
#define PROGRESSUPDATE 100 // = 1 Sekunde - jede Sekunde die Progressbar updaten bei BT-Inquiry |
//PKT_Progress_Init( max, yoffs, width,height) |
PKT_Progress_Init( 4 + ((BT_SEARCHTIME/PROGRESSUPDATE)+1), 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
//------------------------ |
//------------------------ |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 2 |
send_cmd( BT_TEST, NULL); |
fifo_clear( &in_fifo ); |
if( !send_cmd( BT_FIND_DEVICES, NULL) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 3 |
//------------------------ |
//------------------------ |
do |
{ |
uart_receive(); |
Timeout--; |
_delay_ms(1); |
//write_ndigit_number_u( 0,7,fifo_getcount( &in_fifo) ,5,0,0); |
if( fifo_is_full(&in_fifo) ) |
break; |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
keyesc = true; |
break; |
} |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
fifo_clear( &in_fifo ); |
//------------------------ |
//------------------------ |
if( Timeout!=0 && !keyesc ) |
{ |
// 25.06.2014 OG: umgestellt auf zeitliches Timeout anstatt einer Zaehlschleife |
timer3 = BT_SEARCHTIME; // 75 Sekunden BT Device suchen |
while( timer3 && (!fifo_search( &in_fifo, PSTR("Inquiry End")))) |
{ |
if( (timer3 / PROGRESSUPDATE) != progress ) // Progressbar: next step |
{ |
progress = (timer3 / PROGRESSUPDATE); |
PKT_Progress_Next(); |
} |
uart_receive(); |
_delay_ms(2); // 25.06.2014 OG: diese Verzoegerung muss aktuell sein - sonst werden scheinbar keine Geraete gefunden :-( |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
//keyesc = true; |
break; |
} |
} |
} |
PKT_Progress_Next(); // Progressbar: Extra 4 |
//set_beep( 300, 0xffff, BeepNormal ); _delay_ms(3000); // Test um das Ende der Progressbar zu ueberpruefen |
//------------------------ |
//------------------------ |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
// write_ndigit_number_u( 0,6,devcount ,5,0,0); |
// set_beep( 300, 0xffff, BeepNormal ); |
// _delay_ms(1000); |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
if( strstr_P( buffer, PSTR("Inquiry End"))) |
{ |
break; |
} |
copy_DevName( &buffer[3], device_list[devcount].DevName); |
copy_mac( &buffer[3], device_list[devcount].mac); |
devcount++; |
if( devcount >= NUTS_LIST ) |
{ |
break; |
} |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
return devcount; |
} |
//-------------------------------------------------------------- |
uint8_t bt_showsettings( void ) |
{ |
char buffer[512]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t Timeout = 100; |
bt_rxerror = 0; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart1_flush(); |
PKT_Progress_Init( 4 , 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("1 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("1 E A off") ); |
PKT_Progress_Next(); // Progressbar: Extra 2 |
EchoAnswerOn = true; |
if( !send_cmd( BT_SHOW_SETTINGS, NULL) ) |
ScrollBox_Push_P( MNORMAL, PSTR("Command Error") ); |
PKT_Progress_Next(); // Progressbar: Extra 3 |
do |
{ |
uart_receive(); |
// write_ndigit_number_u( 0,6,fifo_getcount( &in_fifo) ,5,0,0); |
Timeout--; |
_delay_ms(2); |
if( fifo_is_full(&in_fifo) ) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("Fifo is full") ); |
break; |
} |
} while(Timeout != 0 ); |
// } while( (Timeout != 0) && (!fifo_search(&in_fifo, PSTR("+++"))) ); |
PKT_Progress_Next(); // Progressbar: Extra 4 |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
ScrollBox_Push( MNORMAL, buffer ); |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("2 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("2 E A off") ); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
/* |
uint16_t bt_discover_OLD( char result[8][12] ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t pos = 0; |
uint16_t Timeout = 10000; |
uint16_t posC = 0; |
uint8_t Result=false; |
PKT_Progress_Init( 25, 0, 0,0); // 25 Progress Steps |
if (!bt_set_mode(BLUETOOTH_MASTER)) Result = false; |
PKT_Progress_Next(); // Progressbar |
send_cmd(BT_TEST, NULL); |
if( !send_cmd(BT_FIND_DEVICES, NULL)) Result = false; |
PKT_Progress_Next(); // Progressbar |
do |
{ |
uart_receive(); |
Timeout--; |
posC++; |
_delay_ms(1); |
write_ndigit_number_u(0,5,fifo_getcount(&in_fifo),5,0,0); |
if (posC ==1000) |
{ |
PKT_Progress_Next(); // Progressbar |
posC = 0; |
} |
if (fifo_is_full(&in_fifo)) break; |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
if (Timeout==0) |
{ |
Result = false; |
} |
else |
{ |
for (uint16_t i = 0; i < 40000; i++) |
{ |
if( (i % 2000) == 0 ) |
PKT_Progress_Next(); // Progressbar |
uart_receive(); |
write_ndigit_number_u( 0,5,fifo_getcount(&in_fifo),5,0,0); |
_delay_ms(1); |
} |
} |
while( !fifo_is_empty(&in_fifo) ) |
{ |
// Get next line |
while (!fifo_cmp_pgm(&in_fifo, PSTR("\r\n"))) |
{ |
fifo_read(&in_fifo, bufferhead); |
bufferhead++; |
} |
// terminate string |
*bufferhead = 0; |
//reset bufferhead |
bufferhead = buffer; |
if (strlen(buffer) == 0) |
continue; //the empty line before end of inquiry |
if( strstr_P(buffer, PSTR("Inquiry End"))) |
//if (searchend) |
{ |
fifo_clear(&in_fifo); |
return true; |
} |
copy_DevName(&buffer[3],device_list[pos].DevName); |
copy_mac(&buffer[3], device_list[pos].mac); |
pos++; |
} // end: while( !fifo_is_empty(&in_fifo) ) |
return Result; |
} |
*/ |
//-------------------------------------------------------------- |
// ermittelt die MAC vom BTM2222 |
//-------------------------------------------------------------- |
uint8_t bt_getID( void ) |
{ |
//char buffer[255]; //oversized, but who cares? |
char buffer[80]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint8_t i; |
while_timeout( !fifo_strstr_pgm(&in_fifo, PSTR("r\n")), 2000) |
uart_receive(); |
while( !fifo_is_empty(&in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) |
continue; // the empty line before end of inquiry |
copy_localID( &buffer[0], &localID[0] ); |
for( i = 0; i < 13; i++) |
{ |
//lcd_putc (i, 6, localID[i],0); |
Config.bt_Mac[i] = localID[i]; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("OK")) ) |
{ |
fifo_clear( &in_fifo ); |
return true; |
} |
else |
{ |
return false; |
} |
} // end: if( !fifo_is_empty(&in_fifo) ) |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_downlink_init(void) |
{ |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
_delay_ms(100); |
// debug_pgm(PSTR("bt_downlink_init")); |
uart_receive(); |
fifo_clear(&in_fifo); |
// send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init Start")); |
#endif |
if( Config.BTIsSlave == true ) // nur Init wenn BT ist Slave |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:try to set Master")); |
#endif |
//comm_mode = BT_NOECHO; |
// |
//if (!send_cmd (BT_SET_ECHO,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Echo!")); |
// #endif |
//} |
// |
//comm_mode = BT_NOANSWER; |
//if (!send_cmd(BT_SET_ANSWER,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Answer!")); |
// #endif |
// |
//} |
comm_mode = BT_CMD; |
//send_cmd(BT_TEST, NULL); |
bt_set_EchoAnswer(true); |
if( !bt_set_mode(BLUETOOTH_MASTER) ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Couldn't set master!")); |
#endif |
return; |
} |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:master is set ")); |
#endif |
//WriteBTMasterFlag(); // Master merken |
comm_mode = BT_CMD; |
} |
else |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Master was set")); |
#endif |
comm_mode = BT_CMD; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_searchDevice( void ) //Bluetoothgeräte suchen |
{ |
bt_devicecount = bt_discover(); |
} |
// |
////-------------------------------------------------------------- |
//uint16_t bt_receiveNMEA2(void) |
//{ |
// |
// char received; |
// static uint8_t line_flag = 1; |
// static char* ptr_write = bt_rx_buffer; |
// uint16_t timeout_ms=2000; |
// |
//// while_timeout(true, timeout_ms) { |
//while(1){ |
// |
// uart_receive(); |
// if (fifo_is_empty(&in_fifo)) |
// continue; |
// fifo_read(&in_fifo, &received); |
// USART_putc(received); |
//// _delay_ms(1); |
// } |
//// |
//return true; |
//} |
#endif |
#endif |
//#endif |
#endif // end: #ifdef USE_BLUETOOTH |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/bluetooth.h |
---|
0,0 → 1,182 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY bluetooth.h |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover() Parameter |
//# |
//# 16.06.2014 OG |
//# - add: BTM222_Initalize() |
//# - del: bt_init() |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() |
//############################################################################ |
#ifndef _BLUETOOTH_H_ |
#define _BLUETOOTH_H_ |
#include <avr/io.h> |
//#include <common.h> |
#include "fifo.h" |
#define SQUIRREL |
#define NUTS_LIST 16 |
#define EXTENSIONS_LIST 16 |
#define RXD_BUFFER_SIZE 150 |
//void InitBT(void); |
extern char bt_rx_buffer[RXD_BUFFER_SIZE]; |
extern volatile uint8_t bt_rx_len; |
extern volatile uint8_t bt_rx_ready; |
//extern char data_decode[RXD_BUFFER_SIZE]; |
extern uint8_t bt_rxerror; |
extern uint8_t bt_frameerror; |
extern uint8_t bt_overrun; |
extern uint8_t bt_bufferoverflow; |
extern fifo_t in_fifo; |
typedef struct _device_info device_info; |
// device info struct, holds mac , class and extensions + values of a device |
struct _device_info |
{ |
char DevName[20]; |
char mac[14]; |
// uint8_t class; |
// uint8_t extension_types[EXTENSIONS_LIST]; |
// uint16_t values_cache[EXTENSIONS_LIST]; |
}; |
extern uint8_t bt_devicecount; |
extern device_info device_list[NUTS_LIST]; |
extern char localID[15]; |
#define valid(num) (num < NUTS_LIST && (device_list[num].mac[0] != 0 || device_list[num].mac[1] != 0 || device_list[num].mac[2] != 0 || device_list[num].mac[3] != 0 || device_list[num].mac[4] != 0 || device_list[num].mac[5] != 0 || device_list[num].mac[6] != 0 || device_list[num].mac[7] != 0 || device_list[num].mac[8] != 0 || device_list[num].mac[9] != 0 || device_list[num].mac[10] != 0 || device_list[num].mac[11] != 0)) |
extern void bt_start(void); // EchoAnswervariable setzen |
void bt_set_EchoAnswer (uint8_t onoff); |
extern uint8_t bt_getID (void); |
//extern static communication_mode_t update_comm_mode(uint16_t timeout_ms); |
// Bluetooth mode ENUM |
typedef enum |
{ |
BLUETOOTH_MASTER, // < Master Mode (to create outgoinng connections). |
BLUETOOTH_SLAVE // < Slave Mode (to wait for incoming connections). |
} bt_mode_t; |
// set baudrate bluetooth |
// @return true = ok |
// false = error |
//extern uint16_t bt_setbaud (uint8_t baudrate); |
uint16_t bt_setbaud(uint8_t baudrate); |
// init bluetooth driver |
// @return always true |
// |
//extern uint16_t bt_init (void (*upate_percentage) (uint16_t)); |
//extern uint16_t bt_init (void); |
// Set the Bluetooth mode |
// @param mode bt_mode_t Bluetooth Mode ENUM (BLUETOOTH_MASTER or BLUETOOTH_SLAVE) |
// @return true if mode change was succesful, false if not |
// |
extern uint8_t bt_set_mode (const bt_mode_t mode); |
// recieve data over bluetooth |
// @param data pointer to memory for data storage |
// @param length value of length after call holds the actual recived data length |
// @param timeout_ms timeout in ms after the recive function aborts and returns with false |
// @return false if recived length > length parameter or it timeouted, true otherwise |
// |
extern uint16_t bt_receive (void * data, uint8_t length, uint16_t timeout_ms); |
// send data over bluetooth |
// @param data pointer to the data to send |
// @param length length of the data to be send |
// @return true if sendingn was successful, false otherwise |
// |
extern uint16_t bt_send (void * data, const uint8_t length); |
// squirrelt only functions |
#ifdef SQUIRREL |
// open bluetoot connection (only one at a time possible) |
// @param address connection is opened to this device mac address |
// @return true if connection was established, false otherwise |
// |
extern uint16_t bt_connect (const char *address); |
// closes bluetooth connection |
// @return false if failed, true otherwise |
// |
extern uint16_t bt_disconnect (void); |
// discover bluetooth devices |
// @param result in a 2 dimensional array first index are devicecs (max 8) second is mac address string |
// @param update_callback to inform of progress (in % from 0 to 100) |
// @return true if successful, false if error occured |
// |
//extern uint16_t bt_discover(void); |
//extern uint16_t bt_discover (char result[8][12], void (*update_callback)(const uint16_t progress)); |
extern void bt_downlink_init(void); // Auf Master stellen für Devicesuche und GPS Empfang |
extern void bt_searchDevice(void); //Bluetoothgeräte suchen |
#endif // SQUIRREL |
void uart_receive(void); |
void bt_fixname(void); |
void BTM222_Initalize( void ); |
uint8_t bt_showsettings( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/error.c |
---|
0,0 → 1,218 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
** |
* Error handling functions |
*/ |
//############################################################################ |
//# HISTORY error.c |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//########################################################################## |
#include <stdbool.h> |
#include <avr/pgmspace.h> |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
//-------------------------------------------------------------- |
inline void _send_msg(const char *msg) |
{ |
for (uint8_t i=0; i<255 && msg[i]!='\0'; i++) |
{ |
errordriver_write_c(msg[i]); |
} |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void send_pgm(const char *msg) |
{ |
uint8_t myByte; |
myByte = pgm_read_byte(msg); |
for(int i = 1; myByte != '\0'; i++) |
{ |
errordriver_write_c(myByte); |
myByte = pgm_read_byte(msg+i); |
} |
} |
//-------------------------------------------------------------- |
void error_init(void) |
{ |
error_driver_Init(); |
} |
//-------------------------------------------------------------- |
void error_putc(const char c) |
{ |
errordriver_write_c(c); |
} |
//-------------------------------------------------------------- |
void assert (bool condition, const char *msg) |
{ |
if (!condition) |
{ |
send_pgm(PSTR("ASS:")); |
_send_msg(msg); |
} |
} |
//-------------------------------------------------------------- |
void info (const char *msg) |
{ |
send_pgm(PSTR("INF:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void warn (const char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void debug (const char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void Error (const char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
_send_msg(msg); |
} |
#endif |
#ifdef DEBUG |
//-------------------------------------------------------------- |
void assert_pgm(bool condition, const prog_char *msg) |
{ |
if (condition) { |
send_pgm(PSTR("ASS:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
} |
//-------------------------------------------------------------- |
void info_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("INF:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void warn_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void error_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void debug_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
////-------------------------------------------------------------- |
//void print_hex(uint8_t num) |
//{ |
// if (num<10) |
// error_putc(num+48); |
// else |
// { |
// switch (num) |
// { |
// case 10: |
// error_putc('A'); break; |
// case 11: |
// error_putc('B'); break; |
// case 12: |
// error_putc('C'); break; |
// case 13: |
// error_putc('D'); break; |
// case 14: |
// error_putc('E'); break; |
// case 15: |
// error_putc('F'); break; |
// default: |
// error_putc('#'); break; |
// } |
// } |
//} |
// |
// |
////-------------------------------------------------------------- |
//void byte_to_hex(uint8_t byte) |
//{ |
// uint8_t b2 = (byte & 0x0F); |
// uint8_t b1 = ((byte & 0xF0)>>4); |
// print_hex(b1); |
// print_hex(b2); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/error.h |
---|
0,0 → 1,79 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
* |
* Error handling functions. |
*/ |
#ifndef __ERROR__ |
#define __ERROR__ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include "../main.h" |
#ifdef DEBUG |
void error_init(void); |
void error_putc(const char c); |
void assert (bool condition, const char *msg); |
void info (const char *msg); |
void warn(const char *msg); |
void debug(const char *msg); |
void Error(const char *msg); |
void assert_pgm(bool condition, const prog_char *msg); |
void info_pgm (const prog_char *msg); |
void warn_pgm(const prog_char *msg); |
void debug_pgm(const prog_char *msg); |
void error_pgm(const prog_char *msg); |
void byte_to_hex(uint8_t byte); |
#else |
#define error_init() {} |
#define error_putc(c) {} |
#define assert(cond, msg) {} |
#define info(msg) {} |
#define warn(msg) {} |
#define debug(msg) {} |
#define error(msg) {} |
#define assert_pgm(cond, msg) {} |
#define info_pgm(msg) {} |
#define warn_pgm(msg) {} |
#define debug_pgm(msg) {} |
#define error_pgm(msg) {} |
#define byte_to_hex(byte) {} |
#endif |
#endif //__ERROR__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/error_driver.c |
---|
0,0 → 1,27 |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
//#include "cpu.h" |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
void errordriver_write_c(char c) |
{ |
USART_putc(c); |
} |
void error_driver_Init(void) |
{ |
// USART_Init(UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/error_driver.h |
---|
0,0 → 1,22 |
/* |
* Functions to write error message to FTDI or USART |
*/ |
#ifndef __ERROR_DRIVER__ |
#define __ERROR_DRIVER__ |
#include <avr/io.h> |
#include "../main.h" |
#ifdef DEBUG |
extern void errordriver_write_c(char c); |
extern void error_driver_Init(void); |
#else |
#define errordriver_write_c(c) {} |
#define error_driver_init() {} |
#endif |
#endif //__ERROR_DRIVER__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/fifo.c |
---|
0,0 → 1,197 |
/** |
* a simple Fifo |
* @file fifo.c |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//############################################################################ |
#include <string.h> |
#include "fifo.h" |
//#if defined HWVERSION1_3W || defined HWVERSION3_9 || defined HWVERSION1_2W |
//-------------------------------------------------------------- |
void fifo_init (fifo_t * fifo, uint8_t * buffer, uint16_t size) |
{ |
fifo->size = size; |
fifo->buffer = buffer; |
fifo->head = 0; |
fifo->count = 0; |
} |
//-------------------------------------------------------------- |
uint16_t fifo_getcount (const fifo_t * fifo) |
{ |
return fifo->count; |
} |
//-------------------------------------------------------------- |
bool fifo_is_empty (const fifo_t * fifo) |
{ |
return (fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_is_full (const fifo_t * fifo) |
{ |
return (fifo->size - fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_read (fifo_t * fifo, char *data) |
{ |
if (fifo_is_empty (fifo)) |
return false; |
uint8_t *head = fifo->buffer + fifo->head; |
*data = (char) * head; |
fifo->head = ( (fifo->head + 1) % fifo->size); |
fifo->count--; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_write (fifo_t * fifo, const char data) |
{ |
if (fifo_is_full (fifo)) |
return false; |
uint8_t *end = fifo->buffer + ( (fifo->head + fifo->count) % fifo->size); |
*end = (uint8_t) data; |
fifo->count++; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_clear (fifo_t * fifo) |
{ |
fifo->count = 0; |
fifo->head = 0; |
return true; |
} |
//-------------------------------------------------------------- |
static bool fifo_cmp_pgm_at (fifo_t * fifo, const char * pgm, const uint16_t index) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= (i + index)) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i + index) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// We found the string, lets move the pointer |
fifo->head = ( (fifo->head + i + index) % fifo->size); |
fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_search (fifo_t * fifo, const char * pgm) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= i) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// // We found the string, lets move the pointer |
// fifo->head = ( (fifo->head + i + index) % fifo->size); |
// |
// fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t * fifo, const char * pgm) |
{ |
return fifo_cmp_pgm_at (fifo, pgm, 0); |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t * fifo, const char * pgm) |
{ |
for (uint16_t i = 0; i < fifo->count; i++) |
{ |
if (fifo_cmp_pgm_at (fifo, pgm, i)) |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Idee: buffer nach str durchsuchen ohne die fifo Zeiger zu aendern |
// ungetestet |
//-------------------------------------------------------------- |
bool fifo_isstr( fifo_t * fifo, const char * str) |
{ |
/* |
void *memmem(const void *s1, |
size_t len1, |
const void *s2, |
size_t len2); |
The memmem() function finds the start of the first occurrence of the substring s2 of length len2 in the memory area s1 of length len1. |
*/ |
return memmem( fifo->buffer + fifo->head, fifo->count, str, strlen(str) ) != NULL; |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth/fifo.h |
---|
0,0 → 1,107 |
/** |
* a simple Fifo |
* @file fifo.h |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//############################################################################ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#ifndef _FIFO_H_ |
#define _FIFO_H_ |
/** |
*fifo data structure all vital fifo information |
*/ |
typedef struct |
{ |
uint16_t count; /**< current number of elements */ |
uint16_t head; /**< position of the head element */ |
uint16_t size; /**< size equals max number of entrys*/ |
uint8_t* buffer; /**< pointer to memory area where the fifo is to be saved */ |
} fifo_t; |
uint16_t fifo_getcount (const fifo_t * fifo); |
/** \brief initialize of a fifo |
* sets all the information in your given fifo structure |
* @param fifo pointer to an allocated fifo_t structure |
* @param buffer pointer to an a allocated memory space for the fifo of size = sizeof(uint8_t) * size |
* @param size max number of entrys the fifo will hold |
*/ |
void fifo_init (fifo_t *fifo, uint8_t *buffer, uint16_t size); |
/** \brief checks if fifo is empty |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if empty otherwise false |
*/ |
bool fifo_is_empty (const fifo_t *fifo); |
/** \brief checks if fifo is full |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if full otherwise false |
*/ |
bool fifo_is_full (const fifo_t *fifo); |
/** \brief clears the fifo |
* resets your fifo structure to 0 elements |
* @param fifo pointer to your initialized fifo_t structure |
* @return always true (never fails) |
*/ |
bool fifo_clear (fifo_t *fifo); |
/** \brief reads head of fifo |
* reads the first element and removes it |
* @param fifo pointer to your initialized fifo_t structure |
* @return false if fifo is empty false otherwise |
*/ |
bool fifo_read (fifo_t *fifo, char *data); |
/** \brief inserts a char into the fifo |
* adds a char to the end of the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param data the char data to be inserted |
* @return false if fifo is full true otherwise |
*/ |
bool fifo_write (fifo_t *fifo, const char data); |
/** \brief compares first elements with prog_char string |
* if pgm equals the first elements of the fifo these elements are removed |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char string for comparison |
* @return true if pgm is equal to the first entrys in the fifo, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t* fifo, const char* pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_search (fifo_t * fifo, const char * pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t *fifo, const char *pgm); |
bool fifo_isstr( fifo_t * fifo, const char * str); |
#endif /* _FIFO_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/bluetooth |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/connect.c |
---|
0,0 → 1,436 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.c |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: wait_and_restore() redundanten Code einsparen |
//# - add: Port_BLE2Wi() (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 28.02.2014 OG |
//# - chg: alle Anzeigescreens von 'PKT Connect' auf Connect_Screen() umgestellt |
//# - add: Connect_Screen() |
//# |
//# 12.02.2014 OG |
//# - chg: Port_WiFly2Wi() Auskommentiert: "unused variable 'i'" |
//# |
//# 15.07.2013 Cebra |
//# - chg: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC, Texte geändert |
//# |
//# 02.07.2013 Cebra |
//# - add: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# - add: Port_WiFly2Wi, Wlan-Wi232 Verbindung |
//# |
//# 24.6.2013 Cebra |
//# - fix: Fehler beim UART1 bei der Rückschaltung von USB2FC,USB2WI,BT2FC,BT2WI |
//# |
//# 19.06.2013 OG |
//# - chg: Screen-Layout von Port_BT2Wi(), Port_USB2Wi() |
//############################################################################ |
#include "cpu.h" |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "lcd/lcd.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lipo/lipo.h" |
#include "main.h" |
#include "bluetooth/bluetooth.h" |
#include "wifly/PKT_WiFlyHQ.h" |
#include "uart/uart1.h" |
#include "utils/xutils.h" |
//#if defined HWVERSION3_9 |
//-------------------------------------------------------------- |
void Change_Output(uint8_t UartMode) // Schaltet die Rx/Tx Richtungen |
{ |
// hiermit werden die 74HTC125 (IC5) Gatter geschaltet |
clr_USB2FC(); // PC2 aus IC5 A/D |
clr_USB2Wi(); // PB0 aus IC5 B/C |
clr_Uart02FC(); // PC6 aus IC4 C/D |
clr_Uart02Wi(); // PC5 aus IC4 A/B |
switch (UartMode) |
{ |
case USB2FC: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2FC(); |
break; |
case Uart02Wi: |
set_Uart02Wi(); |
break; |
case Uart02FC: |
set_Uart02FC(); |
break; |
case USB2Wi: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2Wi(); |
break; |
} |
} |
//-------------------------------------------------------------- |
// gibt eine uebergebene Baud-Rate als uint32_t zurueck |
//-------------------------------------------------------------- |
uint32_t Baud_to_uint32( uint8_t baud ) |
{ |
switch( baud ) |
{ |
case Baud_2400: return( 2400 ); |
case Baud_4800: return( 4800 ); |
case Baud_9600: return( 9600 ); |
case Baud_19200: return( 19200 ); |
case Baud_38400: return( 38400 ); |
case Baud_57600: return( 57600 ); |
case Baud_115200: return( 115200 ); |
} |
return( 0 ); |
} |
//-------------------------------------------------------------- |
// Connect_Screen( |
// |
// Parameter: |
// title : PROGMEM |
// message : RAM oder PROGMEM je nach msg_progmem |
// msg_progmem: true/false |
// true = message im PROGMEM |
// false = message im RAM |
//-------------------------------------------------------------- |
void Connect_Screen( const char *title, const char *message, uint8_t msg_progmem ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, title, MINVERS); // Titel |
show_Lipo(); |
lcdx_printp_center( 2, title , MNORMAL, 0,-3); // |
lcdx_printp_center( 3, strGet(STR_ACTIVE), MNORMAL, 0,1); // "AKTIV!" |
if( (!msg_progmem && strlen(message)>0) || (msg_progmem && strlen_P(message)>0) ) |
{ |
if( msg_progmem ) |
lcdx_printp_center( 5, message, MNORMAL, 0,1); |
else |
lcdx_print_center( 5, (unsigned char *) message, MNORMAL, 0,1); |
lcd_rect_round( 0, 5*7+2, 127, 8+6, 1, R2); // Rahmen |
} |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// _port_wait_and_restore() |
// |
// Wartet bis der Benutzer beendet und restauriert Einstellungen |
// |
// gemeinsamer Code um Speicherplatz zu sparen |
//-------------------------------------------------------------- |
void wait_and_restore( void ) |
{ |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
} |
uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); |
SetBaudUart1( Config.PKT_Baudrate ); |
if( Config.U02SV2 == 1 ) |
Change_Output( Uart02FC ); |
else |
Change_Output( Uart02Wi ); |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Function: BT2FC() |
// Purpose: Connect BT direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2FC(void) |
{ |
/* |
// mit Anzeige von 'Baud' |
// verbraucht einige Bytes mehr... |
char *message; |
uint32_t baud; |
switch( Config.PKT_Baudrate ) |
{ |
case Baud_2400: baud = 2400; break; |
case Baud_4800: baud = 4800; break; |
case Baud_9600: baud = 9600; break; |
case Baud_19200: baud = 19200; break; |
case Baud_38400: baud = 38400; break; |
case Baud_57600: baud = 57600; break; |
case Baud_115200: baud = 115200; break; |
} |
message = buffered_sprintf_P( PSTR("%lu Baud"), baud); |
Connect_Screen( PSTR("BT -> SV2 Kabel"), message, false ); |
*/ |
// ohne Anzeige von 'Baud' |
Connect_Screen( PSTR("BT -> SV2 Kabel"), "", false ); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
_delay_ms(1000); |
//_delay_ms(2000); |
if( !Config.BTIsSlave==true ) |
bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2FC); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BT2Wi() |
// Purpose: Connect BT direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2Wi( void ) |
{ |
char *message = "RE-ID: 0000"; |
strcpy( &message[7], Config.RE_ID); |
Connect_Screen( PSTR("BT Extender"), message, false ); |
set_Modul_On( Bluetooth ); // set_BTOn(); |
_delay_ms(1500); |
if( !Config.BTIsSlave==true ) bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: WiFly2Wi() |
// Purpose: Connect WiFly direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_WiFly2Wi( void ) |
{ |
Connect_Screen( PSTR("WLAN Extender"), PSTR("169.254.1.1:2000"), true ); |
set_Modul_On( Wlan ); |
Change_Output( USB2Wi ); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BLE2Wi() |
// Purpose: Connect BLE direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_BLE2Wi( void ) |
{ |
Connect_Screen( PSTR("BLE Extender"), "", false ); |
set_Modul_On(Wlan); // verwendet wie Wlan/WiFly SV2 |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: FC2CFG_BT() |
// Purpose: Connect FC (Tx1 Pin3, Rx1 Pin4) direct to BT |
// Returns: |
//-------------------------------------------------------------- |
void Port_FC2CFG_BT(void) |
{ |
lcd_cls (); |
lcd_printp_at (0, 0, PSTR("BTM-222 Konfigurieren"), 2); |
lcd_printp_at (0, 1, PSTR("FC > MK-USB > BTM-222"), 2); |
lcd_printp_at (0, 3, PSTR("MK-USB an PC anschl. "), 0); |
lcd_printp_at (0, 4, PSTR("Zwischen MK-USB und "), 0); |
lcd_printp_at (0, 5, PSTR("PKT ein gekreuztes "), 0); |
lcd_printp_at (0, 6, PSTR("Kabel anschliessen. "), 0); |
lcd_printp_at(12, 7, strGet(ESC), 0); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
Change_Output(USB2FC); |
while(!get_key_press (1 << KEY_ESC)); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
set_Modul_On(USB); |
// set_BTOff(); |
return; |
} |
//-------------------------------------------------------------- |
// Function: USB2FC() |
// Purpose: Connect USB direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2FC(void) |
{ |
Connect_Screen( PSTR("USB -> SV2 Kabel"), PSTR("MK-USB"), true ); |
Change_Output(USB2FC); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} while(!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
uart1_init (UART_BAUD_SELECT(57600,F_CPU)); |
SetBaudUart1(Config.PKT_Baudrate); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
return; |
*/ |
} |
//-------------------------------------------------------------- |
// Function: USB2Wi() |
// Purpose: Connect USB direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2Wi(void) |
{ |
Connect_Screen( PSTR("USB -> Wi.232"), "", false ); |
Change_Output(USB2Wi); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} |
while( !get_key_press(1 << KEY_ESC) ); |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
get_key_press(KEY_ALL); |
return; |
*/ |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/connect.h |
---|
0,0 → 1,68 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.h |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: Port_BLE2Wi() |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _CONNECT_H |
#define _CONNECT_H |
void Change_Output(uint8_t UartMode); |
void Port_BT2Wi(void); |
void Port_BT2FC(void); |
void Port_WiFly2Wi( void ); |
void Port_FC2CFG_BT(void); |
void Port_USB2FC(void); |
void Port_USB2Wi(void); |
void Port_BLE2Wi( void ); |
uint32_t Baud_to_uint32( uint8_t baud ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/cpu.h |
---|
0,0 → 1,41 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _CPU_H |
#define _CPU_H |
// Quarz Frequenz in Hz |
#define F_CPU 20000000UL |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/eeprom/eeprom.c |
---|
0,0 → 1,830 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.c |
//# |
//# 14.10.2015 Starter |
//# - chg: rename FM_Azimuth and FM_Distance to FM_Offest_lat and FM_Offset_long |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# Initialisierung der neuen Parameter hinzugefügt |
//# |
//# 17.06.2014 OG |
//# - chg: ReadParameter() - Updatecode auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: Delete_EEPROM(() Unterstuetzung Config.PKTOffTimeout (ehemals LCD_DisplayMode) |
//# |
//# 30.05.2014 OG |
//# - chg: Delete_EEPROM() Config.Lipo_UOffset auf 10000 gesetzt (vorher 8500) |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# - fix: ReadParameter() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# ab EEprom-Version 138 |
//# - fix: Delete_EEPROM() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# - fix: ReadParameter() - Kommentare zu Version 136 und 137 korrigiert |
//# bzgl. der PKT-Versionsangabe und Datum ergaenzt |
//# |
//# 06.04.2014 OG |
//# - chg: ReadParameter(): umgestellt auf lcdx_printp_center() |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: ReadParameter(): upgrade Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# - add: ReadParameter(): upgrade Config.UseBLE, Config.MKParam_Favs |
//# - add: Delete_EEPROM(): init von Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# |
//# 01.04.2014 OG |
//# - add: Delete_EEPROM(): init von Config.UseBLE |
//# - add: Delete_EEPROM(): init von Config.MKParam_Favs |
//# |
//# 27.03.2014 OG |
//# - chg: ReadParameter() Niederländisch wird auf Englisch umgeschaltet |
//# -> Vorbereitung zum entfernen der Niederländischen Sprache |
//# |
//# 11.02.2014 Cebra |
//# - add: Delete EEProm: Config.OSD_ShowCellU = false; |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU in ReadParameter (EEpromversion 0x87) |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 07.07.2013 OG |
//# - chg: Strings fuer WLan gekuerzt |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 0x85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 0x84 |
//# |
//# 03.07.2013 OG |
//# - chg: ReadParameter() - PKT-Upgrade restrukturiert bzgl. inkrementelle |
//# Updates vorheriger Versionen |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 02.07.2013 OG |
//# - chg: frei geworden: Config.OSD_HomeMKView (nicht mehr benoetigt) |
//# |
//# 26.06.2013 OG |
//# - chg: frei geworden: Config.PKT_StartInfo (nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - fix: ReadParameter(): ergaenzt um bt_fixname() um ggf. ungueltige |
//# Zeichen im Bluetooth-Namen zu eliminieren |
//# |
//# 23.06.2013 OG |
//# - chg: Delete_EEPROM(): Default von Config.Lipo_UOffset von 6000 auf 8500 gesetzt |
//# - chg: Delete_EEPROM(): Default von Config.bt_name auf "PKT" gekuerzt |
//# - chg: Delete_EEPROM(): Config.bt_name -> nicht mehr mit Leerzeichen auffuellen! |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 13.06.2013 OG |
//# - fix: Config.PKT_Accutyp hinzugefuegt in Delete_EEPROM() |
//# - chg: Default GPS-LastPosition auf 0/0 gesetzt in Delete_EEPROM() |
//# - chg: Default Config.PKT_StartInfo auf false gesetzt in Delete_EEPROM() |
//# - chg: Code Layout |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung auf Version 81, Versionsanpassung angepasst |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup EEPROM Parameter |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <avr/interrupt.h> |
#include <avr/eeprom.h> |
#include <stdbool.h> |
#include <avr/wdt.h> |
#include "../lcd/lcd.h" |
#include "../main.h" |
#include "../timer/timer.h" |
#include "eeprom.h" |
#include "../wi232/Wi232.h" |
#include "../setup/setup.h" |
#include "../bluetooth/bluetooth.h" |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../osd/osd.h" |
#include "../uart/uart1.h" |
#include "../messages.h" |
#include "../osd/osddata.h" |
#include "../mksettings/paramset.h" |
//------------------------------------------------------------------------------------------ |
ST EEMEM EEStruct; |
ST Config; |
//############################################################# |
// ANMERKUNG OG: |
// |
// es ware besser wenn die gesamte Config-struct zum PC |
// gesendet wird anstatt aufbreitete Textlines. Das spart |
// Speicher im PKT (der ist limitiert). |
// |
// Die Aufbereitung der Daten (Umrechnungen) kann dann auf |
// der PC Seite erfolgen. |
// |
// Nachteil dabei: wenn sich die Config-struct des PKT |
// ändert muss auch das PKT-Tool auf dem PC angepasst werden. |
// |
//CB: deshalb ist das auch jetzt auskommentiert |
// |
//############################################################# |
//char printbuff[100]; |
//void print_data_int8( const char *text, int8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
//// |
//// |
//void print_data_uint8( const char *text, uint8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_uint16( const char *text, uint16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_int16( const char *text, int16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_char( const char *text, char *variable, uint8_t length) |
//{ |
// uart1_puts_p(text); |
// uart1_puts(variable); |
// uart1_puts("\r\n"); |
//} |
// |
// |
//void print_time( const char *text, PKTdatetime_t variable) |
//{ |
// uart1_puts_p(text); |
// sprintf(printbuff, "%d", variable.day); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.month); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.year); |
// uart1_puts(printbuff); |
//// uart1_puts(" "); |
//// sprintf(printbuff, "%d", variable.seconds); |
//// uart1_puts(printbuff); |
// uart1_puts("\r\n"); |
//} |
// |
// |
// |
//void print_OSD_Statistic(void) |
//{ |
// print_time (PSTR("begin_StatTime:"),Config.OSD_Statistic.begin_StatTime); |
// print_time (PSTR("end_StatTime:"),Config.OSD_Statistic.end_StatTime); |
// print_data_uint16 (PSTR("total_FlyTime:"),&Config.OSD_Statistic.total_FlyTime); |
// print_data_uint16 (PSTR("last_FlyTime:"),&Config.OSD_Statistic.last_FlyTime); |
// print_data_uint16 (PSTR("count_Errorcode:"),&Config.OSD_Statistic.count_Errorcode); |
// print_data_int16 (PSTR("max_Altimeter:"),&Config.OSD_Statistic.max_Altimeter); |
// print_data_uint16 (PSTR("max_GroundSpeed:"),&Config.OSD_Statistic.max_GroundSpeed); |
// print_data_uint16 (PSTR("max_Distance:"),&Config.OSD_Statistic.max_Distance); |
// print_data_uint16 (PSTR("max_Current:"),&Config.OSD_Statistic.max_Current); |
// print_data_uint16 (PSTR("max_Capacity:"),&Config.OSD_Statistic.max_Capacity); |
// print_data_int16 (PSTR("max_Variometer:"),&Config.OSD_Statistic.max_Variometer); |
// print_data_int8 (PSTR("max_AngleNick:"),&Config.OSD_Statistic.max_AngleNick); |
// print_data_int8 (PSTR("max_AngleRoll:"),&Config.OSD_Statistic.max_AngleRoll); |
// print_data_uint8 (PSTR("RC Quality:"),&Config.OSD_Statistic.max_RCQuality); |
// print_data_int16 (PSTR("max_TopSpeed:"),&Config.OSD_Statistic.max_TopSpeed); |
// print_data_int8 (PSTR("min_AngleNick:"),&Config.OSD_Statistic.min_AngleNick); |
// print_data_int8 (PSTR("min_AngleRoll:"),&Config.OSD_Statistic.min_AngleRoll); |
// print_data_uint8 (PSTR("min_RCQuality:"),&Config.OSD_Statistic.min_RCQuality); |
// print_data_uint8 (PSTR("min_UBat:"),&Config.OSD_Statistic.min_UBat); |
// print_data_uint8 (PSTR("LiPoCells:"),&Config.OSD_Statistic.LiPoCells); |
// print_data_uint8 (PSTR("BL_Count:"),&Config.OSD_Statistic.BL_Count); |
//} |
//void SendConfigData(void) |
//{ |
// print_data_uint8 (strGet(START_VERSIONCHECK),&Config.Version ); |
// print_data_uint8 (strGet(LOWBAT),&Config.MK_LowBat ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayTimeout ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayLanguage ); |
// print_data_uint8 (strGet(WITXRX),&Config.WiTXRXChannel); |
// print_data_uint8 (strGet(WINETWG),&Config.WiNetworkGroup); |
// print_data_uint8 (strGet(WINETWM),&Config.WiNetworkMode); |
// print_data_uint8 (strGet(WITIMEOUT),&Config.WiTXTO); |
// print_data_uint8 (strGet(WIUART),&Config.WiUartMTU); |
// print_data_uint8 (strGet(DISPLAY8),&Config.OSD_ShowMKSetting); |
// print_data_uint8 (strGet(DISPLAY7),&Config.LCD_DisplayMode); |
// print_data_uint8 (strGet(DISPLAY6),&Config.LCD_Kontrast); |
// print_data_uint8 (strGet(DISPLAY5),&Config.LCD_Helligkeit); |
// print_data_uint8 (PSTR("USB-Betrieb:"),&Config.USBBT); |
// print_data_uint8 (PSTR("Uart-FC/Wi:"),&Config.U02SV2); |
// print_data_uint8 (PSTR("PKT-Debug:"),&Config.Debug); |
// print_data_uint8 (PSTR("Wi232 eingebaut:"),&Config.UseWi); |
// print_data_uint8 (PSTR("BTM222 eingebaut:"),&Config.UseBT); |
// print_data_uint8 (PSTR("Wi232 ok:"),&Config.WiIsSet); |
// print_data_uint8 (PSTR("BTM222 ok:"),&Config.BTIsSet); |
// print_data_uint8 (PSTR("BTM222 Slave:"),&Config.BTIsSlave); |
// |
// print_data_char (PSTR("BTM222 Pin:"),&Config.bt_pin[0],bt_pin_length ); |
// print_data_char (PSTR("BTM222 Name:"),&Config.bt_name[0],bt_name_length); |
// print_data_char (PSTR("BTM222 REID:"),&Config.RE_ID[0],RE_ID_length ); |
// print_data_char (PSTR("BTM222 MAC:"),&Config.bt_Mac[0],bt_mac_length); |
// print_data_char (PSTR("GPS DevName:"),&Config.gps_UsedDevName[0],20); |
// print_data_char (PSTR("GPS MAC:"),&Config.gps_UsedMac[0],14); |
// |
// print_data_int32 (PSTR("LastLongitude:"),&Config.LastLongitude); |
//// print_data_int32(PSTR("LastLatitude:"),&Config.LastLatitude); |
// |
// print_data_uint8 (PSTR("PKT_IdleBeep:"),&Config.PKT_IdleBeep); |
// print_data_uint8 (strGet(DISPLAY2),&Config.PKT_StartInfo); |
// print_data_uint16 (strGet(LIPO3),&Config.Lipo_UOffset); |
// print_data_uint8 (strGet(LIPO2),&Config.PKT_Accutyp); |
// print_data_uint8 (strGet(DISPLAY9),&Config.OSD_RCErrorbeep); |
// print_data_uint8 (strGet(OSD_Invert_Out),&Config.OSD_InvertOut); |
// print_data_uint8 (strGet(OSD_LED_Form),&Config.OSD_LEDform); |
// print_data_uint8 (strGet(OSD_Send_OSD),&Config.OSD_SendOSD); |
// print_data_uint8 (strGet(FALLSPEED),&Config.OSD_Fallspeed); |
// print_data_uint8 (strGet(OSD_VARIOBEEP),&Config.OSD_VarioBeep); |
// print_data_uint8 (strGet(OSD_HOMEMKVIEW),&Config.OSD_HomeMKView); |
// print_data_uint16 (strGet(OSD_MAHWARNING),&Config.OSD_mAh_Warning); |
// print_data_uint8 (strGet(OSD_SCREENMODE),&Config.OSD_ScreenMode); |
// print_data_uint8 (strGet(OSD_LIPOBAR),&Config.OSD_LipoBar); |
// print_data_uint8 (strGet(PKT_BAUDRATE),&Config.PKT_Baudrate); |
// print_data_uint16 (strGet(FOLLOWME_1),&Config.FM_Refresh); |
// print_data_uint16 (strGet(FOLLOWME_2),&Config.FM_Speed); |
// print_data_uint16 (strGet(FOLLOWME_3),&Config.FM_Radius); |
// print_data_uint8 (strGet(HWSOUND),&Config.HWSound); |
// print_data_uint8 (strGet(HWBEEPER),&Config.HWBeeper); |
// print_data_uint8 (PSTR("sIdxSteps:"),&Config.sIdxSteps); |
// print_data_uint8 (strGet(SV_TEST3),&Config.servo_frame); |
// print_data_uint8 (strGet(SV_SINGLESTEP),&Config.single_step); |
// print_data_uint8 (strGet(SV_COUNTTEST),&Config.repeat); |
// print_data_uint8 (strGet(SV_PAUSEEND),&Config.pause); |
// print_data_uint8 (strGet(SV_PAUSEINC),&Config.pause_step); |
//// print_data_int8 (,&Config.tracking); |
//// print_data_int8 (,&Config.track_tx); |
// print_data_uint16 (PSTR("Stick1_min:"),&Config.stick_min[0]); |
// print_data_uint16 (PSTR("Stick2_min:"),&Config.stick_min[1]); |
// print_data_uint16 (PSTR("Stick3_min:"),&Config.stick_min[2]); |
// print_data_uint16 (PSTR("Stick4_min:"),&Config.stick_min[3]); |
// print_data_uint16 (PSTR("Stick5_min:"),&Config.stick_min[4]); |
// print_data_uint16 (PSTR("Stick1_max:"),&Config.stick_max[0]); |
// print_data_uint16 (PSTR("Stick2_max:"),&Config.stick_max[1]); |
// print_data_uint16 (PSTR("Stick3_max:"),&Config.stick_max[2]); |
// print_data_uint16 (PSTR("Stick4_max:"),&Config.stick_max[3]); |
// print_data_uint16 (PSTR("Stick5_max:"),&Config.stick_max[4]); |
// print_data_uint8 (PSTR("Stick1_typ:"),&Config.stick_typ[0]); |
// print_data_uint8 (PSTR("Stick2_typ:"),&Config.stick_typ[1]); |
// print_data_uint8 (PSTR("Stick3_typ:"),&Config.stick_typ[2]); |
// print_data_uint8 (PSTR("Stick4_typ:"),&Config.stick_typ[3]); |
// print_data_uint8 (PSTR("Stick5_typ:"),&Config.stick_typ[4]); |
// print_data_uint8 (PSTR("Stick1_neutral:"),&Config.stick_neutral[0]); |
// print_data_uint8 (PSTR("Stick2_neutral"),&Config.stick_neutral[1]); |
// print_data_uint8 (PSTR("Stick3_neutral:"),&Config.stick_neutral[2]); |
// print_data_uint8 (PSTR("Stick4_neutral:"),&Config.stick_neutral[3]); |
// print_data_uint8 (PSTR("Stick5_neutral:"),&Config.stick_neutral[4]); |
// print_data_uint8 (strGet(LIPO_MESSUNG),&Config.Lipomessung); |
// print_OSD_Statistic(); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWiInitFlag(void) |
{ |
Config.WiIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTInitFlag(void) |
{ |
Config.BTIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWLInitFlag(void) |
{ |
Config.WLIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTSlaveFlag(void) |
{ |
Config.BTIsSlave = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTMasterFlag(void) |
{ |
Config.BTIsSlave = false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteLastPosition( uint32_t ELongitude, uint32_t ELatitude) |
{ |
Config.LastLongitude = ELongitude; |
Config.LastLatitude = ELatitude; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ReadParameter( void ) |
{ |
eeprom_read_block( (void*)&Config, (const void*)&EEStruct, sizeof(ST) ); |
// DEBUG !! |
//Config.Version = 0x82; |
//WriteParameter(); |
//return; |
//------------------- |
// ggf. ungueltige Zeichen aus dem |
// Bluetooth-Namen eliminieren |
//------------------- |
#ifdef USE_BLUETOOTH |
bt_fixname(); |
#endif |
//------------------- |
// 27.03.2014 OG |
// Als Vorbereitung die Niederländische Sprache |
// zu entfernen wird hier ggf. Niederländisch |
// auf Englisch umgeschaltet |
//------------------- |
if( Config.DisplayLanguage == 2 ) // 2 = ehemals Niederländisch |
Config.DisplayLanguage = 1; // -> wird zu 1 = Englisch |
//------------------- |
// 0. EEprom-Version nicht geaendert |
//------------------- |
if( Config.Version == EEpromVersion ) // nichts zu tun... |
{ |
return; // !!! EXIT !!! |
} |
//------------------- |
// 1. PKT-Upgrade NICHT Update faehig |
// ODER Downgrade |
// |
// 0x77 = PKT Version 3.6.6a |
//------------------- |
if( (Config.Version < 0x77) || (Config.Version > EEpromVersion) ) |
{ |
Delete_EEPROM(); |
return; // !!! EXIT !!! |
} |
//------------------- |
// 2. PKT-Upgrade |
//------------------- |
//++++++++++++++++++++++++++++ |
// bei Eeprom-Versionwechsel |
// IMMER loeschen |
//++++++++++++++++++++++++++++ |
#ifdef USE_WAYPOINTS |
PointList_Clear(); // Init Waypoints |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
//++++++++++++++++++++++++++++ |
// inkrementelle Updates |
//++++++++++++++++++++++++++++ |
//----------------------------- |
//--- ab PKT v3.6.9bX6 |
//----------------------------- |
if( Config.Version < 0x81 ) |
{ |
Config.timezone = 1; |
Config.summertime = 1; |
} |
//----------------------------- |
//--- ab PKT v3.7.0b |
//----------------------------- |
if( Config.Version < 0x82 ) |
{ |
Config.OSD_ShowMKSetting = true; |
} |
//----------------------------- |
//--- ab PKT v3.7.0e |
//----------------------------- |
if( Config.Version < 0x83 ) |
{ |
Config.UseWL = false; |
Config.WLIsSet = false; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
} |
//----------------------------- |
//--- ab PKT v3.7.0eX1 |
//----------------------------- |
if( Config.Version < 0x84 ) |
{ |
Config.OSD_UseScreen = 0xffffffff; // alle verfuegbaren OSD-Screens eingeschaltet |
} |
//----------------------------- |
//--- ab PKT v3.7.0f |
//----------------------------- |
if( Config.Version < 0x85 ) |
{ |
Config.Wlan_DHCP = 0; |
Config.Wlan_WPA = 0; |
Config.Wlan_Adhoc = true; |
Config.Wlan_Channel = 1; |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
} |
//----------------------------- |
//--- ab PKT v3.7.3cX5 |
//----------------------------- |
if( Config.Version < 0x87 ) |
{ |
Config.OSD_ShowCellU = false; // |
} |
//----------------------------- |
//--- 01.04.2014 OG |
//--- ab PKT v3.7.4aX7 |
//----------------------------- |
if( Config.Version < 136 ) |
{ |
Config.UseBLE = false; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
memset( Config.MKParam_Favs, param_EOF, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers |
} |
//----------------------------- |
//--- 06.04.2014 OG |
//--- ab PKT v3.7.4aX8 |
//----------------------------- |
if( Config.Version < 137 ) |
{ |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
} |
//----------------------------- |
//--- 07.05.2014 OG |
//--- ab PKT v3.80bX3 |
//----------------------------- |
if( Config.Version < 138 ) |
{ |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers (Vorbelegung jetzt 0) |
} |
//----------------------------- |
//--- 17.06.2014 OG |
//--- ab PKT v3.80cX5 |
//----------------------------- |
if( Config.Version < 139 ) |
{ |
Config.PKTOffTimeout = 0; // autom. PKT ausschalten nach n Minuten (0=immer an) |
} |
//------------------- |
// 3. Update Message |
//------------------- |
Config.Version = EEpromVersion; // Update EEPROM version number |
sei(); |
lcd_cls(); |
lcdx_printp_center( 2, PSTR("EEProm updated to"), MNORMAL, 0,0); |
lcdx_printp_center( 3, PSTR("new Version") , MNORMAL, 0,0); |
lcdx_printp_center( 5, PSTR("check settings!") , MNORMAL, 0,0); |
lcd_printp_at (18, 7, PSTR("OK") , MNORMAL); // Keyline |
while( !get_key_press(1 << KEY_ENTER) ); |
WriteParameter(); |
cli(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteParameter( void ) |
{ |
copy_line(7); |
lcd_printp_at( 0, 7, PSTR(" Write EEPROM "), MNORMAL); |
eeprom_update_block( (const void*)&Config, (void*)&EEStruct, sizeof(ST) ); |
paste_line(7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delete_EEPROM( void ) |
{ |
uint8_t i; |
// EEPROM auf Default setzen |
lcd_cls(); |
lcd_printp_at( 0, 0, PSTR(" EEPROM Parameter "), MINVERS); |
lcd_printp_at( 0, 1, PSTR("werden auf") , MNORMAL); |
lcd_printp_at( 0, 2, PSTR("Standardwerte gesetzt"), MNORMAL); |
Config.MK_LowBat = 137; // 13,7V |
Config.DisplayTimeout = 0; // Display immer an (autom. LCD ausschalten nach n Minuten) |
Config.DisplayLanguage = 5; // default undefined |
Config.WiTXRXChannel = 1; // Kanal 1 MK Standard |
Config.WiNetworkGroup = 66; // Gruppe 66 MK Standard |
Config.WiNetworkMode = NetMode_Normal; // MK Standard |
Config.WiTXTO = TWaitTime16; // MK Standard |
Config.WiUartMTU = UartMTU64; // MK Standard |
Config.OSD_ShowMKSetting= true; // Anzeige MK-Setting beim OSD Start |
Config.PKTOffTimeout = 0; // PKT immer an (autom. PKT ausschalten nach n Minuten) |
Config.LCD_Kontrast = 20; // Kontrast normal |
Config.LCD_Helligkeit = 100; // Helligkeit in % |
Config.USBBT = 0; // USB Betrieb |
Config.U02SV2 = 0; // SV2 (Kabel) Standard |
Config.Debug = 0; // kein Debug |
Config.UseWi = false; // Wi.232 eingebaut? |
Config.UseBT = false; // BT-222 eingebaut? |
Config.WiIsSet = false; // Flag für die Initialisierung Wi232 |
Config.BTIsSet = false; // Flag für die Initialisierung Bluetooth |
Config.BTIsSlave = true; // Slave Flag setzen |
Config.PKT_IdleBeep = 0; // kein Piepsen bei Inaktivität |
Config.PKT_StartInfo = false; // * FREI * (ehemals: PKT Startinfo anzeigen) |
Config.OSD_RCErrorbeep = true; // OSD Receiveerrorbeep |
Config.OSD_InvertOut = false; // LED Anzeige invertiren |
Config.OSD_LEDform = 1; // Form der Anzeige ( + oder schwarz) |
Config.OSD_SendOSD = false; // OSD Daten an SV2 |
Config.OSD_Fallspeed = 40; // maximale Sinkrate |
Config.OSD_VarioBeep = 1; // Vario Beep ein |
Config.OSD_HomeMKView = true; // * FREI * (ehemals: Home Circle from MK View) |
Config.OSD_mAh_Warning = 10000; // mAh Warnschwelle |
Config.OSD_ScreenMode = 0; // Variante des OSD Screen |
Config.OSD_LipoBar = 0; // Bargraphanzeige für MK Lipo |
Config.PKT_Baudrate = Baud_57600; // Baudrate für BT und Wi232 |
Config.PKT_Accutyp = true; // verwendeter Akkutyp (true=Lipo, false=LiON) |
Config.Lipo_UOffset = 10000; // Offset für PKT-Lipomessung |
Config.FM_Offest_Latitude = 0; // FollowMe Azimuth |
Config.FM_Offset_Longitude = 0; // FollowMe Distance |
Config.FM_Speed = 30; // FollowMe Speed in m/s *0.1 |
Config.FM_Radius = 5; // Waypoint Tolerance Radius in meter |
Config.HWSound = 0; // Hardware Sounderweiterung an PD7 |
Config.HWBeeper = 1; // Hardware Beeper an PC7 |
Config.Volume = 0; // Lautstaerke |
Config.LastLongitude = 0x00000000; |
Config.LastLatitude = 0x00000000; |
strcpy_P(Config.bt_pin , PSTR("0000")); |
strcpy_P(Config.bt_name, PSTR("PKT")); // Bluetooth-Name max. 10 Zeichen! - NICHT mit Leerzeichen auffüllen (bt_name_length) |
strcpy_P(Config.bt_Mac , PSTR("0000-00-000000")); |
strcpy_P(Config.RE_ID , PSTR("0000")); |
for( i = 0; i < 20; i++) |
{ |
Config.gps_UsedDevName[i] = 0; // benutztes GPS Device Name |
} |
for( i = 0; i < 14; i++) |
{ |
Config.gps_UsedMac[i] = '0'; // benutztes GPS Device Mac Adresse |
} |
Config.gps_UseGPS = false; // ist GPS aktiv? |
Config.gps_UsedGPSMouse = GPS_Bluetoothmouse1; |
//Config.WiIsSet = false; // 15.07.2013 CB doppelt drin |
//Config.BTIsSet = false; |
Config.Version = EEpromVersion; |
Config.sIdxSteps = STEPS_255; |
Config.servo[0].rev = SERVO_REV; |
Config.servo[0].min = SERVO_I0_RIGHT; |
Config.servo[0].max = SERVO_I0_LEFT; |
Config.servo[0].mid = SERVO_I0_MIDDLE; |
Config.servo[1].rev = SERVO_REV; |
Config.servo[1].min = SERVO_I0_RIGHT; |
Config.servo[1].max = SERVO_I0_LEFT; |
Config.servo[1].mid = SERVO_I0_MIDDLE; |
Config.servo_frame = SERVO_PERIODE; |
Config.single_step = SINGLE_STEP; // nur bei Test-Servo |
Config.repeat = REPEAT; // nur bei Test-Servo |
Config.pause = PAUSE; // nur bei Test-Servo |
Config.pause_step = PAUSE_STEP; // nur bei Test-Servo |
Config.tracking = TRACKING_MIN; |
Config.track_hyst = TRACKING_HYSTERESE; |
Config.track_tx = 0; |
for( i=0; i<5; i++) |
{ |
Config.stick_min[i] = 30+i; // Joystick |
Config.stick_max[i] = 270+i; // Joystick |
Config.stick_typ[i] = 0; // Joystick |
Config.stick_dir[i] = 0; // Joystick |
Config.stick_neutral[i] = 0; // Joystick |
} |
Config.Lipomessung = true; |
Config.timezone = 1; |
Config.summertime = 1; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
Config.WLIsSet = false; // Wlan |
Config.UseWL = false; |
Config.OSD_UseScreen = 0xffffffff; // alle OSD-Screens eingeschaltet |
Config.Wlan_DHCP = 0; // kein DHCP |
Config.Wlan_WPA = 0; // 0 = adhoc 1= WPA2 |
Config.Wlan_Adhoc = true; // Adhoc |
Config.Wlan_Channel = 1; // Wlan Channel |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
Config.OSD_ShowCellU = false; |
Config.UseBLE = false; |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Favoriten MK-Parameter (Anzahl: MAX_MKPARAM_FAVORITES = 10) |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
//------------------------------------------------- |
//------------------------------------------------- |
#ifdef USE_WAYPOINTS |
PointList_Clear(); |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
WriteParameter(); |
//lcd_printp_at (0, 4, PSTR("Waypoints loeschen"), 0); |
//EEWayPointList_Clear(); |
lcd_printp_at( 0, 6, PSTR("!!Check Parameter!! "), MNORMAL); |
lcd_printp_at(18, 7, PSTR("OK") , MNORMAL); |
sei(); |
set_beep( 200, 0x0080, BeepNormal); |
while( !get_key_press (1 << KEY_ENTER) ); |
//#if defined HWVERSION3_9 |
clr_V_On(); |
//#else |
// |
// wdt_enable( WDTO_250MS ); |
// while (1) |
// {;} |
//#endif |
} |
//-------------------------------------------------------------- |
// |
//void EEWayPointList_Clear(void) // löschen der Waypointliste im EEProm |
//{ |
// uint8_t i; |
// PKTWayPoint.Waypoint.Position.Latitude = 0; |
// PKTWayPoint.Waypoint.Position.Longitude = 0; |
// PKTWayPoint.Waypoint.Position.Altitude = 0; |
// PKTWayPoint.Waypoint.Heading = 361; |
// for(i = 0; i < MAX_WPLIST_LEN; i++) |
// { |
// PKTWayPointDirectory.WPList.WPDirectory[i] = 0; |
// } |
// for(i = 0; i < NumberOfWaypoints; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWayPointList[i].WPIndex, i); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Position.Status, INVALID); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Latitude, (void*)&EEWayPointList[i].Waypoint.Position.Latitude, sizeof(EEWayPointList[i].Waypoint.Position.Latitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Longitude, (void*)&EEWayPointList[i].Waypoint.Position.Longitude, sizeof(EEWayPointList[i].Waypoint.Position.Longitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Altitude, (void*)&EEWayPointList[i].Waypoint.Position.Altitude, sizeof(EEWayPointList[i].Waypoint.Position.Altitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Heading, (void*)&EEWayPointList[i].Waypoint.Heading, sizeof(EEWayPointList[i].Waypoint.Heading)); |
// |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.ToleranceRadius, 0); // in meters, if the MK is within that range around the target, then the next target is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.HoldTime, 0); // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Type, POINT_TYPE_INVALID); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Event_Flag, 0); // future implementation |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.AltitudeRate, 0); // no change of setpoint |
// } |
// for(i = 0; i < NumberOfWPLists; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListnumber, i); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListAktiv, false); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.POI_CAM_NICK_CTR, 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.UsePOI, 0); |
// eeprom_write_block ((const void*)&PKTWayPointDirectory.WPList.WPDirectory, (void*)&EEWPDirectory[i].WPList.WPDirectory, sizeof(EEWPDirectory[i].WPList.WPDirectory)); |
// |
// } |
// lcd_printp (PSTR("\r\n"), 0); |
//} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/eeprom/eeprom.h |
---|
0,0 → 1,336 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.h |
//# |
//# 14.10.2015 Starter |
//# - chg: rename FM_Azimuth and FM_Distance to FM_Offest_lat und FM_Offset_long |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# |
//# 25.06.2014 OG |
//# - chg: Kommentare der eeprom Struct aktualisert bzgl. FollowMe und Tracking |
//# |
//# 17.06.2014 OG |
//# - chg: EEpromversion erhoeht auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: LCD_DisplayMode (Obsolete) geaendert zu PKTOffTimeout |
//# |
//# 31.05.2014 OG |
//# - Recherche welche Config-Werte ueberahupt noch im PKT-Code verwendet werden |
//# und ggf. "OBSOLETE" Anmerkung in der Config-Struct |
//# - Config-Struct wurde um etliche Kommentare ergaenzt bzgl. Verwendungszweck |
//# |
//# 30.05.2014 OG |
//# - chg: Kommentare geaendert |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# |
//# 06.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# |
//# 01.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 136 |
//# - chg: ACHTUNG! EEpromversion umgestellt von HEX auf DEZIMAL: 0x87 => 135 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# - add: MAX_MKPARAM_FAVORITES |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU; EEpromversion erhoeht auf 0x87 |
//# |
//# 28.08.2013 Cebra |
//# - chg: LastLatitude war uint32_t, auf int32_t korrigiert |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 84 |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung, Listen/Statistik ganz ans Ende gesetzt |
//# |
//# 31.05.2013 OG |
//# - chg: Eeprom Version auf 81 wegen Erweiterungen der Statistik-Daten |
//# - chg: Code-Layout |
//# |
//# 12.04.2013 CB |
//# - chg: Kommentarergänzung U02SV2 |
//# |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <stdbool.h> |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../waypoints/waypoints.h" |
#include "../osd/osd.h" |
//[General] |
//FileVersion = 2 |
//NumberOfWaypoints = 15 |
//UsePOI = 0 |
//POI_CAM_NICK_CTRL = 0 |
//[POI] |
//Altitude = 1 |
//Latitude = 46.7140763 |
//Longitude = 19.2507334 |
//[Waypoint1] |
//Latitude = 46.7145686 |
//Longitude = 19.2515702 |
//Radius = 10 |
//Altitude = 15 |
//ClimbRate = 0 |
//DelayTime = 4 |
//WP_Event_Channel_Value = 96 |
//Heading = 180 |
#define EEpromVersion 139 // wird nach jeder Parametererweiterung hochgezählt |
// Anmerkung 01.04.2014 OG: umgestellt auf DEZIMALE schreibweise (wiso war das vorher Hex?) |
#define NumberOfWaypoints 55 // Anzahl der Waypoints in der EEPromliste |
#define NumberOfWPLists 5 // Anzahl WP Listen im PKT |
#define MAX_LIST_LEN 31 // Länge Waypointlist |
#define MAX_MKPARAM_FAVORITES 10 // Anzahl der MK-Parameter Favoriten (nicht aendern!) |
#define bt_pin_length 4 |
#define RE_ID_length 4 // Länge der RE-ID |
#define bt_name_length 10 |
#define bt_mac_length 14 |
#define GPS_Bluetoothmouse1 0 // NMEA BT-Mouse |
#define GPS_Mikrokopter 1 // GPS Daten vom MK für Tracking |
#define wlan_password_length 10 // Länge Wlan-Passwort |
#define wlan_ssid_length 10 // Länge Wlan-SSID |
#define wlan_ip_length 15 |
#define wlan_netmask_length 15 |
#define wlan_gateway_length 15 |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
#define MAX_WPLIST_LEN 31 |
//typedef struct { |
// uint8_t rev; |
// uint16_t min; |
// uint16_t max; |
// uint16_t mid; |
//} servo_t; |
typedef struct |
{ |
uint8_t WPIndex; // Index in der EEpromliste |
Point_t Waypoint; // Waypoint |
} WayPoints; |
typedef struct |
{ |
uint8_t WPListnumber; // Nummer der WP Liste im PKT |
uint8_t WPListAktiv; // Liste aktiv |
uint8_t WPDirectory[31]; // Enthält die Indexe der Waypoints im EEPROM |
uint8_t UsePOI; |
uint8_t POI_CAM_NICK_CTR; |
} WPListHeader; |
typedef struct |
{ |
WPListHeader WPList; // Waypointliste im PKT |
} WPListDirectory; |
typedef struct SStructure |
{ |
uint8_t Version; // PKT-Eeprom Version! |
uint8_t MK_LowBat; // MK-LowBat Warnung u.a. fuer osd.c |
uint8_t DisplayTimeout; // autom. LCD ausschalten nach n Minuten |
uint8_t DisplayLanguage; // eingestellte Sprache |
uint8_t WiTXRXChannel; // Wi.232: |
uint8_t WiNetworkGroup; // Wi.232: |
uint8_t WiNetworkMode; // Wi.232: |
uint8_t WiTXTO; // Wi.232: |
uint8_t WiUartMTU; // Wi.232: |
uint8_t OSD_ShowMKSetting; // Anzeige MK-Setting beim OSD Start |
uint8_t PKTOffTimeout; // autom. PKT ausschalten nach n Minuten |
uint8_t LCD_Kontrast; // LCD-Kontrast |
uint8_t LCD_Helligkeit; // LCD-Helligkeit - Aktuell nicht mehr verfuegbar weil auskommentiert! (mit HW 3.9x geht das sowiso nicht mehr!) |
uint8_t USBBT; // ## OBSOLETE ## 31.05.2014 OG: wird nicht verwendet! (wofuer war da in der Vergangenheit?) |
uint8_t U02SV2; // 0=Wi232-Verbindung zum MK, 1 = Kabelverbindung zum MK |
uint8_t Debug; // ??? OBSOLETE ??? 31.05.2014 OG: wird eigentlich nicht mehr verwendet (war zum debuggen vom PKT) |
uint8_t UseWi; // Wi232 wird genutzt |
uint8_t UseBT; // BT wird genutzt |
uint8_t WiIsSet; // Wi232 ist initialisiert |
uint8_t BTIsSet; // BT ist initialisiert |
uint8_t BTIsSlave; // Slave Flag |
char bt_pin[bt_pin_length + 1]; // BT Pinnummer |
char bt_name[bt_name_length + 1]; // BT Name |
char RE_ID[RE_ID_length + 1]; // RE-ID |
char bt_Mac[bt_mac_length + 1]; // MAC-Adresse BTM222 |
char gps_UsedDevName[20]; // benutztes GPS Device Name |
char gps_UsedMac[14]; // benutztes GPS Device Mac Adresse |
uint8_t gps_UseGPS; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() - "ist GPS aktiv?" |
uint8_t gps_UsedGPSMouse; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() und tracking.c/PKT_tracking() - "GPS Maustyp" |
int32_t LastLongitude; // Letzte Position |
int32_t LastLatitude; // Letzte Position |
uint8_t PKT_IdleBeep; // ## OBSOLETE ## 31.05.2014 OG: wurde das jemals verwendet? |
uint8_t PKT_StartInfo; // ## OBSOLETE ## 26.06.2013 OG: ehemals "PKT Startinfo anzeigen" |
uint16_t Lipo_UOffset; // Offset für die Lipospannugsmessung |
uint8_t PKT_Accutyp; // verwendeter Akkutyp |
uint8_t OSD_RCErrorbeep; // Empfangsausffallwarnung im OSD Screen |
uint8_t OSD_InvertOut; // Out1/2 invertiert anzeigen |
uint8_t OSD_LEDform; // Form der Anzeige ( + oder schwarz) |
uint8_t OSD_SendOSD; // OSD Daten an SV2 senden |
uint8_t OSD_Fallspeed; // maximale Sinkrate |
uint8_t OSD_VarioBeep; // Vario Beep im OSD Screen |
uint8_t OSD_HomeMKView; // ## OBSOLETE ## 02.07.2013 OG: ehemals "Home Circle from MK-View" |
uint16_t OSD_mAh_Warning; // mAh Warnschwelle |
uint8_t OSD_ScreenMode; // Variante des OSD-Screen |
uint8_t OSD_LipoBar; // Bargraphanzeige für MK Lipo |
uint8_t PKT_Baudrate; // Baudrate für BT und Wi232 |
int16_t FM_Offest_Latitude; // Azimuth für FollowMe 4.8.2015 CB |
uint16_t FM_Speed; // FollowMe Speed in m/s *0.1 |
uint16_t FM_Radius; // Waypoint Tolerance Radius in meter |
uint8_t HWSound; // Hardware Sounderweiterung an PD7 |
uint8_t HWBeeper; // Hardware Beeper an PC7 |
uint8_t Volume; // Lautstärke |
servo_t servo[2]; // Tracking: |
uint8_t sIdxSteps; // Tracking: |
int16_t FM_Offset_Longitude; // Distance für FollowMe 4.8.2015 CB |
uint8_t servo_frame; // Tracking: |
uint8_t single_step; // Tracking: |
uint8_t repeat; // Tracking: |
uint8_t pause; // Tracking: |
uint8_t pause_step; // Tracking: |
uint8_t tracking; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_hyst; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_tx; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint16_t stick_min[5]; // Joystick: Stickparameter |
uint16_t stick_max[5]; // Joystick: |
uint8_t stick_typ[5]; // Joystick: |
uint8_t stick_dir[5]; // Joystick: |
uint8_t stick_neutral[5]; // Joystick: Stick neutralisierend: ja/nein |
uint8_t Lipomessung; // wenn Lipomessung deaktiviert(Lötbrücke öffnen), kann der Kanal als Stick verwendet werden |
int8_t timezone; // Einstellbereich -12 .. 0 .. 12 Defaultwert: 1 (entspricht unserer Zeitzone) |
uint8_t summertime; // Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) Defaultwert: 1 (entspricht unserer Sommerzeit in D) |
uint8_t UseWL; // WLAN (WiFly) wird an SV2 genutzt (SV2 Patch erforderlich) |
char Wlan_SSID[wlan_ssid_length+1]; // Wlan SSID |
char Wlan_Password[wlan_password_length+1]; // Wlan Passwort |
uint8_t WLIsSet; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Wlan ist initialisiert" |
uint32_t OSD_UseScreen; // welche OSD-Screens nutzen (Bit-codiert) |
uint8_t Wlan_DHCP; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-4" |
uint8_t Wlan_WPA; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-8" |
uint8_t Wlan_Adhoc; // ## OBSOLETE ## 31.05.2014 OG: ehemals "false = AP , true = Adhoc" |
uint8_t Wlan_Channel; // 0-13 |
char Wlan_IP[wlan_ip_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "IP-Adresse" (16 Bytes!) |
char Wlan_Netmask[wlan_netmask_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "SubnetMask" (16 Bytes!) |
char Wlan_Gateway[wlan_gateway_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Defaultgateway" (16 Bytes!) |
uint8_t OSD_ShowCellU; // OSD Azeige: Anzeige UBat als Einzelzellenspannung (ja/nein) |
uint8_t UseBLE; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
char Wlan_HomeSSID[wlan_ssid_length+1]; // Wlan SSID (Home-WLAN) |
char Wlan_HomePassword[wlan_password_length+1]; // Wlan Passwort (Home-WLAN) |
//!!!neue Parameter ab hier anfügen!!!!!! |
// ab hier werden die Parameter bei EEPROMversionsänderungen gelöscht |
Point_t PointList[MAX_LIST_LEN]; // ab EEPROM Version 78 |
osd_statistic_t OSD_Statistic; // ab EEPROM Version 79 & 81 OSD Statistiken |
pkt_gpspos_t GPS_User[MAX_GPS_USER]; // ab EEPROM Version 79 speichert Benutzer GPS-Positionen |
mkerror_t MKErr_Log[MAX_MKERR_LOG]; // ab EEPROM Version 79 speichert auftretende ErrorCode's vom MK |
} ST; |
extern ST Config; |
void ReadParameter (void); |
void WriteParameter (void); |
void ReadLastPosition(void); |
void WriteLastPosition(uint32_t ELongitude,uint32_t ELatitude); |
void WriteWiInitFlag(void); |
void WriteBTInitFlag(void); |
void WriteWLInitFlag(void); |
void WriteBTSlaveFlag(void); |
void WriteBTMasterFlag(void); |
void Delete_EEPROM(void); |
void SendConfigData(void); |
//void EEWayPointList_Clear(void); // l�schen der Waypointliste im EEProm |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/followme/followme.c |
---|
0,0 → 1,698 |
/* |
* FollowMe.c |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY followme.c |
//# |
//# |
//# 14.10.2015 Starter |
//# - Added Offset to FollowMe |
//# - Stuff in FollowMeStep2 only for debug! |
//# |
//# 22.09.2015 Starter |
//# - FollowMeStep2 erweitert mit Kreisberechnung und test auf PKT |
//# - PKT-Pos von lat lon auf latitude und longitude umbenannt |
//# |
//# 20.09.2015 Starter |
//# FollowMeStep2 erweitert auf aktuelle GPS-Daten und followme_calculate_offset(...) |
//# |
//# 03.08.2015 Cebra |
//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe |
//# |
//# 20.07.2015 CB |
//# - chg: FollowMe Datensatz an NC.211 angepasst |
//# |
//# 27.06.2014 OG |
//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX |
//# |
//# 26.06.2014 OG |
//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h) |
//# |
//# 24.06.2014 OG |
//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c) |
//# - del: Variable CheckGPS |
//# |
//# 21.06.2014 OG |
//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der |
//# Entfernung zwischen Kopter und Target |
//# - chg: MK-Timeoutout Erkennung verbessert/angepasst |
//# |
//# 19.06.2014 OG |
//# - erster Prototyp der Follow Me zum Kopter sendet |
//# - etliche Aenderungen im Codeaufbau |
//# |
//# 01.06.2014 OG |
//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# Hauptmenue erledigt wird |
//# |
//# 13.05.2014 OG |
//# - chg: FollowMe() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb |
//# auskommentiert weil man das a) vorallem nicht benoetigt |
//# und b) die load_setting() so nicht mehr existiert |
//# (der Codebereich kann meines erachtens geloescht werden) |
//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es |
//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird |
//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt) |
//# und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio) |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 05.05.2013 Cebra |
//# - chg: #ifdef USE_FOLLOWME |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdio.h> |
#include "../main.h" |
#ifdef USE_FOLLOWME |
#include "../followme/followme.h" |
#include "../osd/osd.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../gps/gps.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
//####################################################################################################################### |
//-------------------- |
// Timings |
//-------------------- |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
#define COSD_WASFLYING 4 |
// global definitions and global vars |
NaviData_t *naviData; |
unsigned char Element; |
uint16_t heading_home; |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
positionOffset followMeOffset; |
// Flags |
//uint8_t COSD_FLAGS2 = 0; |
// |
//GPS_Pos_t last5pos[7]; |
uint8_t FM_error = 0; |
//--------------------- |
// Waypoint Types |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
//--------------------- |
// Status |
// GPS_Pos_t |
// aus MK source |
// |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define INVALID 0x00 |
#define NEWDATA 0x01 |
#define PROCESSED 0x02 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void FollowMe( void ) |
{ |
//uint8_t status; |
GPS_Pos_t currpos; |
uint8_t tmp_dat; |
uint8_t redraw; |
uint8_t drawmode; |
uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
uint32_t send_followme_counter; |
int8_t ok; |
int8_t xoffs; |
int8_t yoffs; |
Point_t FollowMe; |
uint8_t mktimeout = false; |
nmeaPOS NMEApos; |
nmeaPOS NMEAtarget; |
GPS_PosDev_t targetdev; |
//--------------------- |
// 1. Daten GPS-Maus |
//--------------------- |
ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
if( ok <= 0 ) |
{ |
return; // Fehler bzgl. BT GPS-Maus -> exit |
} |
//--------------------- |
// 2. Follow Me |
//--------------------- |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
lcd_cls (); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
timer_mk_timeout = MK_TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
MKLiPoCells_Init(); |
redraw = true; |
NMEA.Counter = 0; |
NMEA_GPGGA_counter_old = 0; |
send_followme_counter = 0; |
while( (receiveNMEA) ) |
{ |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
if( redraw ) |
{ |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance) |
lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:" |
draw_icon_mk( 1, 18); |
draw_icon_target_round( 1, 50); |
redraw = false; |
} |
//----------------------------------------- |
// neue MK Daten vorhanden |
//----------------------------------------- |
if( rxd_buffer_locked ) // neue MK Daten |
{ |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
if( mktimeout ) redraw = true; |
mktimeout = false; |
FM_error = 0; // noch benoetigt? |
currpos.Latitude = naviData->CurrentPosition.Latitude; |
currpos.Longitude = naviData->CurrentPosition.Longitude; |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln |
//################# |
//# MK |
//################# |
//----------------- |
// Oben: MK-Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: MK-Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 3; |
yoffs = -1; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// MK: Sat Anzahl |
//----------------- |
yoffs = -27; |
if( naviData->SatsInUse > 5 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs); |
draw_icon_satmini( 116+3, 42+yoffs); |
//----------------- |
// MK: GPS |
//----------------- |
writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude |
writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude |
// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude); |
// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude); |
//################# |
//# DISTANCE TO TARGET |
//################# |
//----------------- |
// Target: Distance to Target |
//----------------- |
xoffs = 7; |
yoffs = 4; |
//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition); |
lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) ); |
//################# |
//# TARGET (GPS Maus) |
//################# |
yoffs = 8; |
//----------------- |
// Send Counter |
//----------------- |
//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
//----------------- |
// Target: Fix |
//----------------- |
if( NMEA.SatFix == 1 || NMEA.SatFix == 2 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix |
//----------------- |
// Target: Sat Anzahl |
//----------------- |
if( NMEA.SatsInUse > 5 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs); |
draw_icon_satmini( 116+4, 40+yoffs); |
//----------------- |
// Target: Lat / Long |
//----------------- |
writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
rxd_buffer_locked = FALSE; |
timer_mk_timeout = MK_TIMEOUT; |
} |
//----------------------------------------- |
// if( !NMEA_receiveBT() ) |
// { |
// receiveNMEA = false; |
// } |
// else |
// { |
// NMEA_GetNewData(); |
// } |
// Sourcecode aus NaviCtrl/V2.10e/uart1.c |
// FOLLOW_ME |
// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
// { |
// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
// { |
// TransmitAlsoToFC = 1; |
// // update FollowMe content |
// FollowMe.Position.Longitude = GPSData.Position.Longitude; |
// FollowMe.Position.Latitude = GPSData.Position.Latitude; |
// FollowMe.Position.Status = NEWDATA; |
// FollowMe.Position.Altitude = 1; |
// // 0 -> no Orientation |
// // 1-360 -> CompassCourse Setpoint |
// // -1 -> points to WP1 -> itself |
// FollowMe.Heading = -1; |
// FollowMe.ToleranceRadius = 1; |
// FollowMe.HoldTime = 60; |
// FollowMe.Event_Flag = 1; |
// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list |
// FollowMe.Type = POINT_TYPE_WP; |
// FollowMe.WP_EventChannelValue = 100; // set servo value |
// FollowMe.AltitudeRate = 0; // do not change height |
// FollowMe.Speed = 0; // rate to change the Position (0 = max) |
// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
// FollowMe.Name[0] = 'F'; // Name of that point (ASCII) |
// FollowMe.Name[1] = 'O'; // Name of that point (ASCII) |
// FollowMe.Name[2] = 'L'; // Name of that point (ASCII) |
// FollowMe.Name[3] = 'L'; // Name of that point (ASCII) |
// FollowMe.reserve[0] = 0; // reserve |
// FollowMe.reserve[1] = 0; // reserve |
// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe)); |
// } |
// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
// } |
// |
//----------------------------------------- |
// neue NMEA Daten? |
//----------------------------------------- |
if(NMEA_isdataready() && receiveNMEA) |
{ |
if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
{ |
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
{ |
//Config.FM_Refresh |
// FollowMeStep2 |
followMeOffset.latitude = Config.FM_Offest_Latitude; |
followMeOffset.longitude = Config.FM_Offset_Longitude; |
NMEApos.latitude = NMEA.Latitude; |
NMEApos.longitude = NMEA.Longitude; |
followme_add_offset(&NMEApos, &NMEAtarget, &followMeOffset); |
FollowMe.Position.Status = NEWDATA; |
FollowMe.Position.Longitude = NMEAtarget.longitude; |
FollowMe.Position.Latitude = NMEAtarget.latitude; |
FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
// FollowMe.Event_Flag = 0; // no event |
FollowMe.Event_Flag = 1; // 20.7.2015 CB |
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
FollowMe.Name[1] = 'O'; |
FollowMe.Name[2] = 'L'; |
FollowMe.Name[3] = 'L'; |
// FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
send_followme_counter++; |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
} |
NMEA_GPGGA_counter_old = NMEA.Counter; |
} |
} |
//----------------------------------------- |
// TASTEN |
//----------------------------------------- |
if( get_key_press(1 << KEY_ESC) ) |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
break; |
} |
//----------------------------------------- |
//----------------------------------------- |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
//OSDScreenRefresh = OSD_SCREEN_REDRAW; |
// OSD_MK_Connect( MK_CONNECT ); |
} |
} // end: while( (receiveNMEA) ); |
//--------------------- |
// BEENDEN |
//--------------------- |
OSD_active = false; |
//--------------------- |
// GPS beenden |
//--------------------- |
GPSMouse_Disconnect(); |
} |
// |
#ifdef USE_FOLLOWME_STEP2 |
void Debug_GPS (void) |
{ |
uint8_t redraw = true; |
nmeaPOS NMEApos; |
nmeaPOS NMEATarget; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
#ifdef ONLINE |
int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen -> Quick an Dirty ;-) |
if( retcode <= 0 ) |
{ |
return; |
} |
#endif |
#ifdef DEBUG |
// NMEApos.lat = 520000000; |
// NMEApos.lon = 0; |
// Config.FM_Azimuth = 90; |
// Config.FM_Distance = 10000; |
#endif |
while( true ) |
{ |
NMEApos.latitude = NMEA.Latitude; |
NMEApos.longitude = NMEA.Longitude; |
if( redraw ) |
{ |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMeStep2") ); |
lcdx_printf_center_P( 1, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
lcdx_printf_center_P( 3, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
redraw = false; |
} |
writex_gpspos( 1, 2, NMEApos.latitude, MNORMAL, 0,0 ); // GPS-Maus: Latitude |
writex_gpspos(10, 2, NMEApos.longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
followme_calculate_offset(Config.FM_Offset_Longitude, Config.FM_Offest_Latitude, &followMeOffset); |
#ifdef DEBUG |
writex_gpspos( 1, 6, (int32_t)Config.FM_Offest_Latitude*100, MNORMAL, 0, 0 ); |
writex_gpspos( 10, 6, (int32_t)Config.FM_Offset_Longitude*100, MNORMAL, 0, 0 ); |
writex_gpspos( 1, 7, (int32_t)followMeOffset.latitude*100, MNORMAL, 0, 0 ); |
writex_gpspos( 10, 7, (int32_t)followMeOffset.longitude*100, MNORMAL, 0, 0 ); |
#endif |
followme_add_offset(&NMEApos, &NMEATarget, &followMeOffset); |
writex_gpspos( 1, 4, (int32_t)NMEATarget.latitude, MNORMAL, 0, 0 ); // Ziel Latitude |
writex_gpspos(10, 4, (int32_t)NMEATarget.longitude, MNORMAL, 0, 0 ); // Ziel Longitude |
// Tasten |
if( get_key_press(1 << KEY_ESC) ) |
{ |
#ifdef ONLINE |
GPSMouse_Disconnect(); |
#endif |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
Config.FM_Offest_Latitude -= 10; |
redraw = true; |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
Config.FM_Offest_Latitude += 10; |
redraw = true; |
} |
} |
} |
#endif // FOLLOW_ME_STEP2 |
#endif // #ifdef USE_FOLLOWME |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/followme/followme.h |
---|
0,0 → 1,20 |
/* |
* FollowMe.h |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
#ifndef FOLLOWME_H_ |
#define FOLLOWME_H_ |
#include "../mk-data-structs.h" |
#include <inttypes.h> |
void FollowMe (void); |
void Debug_GPS (void); |
void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old); |
void printFollowMeStatic(void); |
#endif /* FOLLOWME_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/followme |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/gps.c |
---|
0,0 → 1,218 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.c |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT |
//# - add my_abs(...) |
//# |
//# 20.09.2015 Starter |
//# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
//# followme_calculate_offset(...) |
//# |
//# 03.08.2015 cebra |
//# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
//# int nmea_move_horz( |
//# const nmeaPOS *start_pos, /**< Start position in radians */ |
//# nmeaPOS *end_pos, /**< Result position in radians */ |
//# double azimuth, /**< Azimuth (degree) [0, 359] */ |
//# double distance) /**< Distance (km) */ |
//# |
//# 27.06.2014 OG - NEU |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
#include "../mk-data-structs.h" |
#include "../gps/mymath.h" |
#include "gps.h" |
/* |
// definiert in: mk_data-stucts.h |
typedef struct |
{ |
u16 Distance; // distance to target in cm |
s16 Bearing; // course to target in deg |
} __attribute__((packed)) GPS_PosDev_t; |
*/ |
/* |
// definiert in: mk_data-stucts.h |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
*/ |
//-------------------------------------------------------------- |
#define NMEA_PI (3.141592653589793) /**< PI value */ |
#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ |
#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ |
#define R (6371) |
#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ |
#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ |
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ |
#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
// Definitonen für FollowMeStep2 |
#define LONG_DIV 10000000 |
#define LAT_DIV LONG_DIV |
#define FOLLOWME_M2DEG 111111 |
#define FOLLOWME_ROUND_100 100 |
# define NMEA_POSIX(x) x |
/** |
* \fn nmea_degree2radian |
* \brief Convert degree to radian |
*/ |
double nmea_degree2radian(double val) |
{ return (val * NMEA_PI180); } |
//------------------------------------------------------------------------------------------ |
nmeaPOS NMEApos; |
nmeaPOS NMEATarget; |
/** |
* \brief Horizontal move of point position |
*/ |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
) |
{ |
nmeaPOS p1 = *start_pos; |
int RetVal = 1; |
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
azimuth = nmea_degree2radian(azimuth); |
end_pos->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth)); |
end_pos->longitude = p1.longitude + atan2( sin(azimuth) * sin(distance) * cos(p1.latitude), cos(distance) - sin(p1.latitude) * sin(end_pos->latitude)); |
if(NMEA_POSIX(isnan)(end_pos->latitude) || NMEA_POSIX(isnan)(end_pos->longitude)) |
{ |
end_pos->latitude = 0; end_pos->longitude = 0; |
RetVal = 0; |
} |
return RetVal; |
} |
// Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück |
// |
// Benutzt die c_cos_8192 der FC |
// TODO: move to followme.c |
// TODO: *8192 optimieren |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *pFollowMeOffset /**< Position Offset in Millimeters */ |
) |
{ |
nmeaPOS pktPos = *pPktPos; |
positionOffset followMeOffset = * pFollowMeOffset; |
target_pos->latitude = pktPos.latitude + ( ( followMeOffset.latitude * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000; |
target_pos->longitude = pktPos.longitude + ( ( followMeOffset.longitude * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.latitude / LONG_DIV ) ) ) ); |
return 1; |
} |
// schlanke abs-Methode |
// TODO: move to mymath.h |
int16_t my_abs(int16_t input) |
{ |
if(input < 0) |
return -input; |
else |
return input; |
} |
// Rechnet einen Offset aus Radius und Winkel nach Lat/Long |
// Benutzt die c_cos_8192 und c_sin_8192 der FC |
// TODO: move to followme.c |
uint8_t followme_calculate_offset( |
int32_t radius, // in mm |
int16_t angle, // in Grad ° |
positionOffset *followMeOffset |
) |
{ |
angle %= 360; // map angle to 0° - 360° |
followMeOffset->latitude = ( radius * c_cos_8192( angle ) ) / 8192; |
followMeOffset->longitude = ( radius * c_sin_8192( angle ) ) / 8192; |
return 1; |
} |
//############################################################################################### |
//-------------------------------------------------------------- |
GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
GPS_PosDev_t PosDev; |
lon1 = pos1.Longitude; |
lat1 = pos1.Latitude; |
lon2 = pos2.Longitude; |
lat2 = pos2.Latitude; |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = (1113 * (lat1 - lat2) / 10000); |
PosDev.Bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ); // |
//PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10; // *10 um von dm auf cm zu kommen |
return PosDev; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/gps.h |
---|
0,0 → 1,81 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.h |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - PKT-Pos von lat lon auf latitude und longitude umbenannt |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPS_H_ |
#define GPS_H_ |
/** |
* Position data in fractional degrees or radians |
*/ |
typedef struct _nmeaPOS |
{ |
int32_t latitude; // Latitude in 1e-7 Grad |
int32_t longitude; // Longitude in 1e-7 Grad |
} nmeaPOS; |
typedef struct _positionOffset |
{ |
int32_t latitude; // latitude offset in mm |
int32_t longitude; // longitude offset in mm |
}positionOffset; |
GPS_PosDev_t gps_Deviation( |
GPS_Pos_t pos1, |
GPS_Pos_t pos2 |
); |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
int nmea_move_horz1( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *followMeOffset /**< Position Offset in Millimeters */ |
); |
uint8_t followme_calculate_offset( |
int32_t radius, |
int16_t angle, |
positionOffset *followMeOffset |
); |
int16_t my_abs(int16_t input); |
#endif // #define GPS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/gpsmouse.c |
---|
0,0 → 1,532 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.c |
//# |
//# 30.07.2015 CB |
//# - add: Anzeige von CRC Fehlern im NMEA Datensatz in GPSMouse_ShowData |
//# |
//# 28.07.2015 CB |
//# - fix: Überprüfung von führenden 00 in der MAC-Adresse der BT Maus abgeschaltet |
//# |
//# 28.06.2014 OG |
//# - fix: GPSMouse_ShowData() - wenn BT-Datenverlust dann immer GPSMouse_Disconnect() |
//# auch wenn waitsatfix Modus aktiv ist |
//# - fix: GPSMouse_ShowData() - Rueckgabe von 0 wenn Verbindung bereits vorhanden war |
//# und dabei aber ein Verbindungsverlust zur BT GPS-Maus auftrat |
//# |
//# 27.06.2014 OG |
//# - chg: GPSMouse_Disconnect() - Anzeige von BT-Datenverlust optimiert |
//# - chg: GPSMouse_ShowData() - nach GPSMouse_Connect ein clear_key_all() eingefuegt |
//# |
//# 24.06.2014 OG |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 23.06.2014 OG |
//# - chg: GPSMouse_ShowData() - neue Parameter und Return-Codes fuer |
//# hoehere Flexibilitaet |
//# |
//# 22.06.2014 OG - NEU |
//# ehemals BT_ShowGPSData() in setup.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../setup/setup.h" |
#include "../eeprom/eeprom.h" |
#include "../lipo/lipo.h" |
#include "../timer/timer.h" |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../uart/uart1.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../messages.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
static const char STR_FIX0[] PROGMEM = " no"; |
static const char STR_FIX1[] PROGMEM = " GPS"; |
static const char STR_FIX2[] PROGMEM = "DGPS"; |
static const char STR_FIX6[] PROGMEM = "esti"; |
static const char STR_FIX[] PROGMEM = " ---"; |
//-------------------------------------------------------------- |
// retcode = GPSMouse_Connect() |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: verbunden |
//-------------------------------------------------------------- |
int8_t GPSMouse_Connect( void ) |
{ |
uint8_t status; |
if( !BT_CheckBTM222() ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return -2; // -2: kein BTM222 eingebaut |
} |
// deaktiviert, MAC Adressen von Handys haben keine 00 in der MAC-Adresse |
// if( (Config.gps_UsedMac[5] == '0') || (Config.gps_UsedMac[6] == '0') ) |
// { |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// PKT_Message_P( strGet(STR_NOGPSMOUSE), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "keine GPS-Maus!" |
// return -1; // -1: keine GPS Maus konfiguriert |
// } |
//-------------------- |
// verbinde BT GPS-Maus |
//-------------------- |
PKT_Title( Config.gps_UsedDevName, true, true); // PKT_Title( text, lShowLipo, clearscreen ) |
lcdx_printp_center( 2, strGet(STR_GPSMOUSECONNECT), MNORMAL, 0,1); // "suche GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
if( Config.BTIsSlave ) |
{ |
bt_downlink_init(); |
} |
status = bt_connect( Config.gps_UsedMac ); |
uart1_flush(); |
fifo_clear(&in_fifo); |
UART1_receiveNMEA = true; // Uartflag setzen zur Erkennung der NMEA Datensätze |
PKT_Gauge_End(); // Gauge: Ende |
if( !status ) // keine Verbindung zum BT-Device |
{ |
set_BTOff(); |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_GPSMOUSECONNECTION), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "GPS-Maus Verbindung" |
return 0; // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
} |
return 1; // 1 = ok, Verbindung steht |
} |
//-------------------------------------------------------------- |
// GPSMouse_Disconnect() |
//-------------------------------------------------------------- |
void GPSMouse_Disconnect( void ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
//--------------------- |
// GPS beenden |
//--------------------- |
UART1_receiveNMEA = false; |
if( !receiveNMEA ) // ggf. anders implementieren, z.B. via Parameter; aber erstmal geht's auch so |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 7, MINVERSX, 0 ); |
lcdx_printp_center( 7, strGet(STR_BT_LOSTDATA), MINVERSX, 0,0); // "BT Datenverlust" |
set_beep( 600, 0xffff, BeepNormal); |
} |
lcdx_printp_center( 2, strGet(STR_GPSMOUSEDISCONNECT), MNORMAL, 0,1); // "trenne GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
receiveNMEA = false; // Uartflag zurücksetzen, es kommen keine NMEA Datensätze mehr |
if (!bt_disconnect()); |
{ |
// lcdx_printp_center( 2, PSTR("Error on disconnect"), MNORMAL, 0,1); // Fehler beim Disconncet aufgetreten |
// _delay_ms(2000); // 09.08.2015 cebra, muss noch untersucht werden, ist aber jetzt kein Problem |
} |
set_BTOff(); |
PKT_Gauge_End(); // Gauge: Ende |
} |
//-------------------------------------------------------------- |
// retcode = GPSMouse_ShowData( mode, waitsatfix ) |
// |
// zeigt GPS-Daten der aktuellen BT-Maus an |
// |
// PARAMETER: |
// show: |
// was soll in der 2. Zeile angezeigt werden |
// GPSMOUSE_SHOW_TIME || GPSMOUSE_SHOW_WAITSATFIX |
// |
// waitsatifix: |
// 0 = Verbindungsaufbau; Daten anzeigen und beenden der |
// BT-Verbindung (fuer setup.c) |
// n = Zeit (via timer) die nach einem erfolgreichen Satfix gewartet wird bis |
// GPSMouse_ShowData() beendet wird - die Verbindung zur GPS-Maus bleibt erhalten |
// und kann weiterverwendet werden! (z.B. in followme.c) |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
// 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// |
// zusammengefasst: |
// >0 : Ok |
// <=0: Fehler |
//-------------------------------------------------------------- |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ) |
{ |
uint8_t redraw = true; |
uint8_t state_posfix = 0; |
const char *pStr; |
int8_t yoffs; |
int8_t retcode; |
//-------------------- |
// BT GPS-Maus verbinden |
//-------------------- |
retcode = GPSMouse_Connect(); |
if( retcode <= 0 ) |
{ |
return retcode; |
} |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
clear_key_all(); |
//--------------------- |
// zeige GPS-Daten |
//--------------------- |
receiveNMEA = true; |
do |
{ |
//------------------------- |
// Screen redraw |
//------------------------- |
if( redraw ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
if( show == GPSMOUSE_SHOW_TIME ) // Zeile 2: GPS-Zeit |
{ |
lcdx_printp_at( 6, 1, PSTR("["), MNORMAL, -1,2); // Mitte: GPS Time |
lcdx_printp_at(15, 1, PSTR("]"), MNORMAL, -2,2); // .. |
lcd_line( 0, 13, 34, 13, 1); // .. |
lcd_line(92, 13, 127, 13, 1); // .. |
} |
if( show == GPSMOUSE_SHOW_WAITSATFIX ) // Zeile 2: "warte auf Satfix..." |
{ |
yoffs = 1; |
lcdx_cls_row( 1, MINVERS, yoffs); |
lcdx_printp_center( 1, strGet(STR_WAITSATFIX), MINVERS, 0,yoffs); // "warte auf Satfix..." |
} |
// yoffs = 1; |
// lcdx_cls_row( 1, MINVERS, yoffs); |
// lcdx_printp_center( 1,strGet(STR_WAITNMEA), MINVERS, 0,yoffs); // "warte auf NMEA..." |
lcdx_printp_at( 0, 2, PSTR("Fix:") , MNORMAL, 0,3); // Links; Fix |
lcdx_printp_at( 0, 3, PSTR("Sat:") , MNORMAL, 0,3); // Links; Sat |
lcdx_printp_at( 0, 4, PSTR("Alt:") , MNORMAL, 0,3); // Links; Alt |
lcdx_printp_at(11, 2, PSTR("RCnt:"), MNORMAL, 0,3); // Rechts: RCnt - Zaehler empfangener GPGGA Datenpakete |
lcdx_printp_at(11, 3, PSTR("CErr:"), MNORMAL, 0,3); // Rechts: RErr - Zaehler fehlerhafte NMEA Pakete |
lcdx_printp_at(11, 4, PSTR("HDOP:"), MNORMAL, 0,3); // Rechts: HDOP |
lcdx_cls_row( 5, MINVERS, 4); // Hintergrund invers |
lcdx_printp_at( 1, 5, strGet(START_LASTPOS1), MINVERS, 0,4); // "Breitengr. Längengr." |
lcd_rect( 0, (4*8)+11, 127, (2*8)+4, 1); // Rahmen |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT-Update geht hier nicht... |
// wegen der Hardware-Verbindung |
//------------------------- |
//if( PKT_CtrlHook() ) |
//{ |
// redraw = true; |
//} |
//------------------------- |
//------------------------- |
if(NMEA_isdataready()) // NMEA Daten vorhanden? |
{ |
if( show == GPSMOUSE_SHOW_TIME ) |
{ |
lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
} |
//-------------- |
// Sat-Fix |
//-------------- |
//writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
switch( NMEA.SatFix ) |
{ |
case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
default: pStr = 0; // Sat-Fix Code unbekannt! |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA.SatFix); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
break; |
} |
if( pStr ) |
{ |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
} |
if( NMEA.SatFix==1 || NMEA.SatFix==2 ) // Sat-Fix OK => einen Haken anzeigen |
lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Sat-Anzahl |
//-------------- |
writex_ndigit_number_u( 6, 3, NMEA.SatsInUse, 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
if( NMEA.SatsInUse >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Altitude |
//-------------- |
lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA.Altitude); // Links: Altitude |
//-------------- |
// empfangene GPGGA Datenpakete |
//-------------- |
lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA.Counter ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
//-------------- |
// NMEA CRC Fehler |
//-------------- |
lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl CRC Fehler im NMEA GPGGA-Pakete |
// lcdx_printf_at( 15, 3, mode, 0, 3, "%3.5ld", (uint32_t)NMEA_getBearing()); |
// writex_ndigit_number_u_10th(15, 3, NMEA_getBearing(), 4,0, MNORMAL, 0,3); |
//-------------- |
// HDOP |
//-------------- |
if( NMEA.HDOP == 0 ) // Rechts:: HDOP |
lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
else |
writex_ndigit_number_u_10th(16, 4, NMEA.HDOP, 4,0, MNORMAL, 0,3); |
//-------------- |
// Lat / Lon |
//-------------- |
writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); // unten rechts: Longitude |
//-------------- |
//-------------- |
if( waitsatfix > 0 ) |
{ |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==0) ) |
{ |
state_posfix = 1; |
timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
} |
if( (NMEA.SatFix!=1 && NMEA.SatFix!=2) && (state_posfix==1) && (timer!=0) ) |
{ |
state_posfix = 0; |
} |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==1) && (timer==0) ) |
{ |
clear_key_all(); |
return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
} |
} |
} |
if (timer_nmea_timeout == 0) receiveNMEA = false; |
} while( !get_key_press(1 << KEY_ESC) && receiveNMEA ); |
//--------------------- |
// GPS beenden |
//--------------------- |
if( (waitsatfix == 0) || !receiveNMEA ) |
{ |
GPSMouse_Disconnect(); |
} |
clear_key_all(); |
return (receiveNMEA ? 1 : 0); // 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
} |
// save, Anzeige der Daten im Format der neuen NMEA Routinen, nach Umbau auf NMEA Structur nicht mehr notwendig |
// { |
// |
// |
// if( show == GPSMOUSE_SHOW_TIME ) |
// { |
//// lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
// |
// writex_ndigit_number_u( 7, 1, NMEA_getHour(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 9, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 10, 1, NMEA_getMinute(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 12, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 13, 1, NMEA_getSecond(), 2,0, MNORMAL, -2,2); |
// |
// } |
// |
// //-------------- |
// // Sat-Fix |
// //-------------- |
// //writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
// switch( NMEA_getGPSfix() ) |
// { |
// case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
// case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
// case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
// case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
// default: pStr = 0; // Sat-Fix Code unbekannt! |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA_getGPSfix()); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
// break; |
// } |
// |
// if( pStr ) |
// { |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
// } |
// |
// if( NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2 ) // Sat-Fix OK => einen Haken anzeigen |
// lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Sat-Anzahl |
// //-------------- |
// writex_ndigit_number_u( 6, 3, NMEA_getSatellites(), 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
// if( NMEA_getSatellites() >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
// lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Altitude |
// //-------------- |
// lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA_getAltitude()); // Links: Altitude |
// |
// |
// //-------------- |
// // empfangene GPGGA Datenpakete |
// //-------------- |
// lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getNMEAcounter() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
// |
// //-------------- |
// // NMEA CRC Fehler |
// //-------------- |
// lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
//// writex_ndigit_number_u(1, 5, UART1_GPGGA , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(5, 5, bt_overrun , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(10, 5, bt_bufferoverflow , 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // HDOP |
// //-------------- |
// if(NMEA_getHDOP() == 0 ) // Rechts:: HDOP |
// lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
// else |
// writex_ndigit_number_u_10th(16, 4, NMEA_getHDOP(), 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // Lat / Lon |
// //-------------- |
// writex_gpspos( 1, 6, NMEA_getLatitude() , MNORMAL, 0,6 ); // unten links: Latitude |
// writex_gpspos(12, 6, NMEA_getLongitude(), MNORMAL, 0,6 ); // unten rechts: Longitude |
// |
// |
// //-------------- |
// //-------------- |
// if( waitsatfix > 0 ) |
// { |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==0) ) |
// { |
// state_posfix = 1; |
// timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
// } |
// |
// if( (NMEA_getGPSfix()!=1 && NMEA_getGPSfix()!=2) && (state_posfix==1) && (timer!=0) ) |
// { |
// state_posfix = 0; |
// } |
// |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==1) && (timer==0) ) |
// { |
// clear_key_all(); |
// return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// } |
// } |
// |
// } |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/gpsmouse.h |
---|
0,0 → 1,46 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.h |
//# |
//# 24.06.2014 OG |
//# - chg: Parameteraenderung bei GPSMouse_ShowData() |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 22.06.2014 OG |
//# - add: GPSMouse_ShowData() |
//# - add: #define GPSMOUSE_SHOWDATA, GPSMOUSE_WAITSATFIX |
//# |
//# 22.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPSMOUSE_H_ |
#define GPSMOUSE_H_ |
//---------------------------------- |
// GPSMouse_ShowData() - show |
//---------------------------------- |
#define GPSMOUSE_SHOW_TIME 1 |
#define GPSMOUSE_SHOW_WAITSATFIX 2 |
//---------------------------------- |
// minimal akzeptierte Sat-Zahl fuer |
// Steuerungsfunktionen des MK |
//---------------------------------- |
#define GPSMOUSE_MINSAT 6 |
//---------------------------------- |
// Export |
//---------------------------------- |
int8_t GPSMouse_Connect( void ); |
void GPSMouse_Disconnect( void ); |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ); |
#endif // #define GPSMOUSE_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/mymath.c |
---|
0,0 → 1,220 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY mymath.c |
//# |
//# 24.04.2013 OG |
//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM' |
//# wegen Compile-Error in AtmelStudio 6 |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <math.h> |
#include <avr/pgmspace.h> |
#include "mymath.h" |
// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29 |
unsigned int sqrt32(uint32_t q) |
{ uint16_t r, mask; |
uint16_t i = 8*sizeof (r) -1; |
r = mask = 1 << i; |
for (; i > 0; i--) { |
mask >>= 1; |
if (q < (uint32_t) r*r) |
r -= mask; |
else |
r += mask; |
} |
if (q < (uint32_t) r*r) |
r -= 1; |
return r; |
} |
// aus Orginal MK source code |
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const int16_t sinlookup[91] = { 0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
8161, 8172, 8181, 8187, 8191, 8192}; |
int16_t c_cos_8192(int16_t angle) |
{ |
return (c_sin_8192(90 - angle)); |
} |
int16_t c_sin_8192(int16_t angle) |
{ |
int8_t m, n; |
int16_t sinus; |
// avoid negative angles |
if (angle < 0) |
{ |
m = -1; |
angle = -angle; |
} |
else m = +1; |
// fold angle to interval 0 to 359 |
angle %= 360; |
// check quadrant |
if (angle <= 90) n = 1; // first quadrant |
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
else {angle = 360 - angle; n = -1;} //fourth quadrant |
// get lookup value |
sinus = sinlookup[angle]; |
// calculate sinus value |
return (sinus * m * n); |
} |
// ---------------------------------------------------------------------------------------------- |
const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, |
75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, |
61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, |
44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, |
18, 14, 10, 0}; |
int16_t c_arccos2(int32_t a, int32_t b) |
{ |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
} |
/* ---------------------------------------------------------------------------------------------- |
atan2.S |
Author: Reiner Patommel |
atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder. |
CORDIC can be used to calculate trigonometric, hyperbolic and linear |
functions and is until today implemented in most pocket calculators. |
The algorithm makes only use of simple integer arithmetic. |
The CORDIC equations in vectoring mode for trigonometric functions are: |
Xi+1 = Xi - Si * (Yi * 1 / 2^i) |
Yi+1 = Yi + Si * (Xi * 1 / 2^i) |
Zi+1 = Zi - Si * atan(1 / 2^i) |
where Si = +1 if Yi < 0 else Si = -1 |
The rotation angles are limited to -PI/2 and PI/2 i.e. |
-90 degrees ... +90 degrees |
For the calculation of atan2(x,y) we need a small pre-calculated table of |
angles or radians with the values Tn = atan(1/2^i). |
We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and |
accumulate the rotated angles or radians in Z. The direction of the rotation |
will be positive or negative depending on the sign of Y after the previous |
rotation and the rotation angle will decrease from step to step. (The first |
step is 45 degrees, the last step is 0.002036 degrees for n = 15). |
After n rotations the variables will have the following values: |
Yn = ideally 0 |
Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector) |
Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians) |
c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to |
approx. 1.64676 for n = 15. |
In order to represent X, Y and Z as integers we introduce a scaling factor Q |
which is chosen as follows: |
1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and |
set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0. |
2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q |
3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2 |
i.e. X and Z are always > 0 and therefore can be unsigned. |
(We do the quadrant correction afterwards based on the initial signs of x and y) |
4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140. |
i.e. |
Xnmax = 65536 --> unsigned int |
Ynmax = +/- 28140 --> signed int |
Znmax = PI/2 * 28140 = 44202 --> unsigned int |
The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad. |
Below is atan2 and atan in C: */ |
#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x]) |
const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; |
int16_t my_atan2(int32_t y, int32_t x) |
{ unsigned char i; |
uint32_t xQ; |
int32_t yQ; |
uint32_t angle = 0; |
uint32_t tmp; |
double x1, y1; |
x1 = abs(x); |
y1 = abs(y); |
if (y1 == 0) { |
if (x < 0) |
return (180); |
else |
return 0; |
} |
if (x1 < y1) { |
x1 /= y1; |
y1 = 1; |
} |
else { |
y1 /= x1; |
x1 = 1; |
} |
xQ = SCALED(x1); |
yQ = SCALED(y1); |
for (i = 0; i < T-1; i++) { |
tmp = xQ >> i; |
if (yQ < 0) { |
xQ -= yQ >> i; |
yQ += tmp; |
angle -= getAtanVal(i); |
} |
else { |
xQ += yQ >> i; |
yQ -= tmp; |
angle += getAtanVal(i); |
} |
} |
angle = RAD_TO_DEG * angle/Q; |
if (x <= 0) |
angle = 180 - angle; |
if (y <= 0) |
return(-angle); |
return(angle); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/mymath.h |
---|
0,0 → 1,16 |
#ifndef _MYMATH_H_ |
#define _MYMATH_H_ |
#include <avr/io.h> |
#define T 16 |
#define Q 28140 |
#define SCALED(X) ((int32_t)((X) * Q)) |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
uint16_t sqrt32(uint32_t qzahl); |
int16_t c_cos_8192(int16_t angle); |
int16_t c_sin_8192(int16_t angle); |
int16_t my_atan2(int32_t y, int32_t x); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Copy of gpsmouse.h |
+ |
+_old_source |
+ |
+Copy of gpsmouse.c |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/hex_beta/GPL_PKT_V3_85h.hex |
---|
0,0 → 1,6912 |
:100000000C948C290C94B4290C94B4290C94B42924 |
:100010000C94B4290C94B4290C94B4290C94B429EC |
:100020000C94B4290C946E4C0C94794C0C94B42917 |
:100030000C94B4290C94B4290C94B4290C94B429CC |
:100040000C94944D0C94B4290C94B4290C94B429B8 |
:100050000C94644B0C94B4290C94B4290C94B429DA |
:100060000C945EA40C94B4290C94B4290C94B42977 |
:100070000C948B480C94B2470C94B4290C94B4297A |
:100080000C94B4290C94B4290C94B4297D5587554B |
:1000900082558C55CC55D855E255F255CB560E5657 |
:1000A0001B56495665567256FF55CB567F56975587 |
:1000B000AF6FB26FBA6FB76FBD6FC06FC36FC66FF0 |
:1000C000CB6FD06FD56FD86FDB6FDE6FE16FE96FED |
:1000D000EC6FF16FF46FF76FFC6F0170B88BBA8B38 |
:1000E000BD8BC08BC38BC68BC98BCC8BCF8BD28B7C |
:1000F000D58BD88BDB8BE28BDE8BFBA20AA313A301 |
:1001000028A344A357A367A383A389A39EBDA2BD2D |
:10011000A6BDAABDAEBDF2BDC3BDC7BDCBBDD5BDDD |
:10012000D9BD5DBD72BD76BD7ABD7EBD82BD86BDC9 |
:100130008ABD8EBD92BD96BD9ABD6EBDE3BDEEBDBE |
:10014000F6BD05A84CCDB2D44EB93836A9020C5034 |
:10015000B9918688083CA6AAAA2ABE000000803F62 |
:100160005265736574204641494C210052657365A0 |
:1001700074204F4B210025533F0052657365742056 |
:100180005769466C7900322E33382E3300342E34C2 |
:100190003100557064617465204F4B2100557064C7 |
:1001A000617465204641494C2100555044415445F5 |
:1001B000204F4B0066747020757064617465207701 |
:1001C00069666C79372D3434312E696D670055506E |
:1001D00044415445204F4B00667470207570646133 |
:1001E0007465207769666C79372D323338332E6920 |
:1001F0006D6700555044415445204F4B0066747064 |
:100200002063757064617465207769666C79372D39 |
:100210003434312E696D6700555044415445204FA8 |
:100220004B00667470206375706461746520776933 |
:10023000666C79372D323338332E696D67007365FC |
:1002400074206674702074696D656F757420383021 |
:100250003000736574206674702070617373205071 |
:100260006173733132330073657420667470207566 |
:1002700073657220726F76696E67007365742066AD |
:10028000747020646972207075626C6963007365B4 |
:1002900074206674702061646472657373203139F0 |
:1002A000382E3137352E3235332E31363100465423 |
:1002B00050205570646174650073657420776C61BB |
:1002C0006E206368616E6E656C203000736574200B |
:1002D000776C616E2070687261736500736574205D |
:1002E0006970206468637020310076657262696E9F |
:1002F0006465204865696D6E65747A005265736542 |
:10030000740025533F0025533A207625530025538A |
:100310003A207625730055706461746520576946EC |
:100320006C7900496E6974204F4B2100496E6974E5 |
:10033000204641494C210025533F0025533A204F88 |
:100340004B005769466C79207625730056657273A9 |
:10035000696F6E206E6F20255321004552524F5217 |
:10036000005769466C79207625730041502D4D6FFA |
:100370006465004164486F6300496E697420576981 |
:10038000466C79003136392E3235342E312E31001B |
:100390003136392E3235342E312E31003235352E6C |
:1003A0003235352E302E30007365742061706D6FDC |
:1003B0006465206C696E6B5F6D6F6E69746F72201F |
:1003C00030007365742069702070726F746F636F92 |
:1003D0006C203078330073657420776C616E206315 |
:1003E00068616E6E656C007365742061706D6F641A |
:1003F000652070617373007365742061706D6F6444 |
:100400006520737369640073657420776C616E2076 |
:1004100072617465203300736574206970206E65A5 |
:10042000746D61736B00736574206970206761740B |
:100430006577617900736574206970206164647206 |
:10044000657373007365742069702064686370203D |
:10045000340073657420776C616E206A6F696E205A |
:10046000370073657420776C616E206368616E007D |
:1004700073657420776C616E2073736964007365B3 |
:1004800074206970206E65746D61736B00736574A0 |
:1004900020697020676174657761790073657420E5 |
:1004A0006970206164647265737300736574206998 |
:1004B00070206468637020300073657420776C610D |
:1004C0006E206A6F696E203400494E4954005665AB |
:1004D0007273696F6E204641494C210056657273F4 |
:1004E000696F6E3A20762573005769466C792056FD |
:1004F000657273696F6E005769466C79206E6F7410 |
:1005000020666F756E6400736574207520622035F7 |
:100510003736303000255320256C752042617564D4 |
:10052000007265626F6F74004173736F6369617409 |
:100530006564006A6F696E0053746F72696E67005C |
:10054000736176650044656661756C7473006661FD |
:1005500063746F727920524553455400414F4B00EC |
:100560006765742076657273696F6E0024242400B9 |
:1005700025642F2564005769466C79204D6F64759A |
:1005800073005769466C79206D6F6465007A656900 |
:1005900067652056657273696F6E0073686F7720A8 |
:1005A00076657273696F6E005353494400506173EE |
:1005B00073776F72740050617373776F7264004B5E |
:1005C000616E616C004368616E6E656C00486F6DB2 |
:1005D00065205353494400486F6D65205061737323 |
:1005E000776F727400486F6D652050617373776F19 |
:1005F0007264005769466C792065696E736368613F |
:100600006C74656E005769466C7920706F776572FF |
:10061000206F6E0052455345543A205769466C7915 |
:10062000005550444154453A205769466C79004979 |
:100630004E49543A204164486F6300494E49543A48 |
:100640002041502D4D6F6465005769466C79005705 |
:1006500069466C79204164486F63005769466C793C |
:100660002041502D4D6F64650022526164696F54C2 |
:10067000726F6E6978220057692E32333220496ECC |
:1006800069740057692E32333220496E6974204FE5 |
:100690004B210057692E323332204572726F723A05 |
:1006A0002025640057692E323332206E6F74206625 |
:1006B0006F756E6400255320256C7520426175644A |
:1006C00000002563256325313773200025632563EA |
:1006D0002531367325632000256325632531375383 |
:1006E00020002563256325313653256320002563CB |
:1006F00025632531345320253373002A2A2A00002C |
:10070000256325632531385300253553002534539F |
:100710000020253230730020253230530020202560 |
:1007200031397300202025313953002A2045525297 |
:100730004F52202A006D656E756374726C2E6300D3 |
:100740004E4F504152454E544D454E55004E4F4927 |
:1007500054454D004D41584954454D004D41584D6B |
:10076000454E55004E4F4D454E550020202020004F |
:100770004E6F206D6F72652052414D004572726F51 |
:10078000723A205363726F6C6C426F7800202A2A91 |
:1007900020504B54204C69506F21202A2A200050B1 |
:1007A0004B5420416C6C67656D65696E00504B540D |
:1007B0002067656E6572616C004F534420416E7A0C |
:1007C00065696765004F534420646973706C617993 |
:1007D000004F53442053637265656E73004F53445A |
:1007E0002073637265656E730056657262696E642C |
:1007F000756E67204D4B00636F6E6E656374696F35 |
:100800006E204D4B0057692E32333200426C7565B5 |
:10081000746F6F746800426C7565746F6F746820D4 |
:10082000424C4500574C414E205769466C79004771 |
:100830005053204D61757300475053206D6F757391 |
:100840006500466F6C6C6F77204D6500504B5420EF |
:1008500055706461746500504B5420526573657423 |
:100860000053707261636865006C616E67756167E3 |
:1008700065005A6569747A6F6E652055544300743B |
:10088000696D65207A6F6E652055544300536F6D16 |
:100890006D65727A6569740073756D6D657220742B |
:1008A000696D6500504B5420617573206E616368FB |
:1008B00000504B54206F6666206166746572004C70 |
:1008C000434420617573206E616368004C4344208B |
:1008D0006F6666206166746572004C4344204B6FFE |
:1008E0006E7472617374004C434420636F6E747253 |
:1008F0006173740048617264776172652042656556 |
:1009000070657200416B6B752054797000416363B0 |
:1009100075207479706500416B6B75204D6573733C |
:10092000756E670041636375206D656173757265EF |
:10093000002D35203D204E657720596F726B002BBE |
:1009400031203D204265726C696E20200054582F82 |
:100950005258204B616E616C0054582F52582043FE |
:1009600068616E6E656C004E6574572E2047727517 |
:10097000707065004E6574572E2047726F75700059 |
:10098000496E697469616C6973696572656E006945 |
:100990006E697469616C697A65004B6F6E66696730 |
:1009A0002E206D697420504300636F6E6669672E58 |
:1009B00020776974682050430042617564726174E5 |
:1009C000652057693233322F4254005374616E648C |
:1009D0006172643A202120353736303020210042C0 |
:1009E000544D2D323232204D4143004D6F64756CB1 |
:1009F0002065696E676562617574006D6F64756C02 |
:100A000065206275696C7420696E004254204E61E5 |
:100A10006D65004254206E616D650042542050693E |
:100A20006E0042542070696E0052452D4944204E9C |
:100A30006F74697A007A65696765204254204B6F4C |
:100A40006E6669670073686F7720425420636F6E2B |
:100A5000666967007A65696765204254204D4143A5 |
:100A60000073686F77204254204D414300456E6407 |
:100A7000206F6620436F6E666967000042544D2DFB |
:100A800032323220436F6E66696700737563686542 |
:100A9000204750532D4D6175730073656172636813 |
:100AA000204750532D6D6F75736500476572E47470 |
:100AB000656C69737465006465766963656C6973F8 |
:100AC0007400616B7475656C6C65204D61757300A5 |
:100AD0006163742E204750532D6D6F757365004709 |
:100AE00050532D4D61757320446174656E004750FD |
:100AF000532D6D6F7573652064617461004F666678 |
:100B0000736574204C6174006F6666736574206C45 |
:100B10006174004F6666736574204C6F6E67006F7A |
:100B20006666736574206C6F6E67005370656564EC |
:100B300000546F6C6572616E7A20526164697573DE |
:100B400000746F6C6572616E6365207261646975B3 |
:100B50007300302E31206D2F7300466F6C6C6F77F1 |
:100B60004D65205365747570005761726E20556E27 |
:100B70007465727370616E6E756E67005761726E28 |
:100B8000204C6F77426174005761726E2052432D82 |
:100B90004665686C6572005761726E2052432D4540 |
:100BA00072726F72007A65696765205A656C6C7342 |
:100BB00070616E6E756E670073686F772063656C29 |
:100BC0006C20766F6C74616765007A656967652073 |
:100BD0004D4B2053657474696E670073686F77209E |
:100BE0006D6B2073657474696E67004E6176692061 |
:100BF000446174656E20616E20535632004E6176FA |
:100C000069206461746120746F20535632003A0089 |
:100C10003E202535642020202020203D3E20253107 |
:100C20002E3264760025533A00204645484C455202 |
:100C30002120312E205A65696368656E21200020CD |
:100C400017202020202016202020202019202020BE |
:100C500020204F4B00430020002020202020200097 |
:100C600025533A003F3F3F003E2025313673003E7A |
:100C70002025313653004C69496F6E004C69706F06 |
:100C800000256C6400253364004164486F630041B3 |
:100C9000502D4D6F646500756E6B6E6F776E004DF5 |
:100CA000696B726F6B6F707465720042542D4D6F7B |
:100CB0007573650057692E3233320025322E316448 |
:100CC0000025336400253564002533640042542038 |
:100CD0005375636865005375636865206475726356 |
:100CE0006866FC6872656E210032303038205468C6 |
:100CF0006F6D6173204B61697365720032303039FA |
:100D00002D32303130205065746572204D61636B37 |
:100D100000323031302053656261737469616E2036 |
:100D2000426F65686D003230313220676562616400 |
:100D30000032303132204D617274696E2052756E0E |
:100D40006B656C002020202020486172616C64205B |
:100D5000426F6E676172747A0032303132204368BC |
:100D6000722E204272616E64746E65720032303190 |
:100D700033204F6C697665722047656D6573690035 |
:100D800020437265646974730000466F6E7420427C |
:100D9000696700536F756E6400493243204D6F746C |
:100DA0006F7274657374004A6F79737469636B0052 |
:100DB000466F6C6C6F77204D6500547261636B6990 |
:100DC0006E67004F53442053746174697374696390 |
:100DD000004F53442033442D4C616765004F53440A |
:100DE0002055736572475053004F5344204D4B5369 |
:100DF0007461747573004F534420456C656374725D |
:100E00006963004F534420576179706F696E747342 |
:100E1000004F5344204E617669676174696F6E00BC |
:100E20004F5344204F6C642053637265656E7300AA |
:100E30004F53442D44617461004D4B2044697370DD |
:100E40006C6179004D4B2044656275672044617484 |
:100E500061004D4B2053657474696E677300574C85 |
:100E6000414E205779466C7900426C7565746F6FFE |
:100E7000746800204D6F64756C657320696E7374BF |
:100E8000616C6C65640025313553202534530000B6 |
:100E9000202020204E4F2057415252414E5459009D |
:100EA00028432920474E552047504C204C69636504 |
:100EB0006E73650000202553205356322D50617407 |
:100EC000636800332E38355F680020504B5420761D |
:100ED00025385320257300504B542055706461749D |
:100EE00065004F4B002573002553002532307300F9 |
:100EF0002532305300332E38355F6800504B542074 |
:100F000076255300253139730025313953001B00F4 |
:100F10001A00F610FF101111251139114D1162112F |
:100F2000741183119311A711B411C111CD11D911ED |
:100F3000E511EE11FE11051210121C122B123412C3 |
:100F400041124F125D126C1277128A129812A5127A |
:100F5000B012C412D012E012EF12F012F112F2121B |
:100F6000F312F4120E0F0E4E0E0F0E4F0E0F0E5305 |
:100F70000E0F0E570E0F0E4E0E0F0E4F0E0F0E537E |
:100F80000E0F0E570E0F0E4E0E0F0E4F0E0F0E536E |
:100F90000E0F0E5700130313061309130C130F1330 |
:100FA00012131513496E666F00554770730044693C |
:100FB000737000253032643A202553002A2A204DD0 |
:100FC0004B2D25532025303264202A2A004D4B20FA |
:100FD00053657474696E6700504B54204F5344003E |
:100FE00057503A2532642F253264001820202020E3 |
:100FF00019000B43006D617820424C202054656D30 |
:10100000703A006D617820424C2020437572723A2C |
:10101000006D41680020202030006B6D6800730077 |
:1010200041006D4168006D4168006D0055475053A7 |
:10103000006D0044693A00253264002532642F2592 |
:1010400032640057503A006D00486F3A00416C3AE4 |
:10105000006D00486F6D653A00416C743A00206B7A |
:101060006D68002020493A004469723A00416C746E |
:101070003A00206D41005750006D73006D202D0027 |
:101080005A69656C002053617400504300202000B1 |
:101090006D202D00486F6D65004B6D68006D00413F |
:1010A000006D41680056005600760053006D004800 |
:1010B000F66865004D4300545200534C00524C00FA |
:1010C00046520054440054550053210047500046F6 |
:1010D0005300454C004659004D52005354004341C3 |
:1010E000004241006F32006F310043480043460028 |
:1010F0005048004148004E6F204572726F720046A2 |
:1011000043206E6F7420636F6D70617469626C65EB |
:10111000004D4B334D6167206E6F7420636F6D70AF |
:101120006174692E006E6F20464320636F6D6D758C |
:101130006E69636174696F6E006E6F204D4B334D45 |
:10114000616720636F6D6D756E69632E006E6F2031 |
:1011500047505320636F6D6D756E69636174696F7D |
:101160006E0062616420636F6D70617373207661DD |
:101170006C7565005243205369676E616C206C6F1B |
:101180007374004643207370692072782065727210 |
:101190006F72006E6F204E4320636F6D6D756E69C8 |
:1011A000636174696F6E004643204E69636B20472C |
:1011B00079726F00464320526F6C6C204779726FD2 |
:1011C00000464320596177204779726F00464320DB |
:1011D0004E69636B2041434300464320526F6C6C61 |
:1011E00020414343004643205A2D4143430050725F |
:1011F0006573737572652073656E736F7200464315 |
:101200002049324300426C204D697373696E670058 |
:101210004D69786572204572726F72004361726524 |
:1012200066726565204572726F7200475053206C7C |
:101230006F7374004D61676E6574204572726F72D2 |
:10124000004D6F746F722072657374617274004226 |
:101250004C204C696D69746174696F6E00576179D7 |
:10126000706F696E742072616E6765004E6F2053F7 |
:10127000442D43617264005344204C6F6767696E6C |
:10128000672061626F7274656400466C79696E678D |
:101290002072616E676521004D617820416C746930 |
:1012A00074756465004E6F2047505320466978007E |
:1012B000636F6D70617373206E6F742063616C690E |
:1012C00062722E00424C2073656C66746573740004 |
:1012D0006E6F206578742E20636F6D70617373007C |
:1012E000636F6D706173732073656E736F7200004E |
:1012F00000000000504B54205258206C6F73740053 |
:101300004E450045200053450053200053570057D9 |
:1013100020004E57004E20004F53442053746174F8 |
:10132000697374696B004F534420737461746973FB |
:101330007469637300424C20205374617469737440 |
:10134000696B00424C202073746174697374696323 |
:1013500073004D4B20204665686C6572004D4B2034 |
:10136000206572726F727300475053205573657217 |
:10137000646174656E0047505320757365726461D3 |
:10138000746100475053206C65747A746520506F07 |
:10139000732E00475053206C61737420706F732E4E |
:1013A000004CD65343484520616C6C652044617401 |
:1013B000656E0044454C45544520616C6C20646169 |
:1013C00074610044656C00253230530020436F6423 |
:1013D00065202530327500253032753A253032755A |
:1013E0003A253032752020253032752E2530327561 |
:1013F0002E2530347500204D4B204572726F72736C |
:101400000025332E366C6425332E366C6400253768 |
:1014100064206D4725352E3164206D420025303221 |
:10142000752E253032752E2530347520202530322A |
:10143000753A253032753A253032750020475053C1 |
:1014400020557365726461746100202025635465C2 |
:101450006D702E25387520256343002020256343B9 |
:10146000757272656E7425342E31752041002531F8 |
:10147000643A256343757272656E7425342E317536 |
:1014800020410020424C2044617461002563424C9D |
:101490002531642054656D702E2025357520256317 |
:1014A00043002563424C2531642043757272656E9A |
:1014B0007425322E317520410025634375727265A3 |
:1014C0006E7425362E316420410025634375727297 |
:1014D000656E7425362E316420410020424C204434 |
:1014E000617461002563526F6C6C25313164202575 |
:1014F00063002563526F6C6C2531316420256300D5 |
:1015000025634E69636B25313164202563002563B3 |
:101510004E69636B25313164202563002563524396 |
:101520002D5175616C6974792535640025635243CA |
:101530002D5175616C697479253564002563566F8A |
:101540006C7461676525362E316420560025634230 |
:101550004C2531642054656D702E2025357520256D |
:101560006343002563424C25316420437572722E1B |
:1015700025342E3175204100256343757272656EE6 |
:101580007425362E316420410025634375727265DF |
:101590006E7425362E3164204100204361706163F2 |
:1015A000697479253764206D41680025635665723A |
:1015B000742E537065656425332E3164206B6D681D |
:1015C00000256347726E642E537065656425332E63 |
:1015D0003164206B6D6800256344697374616E63C8 |
:1015E00065253775206D002563416C7469747564D9 |
:1015F00065253764206D00204C61737420466C793A |
:101600002020253032753A25303275206D696E0004 |
:10161000204F5344204461746100456E643A202099 |
:1016200020253032753A253032752020253032752C |
:101630002E253032752E00426567696E3A202530BE |
:1016400032753A253032752020253032752E2530FE |
:1016500032752E0020202020206E6F20646174617E |
:101660002E2E2E001D00200020001D00416C6C005D |
:101670001D001A201B0056616C75653A2020203031 |
:10168000004D6F746F723A202020310020424C2DA3 |
:101690004374726C20546573742020202020202015 |
:1016A0002000ACAA904221000044214821ADAA901C |
:1016B0004221000051215121AAAA90422100005646 |
:1016C000215621ABAA90422100005B215B21AEAAEA |
:1016D000906021000068216821AFAA90602100007D |
:1016E0006F216F21B0AA906021000076217621B190 |
:1016F000AA90602100007D217D21B2AA9060210086 |
:101700000084218421B3AA90602100008B218B21C9 |
:10171000B4AA906021000092219221B5AA90602184 |
:101720000000992199215AAA90A0210000A621BC6D |
:1017300021DFAA90D0210000D321D321CBAA90D0C1 |
:10174000210000E221F821E2AA90D02100000E221F |
:101750001C22E6AA90D021000026223422EBAA9077 |
:10176000D021000043224322E9AA90D02100004D5D |
:10177000225B22EAAA90D021000069227522ECAAFD |
:1017800090D021000081229322EDAA90D021000068 |
:10179000A222B622E8AA90D0210000C922C9220DB7 |
:1017A000AA90D6220014D822D8220EAA90D62200BF |
:1017B00014E422E4220FAA90F02200F7F322F3228D |
:1017C00057AA90F02200F7FA220923D2AA90D0213A |
:1017D000000016232323D3AA90D02100002E233CFF |
:1017E00023D4AA90D021000049235723D5AA90D012 |
:1017F0002100006423732331AA90F02200F7812393 |
:10180000932332AA90D62200F7A723B82333AA90B5 |
:10181000D62200F7CB23D52337AA90D62200F7E0B3 |
:1018200023F12338AA90D62200F7FF231024E6AA3A |
:1018300090D021000026223422C8AA90D021000096 |
:101840001E242E24C9AA90D021000040244B24E756 |
:10185000AA90D021000058246624CAAA90D0210062 |
:10186000007424742406AA908124000088249624FD |
:1018700004AA90D62200F7A524AE2407AA90F0224D |
:1018800000F7BC24C32405AA90F02200F7CE24DA86 |
:101890002409AA90F02200F7E724E7245EAA90F03A |
:1018A0002200F7ED24032508AA90D62200F7192577 |
:1018B00028250AAA90D62200F732253E250BAA90A9 |
:1018C000F02200F74E254E2576AA90F02200F7541C |
:1018D0002554250CAA90D62200A06625782562AA58 |
:1018E000908A2500008E25A32563AA90D62200F7B2 |
:1018F000B925B92577308CCF250000D125D12523F6 |
:10190000AA90F02200F7E525F62524AA90F02200FF |
:10191000F706261826EEAA90D62200F729263C269E |
:10192000E3AA90D021000050265F26BEAA90D021C5 |
:10193000000075268326C0AA90D02100009726A417 |
:101940002625AA90D62200F7B126B12626AA90D63F |
:101950002200F7C426C42660AA90D6220019D726F2 |
:10196000E32627AA90F02200F7EF26002728AA9066 |
:10197000F02200F711272327EFAA90D62200F7358F |
:10198000274827BFAA90D02100005C276A2729AAF0 |
:1019900090D62200F77E277E272AAA90D62200F72B |
:1019A0008B278B2761AA90D62200199827A4272B72 |
:1019B000AA90D6220208B027C6272CAA90D62200C9 |
:1019C000F7D427EB272EAA90F92700F5FD27FD274E |
:1019D000F0AA90D62200F705281B282FAA90F927F5 |
:1019E00000F532283228F1AA90D62200F73A285082 |
:1019F0002830AA90F02200F767286728F2AA90D62C |
:101A00002200F76F28852870AA90D62200F79C281C |
:101A10009C2871AA90D62200F7B228B22872AA9008 |
:101A2000D62200F7C828C82873AA90D62200F7DE6D |
:101A300028DE28EBAA90D02100004322432247AAA7 |
:101A400090F4280000FC28102948AA90F02200F702 |
:101A50002129372951AA90D62200F74D2960295013 |
:101A6000AA90D62200F77429842949AA90F022006E |
:101A7000F7952995294AAA90F02200F79B299B29DE |
:101A80004BAA90F02200F7A129A1294CAA90F0229C |
:101A900000F7A729A7294DAA90F02200F7B329B390 |
:101AA000294EAA90F02200F7BF29BF294FAA90F033 |
:101AB0002200F7CB29CB2952AA90F02200F7D32994 |
:101AC000E92953AA90F02200F7FF29102A74AA905E |
:101AD000D62200FA212A372A54AA90F02200F74D84 |
:101AE0002A592A55AA90F02200F7652A722A56AA86 |
:101AF00090D62200F77E2A942ACEAA90D021000008 |
:101B0000AA2AAA2ADDAA90D0210000B52AB52A5C0B |
:101B1000AA90D62200F7C42ADA2AF4AA90F02A0062 |
:101B200003F52A042B79AA90132B00001B2B1B2BE7 |
:101B30007AAA90132B0000302B302B6FAA90F02242 |
:101B400000F7402B402B75AA90D62200FA562B5650 |
:101B50002B40028E000000006C2B7B2B41AA90F0E2 |
:101B60002200F7892B892BD6AA90D02100009F2B29 |
:101B7000B12BD7AA90D0210000C42BD12BE1AA9081 |
:101B8000D0210000E32BF82B46AA90F02200F70E9C |
:101B90002C0E2C6DAA90F02200F70E2C0E2C6EAAA3 |
:101BA00090F02200F7242C242C42028E000000002A |
:101BB0003A2C492C43AA90F02200F7572C572CD9E5 |
:101BC000AA90D02100009F2BB12BD8AA90D0210041 |
:101BD00000C42BD12B44028E000000006D2C7C2C05 |
:101BE00045028E00000000902C9F2C10AA90D62257 |
:101BF00000F7B32CBC2C11AA90D62200F7CA2CD324 |
:101C00002C13AA90F02200F7E12CF02C64038D0035 |
:101C1000000000FF2CFF2CF3AA90D0210000152D0E |
:101C2000152D59AA9081240000282D3B2DCFAA9074 |
:101C3000D02100004C2D4C2D1AAA90D62200F75925 |
:101C40002D6F2D66AA90D62200F7852D852D65AAC9 |
:101C500090D62200F79B2D9B2D1CAA90D62200F730 |
:101C6000B12DC72DDEAA90D0210000DD2DF02D1B57 |
:101C7000AA90D62200F7032E0B2E5DAA90D6220042 |
:101C8000F71A2E302E5FAA90D6220010462E5C2E18 |
:101C9000CDAA90D0210000722E842ED0AA90D021FF |
:101CA0000000972EAD2EDCAA90D0210000C22ED5C8 |
:101CB0002EE0AA90D0210000EA2EFC2E14AA90F06B |
:101CC0002200F7112F112F17AA90F02200F7F3220C |
:101CD000F32215AA90F02200F7182F182F18AA90B7 |
:101CE000F02200F71F2F1F2F16AA90F02200F726D0 |
:101CF0002F262F3BAA90F02200F72D2F432FEDAA7D |
:101D000090D0210000A222B62212AA90D62200F77B |
:101D1000542F542F39AA90D62200F7642F7A2F1E01 |
:101D2000AA90F02200F7902F9E2F3AAA90D6220078 |
:101D3000F7A52FB82F19AA90D62200F7CB2FCB2FBB |
:101D40005BAA90D62200F7D62FE52F1FAA90F0228B |
:101D500000F7F22FF22F20AA90F02200F7FE2FFEBC |
:101D60002F21AA90F02200F70A300A3022AA90F020 |
:101D70002200F7163016303CAA90F02200F72230ED |
:101D800022303DAA90F02200F72E302E303EAA904D |
:101D9000F02200F73A303A303FAA90F02200F7469E |
:101DA000304630ECAA90D02100008122932234AA40 |
:101DB00090F02200F75230653035AA90F02200F7FB |
:101DC00078308B3036AA90F02200F79E30AD30FF8D |
:101DD000000000000000000000004E4F2052414D66 |
:101DE00021004661766F726974656E004661766F98 |
:101DF0007269746573004B616EE46C6500436861E1 |
:101E00006E6E656C004B6F6E6669677572617469A2 |
:101E10006F6E00436F6E66696775726174696F6E8D |
:101E200000537469636B004C6F6F70696E67004894 |
:101E3000F6686500416C746974756465004B616D8A |
:101E40006572610043616D657261004E6176692D56 |
:101E50004374726C0041757367E46E6765004F757B |
:101E60007470757473005665727363686965646530 |
:101E70006E6573004D697363656C6C616E656F753B |
:101E800073004779726F0042656E75747A657200EF |
:101E90005573657200416368736B6F70706C756E1B |
:101EA0006700436F75706C204178657300456173FE |
:101EB00079205365747570006E6963687420766567 |
:101EC0007266FC676261720021204552524F5220B7 |
:101ED0002100767374723A20257300767374723A17 |
:101EE0002025730076616C75653A257500666F7202 |
:101EF0006D61743A202573002553006E6963687420 |
:101F00002076657266FC67626172004E4121003E78 |
:101F10002025346400C46E6465726E00253464004C |
:101F20003E2025336400C46E6465726E0025336400 |
:101F3000003D202530337500120028426974313D80 |
:101F400049646C6529001800302F312020201920A9 |
:101F5000202020202020202020204F4B00C46E6411 |
:101F600065726E0025303375003E202531387300D0 |
:101F7000C46E6465726E0056616C75653A202533D7 |
:101F80007500436F6465203A204525303275006D39 |
:101F90006B706172616D65746572732E63002045AC |
:101FA00052524F52200045253032750025337500BE |
:101FB00020204E004E65696E004E6F0020204A00C2 |
:101FC000202059004A610059657300205025750092 |
:101FD000506F74692025750043253275004B616E82 |
:101FE000616C202575004368616E6E656C202575F7 |
:101FF0000053253275005365722E204B616E616C63 |
:10200000202575005365722E204368616E6E656CE5 |
:1020100020257500417573004F666600496E6100AA |
:10202000496E616B746976005750450057502D45D5 |
:1020300076656E74004D696E004D696E696D756DE3 |
:10204000004D6964004D69747465004D6964646C89 |
:1020500065004D6178004D6178696D756D002041B6 |
:102060006E00204F6E004672650046726565002066 |
:10207000434800436F6D696E6720486F6D65202887 |
:102080004348290020504800506F736974696F6E8F |
:1020900020486F6C6420285048290044697300442C |
:1020A000697361626C6564004F75253175004F7509 |
:1020B0007420253175006BC46E006E4368004B655B |
:1020C000696E6520C46E646572756E67006E6F2000 |
:1020D0006368616E676500467A480046744800464A |
:1020E000726F6E74207A7520486F6D650066726F2E |
:1020F0006E7420746F20686F6D6500487A480052D6 |
:102100007448004865636B207A7520486F6D6500E0 |
:102110007265617220746F20686F6D65007753740B |
:1021200000734F7200776965206265696D20537492 |
:102130006172740073616D652061732073746172E4 |
:1021400074004300476173007468726F74746C6547 |
:102150000047656172004E69636B00526F6C6C00E2 |
:1021600041435357554D5800506F7469203100500A |
:102170006F7469203200506F7469203300506F749F |
:1021800069203400506F7469203500506F746920E5 |
:102190003600506F7469203700506F746920380022 |
:1021A000444353574F004D6F746F722053696368F7 |
:1021B0006572682E536368616C742E006D6F746F66 |
:1021C000722073616665747920737769746368003F |
:1021D0003031004D6F746F72205377692E4D6F64EC |
:1021E00065004572772E20456D70662E5369672E07 |
:1021F0005072FC66756E6700656E682E20726563AE |
:102200002E207369676E2E20636865636B00416CD6 |
:102210006C657320416E736167656E0073706561F4 |
:102220006B20616C6C0048F668656E726567656C62 |
:10223000756E670068656967687420636F6E747295 |
:102240006F6C0047505320416B746976004B6F6D83 |
:102250007061737320416B74697600636F6D706198 |
:10226000737320616B746976004B6F6D706173736B |
:102270002046697800636F6D7061737320666978BA |
:10228000004163687328656E74296B6F70706C759C |
:102290006E670028446529436F75706C204178652E |
:1022A000730044726568726174656E626567726519 |
:1022B0006E7A756E6700726F7461727920726174E4 |
:1022C00065206C696D69742E0048656164696E678C |
:1022D00020486F6C640076004E69636B2F526F6C00 |
:1022E0006C2050004E69636B2F526F6C6C20440061 |
:1022F000765000476965722050004578742E204B57 |
:102300006F6E74726F6C6C65006578742E20636FED |
:102310006E74726F6C004C6F6F70696E67204F62E5 |
:10232000656E006C6F6F70696E67207570004C6F22 |
:102330006F70696E6720556E74656E006C6F6F709C |
:10234000696E6720646F776E004C6F6F70696E679F |
:10235000204C696E6B73006C6F6F70696E67206CD8 |
:10236000656674004C6F6F70696E672052656368B4 |
:102370007473006C6F6F70696E6720726967687440 |
:10238000004C6F6F70696E6720476173204C696DF8 |
:102390006974006C6F6F70207468726F74746C6510 |
:1023A000206C696D697400416E7370726563687347 |
:1023B000636877656C6C6500726573706F6E7365CA |
:1023C000207468726573686F6C6400487973746513 |
:1023D00072657365006879737465726573697300FB |
:1023E0004E69636B20556D6B65687270756E6B74AA |
:1023F000006E69636B207475726E6F766572005241 |
:102400006F6C6C20556D6B65687270756E6B7400C7 |
:10241000726F6C6C207475726E6F7665720048F620 |
:1024200068656E62656772656E7A756E670068656D |
:1024300069677468206C696D69746174696F6E0096 |
:10244000566172696F2D48F6686500766172696F32 |
:102450002068656967746800536368616C746572AD |
:102460002048F66865006865696768742073776955 |
:1024700074636800616B7573742E20566172696FA6 |
:1024800000444353574F4E0048F6686520536F6C25 |
:102490006C7765727400686967687420736574701E |
:1024A0006F696E74004D696E2E20476173006D690F |
:1024B0006E2E207468726F74746C650048F66865DF |
:1024C000205000616C7469747564652050004C750F |
:1024D0006674647275636B2044006261726F6D652F |
:1024E000747269632044005A2D414343004D617862 |
:1024F0002E2048F668652020202020202020205B08 |
:102500006D5D006D61782E20616C74697475646511 |
:1025100020202020205B6D5D005665727374E4728C |
:102520006B2E2F52617465006761696E2072617451 |
:102530006500536368776562652D47617300686F56 |
:102540006F766572207468726F74746C65004750A2 |
:1025500053205A0054696C7420436F6D70656E731C |
:102560006174696F6E00537469636B204E65757496 |
:10257000722E50756E6B7400737469636B206E6598 |
:102580007574722E706F696E740044435300417508 |
:10259000746F2053746172744C616E64204B616E71 |
:1025A000616C006175746F2073746172742F6C615B |
:1025B0006E64206368616E2E004C616E64696E67A4 |
:1025C0002053706565642020302E316D2F730044D8 |
:1025D00000414343205A204C616E64696E6720506D |
:1025E000756C7365004E69636B20416E737465751D |
:1025F0006572756E67006E69636B20736572766FC6 |
:10260000206374726C004E69636B204B6F6D706554 |
:102610006E736174696F6E006E69636B20736572AF |
:10262000766F20636F6D702E004E69636B2046617C |
:10263000696C736176652057657274006E69636BAF |
:10264000206661696C736176652076616C756500E2 |
:102650004E69636B204B6F6D702E20417573006E59 |
:1026600069636B20636F6D70656E736174696F6E03 |
:10267000206F6666004E69636B20556D6B656872EE |
:10268000656E006E69636B20696E762E20646972D8 |
:10269000656374696F6E004E69636B2052656C618F |
:1026A000746976006E69636B2072656C617469761B |
:1026B000004E69636B20536572766F204D696E69B9 |
:1026C0006D756D004E69636B20536572766F204D9A |
:1026D0006178696D756D004E69636B2046696C7435 |
:1026E0006572006E69636B2066696C746572005276 |
:1026F0006F6C6C20416E737465756572756E6700E2 |
:10270000726F6C6C20736572766F206374726C2EBE |
:1027100000526F6C6C204B6F6D70656E73617469E5 |
:102720006F6E00726F6C6C20636F6D70656E73619D |
:1027300074696F6E00526F6C6C204661696C7361D6 |
:102740007665205765727400726F6C6C20666169E3 |
:102750006C736176652076616C756500526F6C6C88 |
:1027600020556D6B656872656E00526F6C6C2069E8 |
:102770006E762E20646972656374696F6E00526FA5 |
:102780006C6C204D696E696D756D00526F6C6C20BC |
:102790004D6178696D756D00526F6C6C2046696C87 |
:1027A00074657200726F6C6C2066696C746572007F |
:1027B000416E73742E20476573636877696E646930 |
:1027C000676B65697400736572766F2072656672F7 |
:1027D000657368004D616E752E2047657363687779 |
:1027E000696E6469676B6569742E006D616E7565ED |
:1027F0006C6C2053706565640076545000536572AC |
:10280000766F203300536572766F203320466169FE |
:102810006C73617665205765727400536572766FCC |
:102820002033206661696C736176652076616C7512 |
:102830006500536572766F203400536572766F20A1 |
:1028400034204661696C7361766520576572740047 |
:10285000536572766F2034206661696C73617665AA |
:102860002076616C756500536572766F2035005374 |
:102870006572766F2035204661696C7361766520DC |
:102880005765727400536572766F203520666169F2 |
:102890006C736176652076616C7565005365727640 |
:1028A0006F2033204F6E20202020204F7574312F51 |
:1028B0003200536572766F2033204F6666202020E9 |
:1028C000204F7574312F3200536572766F2034209B |
:1028D0004F6E20202020204F7574312F3200536519 |
:1028E00072766F2034204F6666202020204F75744A |
:1028F000312F32004443535746485100475053202C |
:102900004D6F64757320537465756572756E6700DD |
:10291000475053206D6F646520636F6E74726F6CE7 |
:1029200000475053205665727374E4726B756E677E |
:102930002020205B255D00475053206761696E2091 |
:102940002020202020202020205B255D00475053A0 |
:1029500020537469636B2D53636877656C6C6500F5 |
:1029600047505320737469636B2074687265736891 |
:102970006F6C64004D696E2E20536174656C6C69D8 |
:1029800074656E006D696E696D756D207361746537 |
:102990006C697465004750532D50004750532D49C2 |
:1029A000004750532D44004750532D50204C696D23 |
:1029B0006974004750532D49204C696D6974004774 |
:1029C00050532D44204C696D697400475053204189 |
:1029D0006363004750532057696E646B6F72726572 |
:1029E0006B747572205B255D004750532077696ECC |
:1029F0006420636F72726563742E205B255D0041F5 |
:102A00004343204B6F6D70656E736174696F6E0028 |
:102A100041434320636F6D70656E736174696F6EBF |
:102A2000004D61782E20466C75677261646975731C |
:102A3000205B31306D5D004D61782E20466C6967FA |
:102A400068747261642E205B31306D5D006D617859 |
:102A50002E20526164697573006D61782E20726159 |
:102A6000646975730057696E6B656C204C696D699C |
:102A70007400616E676C65206C696D697400504804 |
:102A8000204C6F67696E205A6569742020202020D1 |
:102A90005B735D005048206C6F67696E2074696DD0 |
:102AA0006520202020205B735D0044796E616D6994 |
:102AB00063205048004D61782E526164697573201F |
:102AC00064504800436F6D2E486F6D652048F6686E |
:102AD0006520202020205B6D5D00636F6D696E674F |
:102AE00020686F6D6520616C74692E205B6D5D00E0 |
:102AF0004B565247004348204175737269636874AE |
:102B0000756E67004348206F7269656E7461746901 |
:102B10006F6E0049435357554D580053696E676CAB |
:102B200065205750204374726C204368616E2E00FC |
:102B30004E657874205750204368616E6E656C0056 |
:102B400053696E676C65575020537065656420301B |
:102B50002E316D2F73004661696C2053696E6B5284 |
:102B60006164697573205B31306D5D004F75743140 |
:102B70003A204269746D61736B65004F7574313A28 |
:102B8000206269746D61736B004F7574313A205423 |
:102B9000696D696E672020205B31306D735D006E5A |
:102BA00075722077656E6E204D6F746F7220416E66 |
:102BB000006F6E6C792077697468206D6F746F7226 |
:102BC000206F6E00201920736F666F727420616E23 |
:102BD00000201920696D6D6564696174656C7920E8 |
:102BE0006F6E006D69742057502D4576656E7420A8 |
:102BF000207665726B6E2E00636F6D62696E652064 |
:102C0000776974682057502D4576656E740041755C |
:102C1000746F205472696767657220446973742EFB |
:102C20005B6D5D004175746F2054726967676572F2 |
:102C300020416C742E205B6D5D004F7574323A201C |
:102C40004269746D61736B65004F7574323A20622E |
:102C500069746D61736B004F7574323A2054696DFD |
:102C6000696E672020205B31306D735D004F757495 |
:102C700031205761726E2055426174004F75743176 |
:102C800020756E646572766F6C74207761726E0069 |
:102C90004F757432205761726E2055426174004F37 |
:102CA00075743220756E646572766F6C742077610E |
:102CB000726E004D696E2E20476173006D696E2E35 |
:102CC000207468726F74746C65004D61782E2047B3 |
:102CD0006173006D61782E207468726F74746C6516 |
:102CE000004B6F6D706173737769726B756E6700FF |
:102CF000636F6D706173732065666665637400430E |
:102D00006F6D70617373204F666673657420202049 |
:102D1000205B0B5D0044697361626C6520446563F0 |
:102D20006C2E2043616C6300436172654672656579 |
:102D300020537465756572756E67006361726546D0 |
:102D400072656520636F6E74726F6C0054656163A9 |
:102D50006861626C6520434600556E7465727370DD |
:102D6000616E6E756E6720205B302E31565D00758A |
:102D70006E646572766F6C746167652020205B30CD |
:102D80002E31565D00436F6D696E6720482E2056C8 |
:102D90006F6C74205B302E31565D004175746F6C22 |
:102DA000616E642E20566F6C74205B302E31565D40 |
:102DB000004E4F542D476173205A656974202020BE |
:102DC0005B302E31735D00456D6572672E546872FD |
:102DD0006F2E54696D655B302E31735D004E4F541C |
:102DE0002D47617320566172696F2048F66865004F |
:102DF000456D6572672E7468722E766172696F20F8 |
:102E0000682E004E4F542D47617300456D65726703 |
:102E10002E5468726F74746C65004661696C732E11 |
:102E2000204348205A656974202020205B735D0090 |
:102E30006661696C732E2043482074696D6520209B |
:102E400020205B735D004661696C732E20436861CE |
:102E50006E6E656C2020303D417573006661696C53 |
:102E6000732E206368616E6E656C2020303D6F6646 |
:102E700066006B2E53756D6D6572206F2E53656EF7 |
:102E8000646572006E6F2062656570207769746892 |
:102E90006F7574205458004B6F6D706173732046CA |
:102EA00065686C65722069676E6F722E0069676E67 |
:102EB0006F726520636F6D70617373206572726FDE |
:102EC00072006B2E5374617274206F2E53442D4B1D |
:102ED00061727465006E6F20737461727420772F55 |
:102EE0006F2053442D63617264006B2E53746172C2 |
:102EF00074206F2E47505320466978006E6F207300 |
:102F00007461727420772F6F204750532046697880 |
:102F1000004779726F2050004779726F204900474F |
:102F20006965722049004779726F20440044796EC8 |
:102F3000616D69736368652053746162696C69745B |
:102F4000E4740064796E616D696320737461626911 |
:102F50006C6979004143432F4779726F2046616B5A |
:102F6000746F72004143432F4779726F204B6F6D2E |
:102F7000702E20205B312F785D004143432F47792D |
:102F8000726F20436F6D702E20205B312F785D00B3 |
:102F900048617570747265676C65722049006D6177 |
:102FA000696E20490044726966742D4B6F6D7065BF |
:102FB0006E736174696F6E00647269667420636F0A |
:102FC0006D70656E736174696F6E004779726F2002 |
:102FD000737461622E004D6F746F7220476CE474DD |
:102FE00074756E67006D6F746F7220736D6F6F74A0 |
:102FF0006800506172616D657465722031005061C6 |
:1030000072616D65746572203200506172616D6528 |
:10301000746572203300506172616D657465722051 |
:103020003400506172616D657465722035005061C5 |
:1030300072616D65746572203600506172616D65F4 |
:10304000746572203700506172616D65746572201D |
:1030500038004769657220706F732E204B6F707057 |
:103060006C756E67006769657220706F732E2063E0 |
:103070006F75706C696E67004E69636B2F526F6C71 |
:103080006C204B6F70706C756E67006E69636B2F90 |
:10309000726F6C6C20636F75706C696E6700476946 |
:1030A0006572204B6F7272656B747572006769652B |
:1030B0007220636F7272656374696F6E00253135BB |
:1030C00073004E4F2052414D2100FE6F6B0CFE01EC |
:1030D000FF00FE706B0CFE01FF00FE726B0CFE0128 |
:1030E000FF000101020C03010401050106010701B3 |
:1030F000080109010A010B010C010D010E010F016C |
:10310000100111011201130114011501160117011B |
:10311000180119011A011B011C011D011E011F01CB |
:10312000200121012201230124012501260127017B |
:10313000280129012A012B012C012D012E012F012B |
:1031400030013101320133013401350136013701DB |
:10315000380139013A013B013C013D013E013F018B |
:10316000400141014201430144014501460147013B |
:10317000480149014A014B014C014D014E014F01EB |
:10318000500151015201530154015501560157019B |
:10319000580159015A015B015C015D015E015F014B |
:1031A00060016101620163016701680169016A01EF |
:1031B0006B0C6C01FF000101020C0301040105010D |
:1031C00006010701080109010A010B010C010D01AB |
:1031D0000E010F011001110112011301140115015B |
:1031E00016011701180119011A011B011C011D010B |
:1031F0001E011F01200121012201230124012501BB |
:1032000026012701280129012A012B012C012D016A |
:103210002E012F013001310132013301340135011A |
:1032200036013701380139013A013B013C013D01CA |
:103230003E013F014001410142014301440145017A |
:1032400046014701480149014A014B014C014D012A |
:103250004E014F01500151015201530154015501DA |
:1032600056015701580159015A015B015C015D018A |
:103270005E015F016001610162016301640165013A |
:103280006701680169016A016B0C6C01FF000101B3 |
:10329000020C0301040105010601070108010901EF |
:1032A0000A010B010C010D010E010F0110011101AA |
:1032B000120113011401150116011701180119015A |
:1032C0001A011B011C011D011E011F01200121010A |
:1032D00022012301240125012601270128012901BA |
:1032E0002A012B012C012D012E012F01300131016A |
:1032F000320133013401350136013701380139011A |
:103300003A013B013C013D013E013F0140014101C9 |
:103310004201430144014501460147014801490179 |
:103320004A014B014C014D014E014F015001510129 |
:1033300052015301540155015601570158015901D9 |
:103340005A015B015C015D015E015F016001610189 |
:103350006201630164016501660167016801690139 |
:103360006A016B0C6C01FF000101020C03010401F6 |
:10337000050106010701080109010A010B010C0101 |
:103380000D010E010F0110011101120113011401B1 |
:10339000150116011701180119011A011B011C0161 |
:1033A0001D011E011F012001210122012301240111 |
:1033B000250126012701280129012A012B012C01C1 |
:1033C0002D012E012F013001310132013301340171 |
:1033D000350136013701380139013A013B013C0121 |
:1033E0003D013E013F0140014101420143014401D1 |
:1033F00045016D014701480149014A014B014C015A |
:103400004D014E014F015001510152015301540130 |
:10341000550156015701580159015A015B015C01E0 |
:103420005D015E015F016001610162016301640190 |
:10343000650166016E016F016701680169016A013A |
:103440006B0C6C01FF00FE7B6B0CFE01FF000101A9 |
:10345000020C03010401050106010701080109012D |
:103460000A010B010C010D010E010F0110011101E8 |
:103470001201130114011501160117011801190198 |
:103480001A011B011C011D011E011F012001210148 |
:1034900022012301240125012601270128012901F8 |
:1034A0002A012B012C012D012E012F0130013101A8 |
:1034B0003201330134013501360137013801390158 |
:1034C0003A013B013C013D013E013F014001410108 |
:1034D00042014301440145016D0147014801490191 |
:1034E0004A014B014C014D014E014F015001510168 |
:1034F00052015301740155015601750157015801DC |
:1035000059015A015B015C015D015E015F016001CF |
:103510006101700171017201730162016301640153 |
:10352000650166016E016F016701680169016A0149 |
:103530006B0C6C01FF000101020C03010401050189 |
:1035400006010701080109010A0176010C010D01BC |
:103550000E010F01100111011201130114011501D7 |
:1035600016011701180119011A011B011C011D0187 |
:103570001E011F0120012101220123012401250137 |
:1035800026012701280129012A012B012C012D01E7 |
:103590002E012F0130013101320133013401350197 |
:1035A00036013701380139013A013B013C013D0147 |
:1035B0003E013F01400141014201430144014501F7 |
:1035C0006D014701480149014A014B014C014D0180 |
:1035D0004E014F0150015101520153017401550137 |
:1035E000560175015701580159015A015B015C01EF |
:1035F0005D015E015F016001610170017101720195 |
:103600007301620163016401650166016E016F016E |
:103610006701680169016A016B0C6C01FF0001011F |
:10362000020C03010401050106010701080109015B |
:103630000A0176010C010D010E010F0110011101AB |
:1036400012011301140115011601170118011901C6 |
:103650001A011B011C011D011E011F012001210176 |
:103660002201230124012501260127012801290126 |
:103670002A012B012C012D012E012F0130013101D6 |
:103680003201330134013501360137013801390186 |
:103690003A013B013C013D013E013F014001410136 |
:1036A00042014301440145016D01470148014901BF |
:1036B0004A014B014C014D014E014F015001510196 |
:1036C000520153017401550156017501570158010A |
:1036D00059015A015B015C015D015E015F016001FE |
:1036E0006101700171017201730162016301640182 |
:1036F000650166016E016F0177016701680169016B |
:103700006A016B0C6C01FF000101020C0301040152 |
:10371000050106010701080109010A0176010C01F2 |
:103720000D010E010F01100111011201130114010D |
:10373000150116011701180119011A011B011C01BD |
:103740001D011E011F01200121012201230124016D |
:10375000250126012701280129012A012B012C011D |
:103760002D012E012F0130013101320133013401CD |
:10377000350136013701380139013A013B013C017D |
:103780003D013E013F01400141014201430144012D |
:1037900045016D014701480149014A014B014C01B6 |
:1037A0004D014E014F01500151015201530174016D |
:1037B0005501560175015701580159015A015B0124 |
:1037C0005C015D015E015F016001610170017101D9 |
:1037D000720173017805620163016401650166018C |
:1037E0006E016F0177016701680169016A016B0C65 |
:1037F0006C01FF000101020C030104010501060137 |
:103800000701080109010A0176010C010D010E01F1 |
:103810000F0110011101120113011401150116010C |
:103820001701180119011A011B011C011D011E01BC |
:103830001F0120012101220123012401250126016C |
:103840002701280129012A012B012C012D012E011C |
:103850002F013001310132013301340135013601CC |
:103860003701380139013A013B013C013D013E017C |
:103870003F014001410142014301440145016D0105 |
:103880004701480149014A014B014C014D014E01DC |
:103890004F0150015101520153017401550156016C |
:1038A00075015701580159015A015B015C015D0125 |
:1038B0005E015F01600161017001710172017301BC |
:1038C0007805620163016401650166016E016F01A3 |
:1038D000770179017A016701680169016A016B0C5E |
:1038E0006C01FF00BE6801010000BF68010200001A |
:1038F000C06801040000C86901010000C969020133 |
:103900000000CA6901020000CB6901040000CC6913 |
:1039100001080000CD6901100000CE6901200000FF |
:10392000CF6901400000D06901800000D267010129 |
:103930000000D36701020000D46701040000D567CE |
:1039400001080000D66701100000D767012000655C |
:10395000D86701400065D96701800000DC6A010179 |
:103960000000DD6A01020064DE6A01040000DF6A13 |
:1039700001080000E06A01100000E16A0120000077 |
:10398000E26A01400000E36A01806000E603010191 |
:103990000000E70301020000E80301040000E9035E |
:1039A00001080000EA0301100000EB030120000001 |
:1039B000EC0301400000ED0301800000AA020300B7 |
:1039C0000000AB0203010000AC0203020000AD02E4 |
:1039D00003030000AE0203040000AF020305000071 |
:1039E000B00203060000B10203070000B2020308A0 |
:1039F0000000B30203090000B402030A0000B5028C |
:103A0000030B0000F36403000000F46803000000EF |
:103A1000EE7803000000EF7803010000F078030265 |
:103A20000000F17803030000F27803040000FF00B7 |
:103A30000000000052656672657368004D4B2053AC |
:103A4000657474696E67004E433A00205265762DA6 |
:103A5000455252210025303375005265763A0046B2 |
:103A6000433A004D4B20496E666F00257500257561 |
:103A7000202573002A202553202A002531752E2564 |
:103A80003032752563002D2D2D2D2D002531752EFD |
:103A9000253032752563004643004E4300255320F0 |
:103AA000255320255300202532532025313653003D |
:103AB00018202020201900446973706C6179004639 |
:103AC00043004E4300253464002533646B6D480089 |
:103AD00025322E326C646D0025322E326400253280 |
:103AE00075006E6F00324400334400537065643AD1 |
:103AF00000416363753A0050444F503A00416C7482 |
:103B00003A005361743A004669783A004D4B2047B9 |
:103B1000505320496E666F000000000000007815C9 |
:103B20001415780020555455780038454445380020 |
:103B30003049484930003C4140413C003841402137 |
:103B400078007E1515150A0022170F172200008431 |
:103B500082FF82841C141C0000000007050700007F |
:103B600010385410101E180C1800000010101010FF |
:103B700010101010107C101008100800000008141D |
:103B80000000140810080404081008102020100871 |
:103B900000081422410000412214080004027F02A0 |
:103BA000040010207F201000103854101010101046 |
:103BB0001054381010181C1C181008183838180821 |
:103BC00000081C3E7F00007F3E1C08000609090912 |
:103BD0000600102010080402000000000000000091 |
:103BE0002F000000000700070000147F147F14005E |
:103BF000242A6B2A120023130864620036495522D6 |
:103C00005000000503000000001C2241000000419C |
:103C1000221C000014083E08140008083E08080092 |
:103C2000005030000000080808080800006060002C |
:103C300000002010080402003E5149453E000042A9 |
:103C40007F4000004261514946002141454B31000F |
:103C50001814127F10002745454539003C4A494950 |
:103C60003000030171090700364949493600064909 |
:103C700049291E00003636000000005636000000BC |
:103C800008142241000014141414140000412214DA |
:103C90000800020151090600324979413E007E11B7 |
:103CA00011117E007F49494936003E4141412200C1 |
:103CB0007F4141221C007F49494941007F09090990 |
:103CC00001003E4149497A007F0808087F00004111 |
:103CD0007F4100002040413F01007F081422410045 |
:103CE0007F40404040007F020C027F007F040810AC |
:103CF0007F003E4141413E007F09090906003E41E7 |
:103D000051215E007F091929460046494949310081 |
:103D100001017F0101003F4040403F001F20402043 |
:103D20001F003F4038403F00631408146300070839 |
:103D3000700807006151494543007F414100000080 |
:103D40000204081020000041417F0000040201022B |
:103D500004004040404040000001020400002054A4 |
:103D6000545478007F484444380038444444200088 |
:103D7000384444487F00385454541800087E0901E0 |
:103D800002000C5252523E007F08040478000044A6 |
:103D90007D4000002040443D00007F10284400008A |
:103DA00000417F4000007C04180478007C08040473 |
:103DB00078003844444438007C1414140800081473 |
:103DC00014187C007C080404080048545454200053 |
:103DD000043F444020003C4040207C001C20402008 |
:103DE0001C003C4038403C004428102844000C5043 |
:103DF00050503C004464544C4400000836410000DC |
:103E000000007F00000000413608000008082A1C5E |
:103E10000800081C2A0808006E6F7420617661692A |
:103E20006C61626C652100746869732066756E63ED |
:103E300074696F6E206973002A2A204F5554204FF1 |
:103E4000462052414D202A2A0020202E20202E20BC |
:103E500020202000201A202020201B20202020208D |
:103E600045786974002020303132333435363738A4 |
:103E7000394142434445460D0A0020206E6F002020 |
:103E80004750530044475053006573746900202025 |
:103E90002D2D2D0025356C640025356C64002534EE |
:103EA0002E31640025345300203F2025640048440F |
:103EB0004F503A00434572723A0052436E743A00D2 |
:103EC000416C743A005361743A004669783A005D77 |
:103ED000005B005556F732EE1AAB0DDC066F03B8E7 |
:103EE00001DC006E0037001B000E0007000300021B |
:103EF000000100463A2531640054783A25356C6457 |
:103F00000044693A20253364206D00416C3A004634 |
:103F10006F6C6C6F774D65004F4B002121436865D6 |
:103F2000636B20506172616D6574657221212000A0 |
:103F300000003136392E3235342E3030312E3030CB |
:103F400031003235352E3235352E3030302E30308E |
:103F500030003136392E3235342E3030312E30307B |
:103F60003100313233343536373800504B5400305D |
:103F700030303000303030302D30302D3030303077 |
:103F8000303000504B540030303030005374616E8C |
:103F90006461726477657274652067657365747AAD |
:103FA000740077657264656E206175660020454512 |
:103FB00050524F4D20506172616D657465722020C2 |
:103FC00020200020202057726974652045455052FA |
:103FD0004F4D20202020004F4B00636865636B200D |
:103FE00073657474696E677321006E65772056651A |
:103FF0007273696F6E00454550726F6D2075706405 |
:104000006174656420746F0000003136392E3235DA |
:10401000342E3030312E303031003235352E3235BD |
:10402000352E3030302E303030003136392E3235AA |
:10403000342E3030312E3030310031323334353699 |
:10404000373800504B54004F4B000D0A00720A00E5 |
:104050000D0A004669666F2069732066756C6C00F6 |
:10406000436F6D6D616E64204572726F72004552D0 |
:10407000524F520D0A004F4B0D0A000D004154499A |
:1040800031004154423F00415453310041544E3DB0 |
:10409000004154523100415452300041544F3000DD |
:1040A00041544F31004154463F004154443D00418A |
:1040B00054443000415448004154410041540041AF |
:1040C0005445310041545130004154453000415471 |
:1040D00051310041544C350041544C340041544C52 |
:1040E000330041544C320041544C310041544C3067 |
:1040F0000041544C2A0041545A30004154503D0074 |
:10410000496E717569727920456E64000D0A004927 |
:104110006E717569727920456E6400496E717569BA |
:10412000727920526573756C74733A0D0A00546984 |
:104130006D65206F75742C4661696C20746F206307 |
:104140006F6E6E6563742100434F4E4E45435400BD |
:10415000444953434F4E4E454354000D0A004254C8 |
:104160004D32323220496E69740042544D204572FE |
:10417000726F723A2025640042544D323232204927 |
:104180006E6974204F4B210042544D323232206E02 |
:104190006F7420666F756E642100255320256C7541 |
:1041A000204261756400696E697469616C69736944 |
:1041B0006572656E3F00255320256C752042617540 |
:1041C0006400504B5400010000000A00000064002D |
:1041D0000000E803000010270000A0860100404214 |
:1041E0000F008096980000E1F50500CA9A3B4857F9 |
:1041F00020332E3978202831312E323031322900C7 |
:10420000485720332E396D202830392E3230313145 |
:104210002900535632205061746368005553422080 |
:104220002D3E2057692E323332004D4B2D555342CF |
:1042300000555342202D3E20535632204B6162657B |
:104240006C004B6162656C20616E7363686C6965BC |
:104250007373656E2E202000504B542065696E20CC |
:1042600067656B7265757A746573202020005A77D4 |
:1042700069736368656E204D4B2D55534220756EF2 |
:10428000642020004D4B2D55534220616E20504339 |
:1042900020616E7363686C2E20004643203E204DE3 |
:1042A0004B2D555342203E2042544D2D3232320088 |
:1042B00042544D2D323232204B6F6E6669677572F3 |
:1042C000696572656E00424C4520457874656E6480 |
:1042D0006572003136392E3235342E312E313A3274 |
:1042E00030303000574C414E20457874656E64651F |
:1042F0007200425420457874656E646572004254C1 |
:10430000202D3E20535632204B6162656C00537560 |
:10431000636865204D696B726F6B6F7074657200B6 |
:10432000736561726368204B6F70746572004F53E0 |
:104330004420446174656E004F53442044617461AD |
:1043400000504B5420436F6E6E65637400504B54A5 |
:1043500020536574757000504B5420496E666F0091 |
:104360004D4B20496E666F004D4B20536574746948 |
:104370006E6773004D4B20446973706C6179004D1A |
:104380004B20446174656E004D4B204461746100A4 |
:10439000504B54205370657A69616C00504B542027 |
:1043A0005370656369616C004F5344004D4B204767 |
:1043B000505320496E666F00425420202045787487 |
:1043C000656E64657200424C452020457874656EC8 |
:1043D00064657200574C414E20457874656E646583 |
:1043E000720055534220202D3E205769323332004F |
:1043F00055534220202D3E20535632004254202057 |
:10440000202D3E2053563200504B5420436F6E6E89 |
:1044100065637400466F6C6C6F77204D6500466F66 |
:104420006C6C6F77204D65205365747570004D4B33 |
:10443000204D6F746F72746573740067467D469388 |
:1044400046A946BF46D146E346ED46F7460D47230B |
:10445000472A47314741474E475A4768477847867A |
:10446000479B47B047C647DB47EB47F9470E482213 |
:104470004827482B482E48314838483E4844484A47 |
:1044800048544859486D4877488148894892489728 |
:10449000489D48A448AD48B348B948BD48C148C63E |
:1044A00048CB48D348E848FB48014907490D491320 |
:1044B00049134917492849364940494A4954495EF0 |
:1044C000496D497A498F49A149B449C649D649E558 |
:1044D00049F049FB490B4A1A4A204A264A264A2CE7 |
:1044E0004A2C4A334A3B4A434A4D4A574A624A6F2A |
:1044F0004A814A8D4A954A9C4AA54AAD4AB24AB772 |
:104500004ABC4AC14AC34AC54AC74AC94AC94ACB32 |
:104510004ACB4ACD4ACD4AD04AD04AD34AE44AF29D |
:104520004AFD4A064B164B244B374B444B5A4B70B3 |
:104530004B7E4B8C4B9A4BA84BB64BC44BD24BE0AB |
:104540004BEE4BFC4B0A4C184C264C344C3B4C422B |
:104550004C494C504C624C734C824C8F4C9E4CAC32 |
:104560004CBC4CCA4CD54CDE4CE74CEF4CEF4CF7F6 |
:104570004CF74C024DF74C0C4D0C4D154D154D1F85 |
:104580004D1F4D284D284D304D304D3B4D3B4D403E |
:104590004D404D454D454D4A4D5E4D6B4D724D78EC |
:1045A0004D894D974DA84DBC4DD14DE54DF04DF584 |
:1045B0004DF94DFE4D034E064E064E0A4E114E1954 |
:1045C0004E294E364E454E504E644E704E814E8CA6 |
:1045D0004E9D4EAD4EBF4ECD4EE14EEF4E014F0FB4 |
:1045E0004F224F2F4F3E4F4B4F5B4F6B4F7F4F93A1 |
:1045F0004FA74FBB4FCF4FE34FF34F015007500C26 |
:104600005011501950205027503A504E505A50686F |
:10461000507C508F509E50AC50BE50D150E050ED69 |
:10462000500051105121512E514051515166517934 |
:10463000518B519C51AC51BE51D051E451F45102B7 |
:104640005216522B523D5250526052725272528642 |
:104650005290529B52AB52BB52CB52D852DE52E5D3 |
:1046600052F9520E530E53201A20202020201B20D6 |
:10467000202020456E64652020204F4B00201A200A |
:10468000202020201B20202020656E642020202058 |
:104690004F4B0020182020202020192020202045CA |
:1046A0006E64652020204F4B002018202020202001 |
:1046B0001920202020656E64202020204F4B0020F0 |
:1046C0001820202020201920202020456E646520FD |
:1046D00000201820202020201920202020656E6432 |
:1046E000202000456E64652020204F4B00656E64DD |
:1046F000202020204F4B002018202020202019208F |
:10470000202020416262722E20204F4B0020182072 |
:10471000202020201920202063616E63656C2020FA |
:104720004F4B0020416262722E0063616E63656CC4 |
:1047300000546173746520312053656B756E646538 |
:1047400000707265737320627574746F6E006665B5 |
:10475000737468616C74656E2100666F722031201D |
:104760007365636F6E6421004C65747A74652050C4 |
:104770006F736974696F6E006C61737420706F730E |
:104780006974696F6E004272656974656E67722E36 |
:10479000204CE46E67656E67722E006C61746974FC |
:1047A0007564652020206C6F6E67697475646500A0 |
:1047B0006CF6736368656E20202020202020202066 |
:1047C000204E65696E0064656C657465202020204C |
:1047D00020202020202020204E6F00476F6F676C24 |
:1047E000652045696E676162653A00476F6F676C67 |
:1047F0006520496E7075743A007375636865204D65 |
:10480000696B726F6B6F707465722E2E2E007365FC |
:1048100061726368696E67204B6F707465722E2ECB |
:104820002E00456E646500656E64004F4B006F6B33 |
:10483000004665686C6572006572726F7200616B2C |
:1048400074697600616374697600537065696368A2 |
:1048500065726E00736176650077697264206765C2 |
:104860007370656963686572742E2E2E00736176AD |
:10487000696E672E2E2E0056657277657266656EBC |
:104880000064697363617264004B6F70696572657F |
:104890006E00636F7079007375636865007365619E |
:1048A00072636800676566756E64656E00666F7535 |
:1048B0006E64007365747A6500736574006D697465 |
:1048C0000077697468006F686E6500776974686F57 |
:1048D0007574004D6F746F72656E206175737363CC |
:1048E00068616C74656E2100737769746368206D0C |
:1048F0006F746F7273206F6666210045696E2020A9 |
:10490000004F6E202020004175732020006F6666E6 |
:1049100020200045534300504B542061757373634E |
:1049200068616C74656E3F0073687574646F776E50 |
:1049300020504B543F004A612020204E65696E0094 |
:10494000796573202020206E6F004E65696E2020EF |
:10495000204A61006E6F2020202079657300446139 |
:1049600074656E206CF6736368656E3F0064656CF9 |
:1049700065746520646174613F0056657262696E9A |
:10498000646520504B54206D6974205043210043CE |
:104990006F6E6E65637420504B5420746F205043CB |
:1049A000004472FC636B6520275374617274272086 |
:1049B00066FC7200507265737320275374617274C1 |
:1049C0002720666F72006D696E2E20322053656B52 |
:1049D000756E64656E006D696E2E2032207365639E |
:1049E0006F6E647300456E64652053746172740069 |
:1049F000456E6420205374617274004D6F64756C51 |
:104A00002065696E676562617574004D6F64756CD1 |
:104A1000206275696C7420696E004B6162656C0080 |
:104A20006361626C6500536C617665004E6F726DF8 |
:104A3000616C005265766572736500696E766572A9 |
:104A4000736500456E64652020204F4B00456E6401 |
:104A5000202020204F4B00504B542D454550726F65 |
:104A60006D005265616C792064656C6574650077D2 |
:104A700069726B6C696368206CF6736368656E3F7E |
:104A800000504B542D454550726F6D3F0064657565 |
:104A900074736368006765726D616E00656E676C44 |
:104AA0006973636800656E676C697368004A6120AA |
:104AB000200079657320004E65696E006E6F2020BE |
:104AC0000056004600480042004C0052004E69006B |
:104AD000526F004D4B20446174656E7665726C7543 |
:104AE000737421004D4B2044617461206C6F7374AA |
:104AF0002100416B746976696572656E006163744B |
:104B00006976617465004B6F7069657265205365E5 |
:104B10007474696E6700636F70792073657474696B |
:104B20006E6773005769726B6C696368206B6F7096 |
:104B3000696572656E3F007265616C6C7920636FA8 |
:104B400070793F00676577E4686C7465732047503F |
:104B50005320476572E47420200073656C656374AC |
:104B600065642047505320646576696365202000A2 |
:104B70006D61782048F66865202020203A006D613C |
:104B80007820616C7469747564653A006D61782091 |
:104B90004765736368772E203A006D6178207370E3 |
:104BA0006565642020203A006D617820456E74664A |
:104BB00065726E2E3A006D61782064697374616E5F |
:104BC00063653A006D696E205370616E6E756E6735 |
:104BD0003A006D696E20766F6C74616765203A00EB |
:104BE0006D6178205A656974202020203A006D613B |
:104BF000782074696D65202020203A006D6178204E |
:104C00005374726F6D2020203A006D6178206375B7 |
:104C10007272656E74203A00456E742E4B6170613D |
:104C20007A69742E3A0055736564436170616369F3 |
:104C300074793A00506F7469202000706F74692095 |
:104C4000200054617374657200737769746368003F |
:104C50004F53442D20756E6420424C2D4461746581 |
:104C60006E004F53442D20616E6420424C2D4461F0 |
:104C70007461004D4B204665686C65726C69737495 |
:104C800065004D4B206572726F726C6973740055CC |
:104C9000736572204750532D446174656E005573DF |
:104CA0006572204750532D64617461006C65747A9D |
:104CB000746520506F736974696F6E006C617374F2 |
:104CC00020706F736974696F6E00414C4C452044CD |
:104CD0006174656E00414C4C2064617461006CF637 |
:104CE000736368656E3F0064656C6574653F00477B |
:104CF000656E6572616C004E617669676174696F9B |
:104D00006E00576179706F696E747300456C6563EE |
:104D100074726963004D4B2D537461747573005543 |
:104D200073657220475053003344204C616765001F |
:104D300053746174697374696373004F5344300032 |
:104D40004F534431004F534432006B65696E656EBA |
:104D5000204D4B20676566756E64656E21006E6F31 |
:104D6000204D4B20666F756E6421004645484C45CA |
:104D700052004552524F52004D4B20446174656EB3 |
:104D80007665726C75737421004D4B2044617461BB |
:104D9000206C6F737421006C6164652053657474BA |
:104DA000696E67732E2E2E006C6F6164696E6720CA |
:104DB00073657474696E67732E2E2E00616B74694F |
:104DC00076696572652053657474696E672E2E2E40 |
:104DD0000061637469766174652073657474696ECB |
:104DE000672E2E2E004265617262656974656E00E1 |
:104DF0006564697400766F6E0066726F6D006E6137 |
:104E0000636800746F00504B5400414B5449562165 |
:104E10000041435449564521004D6F64756C20761E |
:104E20006F7268616E64656E004D6F64756C6520AD |
:104E3000657869737400496E697469616C69736936 |
:104E40006572656E00496E697469616C697A6500A6 |
:104E50006C616E6765722054617374656E647275FF |
:104E6000636B3A006C6F6E672070726573733A0003 |
:104E70004661762068696E7A75676566FC6774219D |
:104E80000066617620616464656421002A204661C1 |
:104E9000762069737420766F6C6C202A002A206655 |
:104EA00061762069732066756C6C202A002A204682 |
:104EB000617620766F7268616E64656E202A002AC2 |
:104EC00020666176206578697374732A006B656962 |
:104ED0006E2046617620766F7268616E64656E2121 |
:104EE000006E6F20666176206578697374210045D5 |
:104EF000696E7472616720656E746665726E742186 |
:104F0000006974656D2064656C6574656421004595 |
:104F1000696E7472616720656E746665726E656E27 |
:104F20003F0064656C657465206974656D3F006E53 |
:104F300069636874206DF6676C69636821006E6F41 |
:104F40007420706F737369626C65004D696E75745F |
:104F5000656E2028303D41757329006D696E75744A |
:104F600065732028303D6F666629003C342E323050 |
:104F70002045696E7A656C7A656C6C652020003C12 |
:104F8000342E32302073696E676C652063656C6CFB |
:104F90002020003E342E323020476573616D7473DB |
:104FA00070616E6E752E003E342E323020746F7438 |
:104FB000616C20766F6C74616765007665727374DE |
:104FC000656C6C656E2C2062697320646965006194 |
:104FD000646A757374206F666673657420756E7489 |
:104FE000696C005370616E6E756E672070617373CB |
:104FF000742100766F6C7461676520666974732133 |
:1050000000446174656E0064617461006C65736571 |
:105010000072656164696E67004C6162656C730063 |
:105020006C6162656C730065787465726E6573207F |
:105030005356322D4D6F64756C0065787465726ED1 |
:10504000616C20535632206D6F64756C65002A20A8 |
:1050500041434854554E47202A002A204154544584 |
:105060004E54494F4E202A004B6F70746572206178 |
:105070007573736368616C74656E2100737769740E |
:105080006368206F6666206B6F70746572210057CD |
:10509000692E323332204B6F6E6669672E00576976 |
:1050A0002E32333220436F6E666967006D6974205B |
:1050B0005553422076657262756E64656E00636F4B |
:1050C0006E6E656374656420776974682055534219 |
:1050D000007369656865204D4B2D57696B693A000F |
:1050E000736565204D4B2D57696B693A006A65748D |
:1050F0007A7420617573736368616C74656E3F00C8 |
:10510000737769746368206F6666206E6F773F00FF |
:10511000504B54206E6575207374617274656E21F6 |
:10512000007265737461727420504B542100616B7E |
:10513000746976696572652057692E3233322100B1 |
:1051400061637469766174652057692E3233322148 |
:10515000002A20776172746520617566205361743E |
:10516000666978202A002A2077616974696E672051 |
:10517000736174666978202A0076657262696E646C |
:1051800065204750532D4D617573007365617263DF |
:1051900068206770732D6D6F757365007472656E2E |
:1051A0006E65204750532D4D617573006469736AB5 |
:1051B0006F696E206770732D6D6F757365006B6519 |
:1051C000696E2042544D32323220766F72682E0062 |
:1051D0006E6F2042544D32323220696E7374616CAE |
:1051E0006C6564006B65696E65204750532D4D6199 |
:1051F000757321006E6F206770732D6D6F75736509 |
:1052000021004750532D4D617573205665726269B8 |
:105210006E64756E67006770732D6D6F75736520B2 |
:10522000636F6E6E656374696F6E004254204765EC |
:1052300072E474652073756368656E3F0073656121 |
:1052400072636820425420646576696365733F0029 |
:10525000737563686520425420476572E474650085 |
:105260007365617263682042542064657669636582 |
:1052700073006B65696E20476572E474206765662C |
:10528000756E64656E00425420476572E474650073 |
:1052900042542064657669636573002863612E203B |
:1052A00032204D696E7574656E29002863612E2069 |
:1052B00032206D696E757465732900425420446113 |
:1052C00074656E7665726C75737400425420646107 |
:1052D0007461206C6F7373004D65746572006D6549 |
:1052E000746572730057617274652061756620473A |
:1052F000505320446174656E0077616974696E670C |
:1053000020666F72204750532044617461004C61E5 |
:10531000737472696E67000014BE88E10FB6F8946A |
:1053200080936000109260000FBE11241FBECFEF6B |
:10533000D0E4DEBFCDBF14E0A0E0B1E0E2E3FCEAE0 |
:1053400001E00BBF02C007900D92AC39B107D9F74D |
:1053500019E2ACE9B4E001C01D92AE38B107E1F743 |
:105360000E944ACC0C9417D60C940000EF920F9335 |
:105370001F93182F80E062E24FE720E101E092E006 |
:10538000E92E0E9411A9812F90E00E947CCEAC01F1 |
:105390008CE067E020E00E947FAA1D3019F488EEBF |
:1053A00093E002C089E190E06FEF7FEF40E00E9460 |
:1053B000D24C0E94C74C80E10E94844C182F80E29E |
:1053C0000E94844C882311F092E003C0112399F3CA |
:1053D00091E080E0913009F481E01F910F91EF900E |
:1053E000089510929C0410929D048823C1F08DE0D2 |
:1053F00090E00E947CCEAC018CE067E020E00E944F |
:105400007FAA89E190E06FEF7FEF40E00E94D24CED |
:105410000E94C74C80E20E94844C8823D9F30895EF |
:10542000EF92FF920F931F937C01162F80E060E1B3 |
:105430004EE727E000E00E941BA7812F0E94EDCAE3 |
:105440002DB73EB72C5030403EBF2DBFEDB7FEB755 |
:10545000319622E0ADB7BEB711962C93118212821D |
:10546000138225E135E035832483F782E682608765 |
:105470007187828793870E949DAF2DB73EB7245FC7 |
:105480003F4F3EBF2DBF1F910F91FF90EF900895AA |
:10549000EF92FF920F931F93182F7B01022F60E072 |
:1054A000422F0E943FAA812FB70140E020E00E94D6 |
:1054B0008BAA1F910F91FF90EF900895EF920F9399 |
:1054C000CF93DF93EC0130919C04332309F451C056 |
:1054D00020919D042F5F20939D048DB79EB70A975E |
:1054E0009EBF8DBFEDB7FEB7319684E0ADB7BEB7B6 |
:1054F00011968C931182128283E0838380E795E07A |
:105500009583848326831782308711860E949DAFFE |
:105510008DB79EB70A969EBF8DBF2097C1F085EAD2 |
:1055200098E0BE0144E150E020E202E00E94D14454 |
:1055300082E06BE24CE727E000E00E941BA785E0D9 |
:1055400065EA78E040E020E003E00E9480AF80E080 |
:105550006FE14FE726E101E032E0E32E0E9411A95E |
:1055600088E59BE124EF31E0F9013197F1F70197EC |
:10557000D9F7DF91CF910F91EF900895CF93DF93FB |
:10558000EC0170938D1660938C1609C00E947948C7 |
:10559000892B29F00E942748888381E007C08091E9 |
:1055A0008C1690918D16892B89F780E0DF91CF9131 |
:1055B0000895A1E0B0E0EFEDFAE20C9421D2109250 |
:1055C0009808109297088DE00E944348FF2409E94B |
:1055D00018E0EE246E010894C11CD11C37C029814B |
:1055E000FF2019F42C3391F52FC02E3369F08E2D46 |
:1055F00090E001960C9718F0FF24F3942DC0F80169 |
:1056000021938F01E39420C0F801108220919C081F |
:1056100030E02051324080919B084AE0849FC001D5 |
:105620001124280F391F8091990844E6849FC001F6 |
:105630001124805C9241280F391F309398082093E1 |
:10564000970842E0F42E11C0FF24F394C60164E6EB |
:1056500070E094DF882319F680E695E061E048EB7E |
:105660005BE021E001E00E94A86680E0F2E0FF1626 |
:1056700009F481E02196E8E00C943DD2EF92FF928C |
:105680001F93DF93CF930F92CDB7DEB710E07E016B |
:105690000894E11CF11C0CC09981112321F49E3364 |
:1056A00039F411E005C080E0903241F481E006C099 |
:1056B000C70164E670E062DF882371F70F90CF9135 |
:1056C000DF911F91FF90EF9008951F93CF93DF9389 |
:1056D00012E0C4EFD1E08CE695E00E946C4888EEC1 |
:1056E00093E0FE013197F1F70197D9F78DE00E9421 |
:1056F000434880E20E948E4C882311F08FEF10C047 |
:10570000BDDF882349F424EC39E0FE013197F1F73D |
:1057100021503040D1F71150112311F08823D9F2D4 |
:10572000DF91CF911F9108951F93182F85E190E08D |
:105730000E947CCE612F74DE812F0E940048C5DF5D |
:1057400018160CF4812F1F910895A1E0B0E0EBEA48 |
:10575000FBE20C941BD27C014B01FC018491882359 |
:1057600009F18701D82E3E010894611C711CC82ED6 |
:1057700057010894A11CB11C0FC089818D1519F423 |
:105780000F5F1F4F05C08C1511F0870101C0850107 |
:10579000F801D490DD2031F0C301B401EFDE88239D |
:1057A00061F701C081E02196EEE00C9437D2A0E0D1 |
:1057B000B0E0EDEDFBE20C9422D28C01EB017A011A |
:1057C000D22E7CDEC8010E946C48209769F080E2EE |
:1057D0000E94434806C0803209F484E20E94434894 |
:1057E000219688818823B9F78DE00E944348E1140F |
:1057F000F10481F0C70160ED77E0A7DF882351F461 |
:10580000C80161E048EB5BE021E001E00E94A8668E |
:1058100080E005C0DD2011F481E001C02FDFCDB7AD |
:10582000DEB7E7E00C943ED288EB9BE024EF31E05A |
:10583000F9013197F1F70197D9F781E295E060E03E |
:1058400070E040E050E020E0B2DF80E29EE424EF30 |
:1058500031E0F9013197F1F70197D9F781E0089527 |
:10586000BC0183E395E048E255E020E0A0DF882317 |
:1058700021F08DE00E94434801CF089580ED97E02C |
:1058800024EF31E0F9013197F1F70197D9F780E47E |
:1058900095E060E070E048E355E021E088CF8EE4D9 |
:1058A00095E060E070E045E455E020E080DF20E135 |
:1058B00037E244EF51E0FA013197F1F721503040DF |
:1058C000D1F708954CE555E021E071CFEF92FF92BA |
:1058D0000F931F938C01CB0156E7E52E55E1F52E72 |
:1058E000B7014AE050E00E94BFD5C801B701EADF26 |
:1058F0001F910F91FF90EF90089560E070E04CE5EC |
:1059000055E021E054CF0F9383E00E9460C98CE3FF |
:1059100090E090938D1680938C1680918C169091C8 |
:105920008D16892BD1F78DE090E00E947CCEAC01E2 |
:105930008CE067E020E00E947FAA84E0F5DE082F7B |
:105940008823E1F481E0F0DE082F8823B9F485E0B4 |
:10595000EBDE082F882391F483E0E6DE082F88230E |
:1059600069F482E0E1DE082F882341F486E0DCDE82 |
:10597000082F882319F487E0D7DE082F87E060E03E |
:1059800040E00E943FAA07FF09C08091271E0E94A5 |
:10599000004881E00E9460C90FEF41C0002391F4EC |
:1059A0008091271E0E94004881E00E9460C987EF15 |
:1059B00094E061E048EB5BE021E001E00E94A86632 |
:1059C00000E02DC086E190E00E947CCE602F28DDB3 |
:1059D0008091271E90E0202F332727FD30958217D6 |
:1059E000930711F401E01BC087E190E00E947CCE98 |
:1059F0006091271E15DD87E095E07FDF882331F079 |
:105A00003DDF882319F010DF082F01C000E08091EE |
:105A1000271E0E940048002311F057DE082F802F18 |
:105A20000F9108950F931F93CF93DF93CAE7D1E0AF |
:105A3000CE0161E00E94E8AF66DF182F87FD54C0F9 |
:105A4000882359F1CE0161E00E94E8AF8DB79EB77F |
:105A500008979EBF8DBFEDB7FEB7319685E0ADB715 |
:105A6000BEB711968C93118212828FEF838386E7E3 |
:105A700091E095838483D783C6830E949DAF8DB7C1 |
:105A80009EB708969EBF8DBF80E270DC882329F404 |
:105A900081E00E9460C980E02AC08AE791E061E06D |
:105AA0000E94E8AF112361F082E080939C04109281 |
:105AB0009D04F5DE882311F410E002C0B5DE182F36 |
:105AC00080E08FDC81E00E9460C9112321F08CE628 |
:105AD00091E060E003C080E691E061E048EB5BE0CC |
:105AE00021E001E00E94A8660E94C74C812FDF914F |
:105AF000CF911F910F910895EF920F931F9389EE0D |
:105B000094E061E00E94E8AFFEDE182F87FD54C0EC |
:105B1000882311F04EDD182F81E00E9460C9112307 |
:105B200009F441C080E063E24FE720E101E072E068 |
:105B3000E72E0E9411A98DB79EB708979EBF8DBF13 |
:105B4000EDB7FEB7319685E0ADB7BEB711968C9331 |
:105B50001182128213828CED94E09583848389E90B |
:105B600098E0978386830E949DAF8DB79EB7089675 |
:105B70009EBF8DBF8DE090E00E947CCEAC018CE09A |
:105B800067E020E00E947FAA89E190E06FEF7FEF5D |
:105B900040E00E94D24C0E94C74C80E20E94844C9C |
:105BA0008823D9F309C08EEC94E061E048EB5BE018 |
:105BB00021E001E00E94A8660E94C74C812F1F913E |
:105BC0000F91EF900895FF920F931F93CF93DF9360 |
:105BD000F82E86E193E061E00E94E8AF94DE082FA2 |
:105BE00087FF04C00E94C74C802F1DC1882309F481 |
:105BF00032C1DFDC882309F42EC122E0F21619F04D |
:105C00000DE811E002C006E811E0C6E1D3E0CE01E4 |
:105C100061E00E94E8AF8DE590E00E947CCEADB7D8 |
:105C2000BEB71A97BEBFADBFEDB7FEB7319622E043 |
:105C300011962C93118212822CEF23832EE033E0F5 |
:105C4000358324839783868389E998E091878087C9 |
:105C50000E949DAF2DB73EB7265F3F4F3EBF2DBF81 |
:105C60008EE590E00E947CCEADB7BEB71A97BEBF5E |
:105C7000ADBFEDB7FEB7319623E011962C9311829C |
:105C800012822EEF238326E033E03583248397832B |
:105C90008683118700870E949DAF0F900F90EDB70C |
:105CA000FEB7319685E0ADB7BEB711968C931182E1 |
:105CB00012828FEF838382E093E095838483D7837E |
:105CC000C6830E949DAF2DB73EB7285F3F4F3EBFB2 |
:105CD0002DBF80E24BDB882309F0AAC081E00E943F |
:105CE00060C980E0A0C0A0DD882309F4B4C00BDE49 |
:105CF000882309F4B0C082E06AEE72E040E020E060 |
:105D0000C7DB8CED92E0F9DD882309F4A4C08CECAC |
:105D100092E067ED7EE1D6DD882309F49CC089EB33 |
:105D200092E0EBDD882309F496C08CEC9EE198DDCF |
:105D3000882309F490C082E06EEA72E040E020E03F |
:105D4000A7DB8EE892E0D9DD882309F484C08BE7D5 |
:105D500092E0D3DD882309F47EC087E692E0CDDDB2 |
:105D6000882309F478C082E592E0C7DD882309F42E |
:105D700072C08EE392E0C1DD882309F46CC080918B |
:105D8000970890919808885B914088F082E0F81617 |
:105D900039F482E292E060E070E048E152E017C03E |
:105DA0008DEF91E060E070E043EF51E010C092E0D1 |
:105DB000F91639F488ED91E060E070E04EEC51E0C6 |
:105DC00006C084EB91E060E070E04AEA51E020E038 |
:105DD000EEDC882309F43FC062DD182F882361F0D0 |
:105DE00082E390E090938D1680938C1680918C16B0 |
:105DF00090918D16892BD1F780E0F3DA81E00E9433 |
:105E000060C9112321F48DE991E061E003C082E9CA |
:105E100091E060E048EB5BE021E001E00E94A866D1 |
:105E20000E94C74C812FCDB7DEB7E5E00C9440D27D |
:105E3000CE0161E00E94E8AF80E180939C04109263 |
:105E40009D0482E06CEF72E040E020E021DB27DD82 |
:105E5000882309F048CF10E0CFCFBF92CF92DF92D6 |
:105E6000EF92FF920F931F937C016A01B22E06E717 |
:105E700015E1C80144E150E00E94CED4C701B80149 |
:105E8000A6012B2D94DC1F910F91FF90EF90DF90D6 |
:105E9000CF90BF9008954CE555E021E0DECF1F93F1 |
:105EA000182F82E069EC74E040E020E0F1DA123073 |
:105EB00009F038C089E080939C0410929D0489EB1E |
:105EC00094E01BDD882309F488C08BEA94E015DD9B |
:105ED000882309F482C08CE994E064E873E0DBDF96 |
:105EE000882309F47AC08DE894E060E973E0D3DF99 |
:105EF000882309F472C08EE794E06CE973E0CBDF8D |
:105F0000882309F46AC080E794E061E77EE1DADC87 |
:105F1000882309F455C060918F1E82E694E070E0FA |
:105F2000D5DC4EC0113009F04DC08DE080939C044B |
:105F300010929D0482E594E0E0DC882309F44DC0D2 |
:105F400084E494E0DADC882309F447C085E394E034 |
:105F500064E873E0A0DF882309F43FC086E294E0A0 |
:105F600060E973E098DF882309F437C087E194E0A3 |
:105F70006CE973E090DF882381F187E094E0BDDC79 |
:105F8000882359F187EF93E061E77EE19BDC88236A |
:105F900021F187EE93E06CE77EE194DC8823E9F061 |
:105FA00060918F1E86ED93E070E090DC8823A9F06D |
:105FB00082EC93E0A2DC882381F088EA93E09DDC08 |
:105FC000882359F05BDC182F882311F02DDC182F63 |
:105FD00080E007DA812F1F91089510E0F9CFA0E04B |
:105FE000B0E0E5EFFFE20C9421D2082F823019F0E7 |
:105FF000CBE6D3E002C0C3E7D3E089E793E061E0FA |
:106000000E94E8AF80DC182F87FDE7C0882309F4E1 |
:1060100077C0CFDA882309F473C0F9E7CF2EF3E015 |
:10602000DF2EE9E9EE2EE8E0FE2E023069F48091E1 |
:10603000970890919808883E910509F463C08E3EB8 |
:10604000910509F45FC00BC0013009F05BC080917D |
:10605000970890919808895B914009F453C081E0BA |
:106060000E9460C989E793E061E00E94E8AF8DB7C4 |
:106070009EB708979EBF8DBFEDB7FEB7319682E001 |
:10608000ADB7BEB711968C93118212828CEF8383C9 |
:1060900081E693E095838483F782E6820E949DAF38 |
:1060A0000F900F90EDB7FEB7319683E0ADB7BEB756 |
:1060B00011968C93118212821FEF13838BE593E06C |
:1060C000958384830E949DAF00D0EDB7FEB73196D3 |
:1060D00085E0ADB7BEB711968C93118212821383FF |
:1060E0008CE493E095838483D783C6830E949DAF1D |
:1060F0008DB79EB708969EBF8DBF8DE037D952C031 |
:1061000010E059C089E793E061E00E94E8AFADB7C5 |
:10611000BEB71897BEBFADBFEDB7FEB7319682E0F0 |
:1061200011968C93118212828CEF838382E493E028 |
:1061300095838483F782E6820E949DAFEDB7FEB718 |
:10614000319683E0ADB7BEB711968C93118212825F |
:106150008EEF83838BE393E095838483D783C68319 |
:106160000E949DAFEDB7FEB7319685E0ADB7BEB7E3 |
:1061700011968C93118212828FEF838387E393E0D1 |
:1061800095838483D782C6820E949DAF8DB79EB7C8 |
:1061900008969EBF8DBF80E2E9D8882329F481E06C |
:1061A0000E9460C980E01CC0C60161E00E94E8AFA7 |
:1061B000802F75DE182F81E00E9460C9112321F421 |
:1061C0008CE293E061E003C083E293E060E048EB9F |
:1061D0005BE021E001E00E94A8660E94C74C812F8D |
:1061E000CDB7DEB7E8E00C943DD20F931F930E9429 |
:1061F000C73D8091701E813019F48BE596E007C091 |
:10620000823019F48FE496E002C089E496E029D642 |
:1062100081E061E040E050E026E735E002E815E08B |
:106220000E94A73D8091701E882309F478C087E002 |
:1062300061E040E050E02DE835E00BE915E00E9418 |
:10624000A73D0E94343D82E061E040E050E028EA52 |
:1062500035E089010E94A73D8091701E813051F484 |
:1062600083E061E040E050E02DEA35E006EB15E028 |
:106270000E94A73D84E061E040E050E02FEB35E074 |
:1062800005EC15E00E94A73D8091701E813039F425 |
:1062900085E061E040E050E02BE336E006C085E0B9 |
:1062A00061E040E050E02FE236E00E948E3D0E9427 |
:1062B000343D8BE061E040E050E02DEC35E08901B9 |
:1062C0000E94A73D8CE061E040E050E027ED35E022 |
:1062D00005EE15E00E94A73D88E061E040E050E057 |
:1062E00021E236E00E948E3D0E94343D86E061E06E |
:1062F00040E050E024E136E00E948E3D89E061E01C |
:1063000043E05CEC23EF35E005E016E00E94A73D9A |
:106310008091701E823019F488E060E09FD61F9152 |
:106320000F910895A4E1B0E0E8E9F1E30C941FD2E5 |
:106330005CDF5E010894A11CB11C80E00E94783EE5 |
:10634000B8D4853009F4B6C08CD410925906813087 |
:10635000D1F41091701E83E790E00E947CCE7C0106 |
:10636000812F90E060E070E042E050E02EE001E03C |
:10637000CC24DD240E94D0568093701E1817E9F2B9 |
:10638000E5D533DFDACF873011F4B6DBD6CF8230F4 |
:10639000C1F4C50161E77EE14BE050E00E94B743E4 |
:1063A000C5016AE043E00E9485538091590681301F |
:1063B00009F0C3CF82EA96E10E94D94481E79EE1C9 |
:1063C00019C08330F1F4C5016CE77EE14BE050E089 |
:1063D0000E94B743C5016AE044E00E948553809162 |
:1063E0005906813009F0A9CF82EA96E10E94D9448A |
:1063F0008CE79EE162EA76E14BE050E00E9434D502 |
:106400009CCF8B30C1F4C5016CEC7EE14BE050E0D9 |
:106410000E94B743C5016AE043E00E948553809122 |
:106420005906813009F089CF82EA96E10E94D94469 |
:106430008CEC9EE1DFCF8C30C1F4C50167ED7EE1CD |
:106440004BE050E00E94B743C5016AE044E00E947F |
:10645000855380915906813009F06FCF82EA96E129 |
:106460000E94D94487ED9EE1C5CF843091F480919C |
:106470008F1E90E060E070E04DE050E020E001E031 |
:10648000EE24FF24CC24DD240E94D05680938F1E5E |
:1064900054CF853021F48091701EA1DD4ECF86301F |
:1064A00011F4C0DA4ACF883009F047CF8091701ECE |
:1064B0008ADB43CF4BD56496EAE00C943BD2EF9253 |
:1064C0000F930E94BBAF80E060E04FE727E001E060 |
:1064D0000E941BA786E790E00E947CCE2DB73EB7B6 |
:1064E000275030403EBF2DBFEDB7FEB7319621E0BB |
:1064F000ADB7BEB711962C93118222E0228313828E |
:106500001482968385830E940AAD2DB73EB7295F1A |
:106510003F4F3EBF2DBF87E790E00E947CCEBC017D |
:1065200082E040E020E00EEF0E948BAA88E790E036 |
:106530000E947CCEBC0184E040E020E000E00E94AC |
:106540008BAA85E069E676E040E020E002E00E9468 |
:106550008BAA80E06CE14FE728E101E092E0E92EB0 |
:106560000E9411A98DE090E00E947CCEAC018CE0ED |
:1065700067E020E00E947FAA80E00E94AFCA5E9898 |
:1065800002C00E94ADA480E20E94844C8823C9F31B |
:106590005E9A8091C01D813011F486E001C085E0D3 |
:1065A0000E94AFCA84E190E00E94E0478091271EDC |
:1065B0000E9400480F91EF9008951F93CF93DF93AF |
:1065C000182F0E94DA488FEF0E948A4982E00E94C9 |
:1065D0008A498EEF0E948A49812F0E948A4984EF5E |
:1065E00091E024EF31E0F9013197F1F70197D9F704 |
:1065F00010E0C4EFD1E00E94DA4828EE33E0FE015B |
:106600003197F1F721503040D1F71F5F882319F4FB |
:106610001A3089F707C01A3011F480E003C08630C1 |
:1066200009F08FEFDF91CF911F910895FF920F93A3 |
:106630001F93F82E82E060E041E00E943FAA85E1CE |
:1066400090E00E947CCE8C018F2D0E94EDCA2DB768 |
:106650003EB72C5030403EBF2DBFEDB7FEB7319650 |
:1066600022E0ADB7BEB711962C931182128221E0C1 |
:10667000238325EB36E035832483178306836087E5 |
:106680007187828793870E949DAF2DB73EB7245FA5 |
:106690003F4F3EBF2DBF8F2D0E94634988E78DDF9E |
:1066A00081508E3F18F020E030E002C02F2D30E006 |
:1066B000C9011F910F91FF9008950F931F93CF93DE |
:1066C000DF93082F162F67FD0BC08FEF0E948A49BA |
:1066D00082E00E948A49802F0E948A49812F0EC041 |
:1066E0008FEF0E948A4983E00E948A49802F0E948E |
:1066F0008A498EEF0E948A49812F80580E948A49D8 |
:1067000010E0C4EFD1E00E94DA4890E028EE33E0D8 |
:10671000FE013197F1F721503040D1F7009721F475 |
:106720001F5F1A3081F704C0069711F480E001C0A2 |
:106730008FEFDF91CF911F910F9108950F931F93CA |
:10674000042FBBDF182F0E94EA631111102F812F35 |
:106750001F910F910895EF92FF920F931F93E09076 |
:10676000271E85E790E00E947CCE8C0184E790E0B4 |
:106770000E947CCE9C0183E0B80140E050E007E736 |
:1067800016E00E94AA65882309F447C1C80161E0A8 |
:1067900041E00E94476485E00E94AFCA0E94204AFF |
:1067A0005E9888EE93E024EF31E0F9013197F1F73C |
:1067B0000197D9F78091B71639DFF82E882309F0B1 |
:1067C000FAC087E033DFF82E882309F0F4C081E0B7 |
:1067D0002DDFF82E882309F0EEC082E027DFF82EA7 |
:1067E000882309F0E8C083E021DFF82E882309F030 |
:1067F000E2C084E01BDFF82E882309F0DCC085E0CE |
:1068000015DFF82E882309F0D6C084EA96E061E00F |
:1068100040ED57E021E001E00E94A866C4C08BE48F |
:106820006091B61D42E08ADF282F882309F078C0E6 |
:1068300081E06091B61D43E081DF282F882309F0B5 |
:106840006FC08CE46091B61D44E078DF282F882368 |
:1068500009F066C08DE060E045E070DF282F8823F6 |
:1068600009F05EC082E063E046E068DF282F8823FD |
:1068700009F056C085E06091B91D47E05FDF282F21 |
:10688000882309F04DC080E56091B91D48E056DFCE |
:10689000282F882309F044C089E06091BA1D49E09F |
:1068A0004DDF282F882309F03BC084E56091BA1D95 |
:1068B0004AE044DF282F882399F584E06091B81DD1 |
:1068C0004BE03CDF282F882359F58FE46091B81DF9 |
:1068D0004CE034DF282F882319F588E061E04DE093 |
:1068E0002DDF282F8823E1F48BE061E04EE026DFE6 |
:1068F000282F8823A9F486E06091B71D4FE01EDFA2 |
:10690000282F882369F481E56091B71D40E116DFE7 |
:10691000282F882329F483E06E2D41E10FDF282FF3 |
:1069200080ED97E044EF51E0FA013197F1F70197DC |
:10693000D9F7222329F48EE46E2D42E1FFDE282FC1 |
:1069400084EC99E044EF51E0FA013197F1F70197B7 |
:10695000D9F72223E9F000D000D0EDB7FEB7319689 |
:1069600083E996E0ADB7BEB712969C938E931197CC |
:10697000228313820E9472470F900F900F900F9006 |
:1069800061E040ED57E021E001E00E94B8660BC0F5 |
:1069900083E896E060E048EE53E021E001E00E94E9 |
:1069A000A8660E9425B95E9A8091271E0E946349BD |
:1069B0000E94C74C32C007E716E0C80161E041E021 |
:1069C0000E94476400D000D000D0EDB7FEB73196EA |
:1069D00082E0ADB7BEB711968C931182128281E02E |
:1069E0008383158304830E949DAF8DB79EB706965F |
:1069F0009EBF8DBF82E160E040E020E00E9419630D |
:106A00008F2D0E94634980E06091B61D41E096DEC3 |
:106A1000282F882309F084CF02CF1F910F91FF9078 |
:106A2000EF90089580918E01992787FD90952DED27 |
:106A300031E0829FF001839FF00D929FF00D1124B1 |
:106A4000E554F74F90818381292F281B27FF02C02F |
:106A500093830895263014F095509383089580937E |
:106A60009E04089580918E01089580918E0199274A |
:106A700087FD90952DED31E0829FF001839FF00D11 |
:106A8000929FF00D1124E554F74F8085882339F04B |
:106A900089E190E06FEF7FEF40E00C94D24C0895D5 |
:106AA0000F931F938330A9F0843028F4813041F094 |
:106AB000823049F40BC0843079F0853021F40FC066 |
:106AC00004E617E00EC000E010E00BC00CE517E094 |
:106AD00008C004E517E005C00DE417E002C000E4BB |
:106AE00017E088EE93E06FE070E040E00E94D24C47 |
:106AF00080E197E265E377E040E050E02BE237E0A9 |
:106B00000E94CE631F910F91089520918E0127FF5F |
:106B100004C081E0C5DF80E00895332727FD30956C |
:106B20008DED91E0289FA001299F500D389F500DB9 |
:106B30001124FA01E554F74F808190E02DE030E018 |
:106B4000829FF001839FF00D929FF00D1124E40FBE |
:106B5000F51FE554F74F80A5089520918E0127FF7A |
:106B600005C081E09DDFE0E0F0E047C0622F7727BD |
:106B700067FD70958DED91E0689F9001699F300DE4 |
:106B8000789F300D1124F901E554F74F8081482F8B |
:106B900050E08DE090E0489FF001499FF00D589F34 |
:106BA000F00D1124E20FF31FE554F74F81A581305A |
:106BB00011F4E0E002C0E091B51DF0E0EE0FFF1F20 |
:106BC0008DE090E09C01429FC001439F900D529F39 |
:106BD000900D1124E80FF91F8DED91E09C01629F4B |
:106BE000C001639F900D729F900D1124E80FF91F53 |
:106BF000E554F74F02A4F3A5E02DCF010895682FC7 |
:106C000080918E0187FF04C081E04ADF80E0089513 |
:106C1000992787FD90952DED31E0829FA001839FFC |
:106C2000500D929F500D1124FA01E554F74F8181C8 |
:106C30008823A9F0262F30E090E0281739077CF44C |
:106C40008DE090E0289FF001299FF00D389FF00D16 |
:106C50001124E40FF51FE554F74F83A1089580E058 |
:106C6000089520918E0127FF04C081E019DF80E0A4 |
:106C70000895332727FD30958DED91E0289FA001E1 |
:106C8000299F500D389F500D1124FA01E554F74FFC |
:106C9000808190E02DE030E0829FF001839FF00D35 |
:106CA000929FF00D1124E40FF51FE554F74F83A1D7 |
:106CB000089520918E0127FF04C081E0F1DE80E07D |
:106CC0000895332727FD30958DED91E0289FF00141 |
:106CD000299FF00D389FF00D1124E554F74F86855C |
:106CE000089520918E0127FF02C081E0D9CE843023 |
:106CF000A9F4332727FD30958DED91E0289FF00111 |
:106D0000299FF00D389FF00D1124E554F74F81E0D5 |
:106D10008787718B608B538B428B08958836A9F4DB |
:106D2000332727FD30958DED91E0289FF001299FB5 |
:106D3000F00D389FF00D1124E554F74F81E0848B5E |
:106D4000768B658B508F478B08958530A9F4332758 |
:106D500027FD30958DED91E0289FF001299FF00DE2 |
:106D6000389FF00D1124E554F74F81E0818F738F28 |
:106D7000628F558F448F08958936A1F4332727FDFC |
:106D800030958DED91E0289FF001299FF00D389FFF |
:106D9000F00D1124E554F74F81E0868F70A3678FC3 |
:106DA00052A341A30895482F20918E0127FF02C0CE |
:106DB00081E076CE332727FD30958DED91E0289F39 |
:106DC000F001299FF00D389FF00D1124E554F74F85 |
:106DD00044830895482F20918E0127FF02C081E04F |
:106DE0005FCE332727FD30958DED91E0289FF00190 |
:106DF000299FF00D389FF00D1124E554F74F42877D |
:106E00000895482F20918E0127FF02C081E048CECF |
:106E1000332727FD30958DED91E0289FF001299FC4 |
:106E2000F00D389FF00D1124E554F74F4187089578 |
:106E3000AC0120918E0127FF02C081E031CE3327C3 |
:106E400027FD30958DED91E0289FF001299FF00DF1 |
:106E5000389FF00D1124E554F74F5587448713866A |
:106E60000895AC0120918E0127FF02C081E018CE69 |
:106E7000332727FD30958DED91E0289FF001299F64 |
:106E8000F00D389FF00D1124E554F74F55874487D6 |
:106E900081E08387089520918E0127FF02C081E061 |
:106EA00003C0222311F485E0FBCD2150622F70E056 |
:106EB0008DED91E0689F9001699F300D789F300DB6 |
:106EC0001124F901E554F74F8081482F50E08DE0FF |
:106ED00090E0489FF001499FF00D589FF00D11245C |
:106EE000E20FF31FE554F74F81A5813011F4E0E084 |
:106EF00002C0E091B51DF0E0EE0FFF1F8DE090E0C5 |
:106F0000489FD001499FB00D589FB00D1124EA0F42 |
:106F1000FB1F8DED91E09C01629FC001639F900D6E |
:106F2000729F900D1124E80FF91FE554F74F02A44A |
:106F3000F3A5E02DA80FB91FA554B74F98968C91D3 |
:106F4000882311F0CF018DCFCF0172CF80918E01B8 |
:106F500087FF02C081E0A4CD815080938E01089507 |
:106F6000682F20918E0127FD19C0332727FD30950A |
:106F70008DED91E0A901489F9001499F300D589FE8 |
:106F8000300D1124F901E554F74F8181482F50E06D |
:106F9000F901E252F74F20E030E009C081E080DDE6 |
:106FA0000AC080812F5F3F4F3D96681729F0922FCE |
:106FB00024173507B4F39FEF892F08951F93162FD9 |
:106FC000CFDF87FD21C0482F552747FD50952091E1 |
:106FD0008E01332727FD30958DE090E0489FF0012A |
:106FE000499FF00D589FF00D11248DED91E0AC01FB |
:106FF000249FC001259F900D349F900D1124E80F10 |
:10700000F91FE554F74F17A31F9108951F93162FEB |
:10701000A7DF87FD21C0482F552747FD50952091B8 |
:107020008E01332727FD30958DE090E0489FF001D9 |
:10703000499FF00D589FF00D11248DED91E0AC01AA |
:10704000249FC001259F900D349F900D1124E80FBF |
:10705000F91FE554F74F16A31F9108951F93162F9C |
:107060007FDF87FD25C0482F552747FD509520918C |
:107070008E01332727FD30958DE090E0489FF00189 |
:10708000499FF00D589FF00D11248DED91E0AC015A |
:10709000249FC001259F900D349F900D1124E80F6F |
:1070A000F91FE554F74F15A2112311F481E085A3D0 |
:1070B0001F91089555DF482F87FD11C080918E01E3 |
:1070C000992787FD90952DED31E0829FF001839FF8 |
:1070D000F00D929FF00D1124E554F74F4083089571 |
:1070E000A0E0B0E0E6E7F8E30C9417D2182F809107 |
:1070F0008E0187FF03C081E0D3DC35C4123009F074 |
:1071000031C180E068E046E02FE200E00E941BA76A |
:107110008DE760E042E02FE30E941BA780918E0183 |
:10712000992787FD90952DED31E0829FF001839F97 |
:10713000F00D929FF00D1124E554F74F248535850D |
:107140002115310509F482C080919E04882309F439 |
:1071500047C083858823A1F04DB75EB749505040A2 |
:107160005EBF4DBFEDB7FEB73196ADB7BEB7119656 |
:107170001C92118212831382148284E297E013C05E |
:107180004DB75EB7495050405EBF4DBFEDB7FEB73B |
:107190003196ADB7BEB711961C921182128313823D |
:1071A00014828DE197E096838583308727830E9440 |
:1071B0000AADEDB7FEB73996FEBFEDBF40918E0127 |
:1071C000552747FD509580E060E021E030E000E089 |
:1071D00072E0E72ECC24C394AA240E9435AF3DC0B0 |
:1071E00083858823A1F04DB75EB7495050405EBFFC |
:1071F0004DBFEDB7FEB73196ADB7BEB711961C9235 |
:10720000118212831382148287E197E013C04DB775 |
:107210005EB7495050405EBF4DBFEDB7FEB73196E7 |
:10722000ADB7BEB711961C921182128313821482DD |
:1072300081E197E096838583308727830E940AAD9A |
:10724000EDB7FEB73996FEBFEDBF07C080E060E046 |
:1072500040E827E001E00E941BA787E060E040E0F3 |
:107260000E943FAA81E067E040E020E00E943B6787 |
:1072700083E090E00E947CCEAC018CE067E020E0EF |
:107280000E947FAA80918E01992787FD90952DED10 |
:1072900031E0829FF001839FF00D929FF00D112449 |
:1072A000E554F74F8785882329F1208931892115F5 |
:1072B000310501F14DB75EB7495050405EBF4DBF3B |
:1072C000EDB7FEB7319681E1ADB7BEB711968C939D |
:1072D00087E081831282138214828DE097E0968387 |
:1072E0008583308727830E940AADEDB7FEB73996B4 |
:1072F000FEBFEDBF80918E01992787FD90952DED02 |
:1073000031E0829FF001839FF00D929FF00D1124D8 |
:10731000E554F74F818D882329F1228D338D211576 |
:10732000310501F14DB75EB7495050405EBF4DBFCA |
:10733000EDB7FEB731968CE0ADB7BEB711968C9322 |
:1073400087E0818312821382148289E097E096831A |
:107350008583308727830E940AADEDB7FEB7399643 |
:10736000FEBFEDBF80918E01992787FD90952DED91 |
:1073700031E0829FF001839FF00D929FF00D112468 |
:10738000E554F74F8781882311F00E94ADA4AA2409 |
:1073900098E0592E8DEDC82E81E0D82E0DE0802E7C |
:1073A000912C44244394BFEF2B2EB6E03B2E8091CA |
:1073B0008E01482F552747FD50954C9D90014D9DBE |
:1073C000300D5C9D300D1124F901E554F74F6380B9 |
:1073D0006A0C8181681608F054C28081681611F029 |
:1073E00010E213C0862D90E0889DF001899DF00D7C |
:1073F000989DF00D1124E20FF31FE554F74F85A17E |
:10740000882311F41DE101C015E1C62DD0E0C89D0F |
:10741000F001C99DF00DD89DF00D11244C9DC001C7 |
:107420004D9D900D5C9D900D1124E80FF91FE554C2 |
:10743000F74F86A1882311F00AE201C000E2C89D3F |
:10744000F001C99DF00DD89DF00D11244C9DC00197 |
:107450004D9D900D5C9D900D1124E80FF91FE55492 |
:10746000F74F64A1623019F4ADE1BA2E02C0F0E228 |
:10747000BF2EC89DF001C99DF00DD89DF00D1124BF |
:107480004C9DC0014D9D900D5C9D900D1124E80F09 |
:10749000F91FE554F74F77A1772311F0EFE1BE2EE6 |
:1074A000C89DF001C99DF00DD89DF00D11244C9D93 |
:1074B000C0014D9D900D5C9D900D1124E80FF91FAA |
:1074C000E554F74F81A5813011F4772402C0709004 |
:1074D000B51D633009F04BC02DB73EB72D5030407D |
:1074E0003EBF2DBFADB7BEB71196EDB7FEB7418217 |
:1074F0004C9DF0014D9DF00D5C9DF00D1124E55467 |
:10750000F74F84818F5F8A0D11968C931197129695 |
:107510001C92129713961C92139714961C92149710 |
:1075200020E037E016963C932E93159717961C9300 |
:10753000179718961C92189719960C9319971A96E4 |
:107540001C921A971C963C922E921B970E940AAD91 |
:107550004DB75EB7435F5F4F5EBF4DBF652D6D5F3B |
:1075600082E14AE620E001E00E941BA78AC1643064 |
:1075700009F08AC089E298E16BEF76E00E94BED400 |
:1075800080918E01992787FD9095C89DF001C99D36 |
:10759000F00DD89DF00D1124DC01AC9DC001AD9D16 |
:1075A000900DBC9D900D1124E80FF91FE554F74F85 |
:1075B00085A1882359F483A10E949589E091001840 |
:1075C000F09101188091FF1762E0099580918E017A |
:1075D000992787FD9095EDB7FEB73F97FEBFEDBFAA |
:1075E000ADB7BEB7119641828C9D90018D9D300D37 |
:1075F0009C9D300D1124F901E554F74F84818F5F74 |
:107600008A0D11968C93119712961C9212971396CD |
:107610001C92139714961C9214974EEE56E01696F1 |
:107620005C934E93159717961C93179718961C9278 |
:10763000189719960C9319971A961C921A97E72D7A |
:10764000F0E0EE0FFF1FC89DC001C99D900DD89DB1 |
:10765000900D1124E80FF91FE20FF31FE554F74FC7 |
:1076600082A593A51C969C938E931B9789E298E123 |
:107670001E969C938E931D970E940AADADB7BEB720 |
:107680001F96BEBFADBFFDC0C89D7001C99DF00C67 |
:10769000D89DF00C11244C9D90014D9D300D5C9DAA |
:1076A000300D1124F701E20FF31FE554F74F80A5C9 |
:1076B000882309F447C0623011F0772329F1EDB730 |
:1076C000FEB73F97FEBFEDBFADB7BEB71196418283 |
:1076D0004C9D90014D9D300D5C9D300D1124F901A4 |
:1076E000E554F74F84818F5F8A0D11968C93119723 |
:1076F00012961C92129713961C92139714961C9232 |
:10770000149742EE56E047C0ADB7BEB71D97BEBF57 |
:10771000ADBF1196EDB7FEB74182F901E554F74FC1 |
:1077200084818F5F8A0D11968C93119712961C920B |
:10773000129713961C92139714961C92149748ED67 |
:1077400056E078C0623019F0772309F456C0ADB71F |
:10775000BEB71F97BEBFADBF1196EDB7FEB7418252 |
:107760004C9D90014D9D300D5C9D300D1124F90113 |
:10777000E554F74F84818F5F8A0D11968C93119792 |
:1077800012961C92129713961C92139714961C92A1 |
:1077900014974CEC56E016965C934E9315971796FB |
:1077A0001C93179718961C92189719960C93199773 |
:1077B0001A961C921A97E72DF0E0EE0FFF1FC89D56 |
:1077C000C001C99D900DD89D900D1124E80FF91F9F |
:1077D000E20FF31FE554F74F82A593A51C969C93E7 |
:1077E0008E931B971D96BC921D971E961C920E940D |
:1077F0000AAD8DB79EB70F9642C0ADB7BEB71D9705 |
:10780000BEBFADBF1196EDB7FEB74182F901E55499 |
:10781000F74F84818F5F8A0D11968C931197129682 |
:107820001C92129713961C92139714961C921497FD |
:1078300042EC56E016965C934E93159717961C9360 |
:10784000179718961C92189719960C9319971A96D1 |
:107850001C921A97E72DF0E0EE0FFF1FEE0DFF1DB3 |
:10786000E20FF31FE554F74F82A593A51C969C9356 |
:107870008E931B970E940AAD8DB79EB70D969EBF43 |
:107880008DBF80918E01992787FD90958C9DF00189 |
:107890008D9DF00D9C9DF00D1124E554F74F448112 |
:1078A000A41648F0262D30E0818190E0840F911DD0 |
:1078B000281739073CF086E0652D4AE727E000E00D |
:1078C0000E941BA7A39498E0590EA6E0AA1609F0FF |
:1078D0006ECD80E069E041E02DE200E00E941BA750 |
:1078E00080918E01E82EFF24E7FCF0948DED91E06D |
:1078F000E89EF001E99EF00DF89EF00D1124E5548C |
:10790000F74F6181673078F1C62FD0E08EE091E0CB |
:10791000BE010E94C7D1062F6E3208F00DE22DE2A3 |
:1079200030E0201B31098DED91E0E89EF001E99EE9 |
:10793000F00DF89EF00D1124E554F74F838190E08F |
:107940002697FC012E9FC0012F9F900D3E9F900D0A |
:107950001124BE010E94C7D1675F80E041E0202F63 |
:1079600001E00E941BA7CDB7DEB7E2E10C9433D251 |
:10797000A0E0B0E0EEEBFCE30C9426D21F81E09196 |
:107980008E01E7FF02C081E014C06E2F772767FDEC |
:1079900070958DED91E0689FA001699F500D789FD3 |
:1079A000500D1124FA01E554F74F2181223218F0CD |
:1079B00083E076D83DC030E08DE090E0F901E89FAB |
:1079C0009001E99F300DF89F300D1124240F351FD1 |
:1079D000F901E554F74F888583A3898584A316A20E |
:1079E0008A859B8597A786A78E8180A711A7FE0110 |
:1079F0003C96D901AB51B74F212F30E0220F331FF6 |
:107A00002E0F3F1F04C0819191918D939D93E2179A |
:107A1000F307C9F78DED91E0689FF001699FF00DC4 |
:107A2000789FF00D1124E554F74F81818F5F81839A |
:107A30002096E3E00C9442D200D000D000D0EDB705 |
:107A4000FEB7319691E0ADB7BEB711969C93118207 |
:107A5000828383E08383158214828ADF8DB79EB789 |
:107A600006969EBF8DBF089500D000D000D0EDB720 |
:107A7000FEB7319681E0ADB7BEB711968C931182F7 |
:107A8000128283E083831582148272DF8DB79EB7E2 |
:107A900006969EBF8DBF08952DB73EB72A50304041 |
:107AA0003EBF2DBFEDB7FEB7319691E0ADB7BEB783 |
:107AB00011969C9392E09183828384E08383158264 |
:107AC000148280910618909107189783868380917D |
:107AD000081890910918918780874ADF2DB73EB723 |
:107AE000265F3F4F3EBF2DBF0895ADB7BEB7189775 |
:107AF000BEBFADBFEDB7FEB7319611961C9291E0B7 |
:107B0000918382836383558344833783268330DF65 |
:107B1000EDB7FEB73896FEBFEDBF0895ADB7BEB75F |
:107B20001897BEBFADBFEDB7FEB7319691E0119685 |
:107B30009C93918382836383558344833783268315 |
:107B400017DFEDB7FEB73896FEBFEDBF08950F9370 |
:107B50001F93ADB7BEB71A97BEBFADBFEDB7FEB7A7 |
:107B6000319691E011969C9392E091838283638396 |
:107B7000558344833783268311870087F9DEEDB769 |
:107B8000FEB73A96FEBFEDBF1F910F910895809109 |
:107B90008E0186301CF082E00C9450358F5F80930C |
:107BA0008E01992787FD90952DED31E0AC01429F24 |
:107BB000C001439F900D529F900D11248554974F03 |
:107BC000DC01A9011D9241505040E1F780918E01E6 |
:107BD000992787FD9095829FF001839FF00D929FDA |
:107BE000F00D1124E554F74F168281E087838087DA |
:107BF0000895ADE0B0E0EFEFFDE30C9425D2082F3F |
:107C000010E020918E01332727FD30958DE090E024 |
:107C1000089FF001099FF00D189FF00D11248DEDC4 |
:107C200091E0AC01249FC001259F900D349F900DE1 |
:107C30001124E80FF91FDE011196E252F74F8DE093 |
:107C400001900D928150E1F720918E01332727FD9D |
:107C5000309570E04DE050E0049FD001059FB00DDD |
:107C6000149FB00D11248DED91E0FC012E9FC001F9 |
:107C70002F9F900D3E9F900D1124A80FB91F649F58 |
:107C8000F001659FF00D749FF00D1124E80FF91FAE |
:107C9000A252B74FE252F74F8DE001900D92815002 |
:107CA000E1F78DE090E0689FF001699FF00D789F0B |
:107CB000F00D112480918E01992787FD90952DED6F |
:107CC00031E0AC01429FC001439F900D529F900D47 |
:107CD0001124E80FF91FE252F74FDE0111968DE0F3 |
:107CE0000D9001928150E1F72D96E4E00C9441D281 |
:107CF000A0E0B0E0EEE7FEE30C941BD2C82E80912A |
:107D00008E0187FF02C081E012C0992787FD909500 |
:107D10002DED31E0829FF001839FF00D929FF00DD9 |
:107D20001124E554F74F8181882329F484E00E94CF |
:107D30005035DD2428C3CC2019F085818823A1F497 |
:107D400082E0CED980918E01992787FD90952DED07 |
:107D500031E0829FF001839FF00D929FF00D11247E |
:107D6000E554F74F81E085830E94C74CFDEDEF2E6F |
:107D7000F1E0FF2EEDE0AE2EB12C74E0972E68E618 |
:107D8000862E59E6752E45E0642E80918E01992746 |
:107D900087FD90958E9DF0018F9DF00D9E9DF00DBD |
:107DA0001124E554F74F16860E94EF65882311F0E1 |
:107DB00002E001C000E080918E01992787FD909537 |
:107DC0008E9DF0018F9DF00D9E9DF00D1124E554C8 |
:107DD000F74F8781882311F00E94ADA480918E0116 |
:107DE000992787FD90958E9DF0018F9DF00D9E9DAA |
:107DF000F00D1124E554F74F8185882311F410E02C |
:107E000026C080E80E949F4C8823D9F080918E0183 |
:107E1000992787FD90958E9DF0018F9DF00D9E9D79 |
:107E2000F00D1124E554F74F6081662341F0818104 |
:107E3000882329F0862F6150DCDE11E003C00E9408 |
:107E4000353510E080E80E948E4C811111E0809160 |
:107E50008E01992787FD90958E9DF0018F9DF00DE5 |
:107E60009E9DF00D1124E554F74F8185882359F428 |
:107E700080E80E94844C882341F480E862E00E94FC |
:107E8000BB4C882311F4112369F180918E01282FB6 |
:107E9000332727FD30952E9DF0012F9DF00D3E9D3F |
:107EA000F00D1124E554F74F9081992331F0818131 |
:107EB000882319F09150908312C02E9DF0012F9DC0 |
:107EC000F00D3E9DF00D1124E554F74F8681882377 |
:107ED00021F081818150808302C00E9435350E944B |
:107EE000123501E080918E01992787FD90958E9D36 |
:107EF000F0018F9DF00D9E9DF00D1124E554F74F7C |
:107F00008185882311F410E029C080E40E949F4CF1 |
:107F10008823F1F080918E01992787FD90958E9DA1 |
:107F2000F0018F9DF00D9E9DF00D1124E554F74F4B |
:107F30006081262F30E0818190E001972817390772 |
:107F40002CF4862F6F5F55DE11E003C00E9435359B |
:107F500010E080E40E948E4C811111E080918E012E |
:107F6000992787FD90958E9DF0018F9DF00D9E9D28 |
:107F7000F00D1124E554F74F8185882359F480E4EE |
:107F80000E94844C882341F480E462E00E94BB4C50 |
:107F9000882311F4112321F180918E01992787FD07 |
:107FA00090958E9DF0018F9DF00D9E9DF00D1124FA |
:107FB000E554F74F4081242F30E0818190E0019714 |
:107FC000281739071CF44F5F408307C08681882338 |
:107FD00011F0108202C00E9435350E94123501E076 |
:107FE00080918E01992787FD90958E9DF0018F9D40 |
:107FF000F00D9E9DF00D1124E554F74F108180E1A6 |
:108000000E948E4C882311F4DD2454C080918E018F |
:10801000992787FD9095212F30E02A9DD0012B9D37 |
:10802000B00D3A9DB00D11248E9D90018F9D300DA5 |
:108030009E9D300D1124A20FB31FA554B74F9496E7 |
:108040008C919497833011F4DD2433C095968C91F4 |
:1080500095978823B9F088EC90E06FEF70E040E0EE |
:108060000E94D24C8AE690E00E947CCEBC0188EC53 |
:1080700090E040E050E020E030E000E010E00E94BE |
:10808000CE6311C09E96ED91FC919F97309711F0B1 |
:10809000099509C0F901E554F74F82899389009742 |
:1080A00029F0FC010995DD2402E004C09686DD2458 |
:1080B000D39401E080918E01992787FD90958E9D44 |
:1080C000F0018F9DF00D9E9DF00D1124E554F74FAA |
:1080D0008285882309F4C8C080E10E949F4C8823D0 |
:1080E00009F4C2C080918E01992787FD90958E9DDD |
:1080F000F0018F9DF00D9E9DF00D1124E554F74F7A |
:108100008181882309F4B0C08285813061F588ECD3 |
:1081100090E06FEF7FEF40E00E94D24C89E690E064 |
:108120000E947CCEBC0180E090E041EC56E020E073 |
:1081300030E000E010E00E94CE638FE190E00E940A |
:108140007CCEAC018CE067E022E00E947FAA80E256 |
:108150000E948E4C182F80E10E948E4C882309F0DB |
:1081600082C01123A1F380918E01992787FD9095FC |
:108170008E9DF0018F9DF00D9E9DF00D1124E55414 |
:10818000F74FE08106C01E2F1F5F8E2F612F31DD5C |
:10819000E12F80918E01682F772767FD70956E9D86 |
:1081A000D0016F9DB00D7E9DB00D1124A554B74F29 |
:1081B000ED01219611964C9111972E2F30E0842FCE |
:1081C00090E0019728173907F4F2242F21502883D3 |
:1081D00013964C911397442371F0842F90E00596E9 |
:1081E00030E0821793073CF0415013964C9313975D |
:1081F0008C9181508C936E9DF0016F9DF00D7E9D52 |
:10820000F00D1124E554F74F91818081891710F00A |
:10821000915090830E94123580918E01992787FD9D |
:1082200090958E9DF0018F9DF00D9E9DF00D112477 |
:10823000E554F74F82858230B1F488EC90E06FEF1F |
:108240007FEF40E00E94D24C88E690E00E947CCE16 |
:10825000BC0188EC90E040E050E020E030E000E03D |
:1082600010E00E94CE6302E080918E01992787FD85 |
:1082700090958E9DF0018F9DF00D9E9DF00D112427 |
:10828000E554F74F8285882311F584898823F9F016 |
:1082900080E10E949F4C8823D1F080918E01992724 |
:1082A00087FD90958E9DF0018F9DF00D9E9DF00DA8 |
:1082B0001124E554F74F8789908D009721F0FC0138 |
:1082C000099502E004C0868632E0D32E01E0809159 |
:1082D0008E01992787FD90958E9DF0018F9DF00D61 |
:1082E0009E9DF00D1124E554F74F868D8823F9F0FB |
:1082F00080E20E949F4C8823D1F080918E019927C3 |
:1083000087FD90958E9DF0018F9DF00D9E9DF00D47 |
:108310001124E554F74F81A192A1009721F0FC01AF |
:10832000099502E004C0768622E0D22E01E080E2C8 |
:108330000E948E4C8823D9F080918E01992787FD69 |
:1083400090958E9DF0018F9DF00D9E9DF00D112456 |
:10835000E554F74F848D958D009721F0FC01099528 |
:1083600002E007C0668692E0D92E01E002C0002339 |
:1083700029F0802F0E9470380E94C74CDD2019F42C |
:10838000CC2009F402CD8D2DCDB7DEB7EEE00C94F4 |
:1083900037D22091EC138091EB132817C0F430E012 |
:1083A00086E190E0AC01249FC001259F900D349F91 |
:1083B000900D1124E091E913F091EA13E80FF91FF1 |
:1083C0008AE080838091EC138F5F8093EC13089593 |
:1083D000EF92FF920F931F93CF93DF93C0E0D0E013 |
:1083E00010E086E1E82EF12C4091E9135091EA1358 |
:1083F0008091ED139E01280F311D8091EC1390E0C8 |
:10840000281739071CF52E9DF0012F9DF00D3E9D7C |
:10841000F00D1124E40FF51F20812A3081F486E04D |
:10842000612F48E727E000E00E941BA7212F2D5F66 |
:1084300086E0622F4DE701E00E9403A60EC0319650 |
:1084400081E06C2FAF010E9458A807C086E0612F21 |
:1084500048E727E000E00E941BA72196185F183329 |
:1084600019F68091EC13E82FF0E081E891E0BF016C |
:108470000E94C7D1162F683308F017E327E330E0D6 |
:10848000211B31098091ED1390E03797AC01249FB7 |
:10849000C001259F900D349F900D1124BF010E94B3 |
:1084A000C7D1F62E80E060E041E027E300E00E94C3 |
:1084B0001BA780E06F2D41E0212F01E00E941BA748 |
:1084C00081E067E040E020E00E943B678DE090E0C3 |
:1084D0000E947CCEAC018CE067E020E00E947FAA85 |
:1084E0008091EE13882329F081E167E04FEE53E19C |
:1084F00008C08091F413882341F081E167E045EFE3 |
:1085000053E120E00E9477AF07C081E167E04BE6CE |
:1085100057E020E00E947FAACDB7DEB7E6E00C94DA |
:108520003FD21F930E94C74C8091E9139091EA13A8 |
:10853000892B11F410E062C04BDF0E94EF658823A5 |
:1085400019F00E94BBAF44DF80E40E94844C882372 |
:1085500031F480E462E00E94BB4C8823B9F0409182 |
:10856000ED13242F30E08091EC1390E0079728174B |
:1085700039072CF44F5F4093ED132ADF07C089E1E0 |
:1085800090E06FEF7FEF40E00E94D24C80E80E94C5 |
:10859000844C882331F480E862E00E94BB4C88233D |
:1085A00081F08091ED13882329F081508093ED13A1 |
:1085B0000FDF07C089E190E06FEF7FEF40E00E949E |
:1085C000D24C8091EE13882339F080E10E948E4CCA |
:1085D000882311F014E001C010E08091F413882387 |
:1085E00029F080E10E949F4C811118E680E20E94F0 |
:1085F000844C882319F4112309F49FCF812F1F91F4 |
:108600000895843041F481E08093EE138FEE93E17E |
:1086100045E050E0ACC0883641F481E08093F4132B |
:1086200085EF93E145E050E0A2C00895DF92EF921C |
:10863000FF920F931F93782FD62EFA018901909104 |
:10864000EC138091EB139817F0F486E1989FC0012A |
:108650001124E090E913F090EA13E80EF91E872F39 |
:108660006AEF73E145E19F01BDD1C70101966AEF51 |
:1086700073E145E150E07BD0F701D0828091EC13AB |
:108680008F5F8093EC131F910F91FF90EF90DF901D |
:108690000895A0E0B0E0EFE4F3E40C9427D2FE01EB |
:1086A000389680E06D814E815F819F01BFDF20960B |
:1086B000E2E00C9443D2A0E0B0E0E1E6F3E40C94F5 |
:1086C00027D2FE01389681E06D814E815F819F0146 |
:1086D000ADDF2096E2E00C9443D28091E9139091B3 |
:1086E000EA13009711F00E9401D31092EA1310923E |
:1086F000E91308951F93182F1092EA131092E913AB |
:1087000086E1189FC00111240E9454D29093EA136D |
:108710008093E9130097F1F40E94BBAF81E063E01E |
:108720004CE757E020E00E947FAA81E064E040E748 |
:1087300057E020E00E947FAA84EF91E063E373E3B7 |
:1087400040E00E94D24C80E20E948E4C8823D9F3F4 |
:1087500080E00BC01093EB131092EC131092ED130A |
:108760001092EE131092F41381E01F9108951F935D |
:10877000182FDB01BA01282F392FF90120E030E250 |
:108780000CC08C91882311F421E002C0222311F047 |
:10879000308301C08083119631968E2F811B90E02B |
:1087A0008617970770F3319710821F910895ACE0F8 |
:1087B000B0E0EDEDF3E40C9423D27C01DE011196E0 |
:1087C000EAEFF2E08CE001900D928150E1F7109118 |
:1087D0006E1E00916F1ED701FB0188E001900D9283 |
:1087E0008150E1F7F70186819781892B09F4AAC0AE |
:1087F0008581882309F4A6C08D3008F0A3C040818C |
:10880000518162817381100F212F332727FD30950D |
:1088100080E19EE0FC012E9FC0012F9F900D3E9FA6 |
:10882000900D11248C01222717FD2095322F040F63 |
:10883000151F261F371F0138F1E51F07F1E02F072D |
:10884000F0E03F0734F0005811452140304091E0FE |
:1088500009C037FD02C090E005C000581E4A2E4FE7 |
:108860003F4F9FEFF701008311832283338384817D |
:10887000292F332727FD3095432F532F280F311DE4 |
:10888000411D511D1581612F70E0FE01E60FF71F9C |
:108890008081992787FD9095A92FB92F821793077B |
:1088A000A407B50734F421E030E040E050E091E067 |
:1088B0001AC0121613061406150614F490E013C01D |
:1088C000123028F42FE130E040E050E00BC0FE0110 |
:1088D0003197E60FF71F8081282F332727FD30952A |
:1088E000432F532F9FEFF70124838581492F55276D |
:1088F00047FD5095652F752F480F511D611D711D46 |
:108900004D3051056105710534F041E050E060E003 |
:1089100070E021E00CC0141615061606170634F098 |
:108920004CE050E060E070E02FEF01C020E0F70184 |
:108930004583332727FD309586819781820F931FCA |
:10894000978386832C96E6E00C943FD2A0E0B0E0BB |
:10895000ECEAF4E40C9422D2F82E192FEA01D22E7C |
:10896000EE2039F00E94CED48F2D912F9C01F90179 |
:108970000BC00E9434D5F8CF8081882369F08D1513 |
:1089800011F4015041F031968E2F8F1990E08C1721 |
:108990009D0790F331971082CDB7DEB7E7E00C94D6 |
:1089A0003ED2EF920F93EE24E394D0DF0F91EF903D |
:1089B00008959C01DC010D900020E9F7A81BB90B7C |
:1089C0001297A80FB91F01C01197A217B30719F08A |
:1089D0008C918032C9F38C91803211F08111119603 |
:1089E0001C920895A1E1B0E0E8EFF4E40C9417D2F2 |
:1089F000D62EC72E4B877801882359F087E394E160 |
:108A0000B90148E250E00E94CED410925E1427E3F0 |
:108A100034E119825B85552309F457C23A872987C7 |
:108A20008D2D9C2D9C016901882499246624852F15 |
:108A300090E09F878E87FE013196F98BE88B9981B4 |
:108A4000992399F4E985FA858081853259F4E8897A |
:108A5000F98958E011925A95E9F781E0898390E20B |
:108A6000988727C0F601819320C2913009F053C046 |
:108A7000E985FA858081803339F4FFE58F16F4E1CA |
:108A80009F0631F4888716C2803330F08A33B0F401 |
:108A9000F40181934F010EC28E3281F48D818823BF |
:108AA000D1F59D83F40110828FE594E10E9485D475 |
:108AB0008B832FE5822E24E1922EFCC18C3619F493 |
:108AC000F1E0FE83F7C1843681F0853771F0883795 |
:108AD00061F0823651F08F3641F0833731F08335C3 |
:108AE00021F0833611F08532B1F48A831982F401C2 |
:108AF00010828D81882341F48FE594E10E9485D412 |
:108B00008B83882331F407C08FE594E10E9485D4DC |
:108B10008C83F1E0FF839A81943649F0953739F0E0 |
:108B2000983729F0923619F09F3609F067C18E8187 |
:108B3000882309F456C09F3609F444C0903730F4B6 |
:108B4000923699F1943609F096C006C0953799F09F |
:108B5000983709F090C01CC087010C5F1F4FF701C8 |
:108B6000608171818281938146E654E12AE030E0A0 |
:108B70000E9462D57FC087010C5F1F4FF7016081A3 |
:108B800071818281938146E654E12AE030E026C07B |
:108B900087010C5F1F4FF701608171818281938192 |
:108BA00046E654E120E130E019C087010C5F1F4F19 |
:108BB000F701608171818281938146E654E122E070 |
:108BC00030E00CC087010C5F1F4FF701608171819D |
:108BD0008281938146E654E128E030E00E949CD5F2 |
:108BE00049C09F36D9F1903730F4923661F194360E |
:108BF00009F041C005C0953781F09837E1F518C0FC |
:108C000087010E5F1F4FF7018081918166E674E155 |
:108C10004AE050E00E9443D52DC087010E5F1F4FF0 |
:108C2000F7018081918166E674E14AE050E020C05E |
:108C300087010E5F1F4FF7018081918166E674E125 |
:108C400040E150E015C087010E5F1F4FF7018081A2 |
:108C5000918166E674E142E050E00AC087010E5F50 |
:108C60001F4FF7018081918166E674E148E050E092 |
:108C70000E94BFD578017724809166148D3211F45B |
:108C8000772473943B817C81272F230F90E08D8183 |
:108C9000811191E0A22EA90EE6E6F4E101900020F8 |
:108CA000E9F73197E656F441BE2E6F81662381F1D4 |
:108CB000432F50E02E2F30E0CA01870F911D8217FD |
:108CC00093075CF020E030E0872D90E08417950753 |
:108CD00014F021E030E02223C9F090E010C08D8133 |
:108CE000882329F08B81981711F45EE201C05AE2C3 |
:108CF000F60150830894C11CD11C63949F5F9A15A0 |
:108D000008F0D5C08B85681650F3E3C0662309F4DC |
:108D1000AB2C772029F496E6492E94E1592E05C014 |
:108D2000BA9487E6482E84E1582E11E001E06A2DBE |
:108D300070E0FB01E60DF11DFD87EC872B2C332441 |
:108D400056C0212F2150412F50E08C859D85841BDA |
:108D5000950BEE85FF85E817F9070CF447C08D8168 |
:108D6000882359F08C81281741F4FB01E41BF50B93 |
:108D7000EC0DFD1D5EE2508339C0B01670F0FB01B2 |
:108D8000E41BF50BEC0DFD1DD101A01BB109A40DD9 |
:108D9000B51D8C9180830F5F29C08C81882379F069 |
:108DA000281730F030E090E001962817390739F4A1 |
:108DB000FB01E41BF50BEC0DFD1D80E316C07720D5 |
:108DC00071F08885803211F01A1549F4FB01E41B1B |
:108DD000F50BEC0DFD1D9DE29083772407C0FB0190 |
:108DE000E41BF50BEC0DFD1D888580831F5FA1162C |
:108DF00008F0A7CFC60ED71E6A0C59C0933731F0C2 |
:108E0000933561F0933609F04CC018C0F70120810A |
:108E1000318182E090E0E80EF91E1CC087010E5FF0 |
:108E20001F4FF7016081718186E694E148E250E0CE |
:108E30000E94CED410928D1478010AC0F70180816F |
:108E4000809366141092671482E090E0E80EF91E99 |
:108E500026E634E14B81D9010D900020E9F7119706 |
:108E6000A21BB30B8F81882309F44A2F90E015C011 |
:108E7000892F860DEB858E1778F4F9018081882380 |
:108E800029F0F60180832F5F3F4F03C050E2F601C7 |
:108E900050830894C11CD11C9F5F941748F3640E43 |
:108EA00006C0953239F4F60191936F016394FB8506 |
:108EB0006F1678F4E985FA858191FA87E987882326 |
:108EC00009F0BDCDE0E0F0E006C08D2D9C2D9C01A9 |
:108ED0006901EFEFFFEFEC0DFD1D10826196E2E1FD |
:108EE0000C9433D2A0E0B0E0E8E7F7E40C9423D28E |
:108EF000FE013996219131914FE0E42E44E1F42EA8 |
:108F000081E0B70148E28F016DDDC7012096E6E000 |
:108F10000C943FD2A0E0B0E0E0E9F7E40C9425D255 |
:108F2000FE013C9681E06F81788549852A853B85E5 |
:108F30008F0158DD2096E4E00C9441D2A0E0B0E02F |
:108F4000E4EAF7E40C9425D2FE013C9680E06F81C0 |
:108F5000788549852A853B858F0144DD2096E4E0AC |
:108F60000C9441D21F920F920FB60F920BB60F9234 |
:108F700011248F939F93EF93FF939091E00580913D |
:108F8000E105981769F0E091E105EF5FEF73E09379 |
:108F9000E105F0E0E056FB4F80818093CE0005C0F4 |
:108FA0008091C9008F7D8093C900FF91EF919F91BF |
:108FB0008F910F900BBE0F900FBE0F901F901895C2 |
:108FC0009C011092E0051092E1051092E2051092CA |
:108FD000E30510928F1410928E1410929F0497FF45 |
:108FE00004C082E08093C8003F773093CD00209387 |
:108FF000CC0088E98093C90086E08093CA00089578 |
:109000008430F1F0853038F48230A1F08330A8F458 |
:109010008130E1F40CC0863039F08630A0F0873022 |
:10902000A9F487E092E00EC083E091E00BC081E8F4 |
:1090300090E008C080E490E005C08FE190E002C0BD |
:1090400084E190E0BDCF8AE090E0BACF089590919E |
:10905000E2058091E305981719F420E031E011C092 |
:10906000E091E305EF5FE093E305F0E0E052FB4FB2 |
:1090700030812091E405922F80E0AC01430F511D17 |
:109080009A01C9010895482F9091E0059F5F9F7351 |
:109090008091E1059817E1F3E92FF0E0E056FB4FEE |
:1090A00040839093E0058091C90080628093C9005D |
:1090B0008091E405382F20E0240F311DC901089567 |
:1090C000CF93DF93EC0102C02196DDDF88818823F6 |
:1090D000D9F7DF91CF910895CF93DF93EC0101C0D1 |
:1090E000D2DFFE01219684918823D1F7DF91CF91C1 |
:1090F00008958091E2052091E30590E081509F4F13 |
:10910000821B91096FEF70E00C94C7D18091E30549 |
:109110008093E20508951F920F920FB60F920BB63F |
:109120000F9211241F932F933F934F935F936F934D |
:109130007F938F939F93AF93BF93EF93FF93109180 |
:10914000C8009091CE001871E091E205EF5F809128 |
:10915000E305E81759F480918E1490918F140196CD |
:1091600090938F1480938E1412E00EC080919F0410 |
:10917000882321F0892F0E9450C806C0E093E205A1 |
:10918000F0E0E052FB4F90831093E405FF91EF91E4 |
:10919000BF91AF919F918F917F916F915F914F910F |
:1091A0003F912F911F910F900BBE0F900FBE0F900C |
:1091B0001F90189520913106309132068091330628 |
:1091C000909134062817390711F4E0E01BC0E091B4 |
:1091D0003306F0913406E081809133069091340695 |
:1091E00001969093340680933306809133069091D4 |
:1091F00034068153964031F481EF95E09093340624 |
:10920000809333068E2F0895DC0120913106309132 |
:10921000320680913306909134062817390711F4ED |
:1092200080E00895E0913306F091340680818C93BC |
:109230008091330690913406019690933406809382 |
:10924000330680913306909134068153964011F095 |
:1092500081E0089581EF95E0909334068093330682 |
:1092600081E008959093C5008093C4008091C00070 |
:109270008D7F8093C00086E08093C2008091C10002 |
:109280008B7F8093C10002C08091C6008091C00096 |
:1092900087FDFACF8091C10088618093C1008091E1 |
:1092A000C10080688093C10081EF95E090933406FF |
:1092B0008093330680913306909134069093320662 |
:1092C0008093310608958430F1F0853038F482308F |
:1092D000A1F08330A8F48130E1F40CC0863039F07D |
:1092E0008630A0F08730A9F487E092E00EC083E0DA |
:1092F00091E00BC081E890E008C080E490E005C0F8 |
:109300008FE190E002C084E190E0ACCF8AE090E091 |
:10931000A9CF0895982F8091C00085FFFCCF90932E |
:10932000C60008955091E605565033E043E048C02A |
:10933000E32FF0E0E057FB4E80813F5FE32FF0E04A |
:10934000E057FB4E20812D533F5FE32FF0E0E057C5 |
:10935000FB4E70813F5FE32FF0E03350E057FB4E50 |
:109360006081E42FF0E0922F92959F708D53880FCB |
:10937000880F892BE057FB4E8083E42FEF5F51303D |
:1093800081F0972F9D53F0E02295207F892F8695BD |
:109390008695282BE057FB4E2083E42FEE5F52305A |
:1093A00011F44E2F10C03C5F5350F0E09295990F8E |
:1093B000990F907C6D53962BE057FB4E90834D5F39 |
:1093C000552309F0B5CF83E994E19093E8058093A4 |
:1093D000E70543504093E905089580919508813051 |
:1093E00071F58091C00085FFFCCF8BE18093C600B2 |
:1093F0008091C00085FFFCCF8BE18093C6008091F7 |
:10940000C00085FFFCCF85E58093C6008091C00039 |
:1094100085FFFCCF8AEA8093C6008091C00085FF5B |
:10942000FCCF1092C60081E08093960884EF91E013 |
:1094300024EF31E0F9013197F1F70197D9F7089559 |
:1094400085E08093960884EF91E024EF31E0F90104 |
:109450003197F1F70197D9F70895A0E0B0E0E3E381 |
:10946000FAE40C9422D28B857C8593E29093951537 |
:109470008F59809396158A8580939715DE011D96E6 |
:10948000772329F4EE24FF2440E050E007C0ED8468 |
:10949000FE84DE0151964F855889715003E010E03B |
:1094A00030E07EC0F701E30FF11D90813F5F415036 |
:1094B000504091F4772309F4CAC0FD013296ED9033 |
:1094C000FC90DF0112960190F081E02D71503097F1 |
:1094D00009F4BCC0AF0130E0F701E30FF11DD0800B |
:1094E0003F5F4150504099F4772311F460E023C06E |
:1094F000FD013296ED90FC90DF0112964081518182 |
:1095000071504115510511F460E014C030E0F701CD |
:10951000E30FF11D60813F5F4150504061F47723BC |
:1095200051F0FD013296ED90FC90DF0112964081E2 |
:109530005181715030E0892F86958695835CF801C2 |
:10954000EB56FA4E80832D2D22952F70892F90E0B7 |
:1095500083709070F4E0880F991FFA95E1F7282B3B |
:10956000235CF801EA56FA4E20838D2D90E08F702F |
:109570009070880F991F880F991F262F2295269586 |
:1095800026952370282B235CF801E956FA4E208398 |
:109590006F73635CF801E856FA4E60830C5F1F4FEF |
:1095A0004115510509F07ECF90E020E030E006C083 |
:1095B000EB56FA4E8081280F311D9F5FE92FF0E0B6 |
:1095C000E017F107A8F33F70C90136E0969587953B |
:1095D0003A95E1F7835CF801EB56FA4E8083C801B7 |
:1095E00001962F73235CFC01EB56FA4E2083019603 |
:1095F000FC01EB56FA4E2DE020830196FC01EB5660 |
:10960000FA4E20830196FC01EB56FA4E2083AC0102 |
:109610004F5F5F4F10928F0120E00CC08091C0001F |
:1096200085FFFCCFE22FF0E0EB56FA4E808180936D |
:10963000C6002F5F822F90E08417950778F381E0B2 |
:1096400080938F012096E7E00C943ED230E060E0FA |
:10965000DD2471CFA1E0B0E0E0E3FBE40C9426D27E |
:1096600090919608913061F519822DB73EB7275039 |
:1096700030403EBF2DBFEDB7FEB7319685E7ADB7A1 |
:10968000BEB711968C9312E011839283CE0101969E |
:109690009483838381E090E096838583DEDE10935C |
:1096A000960824EF31E08DB79EB707969EBF8DBF19 |
:1096B00044EF51E0CA010197F1F721503040D1F752 |
:1096C0002196E3E00C9442D21F920F920FB60F92B4 |
:1096D0000BB60F9211241F932F933F934F935F93D9 |
:1096E0006F937F938F939F93AF93BF93CF93DF93AA |
:1096F000EF93FF9380919608853011F0843089F5BF |
:109700004091C60020913106309132068091330697 |
:1097100090913406281B390B37FF02C0205C3F4F65 |
:109720002F3331050CF0B4C0E0913106F0913206D0 |
:1097300040838091310690913206019690933206D3 |
:109740008093310680913106909132068153964084 |
:1097500009F09EC081EF95E0909332068093310628 |
:1097600097C01091C60080916306882331F08091E4 |
:109770001F1E882311F0812F86DC8091E505882348 |
:1097800009F086C06091EE05662371F4133279F416 |
:109790001093901481E08093EE0583E290E0909323 |
:1097A000F0058093EF0574C0663E08F064C0A62FF4 |
:1097B000B0E02091EF053091F0051D3069F0A05721 |
:1097C000BB4E1C936F5F6093EE05210F311D3093EC |
:1097D000F0052093EF055CC0FD01E257FB4E908140 |
:1097E000ED01C157DB4E8881A901491B5109481B76 |
:1097F00051095F705093F0054093EF052081CA0135 |
:1098000036E0969587953A95E1F7835C821729F5BE |
:109810009881842F8F73835C8917F9F4A057BB4E0E |
:109820001C936F5F6093E605909194088091921469 |
:109830009817F1F481E08093E505809192148235C8 |
:10984000B9F42CE088E190E00FB6F894A8958093E5 |
:1098500060000FBE209360000BC08091EA059091DC |
:10986000EB0501969093EB058093EA051092E505D0 |
:109870001092EE050DC08091EC059091ED050196DA |
:109880009093ED058093EC051092EE051092E5059E |
:10989000FF91EF91DF91CF91BF91AF919F918F9108 |
:1098A0007F916F915F914F913F912F911F910F90F9 |
:1098B0000BBE0F900FBE0F901F90189510927F1641 |
:1098C00010927E1682E084BD85E085BD83EC87BD65 |
:1098D000EEE6F0E080818260808308951F920F920F |
:1098E0000FB60F9211245F9A0F900FBE0F901F902A |
:1098F00018951F920F920FB60F9211245F980F9038 |
:109900000FBE0F901F9018952FB7F89490914206B4 |
:1099100089239827909342062FBF08954FB7F89454 |
:109920003FB7F89420914206909141069095892383 |
:1099300082232827209342063FBF4FBF08952FB7A9 |
:10994000F8949091430689239827909343062FBF5C |
:109950002FB7F89490914406892398279093440652 |
:109960002FBF2FB7F89490914206892398279093A0 |
:1099700042062FBF08952FB7F89490914606892389 |
:10998000982790934606609347062FBF089510923C |
:109990004406109246061092450610924306109215 |
:1099A000420608959C0180912F1E813071F5442359 |
:1099B00039F480914E0690914F06892B31F50EC0F7 |
:1099C0004130A9F480914E0690914F06892BE9F41D |
:1099D00081E090E090934F0680934E0630934D06C1 |
:1099E00020934C0670939501609394010895423042 |
:1099F00061F410924F0610924E0610924D0610928E |
:109A00004C06109295011092940108950F931F93A4 |
:109A100080914A0690914B060197D1F488EE93E02D |
:109A200060E270E040E0BEDF1091931D0091941D54 |
:109A300080E062E040E00E943FAA80E060E04DE804 |
:109A400057E022E00E947FAA1093931D0093941D7B |
:109A500040914A0650914B0620919001309191011E |
:109A600084E690E0BC01269FC001279F900D369FA1 |
:109A7000900D11244817590721F410924B061092AB |
:109A80004A0680914A0690914B06892B49F08091B5 |
:109A90004A0690914B06019690934B0680934A0696 |
:109AA000349B04C0109249061092480634991BC09A |
:109AB00080914806909149060196909349068093BB |
:109AC000480680914806909149060A9761F4809172 |
:109AD0004A0690914B06892B31F481E090E09093F7 |
:109AE0004B0680934A0640914806509149062091C2 |
:109AF00092013091930184E690E0BC01269FC00161 |
:109B0000279F900D369F900D11244817590749F44F |
:109B100082E390E06FEF7FEF40E044DF0E9445B9C1 |
:109B200029981F910F9108951F920F920FB60F92CF |
:109B30000BB60F9211241F932F933F934F935F9374 |
:109B40006F937F938F939F93AF93BF93EF93FF9305 |
:109B50003091410620B12095232780915806822319 |
:109B6000809580935806909157069223982790935A |
:109B7000570628232923922F932790934106922357 |
:109B800080914206892B8093420680915206815033 |
:109B9000809352068F5F89F489E08093520680910A |
:109BA000500690915106019690935106809350066D |
:109BB000809196018F5F809396012223C9F0809156 |
:109BC0007D16882311F40E944AC981E080937D1696 |
:109BD000109235061092360610923706109238060B |
:109BE0001092390610923A0610923B0610923C06EB |
:109BF0001091B41D112359F180917D16813039F5F2 |
:109C0000609135067091360680913706909138063E |
:109C10006F5F7F4F8F4F9F4F60933506709336066F |
:109C2000809337069093380620E737E140E050E014 |
:109C30000E94DAD1812F90E0A0E0B0E02817390728 |
:109C40004A075B0721F40E9455C910927D161091B6 |
:109C5000BC1D112361F16091390670913A06809123 |
:109C60003B0690913C066F5F7F4F8F4F9F4F6093F5 |
:109C7000390670933A0680933B0690933C0620E7A2 |
:109C800037E140E050E00E94DAD1812F90E0A0E07F |
:109C9000B0E0281739074A075B0749F40E9445B925 |
:109CA00082E390E06FEF7FEF40E07CDE2998AEDE4C |
:109CB00080912F1E813069F580914C0690914D0660 |
:109CC0000097D9F08B30910530F00A9790934D06AC |
:109CD00080934C0604C010924D0610924C06809161 |
:109CE00094019091950120914C0630914D0682236C |
:109CF0009323892B09F07FC10AC08FEF9FEF9093C8 |
:109D000095018093940110924F0610924E060E9486 |
:109D10003FC940914106242F30E0207F307021154B |
:109D2000310519F486E480935606809156068150D9 |
:109D300080935606882339F4942F907F80914306B0 |
:109D4000892B809343062115310519F481E0809316 |
:109D5000550680915506815080935506882351F40D |
:109D60008FE080935506942F907F80914406892B35 |
:109D7000809344062115310519F486E48093540636 |
:109D800080915406815080935406882351F48FE0CB |
:109D900080935406942F907F80914506892B809361 |
:109DA0004506232B19F486E48093530680915306CD |
:109DB0008150809353068823A9F48091470681300F |
:109DC00011F484E107C0823011F488E003C08330CD |
:109DD00049F481E080935306407F80914606842BAE |
:109DE0008093460680917E1690917F16892B49F0CC |
:109DF00080917E1690917F16019790937F168093A5 |
:109E00007E168091981690919916892B49F0809131 |
:109E10009816909199160197909399168093981699 |
:109E200080918C1690918D16892B49F080918C161B |
:109E300090918D16019790938D1680938C1680913A |
:109E4000A0169091A116892B49F08091A0169091AF |
:109E5000A11601979093A1168093A01680919C164D |
:109E600090919D16892B49F080919C1690919D169A |
:109E7000019790939D1680939C1680919A169091CD |
:109E80009B16892B49F080919A1690919B16019709 |
:109E900090939B1680939A168091901690919116AC |
:109EA000892B49F08091901690919116019790938B |
:109EB0009116809390168091921690919316892B0B |
:109EC00049F0809192169091931601979093931672 |
:109ED000809392168091941690919516892B49F053 |
:109EE0008091941690919516019790939516809372 |
:109EF00094168091801690918116892B49F080915B |
:109F00008016909181160197909381168093801608 |
:109F100080918E1690918F16892B49F080918E1624 |
:109F200090918F16019790938F1680938E16809143 |
:109F3000821690918316892B49F080918216909118 |
:109F40008316019790938316809382168091BB1697 |
:109F50008823B9F08091961690919716892B49F035 |
:109F600080919616909197160197909397168093EB |
:109F700096168091961690919716892B11F40E944F |
:109F80009E6280913D0690913E06A0913F06B09161 |
:109F900040060196A11DB11D80933D0690933E069B |
:109FA000A0933F06B093400660913D0670913E0637 |
:109FB00080913F069091400624E630E040E050E07A |
:109FC0000E94DAD16115710581059105B9F480917E |
:109FD000841690918516A0918616B09187160196E9 |
:109FE000A11DB11D8093841690938516A0938616AB |
:109FF000B093871603C00E9434C98BCEFF91EF91B6 |
:10A00000BF91AF919F918F917F916F915F914F9190 |
:10A010003F912F911F910F900BBE0F900FBE0F908D |
:10A020001F9018950F931F930E94C73D0E944B37B6 |
:10A0300081E061E040E050E02BEE39E00BEF19E009 |
:10A040000E94A73D8091C31D882309F44BC00E9444 |
:10A05000343D82E061E040E050E02BE03AE003E193 |
:10A060001AE00E94A73D83E061E040E050E02BE170 |
:10A070003AE002E21AE00E94A73D86E061E040E09B |
:10A0800050E029E23AE089010E94A73D0E94343D58 |
:10A0900084E061E040E050E020E839E00FE819E0BA |
:10A0A0000E94A73D87E061E049E25BEC2AE939E0E4 |
:10A0B00009EA19E00E94A73D88E061E04EEC50E516 |
:10A0C00025E33AE005E41AE00E94A73D0E94343DF2 |
:10A0D00085E061E040E050E024E53AE001E61AE086 |
:10A0E0000E94A73D1F910F9108950F931F930E9407 |
:10A0F000C73D0E944B3786E290E00E947CCE9C01D7 |
:10A1000081E061E040E050E00E948E3D8091C21D00 |
:10A11000882361F10E94343D82E061E040E050E03C |
:10A120002DE439E009E519E00E94A73D83E061E0F4 |
:10A1300040E050E027E639E004E719E00E94A73D3F |
:10A140000E94343D87E061E040E050E020E839E0E3 |
:10A150000FE819E00E94A73D88E061E04FE552E377 |
:10A160002AE939E009EA19E00E94A73D1F910F9101 |
:10A1700008950F938091C31D882311F081E00CC0D6 |
:10A180008FE790E00E947CCE61E048EE53E021E052 |
:10A1900001E00E94A86680E00F9108950E94BBAF85 |
:10A1A00082E00E9460C987E30E947A43882371F1AC |
:10A1B00000D00F92EDB7FEB711828CE79AE093833F |
:10A1C00082830E945B43EDB7FEB711828BE79AE072 |
:10A1D000938382830E945B430F900F900F900E94A5 |
:10A1E00054C000D00F92EDB7FEB711828DE69AE011 |
:10A1F000938382830E945B430F900F900F900E9485 |
:10A2000091420E946D4381E00C9460C90895782FBB |
:10A2100060E081E090E0072E02C0880F991F0A9449 |
:10A22000E2F7AA2797FDA095BA2F2091881E3091BA |
:10A23000891E40918A1E50918B1E82239323A42352 |
:10A24000B5230097A105B10509F061E0872F0C94B3 |
:10A25000DE37EF92FF920F931F93682FE090881ED6 |
:10A26000F090891E00918A1E10918B1E81E090E073 |
:10A27000062E02C0880F991F0A94E2F79C0144271A |
:10A2800037FD4095542FDA01C9018E219F21A0236B |
:10A29000B1230097A105B10529F42E293F29402BB0 |
:10A2A000512B04C02E253F25402751272093881E7F |
:10A2B0003093891E40938A1E50938B1E862FA7DFF2 |
:10A2C0001F910F91FF90EF9008950E94C73D0E944B |
:10A2D0004B378DE490E00E947CCE9C0181E061E0F0 |
:10A2E00040E050E00E948E3D81E091DF8EE490E0FE |
:10A2F0000E947CCE9C018AE061E040E050E00E9438 |
:10A300008E3D8AE084DF8FE490E00E947CCE9C0149 |
:10A3100082E061E040E050E00E948E3D82E077DF25 |
:10A3200080E590E00E947CCE9C0183E061E040E00B |
:10A3300050E00E948E3D83E06ADF81E590E00E945C |
:10A340007CCE9C0184E061E040E050E00E948E3DC4 |
:10A3500084E05DDF82E590E00E947CCE9C0185E098 |
:10A3600061E040E050E00E948E3D85E050DF83E5F3 |
:10A3700090E00E947CCE9C0186E061E040E050E0ED |
:10A380000E948E3D86E043DF80E00E94783E0E947E |
:10A390005936853021F00E9431365BDFF5CF0C94C1 |
:10A3A000A637A0E0B0E0E7EDF1E50C941DD22091D6 |
:10A3B000191E30911A1EC90162E370E00E94B3D1E8 |
:10A3C000892B99F0C90162E370E00E94B3D182E366 |
:10A3D00090E09C01629FC001639F900D729F900D61 |
:10A3E000112490931A1E8093191EC091191ED091AA |
:10A3F0001A1E81E0E2E09E2E7CEF872E65E2A62EFB |
:10A400006CE0B62E53E0F52E1EEF40E1C42E4CE07A |
:10A41000D42E882309F449C081E00E9424670E9459 |
:10A42000AD35ADB7BEB71997BEBFADBFEDB7FEB77F |
:10A43000319611961C929182128213828482B68286 |
:10A44000A582908787830E940AAD2DB73EB7275F0C |
:10A450003F4F3EBF2DBF80E060E24FE725E101E0C6 |
:10A4600032E0E32E0E9411A98EE690E00E947CCE9D |
:10A47000AC0181E065E020E000E09BEFE92E0E9466 |
:10A4800071AA8FE690E00E947CCEAC0181E066E08C |
:10A4900020E00E9471AA84E090E00E947CCEAC0192 |
:10A4A00080E067E020E00E947FAA8DB79EB70B97FF |
:10A4B0009EBF8DBFEDB7FEB73196ADB7BEB7119653 |
:10A4C0001C92F182128213821483D682C5828091FB |
:10A4D000191E90911A1E9087878380917F06909114 |
:10A4E0008006928781870E940AAD2DB73EB7255F0F |
:10A4F0003F4F3EBF2DBF0E94ADA40E94EF65082FC5 |
:10A5000080E402DA882329F480E462E034DA8823E4 |
:10A5100069F08091191E90911A1E3AE388399307C9 |
:10A5200028F4C29690931A1E8093191E80E8ECD9E5 |
:10A53000882329F480E862E01EDA882359F08091AC |
:10A54000191E90911A1E009729F0C29790931A1E17 |
:10A550008093191E80E1D8D9882311F018DA0FC032 |
:10A5600080E2D2D9882331F4802F002309F451CF1F |
:10A5700081E04FCF0CDAD0931A1EC093191ECDB7CD |
:10A58000DEB7ECE00C9439D2882331F08CE390E014 |
:10A590006FEF7FEF40E006DA8DE090E00E947CCE26 |
:10A5A000AC018CE067E020E00E947FAA04C00E941A |
:10A5B000ADA40E94EF6580E2A7D98823C1F3E7C963 |
:10A5C000CF92DF92EF920F931F93CF93DF93182FC9 |
:10A5D000EB016A010E94BBAF81E00E942467112356 |
:10A5E00041F082E0BE0140E020E002E00E948BAA40 |
:10A5F00007C082E0BE0140E020E002E00E9480AFA0 |
:10A6000010E0C0E0D0E0F601EC0FFD1F812F8E5F5F |
:10A6100065E0408120E000E0EE24EA940E9451A72A |
:10A62000C0FF0BC0CB3048F41F5F812F8E5F65E009 |
:10A630004EE05CE020E00E9471AA2196CD30D10569 |
:10A6400011F01F5FE0CF80E062E24FE720E101E020 |
:10A65000F2E0EF2E0E9411A980E096DFDF91CF910A |
:10A660001F910F91EF90DF90CF9008950F931F935C |
:10A670000E94C73D0E944B3782E061E046E15EE503 |
:10A680002BE83AE00AE91AE00E94A73D81E061E088 |
:10A6900040E95DE52BEA3AE007EB1AE00E94A73DAE |
:10A6A0000E94343D85E061E040E050E022EC3AE079 |
:10A6B00000ED1AE00E94A73D86E061E040E050E036 |
:10A6C0002FED3AE00EEE1AE00E94A73D80E00E94D6 |
:10A6D000783E0E945936853099F00E943136853097 |
:10A6E00039F480E06BEE7DE14FEF5DE169DFEECFA5 |
:10A6F000863061F781E060E070E00E9414B2E6CF3E |
:10A700000E94A6371F910F910895A0E0B0E0EBE8FA |
:10A71000F3E50C9417D2582FF42E282F392FF90176 |
:10A72000A2EAB6E102C081918D938E2F851B861718 |
:10A73000D0F381E011E0EE24D62ED394DD0CE6E0D8 |
:10A74000CE2E70E2B72E69E3862E5AE5252E41E61D |
:10A75000542E31E4432E20E3A22E94E0392EB4EFA0 |
:10A760006B2EB1E07B2E03C0892D911081E08823F0 |
:10A7700081F181E00E942467C2EAD6E100E00EC0C8 |
:10A78000802F815063E0499120E00E9460A8802FD3 |
:10A7900063E047E55CE020E00E947FAA0E5F0D15B4 |
:10A7A00079F71C9DC0011124835066E14AE02AE03C |
:10A7B00001E00E94D5A88DE066E045E55CE020E080 |
:10A7C0000E947FAA80E067E04FE35CE020E00E9407 |
:10A7D0007FAA0E94ADA40E94EF65982E81E0F81632 |
:10A7E00039F083E0F81621F084E0F81609F064C02F |
:10A7F00080E48AD8882329F480E462E0BCD88823E6 |
:10A8000081F1AE2DB0E0FD01EE55F94E80818A3721 |
:10A8100040F58F5F8083803208F4B082FD01EE55F1 |
:10A82000F94E808181528F3008F4A082FD01EE55EF |
:10A83000F94E80818A53873008F44082FD01EE553D |
:10A84000F94E80818B55863008F45082AE55B94E52 |
:10A85000812F63E04C9120E00E9460A881E080930A |
:10A86000590680E851D8882331F480E862E083D823 |
:10A87000882309F471C0AE2DB0E0FD01EE55F94E0C |
:10A880008081813208F468C0815080838B55863086 |
:10A8900008F42082FD01EE55F94E80818A538730FD |
:10A8A00008F48082FD01EE55F94E808181528F308F |
:10A8B00008F047C0B08245C082E0F81609F04CC0ED |
:10A8C00080E422D8882329F480E461E054D88823E6 |
:10A8D00001F1AE2DB0E0FD01EE55F94E80818933D6 |
:10A8E000C0F48F5F8083803208F4B082FD01EE55A2 |
:10A8F000F94E808181528F3008F4A082AE55B94E56 |
:10A90000812F63E04C9120E00E9460A881E0809359 |
:10A91000590680E80E94844C882329F480E861E08D |
:10A920002AD88823C9F0AE2DB0E0FD01EE55F94ECE |
:10A930008081813388F0815080838A53873008F486 |
:10A940008082AE55B94E812F63E04C9120E00E9489 |
:10A9500060A881E08093590680E20E949F4C882382 |
:10A9600019F181E0F81631F083E0F81619F084E06F |
:10A97000F81629F4EE2DF0E0EE55F94EB08282E0A3 |
:10A98000F81629F4EE2DF0E0EE55F94EA082EE2DEA |
:10A99000F0E0EE55F94E812F63E0408120E00E9407 |
:10A9A00060A881E08093590680E20E948E4C882343 |
:10A9B00009F447C081E0F81661F082E0F81619F456 |
:10A9C0003092BA1609C083E0F81619F084E0F81640 |
:10A9D00019F48AE08093BA16212F269530E02F5F74 |
:10A9E0003F4F8091BA1690E028173907A4F01C9DBC |
:10A9F000C0011124835066E14AE02AE000E00E9491 |
:10AA0000D5A883E066E14AE02AE001E00E94D5A8EB |
:10AA100011E0EE2416C01C9DC0011124835066E194 |
:10AA20004AE02AE000E00E94D5A81E5F1C9DC001FC |
:10AA30001124835066E14AE02AE001E00E94D5A893 |
:10AA4000E39480E10E94844C882309F48DCE81E058 |
:10AA5000F816C9F58091A2168032A9F580E065E06C |
:10AA600049E25CE020E000E0ACEFEA2E0E9471AA2F |
:10AA70008CE291E06FE070EF40E00E94D24C80E207 |
:10AA80009EE4F3013197F1F70197D9F780E064E292 |
:10AA90004FE727E000E00E941BA71C9DC001112486 |
:10AAA000835066E14AE02AE00E94D5A883E066E18F |
:10AAB0004AE02AE001E00E94D5A880EF0E94844C81 |
:10AAC00011E0EE2451CE81E0CDB7DEB7E2E10C9487 |
:10AAD00033D2A0E0B0E0EFE6F5E50C9421D2EC0132 |
:10AAE0006B017A01102FE22FF0E0E231F10508F05E |
:10AAF00052C1EA5BFF4F0C944ED200D000D089ECDB |
:10AB00009CE0B5C000D000D085EC9CE009C000D02E |
:10AB100000D081EC9CE0ABC000D000D08BEB9CE07F |
:10AB2000EDB7FEB792838183D483C383AAC0009715 |
:10AB300021F08130910551F403C0C4EBDCE00AC080 |
:10AB400087E290E00E947CCEEC011EC00097D1F419 |
:10AB5000C0E0D0E08091C21D8823B1F465E24FE7E8 |
:10AB60002FE001E0F2E0EF2E0E9411A98BE790E0C8 |
:10AB70000E947CCEBC0186E040E020E009EF0E940C |
:10AB80008BAACAC0C0E0D0E080E065E24FE72FE0CA |
:10AB900000E00E941BA7C0C0009739F081309105EA |
:10ABA00009F4F3C0C7E9DCE0B9C0CBEADCE0B6C029 |
:10ABB000009729F08130910509F0EDC036C088E298 |
:10ABC00090E009C0009729F08130910509F0E3C0B9 |
:10ABD00006C08CE190E00E947CCEEC019DC08BE130 |
:10ABE00090E0F9CF009729F08130910509F0D3C0AA |
:10ABF00003C081E390E0EFCF80E390E0ECCF8130C1 |
:10AC0000910549F08230910519F0009711F3C3C006 |
:10AC1000C9E8DCE083C0CFE8DCE080C0009729F021 |
:10AC20008130910509F0B7C003C089E290E0D3CF2D |
:10AC30008AE290E0D0CF8417950744F088EC90E04A |
:10AC400060E870E040E00E94D24CE701CC15DD05E1 |
:10AC500039F488EC90E060E870E040E00E94D24C6B |
:10AC60008C2F0E9423A500D000D085E89CE0ADB7D2 |
:10AC7000BEB712969C938E9311971496DC93CE9345 |
:10AC800013970E9472479C010F900F900F900F90A6 |
:10AC90007FC00E94EDCA00D000D000D021E83CE087 |
:10ACA000EDB7FEB732832183ADB7BEB713966D9370 |
:10ACB0007D938D939C9316970E9472479C01EDB7EC |
:10ACC000FEB73696FEBFEDBF63C0009729F0813016 |
:10ACD000910509F060C003C08EE290E07CCF8FE266 |
:10ACE00090E079CF009729F08130910509F053C0A9 |
:10ACF00003C084E490E06FCF85E490E06CCF0097D0 |
:10AD000029F08130910509F046C003C0C6E7DCE0B8 |
:10AD100005C0CCE7DCE002C02097E9F18DB79EB713 |
:10AD200009979EBF8DBFEDB7FEB73196ADB7BEB7E1 |
:10AD300011961C9283E081831282138214838FE622 |
:10AD40009CE096838583D087C78317C08DB79EB755 |
:10AD500009979EBF8DBFEDB7FEB73196ADB7BEB7B1 |
:10AD600011961C9283E0818312821382148388E6F9 |
:10AD70009CE096838583308727830E940AADEDB7D8 |
:10AD8000FEB73996FEBFEDBF06C0CFE9DCE0C6CF07 |
:10AD900021153105D9F6CDB7DEB7E8E00C943DD2E8 |
:10ADA000A6E0B0E0E6EDF6E50C9417D29E838D8325 |
:10ADB0004B015A01322E270136019093B916809328 |
:10ADC000B81681E02224FF2414E0C02EDD249A016D |
:10ADD0002C193D093A832983D601A60FB71FBC83DE |
:10ADE000AB8303C08E2DE11081E0882309F48DC070 |
:10ADF00081E00E942467B1E13B1619F42EEFF22E98 |
:10AE000010E041145104B9F061147104B9F480E008 |
:10AE100065E24FE72FE001E092E0E92E0E9411A9E0 |
:10AE200086E0B20140E020E009EF0E948BAA1EEF0D |
:10AE3000F12E10E01CC061147104C9F080E060E2E2 |
:10AE40004FE725E101E0B2E0EB2E0E9411A985E079 |
:10AE5000B20140E020E00BEF0E948BAA86E0B30134 |
:10AE600040E020E00E948BAAACEFFA2E1EEF0E9479 |
:10AE7000323587FD03C00E94AD350EC0ECE03E16B2 |
:10AE800019F084E69CE008C081E698E069E678E085 |
:10AE90000E949CCE222423942DB73EB729503040E7 |
:10AEA0003EBF2DBFEDB7FEB73196ADB7BEB7119619 |
:10AEB0001C92B2E0B18312821382F48220E63CE05D |
:10AEC00036832583908787830E940AAD8DB79EB70E |
:10AED00009969EBF8DBF84E090E00E947CCEAC01BD |
:10AEE00080E067E020E00E947FAA222039F08BE01A |
:10AEF00067E049E55CE020E00E947FAA8D819E81A9 |
:10AF0000B401A501232D012FE4DD0E94EF65E82E99 |
:10AF100080E40E94844C882331F480E462E00E9443 |
:10AF2000BB4C8823B9F0AD81BE81E981FA81EA1773 |
:10AF3000FB071CF49E828D8206C02D813E812C0D64 |
:10AF40003D1D3E832D838D819E81B401A501232D5E |
:10AF5000012FBFDD80E80E94844C882331F480E813 |
:10AF600062E00E94BB4C8823B9F08D819E81AB8149 |
:10AF7000BC818A179B071CF4BE82AD8206C0ED819E |
:10AF8000FE81EC19FD09FE83ED838D819E81B40164 |
:10AF9000A501232D012F9DDD80E10E94844C882393 |
:10AFA00091F4222009F01ECF80E20E94844C882375 |
:10AFB00009F418CF80EF0E94844C2091B81630918C |
:10AFC000B9163E832D838D819E812696E2E10C94F5 |
:10AFD00033D2CF92DF92EF92FF920F931F930E9492 |
:10AFE000C73D0E944B3782E062E04DE958E52FE90A |
:10AFF00037E00DEA17E00E94A73D86E062E040ECF2 |
:10B000005CE529EB37E005EC17E00E94A73D80E105 |
:10B0100062E045E651E521ED37E00DED17E00E94D5 |
:10B02000A73D0E94343D81E062E04BE55AE529EE00 |
:10B0300037E007EF17E00E94A73D83E062E046EFAC |
:10B0400059E525E038E089010E94A73D84E062E0EF |
:10B0500041E25BE52CE038E089010E94A73D809148 |
:10B060008329882311F00E94343D8091832988230D |
:10B0700049F081E162E043EE5AE526E138E08901DA |
:10B080000E94A73D80918329882349F08FE062E0E8 |
:10B0900042E951E324E238E089010E94A73D0E9481 |
:10B0A000343D88E062E046E353E52FE238E008E310 |
:10B0B00018E00E94A73D89E062E041EE5BE522E4F2 |
:10B0C00038E089010E94A73D0E94343D8CE061E098 |
:10B0D00040EC55E62CE438E089010E94A73D8EE063 |
:10B0E00061E041E057E627E538E089010E94A73D8D |
:10B0F00080E00E94783E0E9459368530A9F00E9477 |
:10B1000031368D30A9F78091C11D90E060E070E08C |
:10B1100041E050E020E001E0EE24FF24CC24DD24D7 |
:10B120003FDE8093C11DE4CF0E94A6371F910F918F |
:10B13000FF90EF90DF90CF900895CF92DF92EF9243 |
:10B14000FF920F931F930E94C73D0E944B3783E0ED |
:10B1500061E040E050E021E638E009E618E00E94B6 |
:10B16000A73D8EE061E040E050E022E738E00FE7E5 |
:10B1700018E00E94A73D8FE061E040E050E02DE83C |
:10B1800038E008E918E00E94A73D0E94343D80E1C4 |
:10B1900061E040E050E024EA38E001EB18E00E9472 |
:10B1A000A73D84E061E040E050E02FEB38E00CEC9C |
:10B1B00018E00E94A73D0E94343D86E061E040E037 |
:10B1C00050E02AED38E007EE18E00E94A73D8AE043 |
:10B1D00061E040E050E024EF38E089010E94A73DA3 |
:10B1E0008CE061E040E050E024E039E00DE019E05F |
:10B1F0000E94A73D8DE061E040E050E027E139E0AA |
:10B2000004E219E00E94A73D80E00E94783E0E947F |
:10B210005936853009F4E1C00E9431368330E1F4BB |
:10B220001091B51D812F90E060E070E041E050E0AA |
:10B230002CE001E0EE24FF24CC24DD24B1DD80935A |
:10B24000B51D181709F381E698E069E678E00E94D9 |
:10B250009CCE0E94C866D8CF8E30B9F480916E1E05 |
:10B26000992787FD909564EF7FEF4CE050E020E058 |
:10B2700001E0EFE3EE2EE9E0FE2E31E3C32E39E0EC |
:10B28000D32E8EDD80936E1EBFCF8F3089F48091D8 |
:10B290006F1E90E060E070E041E050E027E001E0E8 |
:10B2A000EE24FF24CC24DD247BDD80936F1EACCF05 |
:10B2B0008430A9F41091B41D8BE690E00E947CCEFE |
:10B2C0007C01812F90E060E070E04EEF50E020E0E4 |
:10B2D00001E0CC24DD2464DD8093B41D95CF803162 |
:10B2E000A9F41091BC1D8BE690E00E947CCE7C01FD |
:10B2F000812F90E060E070E04EEF50E020E001E050 |
:10B30000CC24DD244DDD8093BC1D7ECF8A3089F4B2 |
:10B3100080912F1E90E060E070E041E050E027E077 |
:10B3200001E0EE24FF24CC24DD243ADD80932F1E9F |
:10B330006BCF893089F480912E1E90E060E070E040 |
:10B3400041E050E027E001E0EE24FF24CC24DD249E |
:10B3500027DD80932E1E58CF8B3089F48091301ECC |
:10B3600090E060E070E042E350E020E001E0EE2495 |
:10B37000FF24CC24DD2414DD8093301E45CF86309D |
:10B3800099F48091BD1D90E06AE070E048E250E0E1 |
:10B390002AE001E0EE24FF24CC24DD2401DD8093AB |
:10B3A000BD1D0E9423A530CF8C3089F480911B1ED7 |
:10B3B00090E060E070E041E050E020E101E0EE2448 |
:10B3C000FF24CC24DD24ECDC80931B1E1DCF8D30AC |
:10B3D00009F01ACF0E94D15117CF0E94A6371F91B2 |
:10B3E0000F91FF90EF90DF90CF900895CF92DF9272 |
:10B3F000EF92FF920F931F930E94755080E00E947E |
:10B40000783E0E945936853009F44CC00E9431368E |
:10B4100010925906823089F48091B61D90E060E068 |
:10B4200070E045E050E020E001E0EE24FF24CC2471 |
:10B43000DD24B6DC8093B61DE1CF833089F48091A2 |
:10B44000B71D90E060E070E04FE750E020E001E0E1 |
:10B45000EE24FF24CC24DD24A3DC8093B71DCECFC3 |
:10B46000873019F40E94AB33C9CF813039F610917F |
:10B47000C21D812F90E060E070E041E050E027E0E5 |
:10B4800001E0EE24FF24CC24DD248ADC8093C21D5D |
:10B49000181709F4B3CF882311F00E94AB330E9430 |
:10B4A000A637AACF0E94A6371F910F91FF90EF9069 |
:10B4B000DF90CF900895CF92DF92EF92FF920F939B |
:10B4C0001F930E94C73D0E944B3781E061E040E03E |
:10B4D00050E029EE37E007EF17E00E94A73D0E94F9 |
:10B4E000343D82E061E040E050E029EB39E0890141 |
:10B4F0000E94A73D80E00E94783E0E945936853028 |
:10B5000009F458C00E943136813071F51091C01D88 |
:10B51000812F90E060E070E041E050E021E101E047 |
:10B52000EE24FF24CC24DD243BDC8093C01D811756 |
:10B5300039F0813011F486E001C085E00E94AFCA85 |
:10B540008091C01D8823B1F68091C21D882391F699 |
:10B550008BE790E00E947CCE61E048EB5BE021E06D |
:10B5600001E00E94A866C6CF823021F61091271E06 |
:10B57000812F90E061E070E046E050E02BE001E0D8 |
:10B58000FBECEF2EF9E0FF2ECC24DD2409DC8093C8 |
:10B59000271E811709F4AECF8091C21D882311F0B8 |
:10B5A0000E94AB338091C31D882309F4A3CF0E946E |
:10B5B000D4C2A0CF0E94A6371F910F91FF90EF90A9 |
:10B5C000DF90CF900895CF92DF92EF92FF920F938A |
:10B5D0001F930E94C73D0E944B3781E690E00E9476 |
:10B5E0007CCE9C0181E061E040E050E00E948E3D15 |
:10B5F00080E00E94783E0E9459368530C9F00E9452 |
:10B6000031368130A9F71091C11E83E790E00E9486 |
:10B610007CCE7C01812F90E060E070E041E050E062 |
:10B6200027E001E0CC24DD24BBDB8093C11EE0CF0A |
:10B630000E94A6371F910F91FF90EF90DF90CF905F |
:10B640000895ABE0B0E0E7E2FBE50C941FD20E9466 |
:10B6500012505E010894A11CB11C80E00E94783E4B |
:10B660000E945936853009F4A6C00E9431361092E6 |
:10B6700059068130E1F41091C31D812F90E060E004 |
:10B6800070E041E050E027E001E0EE24FF24CC240C |
:10B69000DD2486DB8093C31D1817F9F2882311F08F |
:10B6A0000E94D4C20E94A6370E941250D6CF823088 |
:10B6B000F1F40E94BAC2C5016CEC7DE14BE050E0B0 |
:10B6C0000E94B743C5016AE041E01FD88091590646 |
:10B6D000813019F682EA96E10E94D9448CEC9DE112 |
:10B6E00062EA76E14BE050E00E9434D5B6CF833079 |
:10B6F00041F58091C71D89838091C81D8A838091FF |
:10B70000C91D8B838091CA1D8C831D82C50164E095 |
:10B7100042E00E94855380915906813009F09DCF07 |
:10B720008091A2168093C71D8091A3168093C81D97 |
:10B730008091A4168093C91D8091A5168093CA1D7F |
:10B740008CCF843019F40E94D4C287CF853041F465 |
:10B7500081E06FED79E04CED5DE10E94E0527DCF3C |
:10B76000863009F07ACF8091D71D89838091D81DCA |
:10B770008A838091D91D8B838091DA1D8C831D82F1 |
:10B78000C50164E042E00E948553809159068130F2 |
:10B7900009F063CF8091A2168093D71D8091A316E4 |
:10B7A0008093D81D8091A4168093D91D8091A516F1 |
:10B7B0008093DA1D52CF0E94A6372B96EAE00C94B4 |
:10B7C0003BD2A0E0B0E0E7EEFBE50C9421D20E9472 |
:10B7D000C73D8AE59BE00E94313783E061E040E0AD |
:10B7E00050E02DEF3AE008E01BE00E94A73D84E026 |
:10B7F00061E040E050E023E13BE00FE11BE00E940C |
:10B80000A73D81E061E040E050E02BE23BE08901B0 |
:10B810000E94A73D82E061E040E050E021E33BE090 |
:10B8200001E41BE00E94A73D0E94343D86E061E0F8 |
:10B8300040E050E02FED3AE00EEE1AE00E94A73D06 |
:10B84000C8EED3E080E00E94783E0E9459368530F1 |
:10B8500009F48FC00E943136833039F58091281E5B |
:10B860009091291E68EE73E00E94C7D1162F072F12 |
:10B8700088E890E00E947CCE7C01812F902F62EEC0 |
:10B880007FEF4EE150E021E001E0CC24DD2488DAB6 |
:10B890009C012C9FC0012D9F900D3C9F900D112469 |
:10B8A0009093291E8093281ECDCF843039F5809146 |
:10B8B000401E9091411E68EE73E00E94C7D1162F82 |
:10B8C000072F88E890E00E947CCE7C01812F902F8A |
:10B8D00062EE7FEF4EE150E021E001E0CC24DD2478 |
:10B8E0005FDA9C012C9FC0012D9F900D3C9F900D15 |
:10B8F00011249093411E8093401EA4CF8130B1F457 |
:10B9000080912A1E90912B1E60E070E044E650E08A |
:10B9100020E001E0A2E5EA2EABE0FA2ECC24DD2403 |
:10B920003FDA90932B1E80932A1E8CCF8230C1F475 |
:10B9300000912C1E10912D1E88E890E00E947CCE74 |
:10B940007C01C80161E070E044E150E020E001E0EA |
:10B95000CC24DD2425DA90932D1E80932C1E72CFEB |
:10B96000863009F06FCF81E060E070E00E9414B291 |
:10B9700069CF0E94A637CDB7DEB7E8E00C943DD280 |
:10B98000CF92DF92EF92FF920F931F930E94C73DD9 |
:10B990000E944B3782E061E040E050E029E63BE066 |
:10B9A0000CE71BE00E94A73D83E061E040E050E02F |
:10B9B00028E83BE007E91BE00E94A73D0E94343DD8 |
:10B9C00081E061E040E050E025EA3BE008EB1BE06D |
:10B9D0000E94A73D84E061E040E050E02AEC3BE0BB |
:10B9E0000BED1BE00E94A73D0E94343D86E061E024 |
:10B9F00040E050E02BEE3BE00DEF1BE00E94A73D46 |
:10BA000080E00E94783E0E945936853009F47FC05C |
:10BA10000E9431368230C1F41091B31D8CE690E063 |
:10BA20000E947CCE7C018DE690E00E947CCE6C0171 |
:10BA3000812F90E060E270E04FEF50E023E001E002 |
:10BA4000AFD98093B31DDCCF813089F48091C01EC3 |
:10BA500090E060E070E041E050E027E001E0EE249B |
:10BA6000FF24CC24DD249CD98093C01EC9CF85300F |
:10BA7000A1F48091231E9091241E60E070E040E3C9 |
:10BA800055E722E004E6EE24FF24CC24DD2488D907 |
:10BA90009093241E8093231EB3CF833089F480912A |
:10BAA0001C1E90E060E070E041E050E026E001E024 |
:10BAB000EE24FF24CC24DD2473D980931C1EA0CF58 |
:10BAC000843089F48091BB1D90E060E070E041E03B |
:10BAD00050E027E001E0EE24FF24CC24DD2460D9EF |
:10BAE0008093BB1D8DCF863009F08ACF80911F1EB9 |
:10BAF00090E060E070E041E050E027E001E0EE24FB |
:10BB0000FF24CC24DD244CD980931F1E79CF0E94C2 |
:10BB1000A6371F910F91FF90EF90DF90CF9008957F |
:10BB20000F931F930E94B950882309F47CC0809121 |
:10BB30008406882351F486ED9CE061E048EE53E0F2 |
:10BB400021E001E00E94A8666EC00E94C73D85E822 |
:10BB500090E00E947CCE0E94313710E02FC0812FF0 |
:10BB600090E09C0175E0220F331F7A95E1F7880F72 |
:10BB7000991F280F391F27523A4D812F61E040E06D |
:10BB800050E00E94753D212F30E0C90155E0880F3B |
:10BB9000991F5A95E1F7220F331F820F931F83518C |
:10BBA0009A4D6FEF7DE14EE050E00E9426D5009760 |
:10BBB00021F4812F61E00E9406381F5F8091840686 |
:10BBC000181768F280E00E94783E813051F50E949B |
:10BBD0003136182F90E0BC0125E0660F771F2A95BB |
:10BBE000E1F7880F991F680F791F63517A4D8FEF26 |
:10BBF0009DE14EE050E00E9434D5812F90E0BC01E1 |
:10BC000015E0660F771F1A95E1F7880F991F680FE7 |
:10BC1000791F67527A4D8BEE9DE144E150E00E941E |
:10BC200034D50E94A6371F910F910895AF92CF92FD |
:10BC3000DF92EF92FF920F931F93CF93DF93809148 |
:10BC4000EB1D882319F4C0E0D1E002C0CBEEDDE1AA |
:10BC500082E390E06FEF7FEF40E00E94D24C82E8F9 |
:10BC600090E00E947CCE7C0186E890E00E947CCE31 |
:10BC70006C010E94AD358C0185E0B701A60121E081 |
:10BC8000EE24E3946E01AA242BD6882309F43DC048 |
:10BC90000E94B9508823C9F18DEC9CE061E041E03D |
:10BCA000F6D583E890E00E947CCEBC0182E040E0C3 |
:10BCB00020E001E00E948BAA0E9490C90E942BC044 |
:10BCC00085E090E00E947CCEAC018BE067E020E054 |
:10BCD0000E947FAA0E94C2C10E94A8C980918406C6 |
:10BCE000882359F484E890E00E947CCE61E040ED26 |
:10BCF00057E021E00E94A86608C082E390E06FEF61 |
:10BD00007FEF40E00E94D24C0BDFDF91CF911F917B |
:10BD10000F91FF90EF90DF90CF90AF900895109229 |
:10BD2000BB160895FF920F931F93CF93DF938091DB |
:10BD30008629813019F4CBE0D1E002C0C0E1D1E026 |
:10BD400080918629823011F4C5E1D1E00E94BBAF19 |
:10BD500087E30E947A43882309F46FC32DB73EB767 |
:10BD6000275030403EBF2DBFEDB7FEB7319682E081 |
:10BD7000ADB7BEB711968C938AEC9EE09283818317 |
:10BD800083EC9EE094838383D683C5830E945B43C8 |
:10BD900080918329EDB7FEB73796FEBFEDBF8823AC |
:10BDA00019F088E190E002C089E190E00E947CCE29 |
:10BDB0009C0100D000D00F92EDB7FEB7319682E023 |
:10BDC000F82EADB7BEB71196FC9285EB9EE092833C |
:10BDD0008183348323830E945B430F900F90EDB7E0 |
:10BDE000FEB7118284EB9EE0938382830E945B43C3 |
:10BDF000ADB7BEB711961C92119780EA9EE01396DC |
:10BE00009C938E9312970E945B43EDB7FEB711820D |
:10BE100080E99EE0938382830E945B43ADB7BEB707 |
:10BE200011961C9211978FE89EE013969C938E9327 |
:10BE300012970E945B43EDB7FEB7F18283E79EE065 |
:10BE4000938382830E945B430F900F900F9080E357 |
:10BE500090E00E947CCE2DB73EB7275030403EBFC9 |
:10BE60002DBFEDB7FEB73196ADB7BEB711961C9298 |
:10BE700006E81EE01283018329E63EE03483238333 |
:10BE8000968385830E945B43EDB7FEB73796FEBF6E |
:10BE9000EDBF80E390E00E947CCE2DB73EB72750E7 |
:10BEA00030403EBF2DBFEDB7FEB73196ADB7BEB740 |
:10BEB00011961C92128301832EE53EE03483238386 |
:10BEC000968385830E945B43EDB7FEB73796FEBF2E |
:10BED000EDBF80E390E00E947CCE2DB73EB72750A7 |
:10BEE00030403EBF2DBFEDB7FEB73196ADB7BEB700 |
:10BEF00011961C921283018322E53EE03483238352 |
:10BF0000968385830E945B43EDB7FEB73796FEBFED |
:10BF1000EDBF80E390E00E947CCE2DB73EB7275066 |
:10BF200030403EBF2DBFEDB7FEB73196ADB7BEB7BF |
:10BF300011961C921283018324E43EE03483238310 |
:10BF4000968385830E945B43EDB7FEB73796FEBFAD |
:10BF5000EDBF80E390E00E947CCE2DB73EB7275026 |
:10BF600030403EBF2DBFEDB7FEB73196ADB7BEB77F |
:10BF700011961C921283018329E33EE034832383CC |
:10BF8000968385830E945B43EDB7FEB73796FEBF6D |
:10BF9000EDBF80E390E00E947CCE2DB73EB72750E6 |
:10BFA00030403EBF2DBFEDB7FEB73196ADB7BEB73F |
:10BFB00011961C921283018320E33EE03483238395 |
:10BFC000968385830E945B43EDB7FEB73796FEBF2D |
:10BFD000EDBF81E390E00E947CCE2DB73EB72750A5 |
:10BFE00030403EBF2DBFEDB7FEB73196ADB7BEB7FF |
:10BFF00011961C921283018320E23EE03483238356 |
:10C00000968385830E945B43EDB7FEB73796FEBFEC |
:10C01000EDBF80E390E00E947CCE2DB73EB7275065 |
:10C0200030403EBF2DBFEDB7FEB73196ADB7BEB7BE |
:10C0300011961C921283018321E13EE03483238315 |
:10C04000968385830E945B43EDB7FEB73796FEBFAC |
:10C05000EDBF80E390E00E947CCE2DB73EB7275025 |
:10C0600030403EBF2DBFEDB7FEB73196ADB7BEB77E |
:10C0700011961C921283018323E03EE034832383D4 |
:10C08000968385830E945B43EDB7FEB73796FEBF6C |
:10C09000EDBF80E390E00E947CCE2DB73EB72750E5 |
:10C0A00030403EBF2DBFEDB7FEB73196ADB7BEB73E |
:10C0B00011961C921283018326EF3DE03483238383 |
:10C0C000968385830E945B43EDB7FEB73796FEBF2C |
:10C0D000EDBF80E390E00E947CCE2DB73EB72750A5 |
:10C0E00030403EBF2DBFEDB7FEB73196ADB7BEB7FE |
:10C0F00011961C921283018329EE3DE03483238341 |
:10C10000968385830E945B43EDB7FEB73796FEBFEB |
:10C11000EDBF80E390E00E947CCE2DB73EB7275064 |
:10C1200030403EBF2DBFEDB7FEB73196ADB7BEB7BD |
:10C1300011961C92128301832DED3DE034832383FD |
:10C14000968385830E945B43EDB7FEB73796FEBFAB |
:10C15000EDBF80E390E00E947CCE2DB73EB7275024 |
:10C1600030403EBF2DBFEDB7FEB73196ADB7BEB77D |
:10C1700011961C921283018321ED3DE034832383C9 |
:10C18000968385830E945B43EDB7FEB73796FEBF6B |
:10C19000EDBF80E390E00E947CCE2DB73EB72750E4 |
:10C1A00030403EBF2DBFEDB7FEB73196ADB7BEB73D |
:10C1B00011961C921283018323EC3DE03483238388 |
:10C1C000968385830E945B43EDB7FEB73796FEBF2B |
:10C1D000EDBF81E390E00E947CCE2DB73EB72750A3 |
:10C1E00030403EBF2DBFEDB7FEB73196ADB7BEB7FD |
:10C1F00011961C92128301832AEB3DE03483238342 |
:10C20000968385830E945B43EDB7FEB73796FEBFEA |
:10C21000EDBF80E390E00E947CCE2DB73EB7275063 |
:10C2200030403EBF2DBFEDB7FEB73196ADB7BEB7BC |
:10C2300011961C921283018320EB3DE0348323830B |
:10C24000968385830E945B43EDB7FEB73796FEBFAA |
:10C25000EDBF81E390E00E947CCE2DB73EB7275022 |
:10C2600030403EBF2DBFEDB7FEB73196ADB7BEB77C |
:10C2700011961C921283018327EA3DE034832383C5 |
:10C28000968385830E945B43EDB7FEB73796FEBF6A |
:10C29000EDBF81E390E00E947CCE2DB73EB72750E2 |
:10C2A00030403EBF2DBFEDB7FEB73196ADB7BEB73C |
:10C2B00011961C921283018329E93DE03483238384 |
:10C2C000968385830E945B43EDB7FEB73796FEBF2A |
:10C2D000EDBF81E390E00E947CCE2DB73EB72750A2 |
:10C2E00030403EBF2DBFEDB7FEB73196ADB7BEB7FC |
:10C2F00011961C921283018323E93DE0348323834A |
:10C30000968385830E945B43EDB7FEB73796FEBFE9 |
:10C31000EDBF81E390E00E947CCE2DB73EB7275061 |
:10C3200030403EBF2DBFEDB7FEB73196ADB7BEB7BB |
:10C3300011961C92128301832AE83DE03483238303 |
:10C34000968385830E945B430F900F900F900F9010 |
:10C35000EDB7FEB7118289E89DE0938382830E9446 |
:10C360005B43ADB7BEB71196FC92119780E89DE094 |
:10C3700013969C938E9312970E945B43EDB7FEB782 |
:10C3800011828DE69DE0938382830E945B43ADB76B |
:10C39000BEB711961C92119789E59DE013969C9368 |
:10C3A0008E9312970E945B43EDB7FEB7118284E42F |
:10C3B0009DE0938382830E945B43ADB7BEB7119625 |
:10C3C0001C92119781E39DE013969C938E93129794 |
:10C3D0000E945B43EDB7FEB7118286E29DE0938336 |
:10C3E00082830E945B43ADB7BEB711961C92119732 |
:10C3F00081E19DE013969C938E9312970E945B437C |
:10C40000EDB7FEB711828CEF9CE0938382830E948C |
:10C410005B43ADB7BEB711961C92119789EE9CE0B5 |
:10C4200013969C938E9312970E945B430F900F90EC |
:10C430000F900E9491420E946D43CDB7DEB7E5E0B8 |
:10C440000C9440D2A7E0B0E0E8E2F2E60C9423D2EC |
:10C450000E947948892B09F46BC084E690E09093A0 |
:10C460008F1680938E1610E0FF24EE2400E025C086 |
:10C470000E9427489C01807080509140F1F001306B |
:10C4800051F0013018F00230C1F40AC02B313105EF |
:10C4900091F401E012C02B31310569F402E00DC0C6 |
:10C4A0002537310511F411E008C02637310519F49C |
:10C4B000EE24E39402C0FF24F39480918E169091B1 |
:10C4C0008F16892B51F0112319F011E001E006C0FD |
:10C4D000FF2039F4EE2061F211C0012FFF2031F06E |
:10C4E000102F85E40E944348012F23C0112321F01F |
:10C4F00085E50E9443481DC0EE2089F086E50E9434 |
:10C5000043488BE890E08E010F5F1F4FB8014AE06F |
:10C5100050E00E9443D5C8010E94604809C0809144 |
:10C520008E1690918F16892B19F484E50E9443484A |
:10C5300000E0802F2796E6E00C943FD2EF92FF9226 |
:10C540000F931F939091C016892F8F5F8093C01611 |
:10C55000883010F01092C0164091BD1642502091C4 |
:10C56000BF1622503DE2F32E9F9D80011124809141 |
:10C57000BC166091BE16EE240E9451AC4091BD16CF |
:10C5800042502091BF1622500091C0160F9D80018D |
:10C5900011248091BC166091BE16EE24E3940E9493 |
:10C5A00051AC88E290E090939716809396161F9175 |
:10C5B0000F91FF90EF900895EF920F931F93982F94 |
:10C5C00080E48093BC1688E28093BE168AE0809354 |
:10C5D000BD1688E08093BF16992311F09093BE1684 |
:10C5E0001092C01688E290E090939716809396166A |
:10C5F00081E08093BB1680E46091BE1648E026E09F |
:10C6000000E010E0EE24E3940E9451AC6091BE166D |
:10C610004091BD162091BF168091BC1690E070E04D |
:10C6200050E030E001E00E94EDAA1F910F91EF90E1 |
:10C630000895EF920F9310925A0680935B066093D1 |
:10C640005C0610925E0680E680935D06605E8DE07B |
:10C6500046E626E001E0EE24E3940E9411A90F9142 |
:10C66000EF900895A0E0B0E0E8E3F3E60C941BD26D |
:10C670003B01EA016901580147014115510521F0CB |
:10C68000FF24F39411E002C0FF2410E0C114D10490 |
:10C6900019F0E2E0FE2E12E0A114B10419F073E0EB |
:10C6A000F72E12E08114910419F064E0F62E13E0E5 |
:10C6B0008F2D8F5F0E94CBA9209749F00BEF011BB4 |
:10C6C00088E08F19BE0142E020E00E948BAAC114CD |
:10C6D000D10449F00CEF011B89E08F19B60142E04B |
:10C6E00020E00E948BAAA114B10449F00DEF011BB8 |
:10C6F0008AE08F19B50142E020E00E948BAA8114E4 |
:10C70000910449F00EEF011B8BE08F19B40142E058 |
:10C7100020E00E948BAA61147104C9F10E94C74CE9 |
:10C7200070928D1660928C1614C080E20E948E4C1E |
:10C730008823A9F480E10E948E4C882381F480E450 |
:10C740000E948E4C882359F480E80E948E4C8823E6 |
:10C7500031F480918C1690918D16892B31F72F2D05 |
:10C7600030E0B9016095709553E0660F771F5A95D8 |
:10C77000E1F7665C2F5F3F4F43E0220F331F4A957E |
:10C78000E1F72B5F80E04FE700E00E941BA70E94CB |
:10C79000C74CCDB7DEB7EEE00C9437D2AF92BF9264 |
:10C7A000CF92DF92EF92FF920F931F93FC01DB0178 |
:10C7B0006A015901780181E0BF01AD01960185014F |
:10C7C00051DF1F910F91FF90EF90DF90CF90BF90BE |
:10C7D000AF9008950F9380915A068F5F80935A0609 |
:10C7E00020915C062E5D90915D0660915B06899FAD |
:10C7F000C001112470E00E94C7D1462F80E1622F52 |
:10C8000022E001E00E941BA781E00F910895FF92B2 |
:10C810000F931F93CF93DF93182FEB01F42E222356 |
:10C8200011F00E94BBAF80E060E04FE727E001E03D |
:10C830000E941BA7112319F424E03FE002C029E065 |
:10C840003FE08DB79EB709979EBF8DBFEDB7FEB78E |
:10C85000319681E0ADB7BEB711968C93118282E01C |
:10C8600082831382148236832583D087C7830E94F4 |
:10C870000AAD8DB79EB709969EBF8DBFFF2011F000 |
:10C880000E94ADA4CDB7DEB7E5E00C9440D2FC0128 |
:10C89000962F242F81E0BF01492FB9CFFC01962F9D |
:10C8A000242F80E0BF01492FB2CF1F93182F00D053 |
:10C8B00000D08CEF9EE0EDB7FEB79283818385EFC9 |
:10C8C0009EE0948383830E9472470F900F900F9095 |
:10C8D0000F90612F41E0E2DF1F91089581E0E5CFE5 |
:10C8E000A0E0B0E0E6E7F4E60C9417D2F82E2B01B6 |
:10C8F0003A01322E4801BE2C83E290E06FEF7FEFC9 |
:10C9000040E00E94D24C81E012E0B4E02B2E88235C |
:10C9100009F4F8C0C114D10411F4E0DF0BC0AA205F |
:10C9200029F0C60161E041E0B2DF04C0C60161E068 |
:10C9300041E0B4DF6114710409F04EC0BB2019F46A |
:10C9400085EE9EE002C088EE9EE02DB73EB72850EF |
:10C9500030403EBF2DBFEDB7FEB73196ADB7BEB785 |
:10C9600011961C931182128213839583848397827C |
:10C9700086820E949DAF2DB73EB7285F3F4F3EBFD6 |
:10C980002DBF80E061E24FE720E101E0A2E0EA2E66 |
:10C990000E9411A9332019F485EE9EE002C088EEB2 |
:10C9A0009EE0ADB7BEB71897BEBFADBFEDB7FEB73F |
:10C9B000319611962C9211821282B6E0B383958340 |
:10C9C0008483578246820E949DAF2DB73EB7285F71 |
:10C9D0003F4F3EBF2DBF5DC0BB2019F485EE9EE0EA |
:10C9E00002C088EE9EE0ADB7BEB71897BEBFADBF20 |
:10C9F000EDB7FEB7319611961C9311821282BFEFEC |
:10CA0000B38395838483978286820E949DAF2DB7DE |
:10CA10003EB7285F3F4F3EBF2DBF80E06BE14FE741 |
:10CA200027E101E0F2E0EF2E0E9411A9332019F472 |
:10CA3000C5EEDEE002C0C8EEDEE08DB79EB7089717 |
:10CA40009EBF8DBFEDB7FEB73196ADB7BEB711969D |
:10CA50002C9211821282BFEFB383D583C483578295 |
:10CA600046820E949DAFEDB7FEB7319625E0ADB787 |
:10CA7000BEB711962C93118212821382D583C48380 |
:10CA8000778266820E949DAF8DB79EB708969EBF43 |
:10CA90008DBF93E0F916B1F09F1538F0A1E0FA16BA |
:10CAA00071F0B2E0FB1671F507C024E0F216C9F090 |
:10CAB00035E0F31639F51CC080E290E014C08FE138 |
:10CAC00090E011C085E090E00E947CCEAC018BE04C |
:10CAD00067E020E00E947FAA83E167E042EE5EE02B |
:10CAE0000EC083E090E00E947CCEAC018CE006C0DA |
:10CAF00085E290E00E947CCEAC018BE067E020E014 |
:10CB00000E947FAA0E94ADA46AD0082F80E20E94F2 |
:10CB10008E4C882369F480E10E94844C882311F0B4 |
:10CB200081E007C0802F002309F4F1CE81E0EFCE31 |
:10CB300080E091E0F91611F0182F04C010E088236E |
:10CB400009F411E00E94C74C812FCDB7DEB7E2E1B6 |
:10CB50000C9433D2AF92CF92DF92EF920F931F9348 |
:10CB6000F901680121E08F01EE24E394AA24A39443 |
:10CB7000B7DE1F910F91EF90DF90CF90AF900895A7 |
:10CB8000EF92FF920F931F9383E290E00E947CCE7E |
:10CB90007C0184E290E00E947CCE8C0182E290E0F5 |
:10CBA0000E947CCE9C0185E0B701A80107ED1EE044 |
:10CBB000D1DF882379F083E50E9443482CE088E1A7 |
:10CBC00090E00FB6F894A895809360000FBE209374 |
:10CBD0006000FFCF1F910F91FF90EF90089580911B |
:10CBE0008E1690918F16892B11F020E00FC02ADC51 |
:10CBF000882311F420E004C00E9445B9C1DF21E080 |
:10CC00008AE090E090938F1680938E16822F08957D |
:10CC1000A0E0B0E0EEE0F6E60C9420D2182FEB0195 |
:10CC2000F42E6901B02EEE2011F00E94BBAF56DE4B |
:10CC3000FF2091F084E26FE045E32AE001E0EE247A |
:10CC4000E3940E9468A888E590E00E947CCEBC0135 |
:10CC500082E042E020E009C08FE590E00E947CCEB7 |
:10CC6000BC0182E040E020E001E00E948BAA112399 |
:10CC700019F48BEE9EE002C080EF9EE000D000D061 |
:10CC8000EDB7FEB792838183D483C3830E9472473A |
:10CC90008C010F900F900F900F900E94D94484E068 |
:10CCA000B80140E020E005E00E9480AF80E060E253 |
:10CCB0004FE720E101E092E0E92E0E9411A9BB209C |
:10CCC00061F0FF2019F088EE93E002C089E190E066 |
:10CCD0006FEF7FEF40E00E94D24CC114D104E9F025 |
:10CCE0008DE090E00E947CCEAC018CE067E020E01B |
:10CCF0000E947FAA0E94C74CD0928D16C0928C16BB |
:10CD000006C080E20E948E4C882339F468DF80914F |
:10CD10008C1690918D16892BA1F70E94C74CCDB728 |
:10CD2000DEB7E9E00C943CD2EF92FF920F937C01C6 |
:10CD3000062F89E590E00E947CCEBC0181E041E0B5 |
:10CD40009701EE24E39464DF0F91FF90EF90089534 |
:10CD5000EF920F93FC01962FDA01522FE02E81E023 |
:10CD6000BF01492F9D01052F53DF0F91EF900895CB |
:10CD7000EF920F93FC01962FDA01522FE02E80E004 |
:10CD8000BF01492F9D01052F43DF0F91EF900895BB |
:10CD9000EF92FF920F931F938C0189E790E00E941E |
:10CDA0007CCE7C018AE790E00E947CCE9C0182E0F0 |
:10CDB000B70140E050E0CEDE882319F00E9445B96B |
:10CDC00029981F910F91FF90EF9008950F931F9353 |
:10CDD0008EE190E00E947CCE8C018FE590E00E9475 |
:10CDE0007CCE9C0182E0B80140E050E000E010E021 |
:10CDF000B1DE882319F00E9445B929981F910F913F |
:10CE00000895EF92FF920F931F938CE290E00E949F |
:10CE10007CCE7C018DE290E00E947CCE8C0184E788 |
:10CE200090E00E947CCE9C0181E0B701A80100E067 |
:10CE300010E090DE882319F00E945AB929981F91BA |
:10CE40000F91FF90EF900895FF920F931F93F82E8C |
:10CE50000E94323587FF02C041DD09C00E94AD3516 |
:10CE60008C010E948535B8014F2D21E0D0DC1F9147 |
:10CE70000F91FF900895DF92EF92FF920F931F930F |
:10CE8000D62E142FF22EE22EE39467E040E15FE00D |
:10CE900020E0012F0E9471AA8D2D67E04EE05FE037 |
:10CEA00020E0EF2C0E9471AA1F910F91FF90EF904C |
:10CEB000DF9008950F931F93CF93DF93282FEB01FB |
:10CEC0008A0160913C176F3078F58823D9F081E0B2 |
:10CED00090E002C0880F991F2A95E2F7AA2797FDD4 |
:10CEE000A095BA2F2091881E3091891E40918A1EEC |
:10CEF00050918B1E82239323A423B5230097A10571 |
:10CF0000B10591F0E62FF0E0EE0FFF1FEE0FFF1FCF |
:10CF1000DF01A35CB84ECD93DC93E15CF84E118346 |
:10CF200000836F5F60933C17DF91CF911F910F914A |
:10CF3000089580913B178F5F80933B1790913C172A |
:10CF4000891710F010923B1781E080937A170895AB |
:10CF500080913B17882311F480913C178150809376 |
:10CF60003B1781E080937A1708956F927F928F929A |
:10CF70009F92AF92BF92CF92DF92EF92FF920F9368 |
:10CF80001F933B014C01C601B5010E9488D166196F |
:10CF9000770988099909A80197010E94FCD17901B4 |
:10CFA0008A01E60CF71C081D191DB701C8011F9165 |
:10CFB0000F91FF90EF90DF90CF90BF90AF909F9038 |
:10CFC0008F907F906F900895E091E717F091E817A8 |
:10CFD00086AD97AD68E671E00E94C7D19C0186A539 |
:10CFE00097A568E671E00E94C7D1281B390B37FF6F |
:10CFF00002C028593E4F3093EA172093E91708954D |
:10D000009C0148978C9710F480E00895C9018454DE |
:10D0100090408D9710F481E00895C90181579040A8 |
:10D020008D9710F482E00895C9018E5990408D9734 |
:10D0300010F483E00895C9018B5C90408D9710F443 |
:10D0400084E00895C901885F90408D9710F485E0D1 |
:10D050000895255231402D32310510F087E00895B2 |
:10D0600086E0089510926A061092680608958091ED |
:10D0700068068823D9F482E080936806E091E71778 |
:10D08000F091E81742E06BE20BC0542F5F5F81AD77 |
:10D0900090E0469F90011124821793071CF0452FC2 |
:10D0A000473098F3409368064093BB2208958AEF77 |
:10D0B000EDEEF2E2DF011D928A95E9F7089584E52D |
:10D0C00091E0E7EEF3E2DF019C011D922150304038 |
:10D0D000E1F7089589E6E4E8F2E2DF011D928A951E |
:10D0E000E9F78FEF8093BA228093B1228EE7809385 |
:10D0F000B7228093B9220895DC011D921D921D92E2 |
:10D100001C92139790E02BC0292F30E0F901EE0F0D |
:10D11000FF1FE20FF31FEE0FFF1FE154FD4DE081F3 |
:10D1200011968C9111978E1720F41196EC9311970C |
:10D130009C93F901EE0FFF1FE20FF31FEE0FFF1F8D |
:10D14000EE53FD4DE08113968C9113978E1730F4BA |
:10D150001396EC93139712969C9312979F5F80916E |
:10D16000BC22981788F20895CF93DF93DC01E091F9 |
:10D17000E717F091E81783A994A919969C938E93C9 |
:10D18000189784A595A51B969C938E931A97ED01ED |
:10D190002C9631968DE0019009928150E1F7E4E8F8 |
:10D1A000F6E188E001900D928150E1F7DF91CF9197 |
:10D1B00008952AE235E2D901F901719791E10190D0 |
:10D1C0000D929150E1F72151304093E2273E39070B |
:10D1D00091F7F9018193A4E8B6E188E00D900192FE |
:10D1E0008150E1F70895E091E717F091E817EC5BC3 |
:10D1F000FF4F808187FF15C08EEC93E2DC01FC01BC |
:10D20000799729E101900D922150E1F7499722E2A7 |
:10D210008D3E920799F7A8DF80EA90E06FEF7FEFED |
:10D2200004C088E592E06FE070E040E00C94D24CDE |
:10D23000E091E717F091E81790A9809166068917A9 |
:10D2400038F48EE190E06FEF7FEF40E00E94D24C27 |
:10D25000E091E717F091E81790A981A9981799F440 |
:10D26000EC5BFF4F808185FF0EC0809167068823AD |
:10D2700051F480E991E06FEF7FEF40E00E94D24CE3 |
:10D2800081E080936706E091E717F091E81790A995 |
:10D2900080916606981711F09093660681A99817F9 |
:10D2A00029F4EC5BFF4F808185FD02C01092670678 |
:10D2B00008956091B31D623330F550916A06533082 |
:10D2C00020F582E080936806E091E717F091E81777 |
:10D2D00042E0ABE20BC0742F7F5F81AD90E04A9FCC |
:10D2E00090011124821793071CF0472F473098F3C1 |
:10D2F00040936806469F802D1124809369065F5FE6 |
:10D3000050936A0602C060936906E091E717F091B6 |
:10D31000E81791AD809169069817A0F480916B068B |
:10D32000853020F48F5F80936B060895ED5BFF4F8F |
:10D33000808180FF07C088EE93E060E270E041E00A |
:10D340000C94D24C0895BF92CF92DF92EF92FF924D |
:10D350000F931F93182FF62E35E0C32EC80E25E02D |
:10D36000E22EE60E8C2D412F2E2D01E00E9403A609 |
:10D370009AE0B92EB10E8C2D6F2D4B2D2E2D0E94C3 |
:10D3800003A68AE0D82EDF0C8C2D6D2D412F2E2D7B |
:10D390000E9403A68C2D6D2D4B2D2E2D0E9403A6D1 |
:10D3A000812F8C5F6E2D1A5F412F2E2D0E9403A6B8 |
:10D3B0002F2D2A5F8C2D94E0F90E6F2D4C2D0E949D |
:10D3C00003A61F910F91FF90EF90DF90CF90BF9039 |
:10D3D0000895EF92FF920F931F93182FF62E042FAC |
:10D3E000E22EE091E717F091E81786AD97AD68E679 |
:10D3F00071E00E94C7D104DEE82FF0E0EE0FFF1FBE |
:10D40000EC56F04F45915491812F6F2D20E00E94F2 |
:10D4100071AA1F910F91FF90EF900895EF92FF92E4 |
:10D420000F931F93182F7B0185E00E94CBA983E007 |
:10D4300060E24CE720E200E00E9403A683E065E39F |
:10D440004CE725E30E9403A68DB79EB70B979EBFBE |
:10D450008DBFEDB7FEB73196ADB7BEB711961C9232 |
:10D4600083E0818382E0828385E083838EEF8483FF |
:10D4700083EB9FE09683858317831086F286E1868F |
:10D480000E940AAD8DB79EB70B969EBF8DBF83E6F7 |
:10D4900090E00E947CCEAC0180E064E022E006E0F7 |
:10D4A000E3E0EE2E0E9471AA8CE065E04EEA5FE0B8 |
:10D4B00022E000E074E0E72E0E9471AA81E165E0BD |
:10D4C00049EA5FE022E00E9471AA82E090E00E94B7 |
:10D4D0007CCEAC0180E067E022E00E947FAA81E17F |
:10D4E00067E044EA5FE022E00E947FAA1F910F916B |
:10D4F000FF90EF9008950F931F93182F83E00E94E1 |
:10D50000CBA988E590E00E947CCE2DB73EB72A508B |
:10D5100030403EBF2DBFEDB7FEB7319626E0ADB728 |
:10D52000BEB711962C9322E02183128228EF238329 |
:10D530002CEB3FE035832483978386831087118605 |
:10D540000E949DAFE12FF0E0EE0FFF1FEE5EF04F67 |
:10D55000659174912DB73EB7265F3F4F3EBF2DBFFB |
:10D5600087E042E020E00CEF0E948BAA1F910F9110 |
:10D5700008950F9383E00E94CBA982E068ED7FE0DD |
:10D5800040E020E000E00E948BAA86E06DEC7FE0A6 |
:10D5900042E020E008EF0E948BAA8FEF69E00E9432 |
:10D5A000559887E062E040E02CEF0E945599809109 |
:10D5B000C818882339F484EF91E06FEF7FEF40E0E3 |
:10D5C0000E94D24C0E94C74C8CE291E090937F164F |
:10D5D00080937E1680917E1690917F16892B29F07C |
:10D5E0008FEF0E94844C8823A9F30E94C74C0E94AD |
:10D5F000BBAF8091C818811181E00F910895A0E020 |
:10D60000B0E0E5E0FBE60C941FD2382FA62ECA014D |
:10D61000B22EF02E66E170E00E94B3D16D5E70E034 |
:10D62000EB01C05AD04FB9E6CB2EBFE0DB2EC60EC1 |
:10D63000D71E132F1F5FFE014491832F6A2D20E018 |
:10D640000B2DEF2C0E9451A72196312FCC15DD0513 |
:10D6500081F7CDB7DEB7EAE00C943BD2FF920F938F |
:10D660001F93182FF62E90E070E06B5F7F4F0596AA |
:10D6700046E050E025E030E001E00E94EDAA2F2DC9 |
:10D680002B5F812F8D5F622F195F412F0E9403A6B0 |
:10D6900012502F2D295F812F93E0F90E6F2D412F0E |
:10D6A0000E9403A61F910F91FF900895AF92CF9211 |
:10D6B000DF92EF92FF920F931F9380917A17813040 |
:10D6C00049F480E060E04FE72FE301E0B2E0EB2EA9 |
:10D6D0000E9411A9E091E717F091E81786A597A598 |
:10D6E00088599E4F26AD37AD821B930B68E671E0DB |
:10D6F0000E94C7D1F82ED92E8AE160E244E620E2EA |
:10D7000001E00E9403A68FE360E04FE32FE30E9455 |
:10D7100003A68DE36BE041E42BE00E9403A688E2C0 |
:10D720006EE148E222E20E9403A686E56EE146E54C |
:10D7300022E20E9403A68DE365E341E425E30E9413 |
:10D7400003A682E390E00E947CCEAC0189E060E019 |
:10D7500020E000E0A4E0EA2E0E9471AA84E390E0B9 |
:10D760000E947CCEAC0183E063E020E0EE240E94C6 |
:10D7700071AA85E390E00E947CCEAC0181E163E078 |
:10D7800020E00E9471AA83E390E00E947CCEAC016D |
:10D7900089E067E020E0FDEFEF2E0E9471AA86E3AA |
:10D7A00090E00E947CCEAC0180E060E020E005E0EB |
:10D7B000E4E0EE2E0E9471AAE091E717F091E817DD |
:10D7C000E05CFF4F4081552747FD509582E060E0C7 |
:10D7D00023E030E000E0EE2477E0C72E94E0A92EAD |
:10D7E0000E94F3AE85E060E04BE020E007E014E04B |
:10D7F000E12E0E9451A787E390E00E947CCEAC010D |
:10D8000080E067E020E005E0BDEFEB2E0E9471AA0A |
:10D81000E091E717F091E817EF5BFF4F4081552744 |
:10D8200047FD509582E067E023E030E000E0EE2421 |
:10D83000ADEFAA2E0E94F3AE85E067E04BE020E05A |
:10D8400007E0FDEFEF2E0E9451A7E091E717F0915E |
:10D85000E81786AD97AD68E671E00E94C7D1AC01CC |
:10D860008FE060E023E030E000E0EE24ECEFCE2E2D |
:10D8700074E0A72E0E9435AF82E160E04BE020E02B |
:10D880000CEF94E0E92E0E9451A783E160E04EEF97 |
:10D8900024E09FDDE091E717F091E817EF5BFF4F81 |
:10D8A0008081992787FD909562E070E00E94C7D142 |
:10D8B000709561957F4F1FE3A12EA60E8291992747 |
:10D8C00087FD909562E070E00E94C7D170956195E8 |
:10D8D0007F4FB0E2CB2EC60E89E08A1518F0AAE081 |
:10D8E000AA2E05C085E78A1510F4F5E7AF2E88E06B |
:10D8F0008C1518F0E9E0CE2E05C086E38C1510F4E7 |
:10D9000076E3C72E809160068823C1F060915F06A0 |
:10D9100090E070E049E050E028E030E000E00E9454 |
:10D92000EDAA0091610610916206809160066091F7 |
:10D930005F0648E027E0EE240E9451AC8A2D90E07B |
:10D940006C2D70E049E050E028E030E001E00E94FA |
:10D95000EDAA8A2D6C2D48E027E00F2D1D2DEE2419 |
:10D96000E3940E9451ACF0926106D0926206C0929C |
:10D970005F06A09260061F910F91FF90EF90DF90DD |
:10D98000CF90AF900895AF92BF92CF92DF92EF9277 |
:10D99000FF920F931F93F82EB62EA42EC22E14DBE7 |
:10D9A00000916106109162060C541F4F8F2D6B2D54 |
:10D9B0004A2D2C2DEE240E9451AC0091E9171091B4 |
:10D9C000EA170C541F4F8F2D6B2D4A2D2C2DEE2452 |
:10D9D000E3940E9451ACDD24C7FCD0948F2D90E0DD |
:10D9E0006B2D70E04A2D50E0960101E00E94EDAAF7 |
:10D9F0008091E9179091EA179093620680936106EF |
:10DA00001F910F91FF90EF90DF90CF90BF90AF905C |
:10DA10000895EF920F93DF93CF9300D000D0CDB74E |
:10DA2000DEB7CE01019668DB8DE390E00E947CCEEC |
:10DA3000AC0180E060E020E00E947FAA60919C221F |
:10DA400070919D22882777FD8095982F0E94B3CFF3 |
:10DA500020E030E040EA51E40E9418CF0E9480CFDD |
:10DA60009B01AC018EE060E0A90124E030E000E021 |
:10DA7000EE240E9429AF82E160E04DE620E002E062 |
:10DA80000E9451A78EE390E00E947CCEAC0180E022 |
:10DA900061E020E00E947FAA2091A2223091A3227F |
:10DAA00040E050E0CA01B901F3E0660F771F881F1C |
:10DAB000991FFA95D1F7620F731F841F951F2AEFE4 |
:10DAC00030E040E050E00E94DAD18FE061E0A9014F |
:10DAD00023E030E000E00E946BAF82E161E04AE1C8 |
:10DAE00050E120E002E00E9471AA8FE390E00E94E2 |
:10DAF0007CCEAC0180E062E020E00E947FAA8091B1 |
:10DB0000A8229091A9226AE070E00E94B3D1AB01F3 |
:10DB10008EE062E024E030E000E00E946BAF82E142 |
:10DB200062E04DE620E002E00E9451A780E490E030 |
:10DB30000E947CCEAC0180E063E020E00E947FAADE |
:10DB40004091BA224F3F41F48EE063E045E150E15D |
:10DB500020E00E947FAA09C08EE063E050E023E04D |
:10DB600030E000E0EE240E94E7AE82E163E046E5AB |
:10DB700020E002E0EE240E9451A783E490E00E949E |
:10DB80007CCEAC0180E064E020E00E947FAA40915E |
:10DB9000B4225091B5228EE064E024E030E000E051 |
:10DBA0000E946BAF82E164E041E150E120E002E0DD |
:10DBB0000E9471AA82E490E00E947CCEAC0180E0D9 |
:10DBC00065E020E00E947FAA4091AC225091AD22F6 |
:10DBD0008DE065E024E030E000E00E94E7AE82E105 |
:10DBE00065E041E420E002E00E9451A780E066E0A9 |
:10DBF00043E050E120E00E947FAA498150E04F5F5E |
:10DC00005F4F86E066E021E030E000E00E946BAF0D |
:10DC10004A818EE066E050E023E030E00E94E7AE0B |
:10DC200082E166E041E420E002E00E9451A780E04A |
:10DC300067E045EF5FE020E00E947FAA4B8150E063 |
:10DC40004F5F5F4F86E067E021E030E000E00E9438 |
:10DC50006BAF4C818EE067E050E024E030E00E9442 |
:10DC60006BAF82E167E042EF5FE020E002E00E94FC |
:10DC700071AA2496DEBFCDBFCF91DF910F91EF90B7 |
:10DC80000895A0E0B0E0E7E4FEE60C9421D2C82EAF |
:10DC9000D62EF42EE22E4091680650E0CF2DD0E033 |
:10DCA000CA01880F991F9A01A5E0220F331FAA9578 |
:10DCB000E1F7820FA82FB0E0E091E717F091E817A5 |
:10DCC00021AD30E02A1B3B0B8AE290E0489F402DBB |
:10DCD0001124642F70E06A1B7B0B2C9FC0012D9FC9 |
:10DCE000900D3C9F900D11240E94C7D1162F67FD07 |
:10DCF00010E0812F992787FD9095C817D9070CF45C |
:10DD00001F2D81E0E81609F4EE242F2D211B013090 |
:10DD100069F48C2D6D2D4E2D00E00E941BA7FD0C8B |
:10DD2000F11A8C2D6F2D4E2D212F0CC0812F8C0DB3 |
:10DD30006D2D422F2E2D00E00E941BA78C2D6D2DE6 |
:10DD4000412F2E2D01E00E941BA7CDB7DEB7E8E0E2 |
:10DD50000C943DD2DF92EF92FF920F931F93142FFA |
:10DD6000E22E96E0899F802D1124180F660F660F12 |
:10DD7000660FE60E812F6E2D46E027E000E00E9440 |
:10DD80001BA7E091E717F091E81785A996A98330C2 |
:10DD900091059CF022E0D22ED10E92E0F92EFE0CDD |
:10DDA0008D2D6E2D412F2F2D01E00E9403A61C5FAB |
:10DDB0008D2D6E2D412F2F2D17C08E5F9F4FB4F4E8 |
:10DDC00082E0D82ED10E06E0F02EFE0C84E0E80EA4 |
:10DDD0008D2D6F2D412F2E2D01E00E9403A61C5F7B |
:10DDE0008D2D6F2D412F2E2D0E9403A61F910F9177 |
:10DDF000FF90EF90DF900895ACE2B0E0E2E0FFE644 |
:10DE00000C9421D2182FF62EC42E522FD02E282F4C |
:10DE100030E0C901880F991F820F931F0197880F67 |
:10DE2000660F660F660F6150FF2029F41E141CF068 |
:10DE300060E027E001C028E0112329F41D141CF044 |
:10DE400080E04CE001C04EE08D0D6E0D552309F4CD |
:10DE50006AC001E00E941BA783EF90E19A83898347 |
:10DE600080EF90E19C838B838DEE90E19E838D8388 |
:10DE70008AEE90E198878F8387EE90E19A87898771 |
:10DE800084EE90E19C878B8781EE90E19E878D8761 |
:10DE90008EED90E1988B8F878BED90E19A8B898B3B |
:10DEA00088ED90E19C8B8B8B85ED90E19E8B8D8B2B |
:10DEB00082ED90E1988F8F8B8FEC90E19A8F898F14 |
:10DEC0008CEC90E19C8F8B8F89EC90E19E8F8D8FF5 |
:10DED00086EC90E198A38F8F83EC90E19AA389A3BD |
:10DEE00080EC90E19CA38BA38DEB90E19EA38DA38E |
:10DEF0008AEB90E198A78FA387EB90E19AA789A777 |
:10DF000084EB90E19CA78BA7EC2DF0E0EE0FFF1FB8 |
:10DF1000EC0FFD1F41815281812F6F2D22E00D2DCD |
:10DF20000E9471AA03C000E00E941BA7AC96E8E023 |
:10DF30000C943DD2EF920F93382F522FE02E842F66 |
:10DF400090E08631910548F4885A9F4FA091E717D9 |
:10DF5000B091E817FC010C944ED220E056C0A65BAD |
:10DF6000BF4F02C0AC5BBF4F2C9122704EC0AC5B68 |
:10DF7000BF4F0EC0A65BBF4F15C0A65BBF4F0DC005 |
:10DF8000A65BBF4F36C0AD5BBF4F38C0AD5BBF4F68 |
:10DF90002C9124703AC0AD5BBF4F2C91287035C0D6 |
:10DFA000AD5BBF4F2C91217030C0AD5BBF4FDCCF5C |
:10DFB000AD5BBF4F1EC0A65BBF4FEACFAC5BBF4F90 |
:10DFC0000DC020E0AC5BBF4F8C9187FD1EC021E0EF |
:10DFD0001CC0AD5BBF4F17C0AD5BBF4F2C9120780D |
:10DFE00014C0AC5BBF4FDECFAC5BBF4FD6CFAC5BDA |
:10DFF000BF4F2C91207109C0AC5BBF4F2C91207298 |
:10E0000004C0AC5BBF4F2C912074832F052FF4DE2E |
:10E010000F91EF900895EF92FF920F931F93162F99 |
:10E0200096E0899FF02C1124110F110F110FE12E92 |
:10E03000E3941B5F8F2D8C5F612F41E00E9472A5DE |
:10E0400012504F2D4B5F8F2D8D5F612F212F01E0DF |
:10E050000E9403A64F2D4A5FF3948F2D8F5F6E2D84 |
:10E060002E2D0E9403A61F910F91FF90EF9008950F |
:10E07000BF92CF92DF92EF92FF920F931F93F82EF1 |
:10E08000162F42E0E42EE80E34E0C32EC80E8E2D8B |
:10E090004C2D212F01E00E9403A626E0B22EBF0CDA |
:10E0A000D12ED3948F2D6D2D4B2D2D2D0E9403A697 |
:10E0B000212F2E5F8E2D622F4C2D0E9403A693E000 |
:10E0C000E92EEF0C8E2D6D2D40E00E9472A583E0AD |
:10E0D000D82ED10EB5E0CB2EC10E4F2D4F5F8E2D19 |
:10E0E0006D2D2C2D0E9403A64F2D4B5F8E2D6D2D77 |
:10E0F0002C2D0E9403A61A5F8F2D612F4B2D212FEF |
:10E100000E9403A61F910F91FF90EF90DF90CF9098 |
:10E11000BF900895FF920F931F93F82E162F8E5FD6 |
:10E1200042E022E001E00E94D5A81E5F8F2D612F02 |
:10E1300046E02FE00E94D5A81F910F91FF9008950F |
:10E14000CF92DF92EF92FF920F931F93182FF62E2C |
:10E1500032E0E32EE60E6E2D44E022E001E00E9464 |
:10E16000D5A8812F885F6E2D44E022E00E94D5A8BB |
:10E17000812F8C5F6F2D44E026E00E94D5A826E019 |
:10E18000E22EE10E97E0D92EDF0C8BE0C82ECF0CEB |
:10E19000412F4E5F8E2D6D2D2C2D0E9403A6412FF9 |
:10E1A000465F8E2D6D2D2C2D0E9403A68CE0F80E5F |
:10E1B000812F8F5F6F2D155F412F2F2D0E9403A69A |
:10E1C0001F910F91FF90EF90DF90CF900895DF9215 |
:10E1D000EF92FF920F931F93182FF62E55E0E52E26 |
:10E1E000E60E6E2D4AE028E001E00E94D5A845E049 |
:10E1F000D42ED10E8D2D6F2D412F2E2D0E9403A6D2 |
:10E20000165F8D2D6F2D412F2E2D0E9403A68AE0C3 |
:10E21000F80E812F86506F2D42E023E00E94D5A892 |
:10E220001F910F91FF90EF90DF9008959F92AF9212 |
:10E23000BF92CF92DF92EF92FF920F931F93D82E4F |
:10E24000162FC42EF22EE091E717F091E81741AD9A |
:10E2500080916906481710F0E0E001C0E2E0E09329 |
:10E2600079178091C01EF4E0BF2EBD0C9C2C9394B6 |
:10E270008823C1F0609168068AE0489FC00111249C |
:10E2800070E00E94C7D1AB018D2D612F23E030E0FB |
:10E2900000E0EE2EAF2C0E9477AE8B2D612F49EA65 |
:10E2A00050E10EC08D2D612F50E023E030E000E002 |
:10E2B000EE2EAF2C0E94AFAE8B2D612F47EA50E1BE |
:10E2C00020917917092DEF2C0E9471AA86E0D89E23 |
:10E2D000C0011124482F485E612F660F660F660F3C |
:10E2E000262F295F00E080917917823009F401E040 |
:10E2F000842F0E9403A61F910F91FF90EF90DF9053 |
:10E30000CF90BF90AF909F900895EF920F93809120 |
:10E310007A17813021F581E260E041E22BE001E0F3 |
:10E320000E9403A68FE560E04FE52BE00E9403A664 |
:10E3300080E06CE04FE729E292E0E92E0E9411A90B |
:10E3400082E067E04BEE5FE020E000E0EE240E9418 |
:10E3500071AA82E16DE040E020E00E943B67E0911D |
:10E36000E717F091E81720A931A9332309F420E039 |
:10E370008DB79EB70B979EBF8DBFEDB7FEB7319699 |
:10E3800087E0ADB7BEB711968C93118212828FEFE2 |
:10E390008383148280EE9FE0968385832783108693 |
:10E3A000318712860E940AAD8DB79EB70B969EBF2D |
:10E3B0008DBF80E060E040E020E038DF80E069E091 |
:10E3C0004EE121E002E05DDCE091E717F091E81713 |
:10E3D00047A950AD80E160E020E0EE240E9402AE4B |
:10E3E0001092CB1780E065E047EB57E120E005E0B5 |
:10E3F000B3E0EB2E0E94D9AC1092B71780E064E036 |
:10E4000043EA57E120E0A2E0EA2E0E94D9AC109244 |
:10E41000A31780E063E04FE857E120E0EE24E394A7 |
:10E420000E94D9AC10928F1780E062E04BE757E171 |
:10E4300020E0EE240E94D9AC0E9418690F91EF9061 |
:10E440000895A0E0B0E0E7E2F2E70C941CD28091DE |
:10E450007A17813009F046C081E260E041E223E1B1 |
:10E4600001E00E9403A68DE560E04DE523E10E94F6 |
:10E4700003A680E063E14FE72CE292E0E92E0E94E0 |
:10E4800011A980E069E24FE729E20E9403A68CE02F |
:10E4900060E042E250E120E00FEFEE240E9471AA1A |
:10E4A0008CE061E040E250E120E012E0E12E0E94C9 |
:10E4B00071AA4091680683E161E050E021E030E01C |
:10E4C00000E0EE24B2E0CB2EA2E0AA2E0E9435AFEF |
:10E4D00084E161E04EE150E120E002E0F2E0EF2E65 |
:10E4E0000E9471AA80E060E040E020E09FDE80E0D2 |
:10E4F00069E04EE121E002E0C4DBE091E717F09132 |
:10E50000E817E05BFF4F408151818091231E90917D |
:10E51000241E8417950710F090E001C092E09093BC |
:10E52000791787E060E025E030E000E0E92EEDEFCC |
:10E53000CE2EAA240E9435AFE091E717F091E8179C |
:10E5400047A950AD80E160E020E002E0EE240E94A7 |
:10E5500002AEE091E717F091E817E25BFF4F4081D0 |
:10E56000518187E061E024E030E000E072E0A72E16 |
:10E570000E94AFAEBB2455E0D52E88249924BB2041 |
:10E5800019F04DEFF42E02C037EFF32E7D2C7A9464 |
:10E59000E401CC0FDD1FCC0FDD1FC80DD91DCE53FC |
:10E5A000D94E11E08B81807F19F149814423A1F07C |
:10E5B000812F672D50E023E030E000E0EE24CC24F2 |
:10E5C000AF2C0E9435AF812F8D5F672D4BE020E08F |
:10E5D00001E0EF2C0E9451A74881812F6D2D50E062 |
:10E5E00023E030E000E0EE24CC24AF2C0E94AFAE5C |
:10E5F00025961B5F1531B1F6B39482E0D80E84E006 |
:10E6000090E0880E991E92E0B91609F0B8CFCDB708 |
:10E61000DEB7EDE00C9438D2AF92CF92EF920F9329 |
:10E6200080917A178130F1F481E260E041E229E0E3 |
:10E6300001E00E9403A68DE560E04DE529E00E941F |
:10E6400003A680E06AE04FE725E3E2E0EE2E0E94B9 |
:10E6500011A98CE060E046E250E120E00FEFEE24EB |
:10E660000E9471AA80E060E040E020E0DFDDE09100 |
:10E67000E717F091E817E05BFF4F408151818091EF |
:10E68000231E9091241E8417950710F090E001C07E |
:10E6900092E09093791787E060E025E030E000E0B9 |
:10E6A000E92E7DEFC72EAA240E9435AFE091E7172F |
:10E6B000F091E81747A950AD80E160E020E002E06A |
:10E6C000EE240E9402AE83E162E042E029EF0EEF09 |
:10E6D00031DC83E164E040E029EF0BEF2BDC83E1E8 |
:10E6E00066E041E029EF08EF25DC83E167E043E0E5 |
:10E6F00029EF0DEF1FDC80E162E046E022EF0EEF34 |
:10E7000019DC80E164E04BE022EF0BEF13DC80E1E9 |
:10E7100066E042E122EF08EF0DDC80E167E04EE0C9 |
:10E7200022EF0DEF07DC8CE062E047E021EF0EEF17 |
:10E7300001DC8CE064E048E021EF0BEFFBDB8CE0D8 |
:10E7400066E049E021EF08EFF5DB8CE067E04AE0A6 |
:10E7500021EF0DEFEFDB89E062E044E02BEE0EEFFE |
:10E76000E9DB89E064E045E02BEE0BEFE3DB89E0D9 |
:10E7700066E044E12BEE08EFDDDB89E067E045E190 |
:10E780002BEE0DEFD7DB86E062E04FE025EE0EEFDB |
:10E79000D1DB86E064E040E125EE0BEFCBDB86E0E9 |
:10E7A00066E041E125EE08EFC5DB86E067E043E186 |
:10E7B00025EE0DEFBFDB0F91EF90CF90AF90089556 |
:10E7C0008F929F92AF92BF92CF92DF92EF92FF9281 |
:10E7D0000F931F93D82EB62EA02E9E2C8C2CCA01E0 |
:10E7E000B9010E94B3CF7B018C0120E030E048E406 |
:10E7F00053E40E94D2D0181654F0C801B70120E0AB |
:10E8000030E048E453EC0E9411CF8823C4F4C801DF |
:10E81000B70120E030E040EA51E40E9418CF0E94A6 |
:10E8200080CF9B01AC018D2D6B2DA90124E030E040 |
:10E8300000E0EA2CC92CA82C0E94F3AE17C0C80136 |
:10E84000B70120E030E040E05FE30E94D6D00E94B4 |
:10E8500080CF9B01AC018D2D6B2DA90123E030E011 |
:10E8600000E0EA2CC92CA82C0E943FAE1F910F910A |
:10E87000FF90EF90DF90CF90BF90AF909F908F90E0 |
:10E8800008952F923F925F926F927F928F929F9204 |
:10E89000AF92BF92CF92DF92EF92FF920F931F93AE |
:10E8A000CF93DF9380917A178130F1F481E260E0B9 |
:10E8B00041E229E001E00E9403A68DE560E04DE51C |
:10E8C00029E00E9403A680E069E04FE726E332E0FA |
:10E8D000E32E0E9411A986E060E04CE250E120E0C6 |
:10E8E00002E0EE240E9471AA80E060E040E020E0B7 |
:10E8F0009DDC8CE060E04EE025E000E01BDBE09179 |
:10E90000E717F091E81747A950AD80E160E020E0FB |
:10E9100002E0EE240E9402AE25EF622E22E2722E69 |
:10E92000C0E0D0E09BE0592E82E0B82E09E1202E15 |
:10E93000312C1CEFD12EDC0E4E0180E0652D4FE70F |
:10E9400027E001E00E941BA7219680E06B2DAE011D |
:10E9500021E030E000E0B2E0EB2EA4E0CA2EAD2CC6 |
:10E960000E9435AF829CF001839CF00D929CF00DCB |
:10E970001124E351FD4D84E06B2DE080F180028194 |
:10E9800013812481358146815781F2E0CF2EAA245C |
:10E990008D2C0E9482AD84E16B2D4AE250E122E091 |
:10E9A00000E0ED2C0E9471AAF301208131814427FF |
:10E9B00037FD4095542F80E16B2D02E0EE24CD2CE5 |
:10E9C000FFDE252D285F81E0622F4DE700E00E94E9 |
:10E9D00003A61B2D1F5FF3012085318542855385DA |
:10E9E00081E0612F0E9450ADF30124813581468181 |
:10E9F00057818CE0612F0E9450ADF1E15F0E82E003 |
:10EA0000B80EE9E1F0E06E0E7F1EFEE35F1609F03E |
:10EA100090CFDF91CF911F910F91FF90EF90DF90FA |
:10EA2000CF90BF90AF909F908F907F906F905F90AE |
:10EA30003F902F900895AF92CF92EF920F931F9334 |
:10EA400080917A17813069F581E260E041E22BE044 |
:10EA500001E00E9403A68FE560E04FE52BE00E94F5 |
:10EA600003A680E06CE04FE723E3A2E0EA2E0E94D9 |
:10EA700011A980E065E24FE525E20E9403A68FE53B |
:10EA800065E24FE52FE30E9403A68FE565E240E0D3 |
:10EA90000E9472A587E062E19ADB87E06DE20E9446 |
:10EAA0002E6B80E060E040E020E0C0DB80E069E0C9 |
:10EAB0004EE121E002E0E5D8E091E717F091E81798 |
:10EAC00047A950AD80E160E020E0EE240E9402AE54 |
:10EAD00087E060E042E02EEF01E02CDA89E060E0C0 |
:10EAE00043E025E027DA8CE060E044E126E022DA2A |
:10EAF000E091E717F091E81723A934A9442737FDDF |
:10EB00004095542F87E062E000E0F5E0EF2ECC2442 |
:10EB100057DE83E062E04DE450E120E008E0EE24BF |
:10EB20000E9471AA8DE062E04DEF20E013D983E0EE |
:10EB300063E049E450E120E0E2E0EE2E0E9471AA99 |
:10EB4000E091E717F091E81784A595A56AE070E0D9 |
:10EB50000E94B3D1AB0187E063E024E030E000E045 |
:10EB6000EE2475E0C72E92E0A92E0E9435AF8BE00F |
:10EB700063E047E450E120E009E012E0E12E0E946A |
:10EB800071AA8DE669E14BE029E00E94C36C8CE636 |
:10EB900063E242E021E001E00E941BA7E091E71759 |
:10EBA000F091E81742A981E165E050E022E030E011 |
:10EBB00000E0EE24CC24CA940E9435AF83E76AE2D9 |
:10EBC00057DA83E065E043E450E120E008E0B2E09A |
:10EBD000EB2E0E9471AAE091E717F091E81720A9A7 |
:10EBE00031A9332309F420E08DB79EB70B979EBF60 |
:10EBF0008DBFEDB7FEB7319687E0ADB7BEB71196C2 |
:10EC00008C9305E001831282038312E014838BE36B |
:10EC100090E19683858327831086318712860E9430 |
:10EC20000AADE091E717F091E817E75BFF4F20810D |
:10EC30008DB79EB70B969EBF8DBF222359F1ADB7FE |
:10EC4000BEB71997BEBFADBFEDB7FEB731968CE02A |
:10EC500011968C930183128388E08383148387E366 |
:10EC600090E196838583278310860E940AAD8DB735 |
:10EC70009EB709969EBF8DBF80E569E24BE529E20C |
:10EC800001E00E9403A68FE469E24FE421E30E94C1 |
:10EC900003A607C08FE469E24DE028E000E00E948F |
:10ECA0001BA783E066E043E350E120E008E0A4E036 |
:10ECB000EA2E0E9471AAE091E717F091E817838D80 |
:10ECC000948D6AE070E00E94B3D1AB0187E066E00A |
:10ECD00024E030E000E0EE24F5E0CF2EE4E0AE2EBC |
:10ECE0000E9435AF8BE066E041E350E120E009E0AF |
:10ECF00074E0E72E0E9471AA0E9418691F910F917B |
:10ED0000EF90CF90AF900895AF92CF92EF920F9384 |
:10ED10001F93CF93DF9380917A17813031F581E291 |
:10ED200060E041E22BE001E00E9403A68FE560E095 |
:10ED30004FE52BE00E9403A680E06CE04FE72CE05B |
:10ED40000E9403A680E062E049E550E120E000E097 |
:10ED5000A2E0EA2E0E9471AA80E065E043E550E15E |
:10ED600020E0F3E0EF2E0E9471AA80E060E040E036 |
:10ED700020E05CDA80E069E04EE121E002E00E9400 |
:10ED8000416EE091E717F091E81786AD97AD68E620 |
:10ED900071E00E94C7D1AC0186E060E022E000E0B3 |
:10EDA0000E94FF6AE091E717F091E81747A950AD7C |
:10EDB00080E160E020E002E0EE240E9402AEE091FB |
:10EDC000E717F091E81723A934A9442737FD4095A8 |
:10EDD000542F80E063E000E0E3E0CE2EF1DC84E03D |
:10EDE00063E041E023E00E94AA6EE091E717F09112 |
:10EDF000E81784A595A56AE070E00E94B3D1AB0145 |
:10EE000080E066E024E030E0CC2474E0A72E0E948D |
:10EE100035AF84E066E041E550E120E002E034E017 |
:10EE2000E32E0E9471AA80E466E24AE126E10E9494 |
:10EE3000C36C85E361E246E12AE000E00E941BA783 |
:10EE400084E360E248E12CE001E00E94D5A88FE372 |
:10EE50006CE342E022E00E941BA7C091E917D09129 |
:10EE6000EA17C43BD10514F411E007C088E691E02D |
:10EE70009C012C1B3D0BE90110E089E064E0AE0130 |
:10EE800023E030E000E0EE2493E0C92EB3E0AB2EA7 |
:10EE90000E9435AF112349F0CD2B39F086E366E2AD |
:10EEA00047E326E201E00E9403A680E162E046E03B |
:10EEB0002DEF00E03FD883E162E042E020E03AD865 |
:10EEC00083E164E040E020E00DEF34D883E166E0C8 |
:10EED00041E020E00AEF2ED883E167E043E020E044 |
:10EEE0000FEF28D880E167E04EE02DEF23D8DF91C7 |
:10EEF000CF911F910F91EF90CF90AF900895AF9267 |
:10EF0000CF92EF920F9380917A17813009F06DC004 |
:10EF100080E06CE14FE72CE101E00E9403A680E075 |
:10EF200063E34FE723E30E9403A681E460E041E44A |
:10EF300022E30E9403A680E064E0ECD887E062E070 |
:10EF400042E750E120E000E0F2E0EF2E0E9471AADB |
:10EF50008CE060E04DE650E120E0EE240E9471AAD2 |
:10EF60008CE061E048E650E120E0EE24E3940E946A |
:10EF700071AA84E161E04BE020E001E00E9451A72A |
:10EF80008CE062E043E650E120E000E0E2E0EE2EBB |
:10EF90000E9471AA84E162E041E420E002E00E9464 |
:10EFA00051A780E061E2CCD886E065E04EE550E113 |
:10EFB00020E000E0EE24E3940E9471AA86E460E27F |
:10EFC00006D984E164E04DE620E002E0EE240E94F0 |
:10EFD00051A784E165E04BE020E001E0EE24E394FA |
:10EFE0000E9451A784E167E016D882E068E04BE018 |
:10EFF00022E001E00E94416E82E060E040E020E01B |
:10F0000015D988E060E046E020E000E00E949A6FB9 |
:10F01000E091E717F091E81747A950AD82E061E071 |
:10F0200020E0EE24E3940E9402AEE091E717F09115 |
:10F03000E817E05BFF4F408151818091231E909142 |
:10F04000241E8417950710F090E001C092E0909381 |
:10F05000791782E062E025E030E000E0E92ECC2480 |
:10F0600072E0A72E0E9435AFE091E717F091E81704 |
:10F0700023A934A9442737FD4095542F80E160E04F |
:10F08000EE249EDB84E160E042E020E00E94AA6E74 |
:10F09000E091E717F091E81786AD97AD68E671E06B |
:10F0A0000E94C7D1AC0181E161E023E030E0AA24F5 |
:10F0B000A3940E9435AFE091E717F091E817E25B67 |
:10F0C000FF4F808191816AE070E00E94B3D1AB0173 |
:10F0D00081E162E023E030E092E0A92E0E9435AFAA |
:10F0E000E091E717F091E81742A984E064E050E06E |
:10F0F00022E030E00E946BAF88E064E04EE02FEF4A |
:10F100000E949A6FE091E717F091E81722AD33ADB6 |
:10F1100040E050E0CA01B901B3E0660F771F881FD5 |
:10F12000991FBA95D1F7620F731F841F951F2AEF9D |
:10F1300030E040E050E00E94DAD183E065E0A901D0 |
:10F1400023E030E0AA24A3940E9435AFE091E717B2 |
:10F15000F091E81784A595A56AE070E00E94B3D10C |
:10F16000AB0181E164E023E030E00E946BAF4091AD |
:10F17000E9175091EA1780E165E024E030E00E9451 |
:10F1800035AF81E067E042E020E00E949A6F84E0C2 |
:10F1900067E040E020E00E949A6F87E067E041E08E |
:10F1A00020E00E949A6F8AE067E043E020E00E943E |
:10F1B0009A6F8DE067E04BE020E00E949A6FE0914B |
:10F1C000E717F091E817EE5BFF4F408181E167E0C0 |
:10F1D00050E023E030E00E946BAF0F91EF90CF90B2 |
:10F1E000AF900895A1E0B0E0E8EFF8E70C9421D2E9 |
:10F1F000082F84E58093940891E099832DB73EB75A |
:10F20000275030403EBF2DBFEDB7FEB7319684E7A3 |
:10F21000ADB7BEB711968C9382E081839283CE0105 |
:10F2200001969483838381E090E0968385830E9496 |
:10F230002D4A1092E50510E010937F1600937E167C |
:10F24000EDB7FEB73796FEBFEDBF80917E16909169 |
:10F250007F16892BD1F78091E505882309F448C0F2 |
:10F260000E949249C090E705D090E805D6018D91A3 |
:10F270009C91815D9740873C9105C8F5F894F60113 |
:10F28000648170E080E090E020E13EE040E050E00A |
:10F290000E9488D17B018C01D60115966C9170E09B |
:10F2A00080E090E02CE330E040E050E00E9488D124 |
:10F2B000E60EF71E081F191FF6018681E80EF11CE5 |
:10F2C000011D111DE0928416F0928516009386169A |
:10F2D0001093871683818093881682818093891684 |
:10F2E0008081918190938B1680938A16789481E027 |
:10F2F00090E02196E8E00C943DD2A0E0B0E0E3E895 |
:10F30000F9E70C9417D2E091E717F091E817ED5B5D |
:10F31000FF4FF0808091901690919116892B51F5B6 |
:10F320000091BC181091BD182091BE183091BF18E3 |
:10F330004091C0185091C1186091C2187091C318C3 |
:10F340008091C4189091C518EE24FEE1CF2EE7E617 |
:10F35000AE2E0E945A97823068F084E143DF892BF9 |
:10F3600019F482E390E002C080EE9EE29093911641 |
:10F370008093901680916C06882309F45FC0809179 |
:10F38000921690919316892B09F058C088E48093C7 |
:10F3900094081092E50580919701882319F48FEF66 |
:10F3A000809397018DB79EB70B979EBF8DBFEDB72A |
:10F3B000FEB7319688E6ADB7BEB711968C93118231 |
:10F3C00082E0828387E991E09483838381E090E007 |
:10F3D0009683858310861782928781870E942D4AA3 |
:10F3E0008FEF8093970184E190E090937F16809354 |
:10F3F0007E16EDB7FEB73B96FEBFEDBF80917E1641 |
:10F4000090917F16892B21F08091E5058823B1F337 |
:10F410008091E505882359F00E949249ABE7B7E156 |
:10F42000E3E9F4E180E501900D928150E1F784E198 |
:10F4300090E09093931680939216B0C080919A16A4 |
:10F4400090919B16892B09F0A9C08BE480939408B6 |
:10F450001092E50584E190E090937F1680937E16EC |
:10F460002F2C3324F1E02F2233248BC08091E5052B |
:10F47000882309F486C00E949249A091E705B091C3 |
:10F48000E8054C9087E0841508F479C095E0499E22 |
:10F49000F0011124EF53F94E85E00D900192815057 |
:10F4A000E1F7C42DD0E0DE01AA0FBB1FAA0FBB1FDE |
:10F4B000AC0FBD1FAF53B94E14968C911497807F3B |
:10F4C00009F45DC02114310409F459C03E01660CF1 |
:10F4D000771C6C0E7D1E660C771CF2EB8F2EFDE107 |
:10F4E0009F2E860C971CF401E55FFA4FE080F180B7 |
:10F4F0000894E11CF11CF182E08211965C90AEE070 |
:10F50000B5E08A0E9B1EF4016081718180E090E07D |
:10F51000252D30E040E050E000E010E0E4E6AE2EC3 |
:10F52000B12CC12CD12C0E94B567D4016D937C9372 |
:10F53000F301E154FD4D8081851508F45082FE01F0 |
:10F54000EE0FFF1FEE0FFF1FEC0FFD1FEF53F94EE5 |
:10F550009281FE01EE0FFF1FEC0FFD1FEE0FFF1F4C |
:10F56000EE53FD4D8081891708F490838091BC2271 |
:10F5700090E0C817D9071CF043944092BC22109227 |
:10F58000E50580917E1690917F16892B09F06ECF4C |
:10F5900083E290E090939B1680939A168FE4809379 |
:10F5A00094081092E50580E991E090939516809378 |
:10F5B0009416CDB7DEB7E2E10C9433D2CF92EF923E |
:10F5C0000F938091E617883231F088E20E94D96863 |
:10F5D00088E28093E6170E94BBAF80E060E04FE7CF |
:10F5E0002AE001E0EE24E3940E9468A888E390E01A |
:10F5F0000E947CCEBC0180E042E021E002E00E945B |
:10F600008BAA88E090E00E947CCEAC0183E061E0B0 |
:10F6100020E000E096E0E92E0E9471AA8BE090E0E5 |
:10F620000E947CCEAC0183E062E020E00E9471AADF |
:10F6300080E061E24FE727E001E00E941BA789E03C |
:10F6400090E00E947CCEAC0181E063E022E000E02B |
:10F65000B9E0EB2E0E9471AA2091131E3091141E66 |
:10F660004091151E5091161E81E064E0EE24ABE03F |
:10F67000CA2E0E9450AD20910F1E3091101E409155 |
:10F68000111E5091121E8CE064E0EE24EA940E9458 |
:10F6900050AD80E060E24FE724E101E00E94D5A890 |
:10F6A0008DE090E00E947CCEAC018CE067E020E031 |
:10F6B0000E947FAA80E090E060E070E042E00E945B |
:10F6C000D24C0F91EF90CF900895A1E0B0E0EBE61F |
:10F6D000FBE70C9426D2182F813069F50E94ED4982 |
:10F6E00019828DB79EB707979EBF8DBFEDB7FEB746 |
:10F6F000319684E6ADB7BEB711968C931182128312 |
:10F70000CE0101969483838381E090E09683858384 |
:10F710000E942D4A83E090E090937F1680937E169E |
:10F720008DB79EB707969EBF8DBF80917E16909134 |
:10F730007F16892BD1F78AE08983ADB7BEB71797BB |
:10F74000BEBFADBFEDB7FEB731968FE611968C9375 |
:10F7500082E0818381E08283CE010196948383835A |
:10F7600081E090E0968385830E942D4A83E090E0BB |
:10F7700090937F1680937E168DB79EB707969EBF97 |
:10F780008DBF80917E1690917F16892BD1F787E0EF |
:10F790008983ADB7BEB71797BEBFADBFEDB7FEB794 |
:10F7A00031968BE611968C93118281E08283CE0193 |
:10F7B00001969483838381E090E0968385830E9401 |
:10F7C0002D4A8FE48093940888EC90E09093811602 |
:10F7D000809380161092E5058DB79EB707969EBF61 |
:10F7E0008DBF2196E3E00C9442D2A0E0B0E0EBEFB5 |
:10F7F000FBE70C9417D20E94BBAF8091BB1D8823FE |
:10F8000029F00E94B96A882309F470C381E5EBE707 |
:10F81000F7E1DF011D928A95E9F710926C068FEFF0 |
:10F820008093970188E2E1ECF6E1DF011D928A9571 |
:10F83000E9F781E04ADF81E08093630610926A066F |
:10F840001092680610926B0610926406109265067C |
:10F850001092E6171092CC1710923B1710923C179B |
:10F860008CE490E00E947CCEBC0180E04FE757E73B |
:10F870000E945A678DE490E00E947CCEBC0181E03A |
:10F8800044E856E70E945A678EE490E00E947CCEDE |
:10F89000BC018AE04BE155E70E945A678FE490E093 |
:10F8A0000E947CCEBC0182E041E252E70E945A678E |
:10F8B00080E590E00E947CCEBC0183E04CE053E701 |
:10F8C0000E945A6781E590E00E947CCEBC0184E0F2 |
:10F8D00041E454E70E945A6782E590E00E947CCEA2 |
:10F8E000BC0185E046E55BE60E945A6783E590E04F |
:10F8F0000E947CCEBC0186E049E05DE60E945A672A |
:10F900009091251E10923B1780913C17981710F488 |
:10F9100090933B1781E080937A1780E991E0909370 |
:10F920009516809394168DE290E090939D168093A7 |
:10F930009C1610929B1610929A1610929116109285 |
:10F940009016109293161092921610E0FF249924AC |
:10F9500088248394B0E96B2EB1E07B2EA9EE4A2E69 |
:10F96000A6E15A2EFCEB2F2EF2E03F2E8091E5050A |
:10F97000882309F4CCC10E949249A9EEB6E1809196 |
:10F98000E7059091E8059C01F90182E501900D924F |
:10F990008150E1F75092E8174092E717809168068E |
:10F9A0008823B1F480912217482F50E022E00AC04A |
:10F9B000322F3F5FBBE22B9FC0011124481759072C |
:10F9C0001CF0232F2730A0F3209368062093BB223E |
:10F9D0000E94E4678DEC97E10E94B4688091DD1786 |
:10F9E0009091DE17A091DF17B091E0178093131E5E |
:10F9F0009093141EA093151EB093161E8091D917D4 |
:10FA00009091DA17A091DB17B091DC1780930F1E4D |
:10FA10009093101EA093111EB093121E8091E617B2 |
:10FA2000883221F41092E6170E94C74CE091E71744 |
:10FA3000F091E817ED5BFF4F808180FFE9C0809176 |
:10FA40008A1690918B16892B79F080918A22909159 |
:10FA50008B22892B49F4A4E8B2E2E4E8F6E188E0DD |
:10FA600001900D928150E1F7C091E717D091E8170E |
:10FA70008FA998AD9093972280939622E090982238 |
:10FA8000F09099220894E11CF11CF0929922E092E6 |
:10FA900098226091AE227091AF2280E090E0C25B2C |
:10FAA000DF4F28813981CE54D04040E050E000E063 |
:10FAB00010E0E4E6AE2EB12CC12CD12C0E94B5672B |
:10FAC0007093AF226093AE222BA93CA980919C2217 |
:10FAD00090919D228217930724F430939D222093C6 |
:10FAE0009C222AAD3BAD8091A2229091A322821745 |
:10FAF000930720F43093A3222093A2222CA53DA5A6 |
:10FB00008091A8229091A9228217930720F4309324 |
:10FB1000A9222093A822C25BDF4F28813981CE54CD |
:10FB2000D0408091AC229091AD228217930720F4AF |
:10FB30003093AD222093AC22C05BDF4F2881398106 |
:10FB4000C055D0408091B4229091B522821793077E |
:10FB500020F43093B5222093B42299AD8091BA223B |
:10FB6000981710F49093BA22C95BDF4F288139812E |
:10FB7000C754D0408091A6229091A7228217930764 |
:10FB800024F43093A7222093A622CE5BDF4F9881E6 |
:10FB9000C254D0408091B022891710F49093B022C3 |
:10FBA000CE5BDF4F9881C254D0408091B12298172C |
:10FBB00010F49093B122C05CDF4F9881C054D040C4 |
:10FBC0008091B722981714F49093B722C05CDF4F4E |
:10FBD0009881C054D0408091B622891714F4909334 |
:10FBE000B622CF5BDF4F9881C154D0408091B922BB |
:10FBF000981714F49093B922CF5BDF4F288180913E |
:10FC0000B822821714F42093B822FF24F39412C070 |
:10FC1000FF2081F080918A1690918B16892B51F0EC |
:10FC2000ACE8B2E2E4E8F6E188E001900D928150A0 |
:10FC3000E1F7FF240E945969992051F580919C16A3 |
:10FC400090919D16892B21F080917A17813001F5D2 |
:10FC500080917A17813011F40E94BBAF80916C06BD |
:10FC6000882371F4E0913B17F0E0EE0FFF1FEE0FD9 |
:10FC7000FF1FE15CF84E0190F081E02D099502C074 |
:10FC80000E9485718DE290E090939D1680939C1662 |
:10FC900010927A17E091E717F091E817EB5BFF4FAE |
:10FCA000E0818091CC17E81751F1E33240F5EE2363 |
:10FCB000D1F08E2F0E94D968E091E717F091E817F4 |
:10FCC000EB5BFF4F80810E947B6A88EE93E06FE0E0 |
:10FCD00070E040E00E94D24C30928D1620928C163B |
:10FCE00032E0932E03C080927A179924E091E717AF |
:10FCF000F091E817EB5BFF4F80818093CC17109257 |
:10FD0000E505FBDA709295166092941609C01123EE |
:10FD100039F080E20E948E4C811181E0082FA4C04E |
:10FD2000992009F4DDC08FEF0E94844C882339F4B8 |
:10FD300080918C1690918D16892B09F0D1C08092FC |
:10FD40007A170E94C74C9924CBC080E20E948E4C47 |
:10FD5000182F811111E0012F80E20E949F4C88230F |
:10FD600069F089E190E06FEF7FEF40E00E94D24CB4 |
:10FD700080927A1780926C061092970180E10E941F |
:10FD80009F4C882311F00E94F36880E10E948E4C02 |
:10FD90008823B1F09920A1F480913B17E82FF0E07F |
:10FDA000EE0FFF1FEE0FFF1FE35CF84E60817181C5 |
:10FDB0000E940E6A70928D1660928C16992493940C |
:10FDC00080E80E94844C882311F00E94A86780E498 |
:10FDD0000E94844C882311F00E949967112309F036 |
:10FDE00042C080916C068823E9F180E20E949F4C1A |
:10FDF000882369F089E190E06FEF7FEF40E00E9497 |
:10FE0000D24C80927A1710926C060E94C74C80E800 |
:10FE10000E948E4C882319F09EEF9093970180E406 |
:10FE20000E948E4C882319F0ADEFA093970180E2D9 |
:10FE30000E948E4C882319F0BBEFB093970180E1AC |
:10FE40000E948E4C882319F067EF60939701809190 |
:10FE500097018F3F39F010929316109292164DDA57 |
:10FE60000E94857100E010E08091801690918116CB |
:10FE7000892B11F482E029DC8091941690919516DB |
:10FE8000892B99F4112309F499DB88EC90E060E860 |
:10FE900070E040E00E94D24C7092951660929416E9 |
:10FEA00080927A1781E011DC11E0002309F45ECD25 |
:10FEB00080913B178093251EFF2079F080918A1650 |
:10FEC00090918B16892B49F0ACE8B2E2E4E8F6E1B8 |
:10FED00088E001900D928150E1F71092630606C010 |
:10FEE00080916C06882309F07CCF2FCFCDB7DEB789 |
:10FEF000E2E10C9433D20F931F930E94C73D0E94FE |
:10FF00004B3781E061E042E355E828E133E106E266 |
:10FF100013E10E94A73D82E061E047ED53E825E34D |
:10FF200033E103E413E10E94A73D83E061E043EE87 |
:10FF300051E822E533E10DE513E10E94A73D84E09D |
:10FF400061E049EC52E828E633E106E713E10E945C |
:10FF5000A73D85E061E049E051E823E833E103E9AA |
:10FF600013E10E94A73D86E061E04DEE50E821EAF2 |
:10FF700033E103EB13E10E94A73D80E00E94783E4D |
:10FF80000E94A6371F910F910895A8E0B0E0EBEC16 |
:10FF9000FFE70C941DD284E8A82E82E2B82E4E0111 |
:10FFA0000894811C911CC401B5010E94D743C980EB |
:10FFB000DA80EB80FC808DB79EB70B979EBF8DBF1C |
:10FFC0000DB71EB70F5F1F4FEDB7FEB7118287E366 |
:10FFD00096E1F80192838183C701B60120E13EE0FA |
:10FFE00040E050E00E94DAD1F80134832383C70156 |
:10FFF000B6012CE330E040E050E00E94DAD1CA01C3 |
:020000021000EC |
:10000000B9012CE330E040E050E00E94DAD1F80181 |
:10001000768365838D81878310868E8181871286A2 |
:100020000E945B438DB79EB70B969EBF8DBFE8E0E5 |
:10003000F0E0AE0EBF1EC401B5010E94D743C980D7 |
:10004000DA80EB80FC808DB79EB70B979EBF8DBF8B |
:100050000DB71EB70F5F1F4FEDB7FEB711828AE1D4 |
:1000600096E1F80192838183C701B60120E13EE069 |
:1000700040E050E00E94DAD1F80134832383C701C5 |
:10008000B6012CE330E040E050E00E94DAD1CA0132 |
:10009000B9012CE330E040E050E00E94DAD1F801F1 |
:1000A000768365838D81878310868E818187128612 |
:1000B0000E945B438DB79EB70B969EBF8DBF0E947B |
:1000C000C9412896ECE00C9439D2EF920F931F931C |
:1000D000182F0E94C74C8CE390E06FEF7FEF40E059 |
:1000E0000E94D24C81E0882309F45AC00E94BBAF21 |
:1000F00080E060E04FE727E001E00E941BA78BE46F |
:1001000090E00E947CCEAC0181E060E022E000E063 |
:10011000EE240E9471AA0E94ADA480E064E14FE742 |
:100120002BE101E092E0E92E0E9411A9133091F039 |
:10013000143028F4113041F01230E9F408C01430C2 |
:1001400061F01530C1F40CC086E490E00BC087E488 |
:1001500090E008C088E490E005C089E490E002C027 |
:100160008AE490E00E947CCEBC0182E040E020E086 |
:1001700009E00E948BAA8BE490E00E947CCEBC0137 |
:1001800084E040E020E004E00E948BAA8AE090E056 |
:100190000E947CCEAC0180E067E020E00E947FAA54 |
:1001A0000E94EF65082F80E80E94844C882321F08C |
:1001B0000E94C74C81E00DC080E10E94844C8823DE |
:1001C00029F4802F002309F48ECF8CCF0E94C74CD6 |
:1001D00080E01F910F91EF90089585E076DF8823EE |
:1001E000B9F00E946A680E945F680E94576860E0E8 |
:1001F00070E080E090E020E030E040E050E00E94DD |
:1002000034B980E991E06FEF7FEF40E00C94D24C7D |
:100210000895CF92EF920F930E94C74C01E0002304 |
:1002200009F462C00E946E6488E090E00E947CCE77 |
:10023000AC0183E061E020E000E0F4E0EF2E0E94FA |
:1002400071AA8BE090E00E947CCEAC0183E062E07A |
:1002500020E00E9471AA80E060E24FE727E001E021 |
:100260000E941BA789E090E00E947CCEAC0181E057 |
:1002700063E022E000E0E8E0EE2E0E9471AA209107 |
:10028000131E3091141E4091151E5091161E81E0D0 |
:1002900064E0EE247AE0C72E0E9450AD20910F1E3C |
:1002A0003091101E4091111E5091121E8CE064E09E |
:1002B000EE24EA940E9450AD80E06FE14FE724E124 |
:1002C00001E00E94D5A883E090E00E947CCEAC01C2 |
:1002D0008CE067E020E00E947FAA82E167E043ECC7 |
:1002E00053E120E00E947FAA0E94ADA480E20E9418 |
:1002F000844C882321F580E10E94844C8823C1F03E |
:1003000084E0E3DE882311F401E013C080E991E08A |
:100310006FEF7FEF40E00E94D24C60E070E080E041 |
:1003200090E020E030E040E050E00E9434B907C0A7 |
:1003300000E00E94EF65882309F471CF73CF0E941B |
:10034000C74C0F91EF90CF9008951F93162F8823DD |
:1003500071F400D00F92EDB7FEB7118284E596E1FB |
:10036000938382830E945B430F900F900F9084E0F1 |
:100370006AE171E00E9401430E9491428430F9F4E5 |
:10038000812FA3DE8823B1F0123039F0133041F011 |
:10039000113041F40E946A6805C00E945F6802C083 |
:1003A0000E94576880E991E06FEF7FEF40E00E9484 |
:1003B000D24C07C00E94BBAF0E94E841DDCF88232A |
:1003C000D9F61F910895A8E0B0E0E9EEF1E80C94A9 |
:1003D00017D20E94BBAF83E50E947A43882309F4B9 |
:1003E000D4C000D00F9282E0ADB7BEB711968C9307 |
:1003F000119786EF93E113969C938E9312970E9428 |
:100400005B4337EE832E33E2932E00E0AA24BB2415 |
:100410000F900F900F9027ED222E23E1322E9CECAF |
:10042000492E93E1592E87EC682E83E1782EF40152 |
:100430008081882309F496C00E94C941B501B4E0C7 |
:10044000660F771FBA95E1F76A0D7B1D68517C4DE9 |
:10045000CE0101960E94D743C980DA80EB80FC80F0 |
:100460008DB79EB70F979EBF8DBF0DB71EB70F5F9D |
:100470001F4FADB7BEB711961C92F8013282218290 |
:10048000C701B60120E13EE040E050E00E94DAD131 |
:10049000D80114963C932E931397C701B6012CE311 |
:1004A00030E040E050E00E94DAD1CA01B9012CE30B |
:1004B00030E040E050E00E94DAD1F80176836583B5 |
:1004C000C701B6012CE330E040E050E00E94DAD1F1 |
:1004D000D80118967C936E9317978D8119968C93FB |
:1004E00019971A961C921A978E811B968C931B97BC |
:1004F0001C961C921C978F8198851E969C938E93B8 |
:100500001D970E945B43EDB7FEB73A96FEBFEDBF65 |
:100510003196ADB7BEB711961C9252824182D4017A |
:100520008C91838314820E945B43F4018081E82FC5 |
:10053000F0E0EE0FFF1FEE5EF04F85919491EDB766 |
:10054000FEB73196ADB7BEB711961C92728261822A |
:10055000948383830E945B4301E00F900F900F9080 |
:100560000F900F900894A11CB11CE1E1F0E08E0EF9 |
:100570009F1EF4E1AF16B10409F059CF0E94C941A2 |
:10058000802F62E0E2DE0E946D432896E2E10C9447 |
:1005900033D2A8E0B0E0EFECF2E80C9417D20E945E |
:1005A000BBAF8DE20E947A43882309F4FCC000D0DF |
:1005B0000F9282E0EDB7FEB781838CE394E19383E1 |
:1005C00082830E945B4375EFA72E72E2B72E00E094 |
:1005D000882499240F900F900F906DE1262E64E1EE |
:1005E000362E5EE0452E54E1552E41E0642E44E166 |
:1005F000742EF50180859185A285B3850097A105AC |
:10060000B10509F4BDC00E94C94189E190E0889E0E |
:10061000B001899E700D989E700D112463517D4D1F |
:10062000CE0101960E94D743C980DA80EB80FC801E |
:10063000EDB7FEB73F97FEBFEDBF0DB71EB70F5F1B |
:100640001F4F1182F801328221828D8183831482AF |
:100650008E81858316828F81988590878783C701D5 |
:10066000B60120E13EE040E050E00E94DAD1F8011E |
:1006700032872187C701B6012CE330E040E050E02B |
:100680000E94DAD1CA01B9012CE330E040E050E029 |
:100690000E94DAD1F80174876387C701B6012CE3A1 |
:1006A00030E040E050E00E94DAD1F80176876587BB |
:1006B0000E945B430F900F900F900F900DB71EB7E5 |
:1006C0000F5F1F4FEDB7FEB71182F80152824182D2 |
:1006D000F501648575858685978528EE33E040E0D1 |
:1006E00050E00E94FCD1F801238334834583568374 |
:1006F000F50160817181882777FD8095982F0E9490 |
:10070000B3CF20E030E040E05FE30E94D6D0F801B4 |
:1007100067837087818792870E945B430DB71EB7FE |
:100720000F5F1F4FEDB7FEB71182F8017282618231 |
:10073000F50160857185828593852AE030E040E08F |
:1007400050E00E94FCD1F801238334834583568313 |
:10075000F50164817581868197812AE030E040E06F |
:1007600050E00E94FCD1F8012783308741875287EF |
:100770000E945B4301E08DB79EB70B969EBF8DBF75 |
:100780000894811C911CE9E1F0E0AE0EBF1EFAE076 |
:100790008F16910409F02DCF0E94C941802F63E08C |
:1007A000D4DD0E946D432896E2E10C9433D22F925F |
:1007B0003F924F925F926F927F928F929F92AF92F1 |
:1007C000BF92CF92DF92FF920F931F93DF93CF934D |
:1007D00000D000D0CDB7DEB70E94BBAF8091BC2265 |
:1007E000880F880F865F0E947A43882309F425C109 |
:1007F000CE0101960E947C6800D00F92F2E0FF2E9D |
:10080000ADB7BEB71196FC9211978BED94E113969C |
:100810009C938E9312970E945B430F900F900F90C2 |
:10082000B4DBEDB7FEB73797FEBFEDBF3196ADB77E |
:10083000BEB711961C928AEC94E1928381830DE0FD |
:1008400010E0148303838091AC229091AD229683B3 |
:1008500085830E945B43EDB7FEB73196ADB7BEB757 |
:1008600011961C9289EB94E1928381838AE090E057 |
:10087000948383838091AE229091AF2264E670E0EE |
:100880000E94B3D1658316820E945B4300D0EDB70E |
:10089000FEB73196ADB7BEB711961C9282EA94E1CD |
:1008A0009283818314830383898190E001969683E8 |
:1008B00085838A81878310860E945B4300D0EDB7D1 |
:1008C000FEB73196ADB7BEB711961C928CE894E195 |
:1008D00092838183148303838B8190E001969683B6 |
:1008E00085838C81878310868EE190E09287818753 |
:1008F0000E945B43EDB7FEB73B96FEBFEDBF0E9483 |
:10090000C9418091BC22882309F492C000D00F9283 |
:10091000ADB7BEB71196FC92119783E894E1139698 |
:100920009C938E9312970E945B43FF240F900F902D |
:100930000F90EEE62E2EE4E13E2E7DE0C72ED12C68 |
:100940006BE5462E64E1562E5AE0652E712C4AE482 |
:10095000842E44E1942E3EE1A32EB12C64C08F2D51 |
:1009600090E0EDB7FEB73997FEBFEDBFADB7BEB7AC |
:100970001196118212963C922E921197019614961E |
:100980009C938E93139701971696DC92CE921597AF |
:100990008C01000F111F080F191F000F111FF80104 |
:1009A000E154FD4D808117968C93179718961C92F1 |
:1009B0000E945B430F900F90EDB7FEB73196ADB735 |
:1009C000BEB711961C9252824182748263820E5489 |
:1009D000124E025F1A4FD8018D919D918D0164E6F0 |
:1009E00070E00E94B3D1658316820E945B4300D001 |
:1009F000EDB7FEB73196ADB7BEB711961C92928295 |
:100A00008182D482C382D8018C9185831682B0867C |
:100A1000A7820E945B43EDB7FEB73996FEBFEDBFDC |
:100A20000E94C941F3948091BC22F81608F497CF34 |
:100A300081E061E08ADC0E946D432496DEBFCDBF79 |
:100A4000CF91DF911F910F91FF90DF90CF90BF90DA |
:100A5000AF909F908F907F906F905F904F903F905E |
:100A60002F900895A4E0B0E0E8E3F5E80C9423D2D9 |
:100A70000E94BBAF89E10E947A43882309F45AC2DD |
:100A8000CE0101960E947C6800D00F9282E0ADB743 |
:100A9000BEB711968C93119780E196E113969C93C3 |
:100AA0008E9312970E945B430F900F900F906DDA18 |
:100AB0002091962230919722EDB7FEB73797FEBF6F |
:100AC000EDBF3196ADB7BEB711961C9287EF95E199 |
:100AD00092838183C9016CE370E00E94B3D1748377 |
:100AE0006383C9016CE370E00E94B3D19683858370 |
:100AF0000E945B430DB71EB70F5F1F4FEDB7FEB7E8 |
:100B0000118287EE95E1D80112969C938E931197EE |
:100B10008DE0E82EF12C1496FC92EE9213976091E2 |
:100B20009C2270919D22882777FD8095982F0E94A6 |
:100B3000B3CF20E030E040EA51E40E9418CF0E9499 |
:100B400080CFF801768365830E945B43EDB7FEB7E3 |
:100B50003196ADB7BEB711961C9287ED95E19283A1 |
:100B60008183F482E3828091A8229091A9226AE095 |
:100B700070E00E94B3D1768365830E945B430DB71A |
:100B80001EB70F5F1F4FEDB7FEB7118281EC95E1E5 |
:100B9000D80112969C938E9311971496FC92EE9224 |
:100BA00013976091A2227091A32280E090E02AE541 |
:100BB00030E040E050E00E9488D12AEF30E040E091 |
:100BC00050E00E94DAD1F801368325830E945B430E |
:100BD0002091A6223091A722EDB7FEB73196ADB78E |
:100BE000BEB711961C928BEA95E192838183F482C1 |
:100BF000E38284E290E0AC01249FC001259F900D28 |
:100C0000349F900D112464E670E00E94C7D1768372 |
:100C100065830E945B438DB79EB707969EBF8DBFCD |
:100C20000E94C94100D000D00F92EDB7FEB73196B7 |
:100C3000ADB7BEB711961C928AE995E19283818384 |
:100C40008091B4229091B522948383830E945B4368 |
:100C500000D0EDB7FEB73196ADB7BEB711961C9276 |
:100C600089E895E192838183F482E3828091AC22CA |
:100C70009091AD22968385830E945B43EDB7FEB7CA |
:100C80003196ADB7BEB711961C9288E795E1928375 |
:100C900081838AE090E0948383838091AE22909157 |
:100CA000AF2264E670E00E94B3D1658316820E9491 |
:100CB0005B4300D0EDB7FEB73196ADB7BEB7119626 |
:100CC0001C9283E695E192838183F482E382898199 |
:100CD00090E00196968385838A81878310860E949F |
:100CE0005B4300D0EDB7FEB73196ADB7BEB71196F6 |
:100CF0001C928DE495E192838183F482E3828B815F |
:100D000090E00196968385838C81878310868EE19F |
:100D100090E0928781870E945B438091BA22EDB771 |
:100D2000FEB73B96FEBFEDBF8F3F19F420E030E0E9 |
:100D300002C0282F30E04DB75EB7475050405EBF2D |
:100D40004DBFEDB7FEB73196ADB7BEB711961C9249 |
:100D50008CE395E19283818380E190E094838383A7 |
:100D6000368325830E945B43EDB7FEB73796FEBFFF |
:100D7000EDBF0E94C9412DB73EB7275030403EBF5E |
:100D80002DBFEDB7FEB73196ADB7BEB711961C9229 |
:100D90008CE295E1928381838DE090E0948383835C |
:100DA0008091B022858316820E945B438091B1229C |
:100DB000EDB7FEB73796FEBFEDBF8F3F19F420E0C9 |
:100DC00030E002C0282F30E04DB75EB747505040AA |
:100DD0005EBF4DBFEDB7FEB73196ADB7BEB711964A |
:100DE0001C928CE195E19283818380E190E0948371 |
:100DF0008383368325830E945B4300D0EDB7FEB723 |
:100E00003196ADB7BEB711961C928EE095E19283F4 |
:100E100081838DE090E0948383838091B62299272B |
:100E200087FD9095968385838BE090E090878783FC |
:100E30000E945B438091B722EDB7FEB73996FEBFA3 |
:100E4000EDBF8E3719F420E030E004C0282F33279F |
:100E500027FD30954DB75EB7495050405EBF4DBF3E |
:100E6000EDB7FEB73196ADB7BEB711961C9280E0D4 |
:100E700095E19283818380E190E09483838336833C |
:100E800025830BE010E0108707830E945B43EDB7DA |
:100E9000FEB73196ADB7BEB711961C9282EF94E1C2 |
:100EA000928381838DE090E0948383838091B82244 |
:100EB000992787FD909596838583108707830E94E5 |
:100EC0005B438091B922EDB7FEB73996FEBFEDBF07 |
:100ED0008E3719F420E030E004C0282F332727FD97 |
:100EE00030954DB75EB7495050405EBF4DBFEDB72E |
:100EF000FEB73196ADB7BEB711961C9284EE94E161 |
:100F00009283818380E190E0948383833683258379 |
:100F10008BE090E0908787830E945B43EDB7FEB73C |
:100F20003996FEBFEDBF0E94C94181E061E00DDA54 |
:100F30000E946D432496E6E00C943FD2CF93DF935A |
:100F4000E82FA6E7B5E180E1ED0119928A95E9F76E |
:100F5000EE2339F4CD0170E040E150E00E940DD560 |
:100F600004C0F0E0EB58FA4E60838DB79EB7079748 |
:100F70009EBF8DBFEDB7FEB7319684E7ADB7BEB764 |
:100F800011968C9381E08183828386E795E1948337 |
:100F9000838380E190E0968385830E942D4ACDB7BC |
:100FA000DEB72796DEBFCDBFDF91CF910895EF92D8 |
:100FB000FF920F931F93182F0E94BBAF81E0809385 |
:100FC000EC178093FE171092FD170E94BBAF8CE8C0 |
:100FD00096E162E00E9488AA82E062E041E856E180 |
:100FE00020E00E947FAA82E063E046E756E120E02D |
:100FF0000E947FAA88E068E240E026E001E00E94CB |
:101000001BA782E090E00E947CCEAC0180E067E00C |
:1010100020E00E947FAA82E167E042E756E120E0FB |
:101020000E947FAA80E062E040E756E120E00E9453 |
:101030007FAA123051F480919508813031F480916B |
:101040009608813011F40E942A4B80E7F82E809197 |
:10105000EC17882369F580E40E94844C882331F4DE |
:1010600080E463E00E94BB4C882311F14091FD179E |
:101070004E3FF0F44F5F4093FD1789E063E050E08E |
:1010800023E030E000E0EE240E946BAF123081F4E8 |
:101090008091FD178F9DC00111246FEF70E00E94B9 |
:1010A000C7D1462F88E068E226E001E00E941BA736 |
:1010B0008091EC17882309F040C080E80E94844C9E |
:1010C000882331F480E863E00E94BB4C8823A9F1B7 |
:1010D0008091FD17882389F1123079F48F9DC0012A |
:1010E00011246FEF70E00E94C7D1862F885F68E2FD |
:1010F00040E026E000E00E941BA74091FD17415010 |
:101100004093FD1789E063E050E023E030E000E029 |
:10111000EE240E946BAF123081F48091FD178F9DF9 |
:10112000C00111246FEF70E00E94C7D1462F88E004 |
:1011300068E226E001E00E941BA78091EC17813055 |
:10114000D9F480E40E94844C882331F480E461E087 |
:101150000E94BB4C882381F04091FE17403160F41F |
:101160004F5F4093FE1789E062E050E023E030E0FB |
:1011700000E0EE240E946BAF8091EC17813041F5C6 |
:1011800080E80E94844C882331F480E861E00E946A |
:10119000BB4C8823E9F04091FE174423C9F041502D |
:1011A0004093FE17442349F089E062E050E023E0D9 |
:1011B00030E000E0EE240E946BAF8091FE178823A0 |
:1011C00039F489E062E04CE656E120E00E947FAA13 |
:1011D00080E10E94844C882329F18091EC178823B8 |
:1011E00089F462E04AE656E120E00E947FAA80E0AE |
:1011F00063E048E656E120E00E947FAA81E0809308 |
:10120000EC1710C080E062E046E656E120E00E9464 |
:101210007FAA80E063E044E656E120E00E947FAAD6 |
:101220001092EC178091FE176091FD1787DE80E227 |
:101230000E94844C882309F40ACF80EF0E94844CDA |
:10124000123019F480E060E079DE1F910F91FF9079 |
:10125000EF9008950F931F930E943136982F81507D |
:101260008E3F08F04DC09E3F89F0A2ECBEE1E0E069 |
:1012700004C0EF5F1196EA3019F08C918917C9F715 |
:10128000F0E0EE53F14E8081891721F0A2ECBEE12F |
:10129000E0E00EC08CE291E06FE070E040E00E9480 |
:1012A000D24C86E690E01FC0EF5F1196EA3021F045 |
:1012B0008C918823C9F70AC084EF91E06FE070E059 |
:1012C00040E00E94D24C85E690E00DC0F0E0EE5385 |
:1012D000F14E908389E190E06FEF7FEF40E00E9454 |
:1012E000D24C84E690E00E947CCEBC0180E991E083 |
:1012F00040E050E020E030E000E010E00E94CE63EB |
:101300001F910F9108958091B51D882329F4409174 |
:1013100006185091071804C04091081850910918F8 |
:1013200080E062E020E00C947FAA1F93E2EAF6E1FD |
:101330001491181721F01F3F19F03B96F9CF182F81 |
:101340008FEF97E1BF014BE050E00E94AAD460917B |
:1013500002187091031885E198E144E150E00E9481 |
:10136000CED480E01F3F09F081E01F910895EF92F5 |
:10137000FF920F931F93F82EE62E142F8DB79EB772 |
:1013800007979EBF8DBFEDB7FEB7319689E298E112 |
:10139000ADB7BEB712969C938E93119784E182836A |
:1013A00086EA9FE194838383F58216820E948A47AE |
:1013B0008DB79EB707969EBF8DBF112309F461C0FC |
:1013C0000E94BBAF81E06EE97FE142E020E00DEFDB |
:1013D0000E948BAA82E06FE87FE140E020E000E01D |
:1013E0000E948BAAADB7BEB71797BEBFADBFEDB712 |
:1013F000FEB7319611961C9284E08183128282E8B6 |
:101400009FE194838383F58216820E94E7ACEDB757 |
:10141000FEB73196ADB7BEB711961C9285E08183B9 |
:10142000128287E79FE194838383E58216820E947C |
:10143000E7AC8DB79EB707969EBF8DBF8DE090E05D |
:101440000E947CCEAC018BE067E020E00E947FAA86 |
:1014500084EF91E06FE070E040E00E94D24C04EF36 |
:1014600011E009C08CED95E0F8013197F1F7019793 |
:10147000D9F70E94EF6580E20E94844C882391F3A3 |
:1014800007C080EA90E06FE070E040E00E94D24C3C |
:101490001F910F91FF90EF9008950F931F93CF939B |
:1014A000DF93EC010E94C73D0E944B3780E00E9411 |
:1014B0002F3588E660E070E04AE259E80E94713614 |
:1014C00000E01FC01E3F31F40E3FC1F08EEF0E94BE |
:1014D0001C3D15C0812F29DF8091FF178F3F29F414 |
:1014E00081E0612F41E043DF22C00E9443968823C0 |
:1014F00029F08091FF170E944C3D01C0102F2196CA |
:10150000012F18811F3FF1F680E00E94783E0E9473 |
:101510005936853061F00E943136182F06DFE09190 |
:101520000018F0910118812F61E00995EDCF0E941C |
:10153000A637DF91CF911F910F910895EF92FF92FF |
:101540000F931F93CF93DF938091C21E882361F482 |
:1015500087E690E00E947CCE60E044EF51E021E01D |
:1015600001E00E94A86657C00E94C73D0E944B3709 |
:1015700080E00E942F3581E00E94EA3681E00E94DF |
:10158000013782ECE82E8EE1F82EE70122C01E3FE3 |
:1015900021F48EEF0E941C3D17C0812FC6DE809182 |
:1015A000FF178F3F29F481E0612F41E0E0DE31C079 |
:1015B000812F0E944C3D812F0E944396882321F465 |
:1015C000812F60E00E942E3821968EE1CC3CD80716 |
:1015D00019F018811123D9F680E00E94783E0E940C |
:1015E0005936853061F00E943136182F9EDEE09129 |
:1015F0000018F0910118812F61E00995EDCF10E0FE |
:10160000812F0E94FF35F70181937F011F5F1A3000 |
:10161000B9F70E94A637CDB7DEB7E6E00C943FD20B |
:101620000F931F938B010E94C73DC8010E9418377A |
:1016300080E00E942F3581E061E040E050E022EE42 |
:101640003DE10CEE1DE10E94A73D0E94343D82E089 |
:1016500061E040E050E026EF3DE10DEF1DE10E942A |
:10166000A73D83E061E040E050E025E03EE103E19A |
:101670001EE10E94A73D84E061E040E050E021E2ED |
:101680003EE189010E94A73D85E061E040E050E035 |
:1016900027E23EE189010E94A73D86E061E040E04B |
:1016A00050E02FE23EE104E31EE10E94A73D87E007 |
:1016B00061E040E050E02DE33EE104E41EE10E94E1 |
:1016C000A73D88E061E040E050E02BE43EE1890185 |
:1016D0000E94A73D89E061E040E050E025E53EE161 |
:1016E0000EE51EE10E94A73D8AE061E040E050E087 |
:1016F00026E63EE104E71EE10E94A73D8BE061E0A3 |
:1017000040E050E022E83EE189010E94A73D8CE0E4 |
:1017100061E040E050E027E83EE100E91EE10E9480 |
:10172000A73D8DE061E040E050E025E93EE102EABE |
:101730001EE10E94A73D8FE061E040E050E02DEA0D |
:101740003EE189010E94A73D80E00E94783E0E9410 |
:101750005936853009F440C00E94313690E0FC01D2 |
:101760003197EF30F10570F5E259FF4F0C944ED2EE |
:10177000E5DEEACF86E093E023C089E193E020C074 |
:1017800082E293E01DC088E293E01AC083E393E015 |
:1017900017C089E493E014C089E693E011C088E89B |
:1017A00093E00EC089E993E00BC080EB93E008C0A2 |
:1017B00081EC93E005C08AEC93E002C08FEC93E0EB |
:1017C0006CDEC2CF88EB9EE160E048EC50E021E0A7 |
:1017D00001E00E94A866B8CF0E94A6371F910F9122 |
:1017E0000895A0E0B0E0E7EFFBE80C9422D2D82EF9 |
:1017F0007B010E940797082F10E00F5F1F4FC80161 |
:101800000E9454D2EC01009759F48AED9DE161E009 |
:1018100040E253E021E001E00E94A86680E01BC0A6 |
:101820006091E7057091E805A8010E9404D58D2D0F |
:10183000B701F6DE6091E7057091E805CE01A801D9 |
:101840000E94F7D48C01CE010E9401D380E00115E3 |
:10185000110509F081E0CDB7DEB7E7E00C943ED288 |
:10186000A0E0B0E0E6E3FCE80C9422D2F82E062FCC |
:101870005CDD8091FF178F3F41F440E0013009F4B7 |
:1018800041E08CE06F2D73DDB9C08F2D0E9457961B |
:10189000182F013031F18DB79EB707979EBF8DBFCE |
:1018A000EDB7FEB7319689E298E1ADB7BEB71296B3 |
:1018B0009C938E93119784E182838CE19FE19483C2 |
:1018C0008383812F90E0880F991F880F991F96833B |
:1018D00085830E948A478DB79EB707969EBF8DBFAE |
:1018E0008DC00E94BBAF02E034E0D32ECFE0DFE139 |
:1018F000023031F580E060E04FE727E001E00E9430 |
:101900001BA781E060E045E15FE122E000E0EE241A |
:101910000E9471AA81E090E00E947CCEAC0180E040 |
:1019200067E020E00E947FAA85E090E00E947CCEE4 |
:10193000AC018BE067E020E00E947FAAE4DC01E0DC |
:101940000E94ADA40130E9F4ADB7BEB71797BEBF92 |
:10195000ADBFEDB7FEB7319611961C92D1821282BF |
:10196000D483C383812F90E0880F991F880F991F1C |
:10197000968385830E94E7AC8DB79EB707969EBF7E |
:101980008DBF80E20E948E4C8823C1F580E10E94C9 |
:101990008E4C882329F08F2D612F0E94A1962EC096 |
:1019A00080E80E948E4C882341F480E862E00E9427 |
:1019B000BB4C882311F400E006C01F3B10F41EEB63 |
:1019C00001C0115001E080E40E948E4C882331F464 |
:1019D00080E462E00E94BB4C882321F01F3F09F0A5 |
:1019E0001F5F01E080EF62E00E94BB4C0E94EF6548 |
:1019F000882309F47DCF0E94BBAF7CCFCDB7DEB783 |
:101A0000E7E00C943ED2A0E0B0E0E9E0FDE80C9401 |
:101A10001FD2F82E062F89DC8091FF178F3F41F4EB |
:101A200040E0013009F441E08CE06F2DA0DCE5C01E |
:101A30008F2D0E945796182F90E0E6E09695879597 |
:101A4000EA95E1F7912F9F77BB24B394B82280FFEA |
:101A500009C091959F77892F90E0CC27DD27C81B7F |
:101A6000D90B02C0C92FD0E0013001F18DB79EB76C |
:101A700007979EBF8DBFEDB7FEB7319689E298E11B |
:101A8000ADB7BEB712969C938E93119784E1828373 |
:101A90008DE29FE194838383D683C5830E948A4726 |
:101AA0008DB79EB707969EBF8DBFA7C00E94BBAFE4 |
:101AB00002E074E0A72E60E2C62E6FE1D62E02305F |
:101AC00031F580E060E04FE727E001E00E941BA7CE |
:101AD00081E060E046E25FE122E000E0EE240E9467 |
:101AE00071AA81E090E00E947CCEAC0180E067E0CA |
:101AF00020E00E947FAA85E090E00E947CCEAC01AD |
:101B00008BE067E020E00E947FAAFDDB01E00E94FD |
:101B1000ADA40130B9F4ADB7BEB71797BEBFADBF26 |
:101B2000EDB7FEB7319611961C92A1821282D48233 |
:101B3000C382D683C5830E94E7AC8DB79EB7079654 |
:101B40009EBF8DBF80E20E948E4C882309F055C055 |
:101B500080E10E948E4C8823D9F020976C2F8C2F27 |
:101B600090E056E0969587955A95E1F78170111FA0 |
:101B70001127111F1B1519F4882329F002C088238F |
:101B800011F4606801C06F778F2D0E94A19635C057 |
:101B900080E80E948E4C882341F480E862E00E9435 |
:101BA000BB4C882311F400E009C09FEFC53CD90766 |
:101BB0001CF4CCE3D0E001C0219701E080E40E9456 |
:101BC0008E4C882331F480E462E00E94BB4C882371 |
:101BD00041F0CC33D1051CF0C4ECDFEF01C02196FD |
:101BE00001E080EF62E00E94BB4C0E94EF65882319 |
:101BF00009F465CF0E94BBAF64CFCDB7DEB7EAE092 |
:101C00000C943BD2A0E0B0E0E8E0FEE80C941AD2DD |
:101C1000D82E062F8ADB8091FF178F3F41F440E0DA |
:101C2000013009F441E08CE06D2DA1DB08C10130E9 |
:101C300029F18D2D0E9457962DB73EB72750304081 |
:101C40003EBF2DBFEDB7FEB7319629E238E1ADB703 |
:101C5000BEB712963C932E93119794E1928324E69B |
:101C60003FE134832383858316820E948A472DB700 |
:101C70003EB7295F3F4F3EBF2DBFE1C00E94BBAFC3 |
:101C80008D2D0E945796C82E02E010E02FE0522EB4 |
:101C900091E3692E9FE1792E87E0882E912CB1E0A7 |
:101CA000AB2EB12C0230E9F580E060E04FE727E091 |
:101CB00001E00E941BA781E060E04DE55FE122E0CA |
:101CC00000E0EE240E9471AA81E067E048E45FE151 |
:101CD00020E00E947FAA85E090E00E947CCEAC01CB |
:101CE0008BE067E020E00E947FAA87E066E046E4A0 |
:101CF0005FE120E00EEFA2E0EA2E0E9471AA30E43C |
:101D0000D31619F082E4D81651F48AE065E04AE36C |
:101D10005FE120E000E0F2E0EF2E0E9471AAF3DA2A |
:101D200001E00E94ADA4013009F040C0C0E0D0E065 |
:101D300008E0EC2CFF24C7010C2E02C09595879576 |
:101D40000A94E2F780FD02C040E301C041E3802F26 |
:101D500064E020E00E9460A82196015061F780E0D5 |
:101D600068E24BE327E00E941BA7812F8F5F65E0AD |
:101D700048E35FE120E00E947FAAADB7BEB71797A6 |
:101D8000BEBFADBFEDB7FEB7319611965C9284E051 |
:101D90008183128274826382F682E5820E94E7ACBC |
:101DA0002DB73EB7295F3F4F3EBF2DBF80E20E9457 |
:101DB0008E4C882309F043C080E10E948E4C88231A |
:101DC00021F08D2D6C2DBDD73AC080E40E948E4C41 |
:101DD000882311F400E007C0173019F401E010E087 |
:101DE00002C01F5F01E080E40E949F4C882339F00D |
:101DF000112319F401E017E002C0115001E080E85E |
:101E00000E948E4C882379F0812F992787FD909529 |
:101E1000D401A81BB90B950102C0220F331FAA954C |
:101E2000E2F7C22601E080EF62E00E94BB4C0E9414 |
:101E3000EF65882309F436CF0E94BBAF35CFCDB70D |
:101E4000DEB7EFE00C9436D2CF92DF92EF92FF92A2 |
:101E50001F93CF93DF93182F6B01EA0179018091D3 |
:101E6000FF17811711F0812F60DA8091FF178F3FE4 |
:101E700019F48FE1612F20C0812F19D7F6018083DB |
:101E8000A5E1B8E14BE050E06C9130E001C03F5F6C |
:101E9000832F90E0849FF001859FF00D949FF00DBB |
:101EA0001124E856FE4F8081861719F08F3F79F78D |
:101EB00002C06F3F29F480E240E059DA80E039C087 |
:101EC000663731F48091041888838091051803C027 |
:101ED000818188838281F70180832881F701908145 |
:101EE000F6018081921728F0821738F0981728F0B1 |
:101EF0000CC0891710F0281740F411968C91882394 |
:101F000019F681E2F6016081D7CF8BE0389FF001AE |
:101F10001124AAE0B8E1E856FE4F01900D928150DD |
:101F2000E1F7888180930B18F701808180930C186A |
:101F300081E0DF91CF911F91FF90EF90DF90CF90E4 |
:101F40000895A3E0B0E0E7EAFFE80C9425D2082F5B |
:101F5000162FBE016F5F7F4FAE014E5F5F4F9E0138 |
:101F60002D5F3F4F71DF882309F481C02981909153 |
:101F70000A18E5E1F8E101C0319680818917E1F79F |
:101F80004A818B81841708F41195612F772767FDAB |
:101F90007095620F711D242F30E0841720F06217B6 |
:101FA000730724F030C02617370794F588E1E53130 |
:101FB000F80741F49F012F5F3F4F8181882319F07B |
:101FC000F901F8CF3197308120E04BE050E001C0BB |
:101FD0002F5F822F90E0849FF001859FF00D949FEA |
:101FE000F00D1124E856FE4F8081831719F08F3FC2 |
:101FF00079F73DC03F3FD9F1363719F48091051884 |
:1020000031C082812FC090E0861797072CF02BC03B |
:1020100090E0681779073CF5DF01119681818823EC |
:1020200011F4A5E1B8E13C9120E04BE050E001C0A3 |
:102030002F5F822F90E0849FF001859FF00D949F89 |
:10204000F00D1124E856FE4F8081831719F08F3F61 |
:1020500079F70DC03F3F59F0363719F480910418D5 |
:1020600001C08181682F802F6CD681E001C080E0A3 |
:102070002396E4E00C9441D2A3E0B0E0E2E4F0E97E |
:102080000C9426D2162FBE016F5F7F4FAE014E5FBC |
:102090005F4F9E012D5F3F4FD7DE882309F456C066 |
:1020A00080910A182B819A81863741F4291788F488 |
:1020B00089E2698140E05BD980E048C089812917C5 |
:1020C00018F0891B898302C0981B998389818F5FCF |
:1020D0008983133071F48091B51D882329F42091F0 |
:1020E00011183091121812C0209113183091141841 |
:1020F0000DC08091B51D882329F420910D183091D1 |
:102100000E1804C020910F18309110188DB79EB78B |
:1021100007979EBF8DBFEDB7FEB7319689E298E174 |
:10212000ADB7BEB712969C938E93119784E18283CC |
:10213000348323838981858316820E948A4781E0C4 |
:102140002DB73EB7295F3F4F3EBF2DBF2396E3E03B |
:102150000C9442D2A0E0B0E0E0EBF0E90C941DD288 |
:10216000182F613011F088DFB1C0DFD88091FF17E0 |
:102170008F3F29F48BE0612F41E0F9D8A7C00E947E |
:10218000BBAF812F94D5982E02E0FF2454E0852E1A |
:1021900049E6A42E4FE1B42E39E2C32E38E1D32E06 |
:1021A000C4EFD1E0023031F580E060E04FE727E096 |
:1021B00001E00E941BA781E060E040E75FE122E0D0 |
:1021C00000E0EE240E9471AA81E090E00E947CCEA3 |
:1021D000AC0180E067E020E00E947FAA85E090E00B |
:1021E0000E947CCEAC018BE067E020E00E947FAAD9 |
:1021F0008AD801E00E94ADA4013089F5812F63E007 |
:102200003BDF2DB73EB7275030403EBF2DBFEDB767 |
:10221000FEB73196ADB7BEB711961C92818212827D |
:10222000B482A382D682C5820E94E7AC80910C184A |
:1022300090E020910B18821B910944962DB73EB770 |
:10224000295F3F4F3EBF2DBF899738F480ED97E05F |
:10225000FE013197F1F70197D9F700E0FF2480E202 |
:102260000E94844C882321F0812F692D6AD52EC0CD |
:1022700080E10E94844C882349F580E40E94844CCC |
:10228000882331F480E462E00E94BB4C882311F083 |
:10229000FF24F39480E80E94844C8823A1F480E812 |
:1022A00062E00E94BB4C882371F4FF2021F0812F53 |
:1022B0006F2D47DE01E00E94EF65882309F472CF9D |
:1022C0000E94BBAF71CFFF24FA94F1CFCDB7DEB738 |
:1022D000ECE00C9439D20F931F93082F0E94C73D56 |
:1022E00082E1089F80011124C801855D974E0E94FC |
:1022F000183780E00E942F3581E00E94D33681E1BB |
:1023000090E00E947CCE9C018EE161E040E050E0D4 |
:102310000E948E3D83E190E00E947CCE9C018FE183 |
:1023200061E040E050E00E948E3D80E00E94783EF7 |
:102330000E945936853011F400E003C00E94313606 |
:10234000082F0E94A63781E00E942F35802F1F9111 |
:102350000F9108951F93182F0E94C73D82E1189F87 |
:10236000C0011124855D974E0E94183780E00E94BD |
:102370002F3581E00E94D3368EEF85D4882359F41F |
:102380008CE590E00E947CCE9C0184E161E040E01D |
:1023900050E00E948E3D163059F089E390E00E9493 |
:1023A0007CCE9C0185E161E040E050E00E948E3DE2 |
:1023B00084E190E00E947CCE9C0186E161E040E0F7 |
:1023C00050E00E948E3D80E00E94783E0E94593687 |
:1023D000853011F410E003C00E943136182F0E949E |
:1023E000A63781E00E942F35812F1F910895FF921B |
:1023F0000F931F93182FF62E0AD5082F8091AA1835 |
:102400009091AB18892B49F4802F90E001960E949F |
:1024100054D28093AA189093AB184091AA18509167 |
:10242000AB184115510559F482EC90E361E040E2AC |
:1024300053E021E001E00E94A86680E01EC0202F4A |
:1024400030E06091E7057091E805163061F4809105 |
:10245000C7188093A9182F5F3F4FCA01A9010E9496 |
:1024600004D586E00AC02F5F3F4FCB01BA01A90116 |
:102470000E9404D5812F6F2D78D61F910F91FF9068 |
:102480000895A0E0B0E0E7E4F2E90C941BD2C82E76 |
:102490000E94BBAF82E1C89EE0011124C55DD74E0A |
:1024A000FC2C11E0DD2485E0982EB3E08B2EADEB03 |
:1024B000AA2EA0E3BA2EF4E07F2EE2E16E2E1123C5 |
:1024C00009F476C080E060E04FE727E001E00E9479 |
:1024D0001BA78AE390E00E947CCEAC0181E060E023 |
:1024E00022E00E947FAA8DE590E00E947CCEAC01A4 |
:1024F00080E062E020E000E0EE24EA940E9471AA0D |
:102500008DB79EB709979EBF8DBFEDB7FEB73196C9 |
:10251000ADB7BEB711969C9282E08183128283820E |
:102520008FEF8483B682A582D087C7830E940AADCD |
:102530008DB79EB709969EBF8DBF8EE169E14AE5D2 |
:1025400027E00E941BA78EE590E00E947CCEAC01A4 |
:1025500080E064E020E079EFE72E0E9471AAADB739 |
:10256000BEB71997BEBFADBFEDB7FEB73196119696 |
:102570009C9271821282838289EF8483B682A582C3 |
:10258000F69CC0011124855D974E908787830E9439 |
:102590000AAD8DB79EB709969EBF8DBF81E090E0D2 |
:1025A0000E947CCEAC0180E067E020E00E947FAA20 |
:1025B00080E20E94844C882309F0B6C080E40E9427 |
:1025C000844C882311F410E009C096E0F91621F438 |
:1025D00011E0FF24F39402C0F39411E080E80E941C |
:1025E000844C882349F0A1E0FA1621F411E046E07A |
:1025F000F42E02C0FA9411E080E10E94844C8823FA |
:1026000009F48AC08BE390E00E947CCEBC0185E097 |
:1026100040E020E001E00E948BAA80E065E24FE705 |
:102620002EE032E0E32E0E9411A980E068E34FE73C |
:1026300027E000E00E941BA780E290E00E947CCE91 |
:10264000AC018CE067E020E00E947FAA89E190E085 |
:102650006FEF7FEF40E00E94D24C80E10E94844CFB |
:10266000882311F004E001C000E080E20E94844C65 |
:10267000811105E00E94EF65002379F3043009F031 |
:1026800046C0B6E0CB1699F08C2D64E10ED5D82E5D |
:102690008C1569F089E590E00E947CCE61E040E213 |
:1026A00053E021E001E00E94A866DD243FC086E0FF |
:1026B000F81619F46AE09BDE39C080E060E240E879 |
:1026C00020E200E00E941BA782E190E00E947CCE05 |
:1026D000BC0184E040E020E009E00E948BAA80E099 |
:1026E00065E24FE72EE001E092E0E92E0E9411A999 |
:1026F00096E0C91621F48F2D6EE179DE06C0A5E0C3 |
:10270000AF1550F08F2D6EE130D5D82E0FC00530AB |
:1027100019F40E94BBAF11E00E94EF65882309F411 |
:10272000CECE0E94BBAFCECE83E6D82E8D2DCDB7B8 |
:10273000DEB7EEE00C9437D283E0E9EAF8E1DF019E |
:102740001D928A95E9F787E998E16FE171E00C94B1 |
:102750001FD58091AA189091AB18009711F00E9494 |
:1027600001D3EACFA0E0B0E0E8EBF3E90C9422D289 |
:102770000DE318E1C1E0D0E020E1D22E9EE2E92E87 |
:1027800091E0F92E8C2F64E190D4882309F458C08D |
:102790008BE67ED22DB73EB7295030403EBF2DBFCD |
:1027A000EDB7FEB73196ADB7BEB712961C930E9338 |
:1027B0001197D282F482E382D683C5839087878380 |
:1027C0000E949E470E5E1F4F21962DB73EB7275F92 |
:1027D0003F4F3EBF2DBF38E10739130799F62091CF |
:1027E000AA183091AB182115310539F19091A9182B |
:1027F0008091C718981711F0ACDF1FC0C9010FD323 |
:102800008BE646D2ADB7BEB71797BEBFADBFEDB72B |
:10281000FEB7319612961C930E931197D28226E33F |
:1028200031E034832383968385830E949E472DB7AE |
:102830003EB7295F3F4F3EBF2DBF8FEF64E135D4D8 |
:10284000CDB7DEB7E7E00C943ED2EF92FF920F9344 |
:102850001F938AE590E00E947CCE60E040E050E06B |
:1028600021E001E00E94A8667DDF082F882361F443 |
:1028700089E590E00E947CCE61E040E253E021E0F7 |
:1028800001E00E94A866C8C00E94C73D0E944B3765 |
:1028900040E052E1E52E0EC0142F1F5F4E9D9001C7 |
:1028A0001124235C374E812F62E040E050E00E940B |
:1028B000753D412F8091AA189091AB18892B19F47E |
:1028C00025E030E002C026E030E0842F90E082175F |
:1028D000930714F3802F0E945A38802F61E00E94E2 |
:1028E000063842E1F42E80E00E94783E0E9459367C |
:1028F000853009F48FC00E943136182F8150863000 |
:1029000090F7812F27DD8431A9F5812F69E1CDD39F |
:102910001F9DB0011124655D774E812F0E94F18BC0 |
:10292000882309F31630F9F2812FD5DC8E31D9F6E0 |
:1029300080E068E340E827E000E00E941BA782E116 |
:1029400090E00E947CCEBC0184E040E020E009E001 |
:102950000E948BAA80E065E24FE72EE001E032E0C2 |
:10296000E32E0E9411A9812F62E3FFD3882309F08F |
:10297000BACF12C08531A1F58BE590E00E947CCEE4 |
:1029800060E040E050E021E001E00E94A866812F75 |
:1029900064D4182F882359F489E590E00E947CCEF6 |
:1029A00061E040E253E021E00E94A8669CCF81E014 |
:1029B00060E00E94063882E060E00E94063883E012 |
:1029C00060E00E94063884E060E00E94063885E0FE |
:1029D00060E00E940638812F61E00E94063883CFB4 |
:1029E000863109F080CF812F4CDD882361F489E5A1 |
:1029F00090E00E947CCE61E040E253E021E001E003 |
:102A00000E94A86603C0833609F46DCF0E94A637E2 |
:102A100081E003C00E94A63780E01F910F91FF90D4 |
:102A2000EF9008950F9300919508013009F045C08B |
:102A30000E94ED498DB79EB707979EBF8DBFEDB73A |
:102A4000FEB731968FE6ADB7BEB711968C9382E094 |
:102A5000818302838AE090E09483838381E090E025 |
:102A6000968385830E942D4A8FE480939408109268 |
:102A7000E50588EC90E090937F1680937E168DB7E5 |
:102A80009EB707969EBF8DBF8091E505882331F4E0 |
:102A900080917E1690917F16892BB1F78091E50584 |
:102AA000882379F00E949249E091E705F091E805CA |
:102AB000ED5BFF4F808180FD04C0C7DE8130E9F30C |
:102AC0000BC08AE190E00E947CCE60E040E951E0DA |
:102AD00021E001E00E94A8660F910895282F862F1B |
:102AE00090E036E0969587953A95E1F7982F9170AA |
:102AF000213049F4862F881F8827881F60E08917B6 |
:102B000071F061E00CC0223051F4442319F4992390 |
:102B100029F002C0992311F4606801C06F77862FF5 |
:102B20000895813069F460936D06862F90E0887176 |
:102B3000907043E0959587954A95E1F7682F0AC014 |
:102B4000823041F4642F660F660F660F80916D0628 |
:102B50008770682B862F0895CF93DF93DB01EA01FE |
:102B6000A9011C921882F9011082E4EEF8E3949115 |
:102B70009817E9F4349694913196249135979923D6 |
:102B800021F08091C7188917B0F0222321F080919D |
:102B9000C718281780F0319684918C9331968491D0 |
:102BA000888331968491FA01808381E005C09F3F3C |
:102BB00011F03696DCCF80E0DF91CF910895A1E04F |
:102BC000B0E0E5EEF5E90C9426D28983162FFA01E0 |
:102BD000D90110821C928091C618882329F199810D |
:102BE000892F805A8F3568F4892FBE016F5F7F4F20 |
:102BF000B3DF882331F4112311F48FEF1AC080E082 |
:102C000018C0E091AC18F091AD18398190E0249192 |
:102C1000319684913197231749F02F3F19F0980F7F |
:102C20003296F5CF3F3F11F09FEF80E0112309F47A |
:102C3000892F2196E3E00C9442D2A2E0B0E0E3E2D7 |
:102C4000F6E90C9427D2AE014F5F5F4F9E012E5FD5 |
:102C50003F4FB5DF2296E2E00C9443D2382F20E0BC |
:102C600003C0E31771F02F5F822F90E0FC01EE0F9D |
:102C7000FF1FE80FF91FE452FC4FE081EF3F89F797 |
:102C80002FEF822F089561E0D8DF811181E0089550 |
:102C900060E0D3DF8F3F19F420E030E006C02091E0 |
:102CA000AE183091AF18280F311DC9010895A2E068 |
:102CB000B0E0EDE5F6E90C9425D2082F60E0AE0116 |
:102CC0004F5F5F4F9E012E5F3F4F79DF8F3F11F4C3 |
:102CD00010E032C0E091AE18F091AF18E80FF11D8E |
:102CE00010818981813031F48A81812309F081E06A |
:102CF000182F0EC0823031F48A81812310E041F414 |
:102D000011E006C0833021F48A81E80FF11D1081A3 |
:102D1000802FA4DF8F3F81F090E0FC01EE0FFF1FBA |
:102D2000E80FF91FE452FC4F0180F281E02D81E0B1 |
:102D3000612F412F0995182F812F2296E4E00C94E2 |
:102D400041D2A2E0B0E0E7EAF6E90C9422D2D82E14 |
:102D5000E62E60E0AE014F5F5F4F9E012E5F3F4F5A |
:102D60002EDFF82E8FEFF81611F480E04CC0298189 |
:102D7000822F8150823010F01E2D18C0E091AE18C5 |
:102D8000F091AF18EF0DF11DE081223011F09E2D72 |
:102D900004C090E0EE2009F491E08A81992319F0B3 |
:102DA0001E2F182B03C0182F10951E23233011F04F |
:102DB00000E001C00A818D2D51DF8F3FD1F090E0FE |
:102DC000DC01AA0FBB1FA80FB91FA452BC4FE09192 |
:102DD000AE18F091AF18E00FF11DEF0DF11D119637 |
:102DE0002D913C91129782E060814E2DF901099559 |
:102DF000182FE091AE18F091AF18E00FF11DEF0D14 |
:102E0000F11D108381E02296E7E00C943ED2809180 |
:102E1000C618882319F08FEF60E00FCF0895FC01EA |
:102E20001092AD181092AC181092AF181092AE1804 |
:102E30001092C6181092C7181092C8181092C9188C |
:102E40000097B1F1DC018D918093C81831813093E6 |
:102E5000C71820E0822F90E0FC01EE0FFF1FE80F63 |
:102E6000F91FE056FD4F8081831721F08F3F01F15C |
:102E70002F5FF0CF3F3FE1F0B093AF18A093AE18B3 |
:102E8000818192819093AD188093AC1881E08093FA |
:102E9000C6188BE6FDDE009741F0A9ECB8E1FC0115 |
:102EA0008CE001900D928150E1F78091C618089551 |
:102EB00080E00895AF92CF92EF920F931F93DF932C |
:102EC000CF93CDB7DEB72A97DEBFCDBF8A2D0023C3 |
:102ED00021F4112311F480E014C00E1539F41C15EF |
:102EE00041F48156481741F482E00BC00E1540F4BE |
:102EF00005C01C1528F402C0481710F481E001C079 |
:102F000083E02A96DEBFCDBFCF91DF911F910F9155 |
:102F1000EF90CF90AF900895EF920F93982F41309C |
:102F200051F1423009F05CC03091B218332329F4DA |
:102F30008091B318882309F44EC04DB75EB74D5049 |
:102F400050405EBF4DBFEDB7FEB73196ADB7BEB7CF |
:102F500011969C93618322830383E4828CE89AE335 |
:102F600096838583378310868091B3188187128674 |
:102F70008091B61822C03091BC18332321F480917F |
:102F8000BD18882341F1ADB7BEB71D97BEBFADBF19 |
:102F9000EDB7FEB7319611969C936183228303832C |
:102FA000E4828BE79AE3968385833783108680914A |
:102FB000BD18818712868091C01890E08F599F4F6D |
:102FC000948783870E940AAD4DB75EB7435F5F4F1A |
:102FD0005EBF4DBF05C0892F46E85AE30E9471AA23 |
:102FE0000F91EF90089592E09093D6182DB73EB7C9 |
:102FF000275030403EBF2DBFEDB7FEB7319685E775 |
:10300000ADB7BEB711968C93918381E0828386ED34 |
:1030100098E19483838381E090E0968385830E9486 |
:103020002D4A84E0809396082AEF30E08DB79EB752 |
:1030300007969EBF8DBF44EF51E0CA010197F1F79B |
:1030400021503040D1F708951F931092D6188DB7B4 |
:103050009EB707979EBF8DBFEDB7FEB7319685E748 |
:10306000ADB7BEB711968C9312E0118381E08283D5 |
:1030700086ED98E19483838381E090E09683858355 |
:103080000E942D4A109396082AEF30E08DB79EB724 |
:1030900007969EBF8DBF44EF51E0CA010197F1F73B |
:1030A00021503040D1F71F910895A1E0B0E0EBE549 |
:1030B000F8E90C941ED28983162F81E58093940839 |
:1030C0001092E50521E7922E01E05E010894A11C13 |
:1030D000B11C91E0C92ED12C89E1E82EF12C29C038 |
:1030E000B3DF8DB79EB707979EBF8DBFEDB7FEB715 |
:1030F0003196ADB7BEB711969C9201830283B4821C |
:10310000A382D682C5820E942D4AF0928D16E0924B |
:103110008C168DB79EB707969EBF8DBF80918C167B |
:1031200090918D16892B21F08091E5058823B1F3CC |
:1031300011508091E505882311F4112389F610922E |
:10314000940880E090E06BDE8091E505882339F0FB |
:103150000E9492498091E7059091E80560DE809198 |
:10316000C8182196EBE00C943AD2A1E0B0E0EBEB6A |
:10317000F8E90C941AD28983E62E8091C618882328 |
:1031800009F467C083E5809394081092E505FF2455 |
:1031900063E7562E6624639452E0752E4E01089420 |
:1031A000811C911C41E0A42EB12C36E4C32ED12CFD |
:1031B00044C00091E7051091E8050F5F1F4F27DE1F |
:1031C0002DB73EB72B5030403EBF2DBFEDB7FEB7F9 |
:1031D0003196ADB7BEB711965C92618272829482CD |
:1031E0008382B682A58210870783818712860E9418 |
:1031F0002D4AD0928D16C0928C162DB73EB7255F02 |
:103200003F4F3EBF2DBF80918C1690918D16892B1C |
:1032100021F08091E5058823B1F38091E5058823AD |
:1032200059F00E949249E091E705F091E805F0809D |
:10323000FF2011F41092E505EA948091E5058823BA |
:1032400029F4EE2019F0FF2009F4B3CF8FEF6FE0DF |
:103250002CDF2196EFE00C9436D21F93182F64E1F7 |
:1032600024DF811711F080E002C062E37EDF1F914E |
:1032700008958BE10E948A498BE10E948A4985E585 |
:103280000E948A498AEA0E948A4980E00E948A490B |
:1032900081E0809396088AEF90E024EF31E0F90115 |
:1032A0003197F1F70197D9F70895A2E1B0E0EBE586 |
:1032B000F9E90C9423D2182F062FF42EE22E209138 |
:1032C000C8182223F9F488E590E00E947CCE2DB73F |
:1032D0003EB7275030403EBF2DBFEDB7FEB7319609 |
:1032E0009E012F5F3F4FADB7BEB712963C932E9312 |
:1032F000119722E1228324E73AE3348323839683E0 |
:1033000085833EC08091C618AE014F5F5F4F882312 |
:1033100009F18DB79EB709979EBF8DBFEDB7FEB778 |
:103320003196ADB7BEB712965C934E93119782E17A |
:1033300082838EE69AE3948383832583168289ECC5 |
:1033400098E1908787830E948A472DB73EB7275F11 |
:103350003F4F1CC08DB79EB707979EBF8DBFEDB77F |
:10336000FEB73196ADB7BEB712965C934E931197E8 |
:1033700082E182838BE69AE394838383258316829A |
:103380000E948A472DB73EB7295F3F4F3EBF2DBFF2 |
:10339000812FBE016F5F7F4F402F2F2D0E2D0E947A |
:1033A00080AF6296E6E00C943FD2A0E0B0E0EBED97 |
:1033B000F9E90C9422D2D82E86E2E0EBF8E1DF01A5 |
:1033C0001D928A95E9F755DF84EF91E024EF31E013 |
:1033D000F9013197F1F70197D9F74BDF86E5809333 |
:1033E00094081092E50591E010E0F6E7FF2EC0E1A9 |
:1033F000D0E06AC0992361F10E946E648FE590E08D |
:103400000E947CCEBC0182E040E020E002E00E940D |
:103410008BAA8CE090E00E947CCEAC0180E063E05F |
:1034200020E000E0E8E0EE2E0E9471AA8DE090E03E |
:103430000E947CCEAC018CE067E020E00E947FAA75 |
:1034400080E06AE24EE728E001E00E94D5A812E0A1 |
:103450000E94ADA400D00F92EDB7FEB73196ADB784 |
:10346000BEB71196FC92118212820E942D4AD0930F |
:103470007F16C0937E160F900F900F9080917E164E |
:1034800090917F16892BD1F782E06CE2412F24E0E6 |
:1034900001E00E941BA71E5F1D3740F082E06CE236 |
:1034A0004BE724E000E00E941BA710E080E20E94AE |
:1034B000844C882321F01092950810E0BDC00E9432 |
:1034C000EF65811181E0982F8091E505882309F44B |
:1034D00091CF8091E505882311F410E081C00E940E |
:1034E000924980919114823611F010E016C082E06A |
:1034F000809395088093960881E08093B018A2EBA2 |
:10350000B8E12091E7053091E805C901FC018AE0A6 |
:1035100001900D928150E1F711E080919114833672 |
:10352000A9F481E080939508809396088093B11860 |
:10353000ACEBB8E12091E7053091E805C901FC0149 |
:103540008AE001900D928150E1F711E08091950899 |
:10355000813009F045C078DD86E5809394081092AB |
:10356000E50508E276E7F72EEE24E394C4E1D0E027 |
:103570001BC000D00F92EDB7FEB73196ADB7BEB706 |
:103580001196FC92E18212820E942D4AD0937F16FE |
:10359000C0937E160F900F900F9080917E169091A1 |
:1035A0007F16892BD1F701508091E505882311F40E |
:1035B0000023F9F68091E505882391F00E94924955 |
:1035C00081E08093B018A2EBB8E12091E70530913B |
:1035D000E805C901FC018AE001900D928150E1F7F4 |
:1035E0008FEF6FE062DD109294081092E5058091F4 |
:1035F000B118882309F03DDE8091C618882361F058 |
:1036000089E190E06FEF7FEF40E00E94D24CDD2037 |
:1036100081F084EF91E00BC088EE93E06FEF7FEFD5 |
:1036200040E00E94D24CDD2021F080E496E061E091 |
:1036300009D080EF0E94844C812FCDB7DEB7E7E040 |
:103640000C943ED2A0E0B0E0E8E2FBE90C9420D27A |
:10365000EC011091B018F090B118009721F0909300 |
:103660007F1680937E168EE591E09093911680935D |
:10367000901601E051E1B52E45E5C42E4AE3D42E63 |
:10368000002309F4AFC00E94BBAF0E94ADA480E04C |
:1036900060E045E628E001E00E941BA781E060E0D1 |
:1036A00043E65AE322E000E0EE240E9471AA8091F2 |
:1036B000B018882329F48091B118882309F473C0C5 |
:1036C00080E06EE14FE726E101E032E0E32E0E9468 |
:1036D00068A880E062E04FE55AE320E004E09BEF59 |
:1036E000E92E0E9471AA84E062E042E020E002E05C |
:1036F00013DC8CE062E04AE55AE320E005E00E943A |
:1037000071AA8DB79EB709979EBF8DBFEDB7FEB763 |
:103710003196ADB7BEB71196BC9282E0818312821A |
:1037200083838BEF8483D682C5828091C718878379 |
:1037300010860E940AAD8DB79EB709969EBF8DBFB9 |
:103740008091C618882351F48BE063E04BE45AE380 |
:1037500022E0BCEFEB2E0E9471AA07C087E464E16F |
:1037600046E328E000E00E941BA780E063E047E416 |
:103770005AE320E004E0ADEFEA2E0E9471AA84E053 |
:1037800063E041E020E002E0C7DB84E06CE37AE341 |
:1037900042E020E001E00E948BAA85E062E040E088 |
:1037A00023E083DD0BC087E590E00E947CCE60E0E3 |
:1037B00040E050E021E000E00E94A8668DE090E04B |
:1037C0000E947CCEAC018CE067E020E00E947FAAE2 |
:1037D000209739F480E067E044E35AE320E00E9458 |
:1037E0007FAA00E00E94ADA480E80E94844C882358 |
:1037F00029F0209719F480E0D8DD01E080E20E94F2 |
:103800008E4C882371F40E94EF65811101E02097AE |
:1038100009F436CF80917E1690917F16892B09F09E |
:103820002FCF80EF0E94844C8091B018181731F48C |
:1038300020E030E08091B118F81611F021E030E07E |
:10384000822FCDB7DEB7E9E00C943CD28F929F92E5 |
:10385000AF92BF92CF92DF92EF920F93CF93DF930D |
:1038600095E0899F50011124650134E0CC0CDD1CEA |
:103870003A95E1F7CA0CDB1CC0E0D0E020E2822ED2 |
:1038800022E0922ECE018A0D9B1D809744F08C2F52 |
:1038900064E140E024E00CE00E9402AA1DC0809197 |
:1038A000960890E09C01289DC001299D900D389DAF |
:1038B000900D1124805292408C0D9D1D4091D7187F |
:1038C0005091D818480F591F80E06C2F20E004E079 |
:1038D0009CE0E92E0E94D9AC219681E190E0C80ECF |
:1038E000D91EC530D10571F6DF91CF910F91EF90C0 |
:1038F000DF90CF90BF90AF909F908F900895CF931F |
:10390000DF9380919608823019F0CAE9DAE302C0A9 |
:10391000C7E9DAE380E790E00E947CCE2DB73EB79E |
:103920002B5030403EBF2DBFEDB7FEB73196ADB73F |
:10393000BEB711961C92118222E022831382148258 |
:1039400026EA3AE336832583D087C7839287818727 |
:103950000E940AAD2DB73EB7255F3F4F3EBF2DBF3A |
:10396000FCD7DF91CF910895A1E0B0E0EAEBFCE94C |
:103970000C9418D2E0919608F0E0E359F94F808159 |
:1039800089830E94BBAFBBDF81E790E00E947CCEC1 |
:103990007C0180919608823019F00AE91AE302C08E |
:1039A00007E91AE382E790E00E947CCE2DB73EB78C |
:1039B0002C5030403EBF2DBFEDB7FEB7319623E00F |
:1039C000ADB7BEB711962C931182128213822DE9E6 |
:1039D0003AE335832483F782E68211870087938751 |
:1039E00082870E949DAF2DB73EB7245F3F4F3EBFF9 |
:1039F0002DBF8DE066E246E626E001E0EE24E3948A |
:103A00000E9411A98DE090E00E947CCEAC018CE078 |
:103A100067E020E00E947FAA81E48093940880ED13 |
:103A200097E024EF31E0F9013197F1F70197D9F7E9 |
:103A30001092E50584E190E090937F1680937E16C6 |
:103A400000E011E091E6392E442443943E010894AD |
:103A5000611C711C81E0882E912CB4E1CB2ED12CFD |
:103A6000A4EFAA2EA1E0BA2EF0E2EF2EF2E0FF2E94 |
:103A7000E1E15E2EB9C08DB79EB707979EBF8DBF9F |
:103A8000EDB7FEB73196ADB7BEB711963C92118235 |
:103A9000428274826382968285820E942D4A11E05E |
:103AA0002DB73EB7295F3F4F3EBF2DBF07C080E215 |
:103AB0000E94844C10E0882309F411E08091E50510 |
:103AC000882341F480917E1690917F16892B11F006 |
:103AD000112369F780917E1690917F16892B09F446 |
:103AE0006DC00E9492499981E091E705F091E80547 |
:103AF0008081981709F054C0892F90E00196AC019D |
:103B0000440F551F480F591F80E168E222E001E091 |
:103B10000E941BA78091960890E08E9D90018F9D3A |
:103B2000300D9E9D300D11248981859DC001112489 |
:103B3000280F391F20523240A091D718B091D818C1 |
:103B4000E091E705F091E805A20FB31F319680E1FF |
:103B500001900D928150E1F78091960890E02981C3 |
:103B600030E08E9DA0018F9D500D9E9D500D112423 |
:103B7000C90164E0880F991F6A95E1F7820F931FCE |
:103B8000480F591FE091D718F091D818E40FF51F8E |
:103B9000E051F240108289818F5F898300E007C085 |
:103BA00088EE93E0F5013197F1F70197D9F7D092BC |
:103BB0007F16C0927E161092E50516C0112371F093 |
:103BC0000F5FD0927F16C0927E160F3060F084EFA8 |
:103BD00091E061E00E94946611E00FC080E20E94D3 |
:103BE000844C882309F411E08981803220F4112368 |
:103BF00009F041CF09C0112339F0E0919608F0E0B7 |
:103C0000E359F94F89818083812F2196E1E10C945A |
:103C100034D2A3E0B0E0EFE0FEE90C9417D280E4E8 |
:103C200094E00E9454D2FC019093D8188093D71846 |
:103C3000009719F40E9406B0C2C180E494E0DF014D |
:103C40009C011D9221503040E1F710926E061092B7 |
:103C50006F060E942A4B88EC90E090937F16809329 |
:103C60007E16E0919608F0E0E359F94F80818032AA |
:103C700020F47ADE882309F49CC180917E1690910D |
:103C80007F16892B09F495C184E4809394081092DF |
:103C9000E50518ECE12EF12CF0927F16E0927E16ED |
:103CA00010929916109298168AE089838DB79EB764 |
:103CB00007979EBF8DBFEDB7FEB7319684E6ADB7CF |
:103CC000BEB711968C93118281E082834E010894D5 |
:103CD000811C911C9482838281E090E0968385838D |
:103CE0000E942D4AF0928116E0928016FF2412E085 |
:103CF000EDB7FEB73796FEBFEDBFB8EC2B2E312CDB |
:103D00009B828A82BA80DB801123D1F1123011F4B8 |
:103D10000E94BBAFF4DD80E069E04FE72CE201E0F8 |
:103D2000A2E0EA2E0E9411A98F2D90DD82E090E0A2 |
:103D30000E947CCEAC0180E067E020E00E947FAA78 |
:103D40004F2D4F5F85E067E00E94C5A980919608DE |
:103D5000823011F083E101C083E067E047E95AE374 |
:103D600020E00E947FAA80919608823011F083E0C3 |
:103D700001C083E167E04AE95AE320E00E947FAA9C |
:103D80008091E505882309F443C00E94924920915F |
:103D9000E7053091E8058091981690919916892B46 |
:103DA00089F585E0F89E2001112432010894611CF8 |
:103DB000711C660C771C620E731E8824992417C030 |
:103DC000D3014D915D913D018FE0682D25E030E0FC |
:103DD00000E0EE24E4E0CE2E7CE0A72E0E94F3AEBD |
:103DE0000894811C911CB5E08B16910429F0C40144 |
:103DF000840D951D809724F3E9E1F0E0F093991686 |
:103E0000E09398161092E50530927F1620927E1668 |
:103E10008091801690918116892B21F5FAE0F98323 |
:103E20002DB73EB7275030403EBF2DBFEDB7FEB790 |
:103E3000319624E6ADB7BEB711962C93118281E07E |
:103E40008283B382D48281E090E0968385830E944E |
:103E50002D4A3092811620928016ADB7BEB71796C4 |
:103E6000BEBFADBF7AD50E94EF65882311F410E084 |
:103E700005C030927F1620927E1612E080E80E94E4 |
:103E8000844C882339F0FA94B6E0BF1510F426E08C |
:103E9000F22E11E080E40E94844C882331F0F394E8 |
:103EA000E7E0FE1609F4FF2411E0809195088130C7 |
:103EB00009F044C080E10E94844C882309F43EC08C |
:103EC00080919608813019F40E942A4B02C00E940A |
:103ED000ED49E0919608F0E0E359F94F8081803296 |
:103EE00020F442DD882309F464C0F4E4F0939408DC |
:103EF0001092E50530927F1620927E162AE02983E3 |
:103F00008DB79EB707979EBF8DBFEDB7FEB73196B1 |
:103F100024E6ADB7BEB711962C93118281E082835F |
:103F2000B382D48281E090E0968385830E942D4AFB |
:103F300011E0ADB7BEB71796BEBFADBF80E20E941D |
:103F4000844C882339F480917E1690917F16892BBA |
:103F500009F0DACE0E94C74C1982EDB7FEB7379749 |
:103F6000FEBFEDBF319684E6ADB7BEB711968C9318 |
:103F7000118281E08283CE0101969483838381E064 |
:103F800090E0968385830E942D4A109294081092A7 |
:103F9000E50580917E1690917F16EDB7FEB73796B6 |
:103FA000FEBFEDBF892B29F484EF91E061E00E9410 |
:103FB00094668091D7189091D8180E9401D32396C7 |
:103FC000E2E10C9433D2A1E0B0E0E9EEFFE90C9419 |
:103FD00017D288E4809394081092E50504EF11E06D |
:103FE00010937F1600937E16EE24EA94E9822CD873 |
:103FF00090917006913079F586E089832DB73EB7B0 |
:10400000275030403EBF2DBFEDB7FEB731968CE64E |
:10401000ADB7BEB711968C9311829283CE010196F3 |
:104020009483838381E090E0968385830E942D4A68 |
:10403000E9828091E5052DB73EB7295F3F4F3EBF2E |
:104040002DBF882341F00E9492491092E5051093FC |
:104050007F1600937E1601E02E010894411C511C2E |
:10406000A1E0CA2ED12CF4EF6F2EF1E07F2EE4EF09 |
:104070008E2EE1E09E2E7EEF272E6DEFF62E5BEF6B |
:10408000352E17EF42E3A42EB12C002359F1809175 |
:104090009608813019F08FEB9AE302C082EC9AE324 |
:1040A00061E00E94E8AF84E060E047EB5AE322E081 |
:1040B0000E947FAA80E06BE04FE729E201E032E056 |
:1040C000E32E0E9411A982E067E040EB5AE320E072 |
:1040D00000E0EE240E9471AA82E16DE040E020E061 |
:1040E0000E943B673AD48DB79EB70B979EBF8DBF9A |
:1040F000EDB7FEB7319628E6ADB7BEB711962C9353 |
:10410000118282E0828354824382D682C5821086E5 |
:104110001782D286C1860E942D4A3FEF398384ECF4 |
:1041200099E0ADB7BEB71B96BEBFADBFF3013197E7 |
:10413000F1F70197D9F78091E5058823A1F10E9455 |
:1041400092491092E31480E065E04FEC54E120E0E6 |
:1041500005E092E0E92E0E94D9AC1092CF1480E0E5 |
:1041600064E04BEB54E120E0EE24E3940E94D9ACF0 |
:104170001092BB1480E063E047EA54E120E0EE24B3 |
:104180000E94D9AC1092A71480E062E043E954E1A8 |
:1041900020E0EE24EA940E94D9AC1092E5059092BA |
:1041A0007F1680927E160E94EF65811181E0082FB4 |
:1041B00080E80E94844C882329F480E862E00E9411 |
:1041C000BB4C8111298280E40E94844C882329F40D |
:1041D00080E462E00E94BB4C8111F98280E20E947F |
:1041E0008E4C8111398280E10E948E4C811119839D |
:1041F00080919508813001F580E10E949F4C8823D1 |
:10420000D9F089E190E06FEF7FEF40E00E94D24C5F |
:1042100080919608813019F40E94249802C029D810 |
:1042200028D8B0928D16A0928C1680918C16909101 |
:104230008D16892BD1F701E080E20E949F4C8823E4 |
:1042400041F089E190E06FEF7FEF40E00E94D24CB7 |
:1042500007C080917E1690917F16892B09F015CFAB |
:10426000109294081092E50580917E1690917F1629 |
:10427000892B29F484EF91E061E00E9494660E940A |
:10428000C74C2196E2E10C9433D2FC0180818093EB |
:10429000DB1881818093DC1882818093DD18938103 |
:1042A0009093DE186091DB187091DC180895A5E6F4 |
:1042B000B0E0EDE5F1EA0C9417D219820E94399929 |
:1042C0000E94F3970E94F3978CE291E090939516E9 |
:1042D000809394168CE09BE361E00E94E8AF80E05D |
:1042E00061E047E05BE320E000E0EE24E3940E941D |
:1042F00071AA80E062E042E05BE320E00E9471AAE4 |
:1043000080E063E04DEF5AE320E00E9471AA89E06B |
:1043100061E047EF5AE320E003E00E9471AA89E0E0 |
:1043200062E041EF5AE320E00E9471AA89E063E075 |
:104330004BEE5AE320E00E9471AA80E064E24FE76E |
:1043400027E001E078D589E090E00E947CCEAC01C6 |
:1043500081E063E022E000E0BCE0EB2E0E9471AA65 |
:1043600080E063E24FE724E101E01FD78DE090E0B9 |
:104370000E947CCEAC018CE067E020E000E0EE24FF |
:10438000E3940E9471AABB247724AA24992410E004 |
:1043900088248394A3E06A2EFAE14F2E512C4C0E10 |
:1043A0005D1EE0ED2E2EEAE33E2E14C1112309F42A |
:1043B0000FC121E0A21609F00AC136E0F31609F098 |
:1043C000E3C18C85823049F0823060F0853050F452 |
:1043D00085E061E048EE5AE309C085E061E045EE22 |
:1043E0005AE304C085E061E042EE5AE320E001E0D8 |
:1043F000EE24E3940E9471AA8D8580FF04C087E0BB |
:1044000061E04FE103C087E061E040E220E003E0CB |
:10441000EE24E39446D58DB79EB709979EBF8DBF16 |
:10442000EDB7FEB7319625E0ADB7BEB711962C9328 |
:1044300032E031831282138284828EED9AE3968376 |
:10444000858389A9878310860E940AADEDB7FEB7E0 |
:1044500031961FE0ADB7BEB711961C9381821282D0 |
:104460001382848228ED3AE3368325838FA52FEFCC |
:10447000829FC00111242EA5820F911D90878783F2 |
:104480000E940AAD8DB79EB709969EBF8DBFC2012F |
:10449000FCDEADB7BEB71B97BEBFADBFEDB7FEB775 |
:1044A000319611961C93B2E0B183128213828482FA |
:1044B0003682258267837087818792870E940AAD42 |
:1044C000EDB7FEB73B9631C0CE014696DEDE2DB786 |
:1044D0003EB7295030403EBF2DBF0DB71EB70F5F0E |
:1044E0001F4F2FE0ADB7BEB711962C93F801618234 |
:1044F00012821382848229EC3AE33683258320E1F9 |
:104500003EE040E050E00E9488D120EA36E841E0F9 |
:1045100050E00E94DAD1D80118963C932E93179759 |
:104520000E940AADEDB7FEB73996FEBFEDBF4FC092 |
:10453000CE014296AADE2DB73EB7295030403EBF8D |
:104540002DBF0DB71EB70F5F1F4FE4E0ADB7BEB76D |
:104550001196EC93D80111966C92119712961C92B9 |
:10456000129713968C9213971496AC921497E5ECCD |
:10457000FAE31696FC93EE93159728EE33E040E0AD |
:1045800050E00E94DAD1D80118963C932E931797E9 |
:104590000E940AADEDB7FEB73996FEBFEDBFCE0162 |
:1045A0000A9673DE9B01AC0181E064E000E0EE243A |
:1045B0007EE0C72E0E9450ADCE01069666DE9B01BE |
:1045C000AC018CE064E0EE24EA940E9450ADC5D1C9 |
:1045D0000D2D1F2DCE0101960E940449882309F458 |
:1045E000ACC0EB2DF0E0E930F10508F0A6C0E358CF |
:1045F000FF4F0C944ED28981853B09F09EC01092EA |
:10460000710610927206D02E9924AA24FF24BB248E |
:10461000B39499C08981823609F092C0F12ED02ED0 |
:1046200062E0B62E90C08981813009F089C09091F6 |
:1046300071069F5F9093710680917206890F809337 |
:104640007206F12ED02EAA24A39453E0B52E7BC07F |
:10465000F980F0E3FF1651F026E0F21639F032E16E |
:10466000F31621F082E0F81609F06AC09091710605 |
:104670009F0D9093710680917206890F8093720648 |
:10468000D02E44E0B42E5FC0D98093E69D1508F487 |
:1046900057C0909171069D0D90937106809172069E |
:1046A000890F80937206F12E35E0B32E4CC08981BC |
:1046B000882309F045C09091710680917206890F98 |
:1046C00080937206F12ED02E26E0B22E3CC0898156 |
:1046D000E2E0F0E0EC0FFD1FE90DF11D8083909109 |
:1046E0007106980F9093710680917206890F8093DE |
:1046F00072069394D02EDA9421F4F12E97E0B92E1D |
:1047000022C0F12E20C07980F12ED02E88E0B82E64 |
:104710001AC080917106781699F499818091720679 |
:10472000981771F4ACE2B1E0B0939516A09394168B |
:10473000F12ED02EBB2411E007C0F12ED02E03C0E5 |
:10474000F12ED02EBB2410E080E20E94844C8823FE |
:1047500039F48091941690919516892B09F026CE04 |
:104760008091941690919516892B29F484EF91E00D |
:1047700061E00E9494660E94C74C0E94ED49CB59AB |
:10478000DF4FE2E10C9433D2B2E1FB1609F49CCE88 |
:10479000E2E0FE1609F01BCFCBCE09981198ECE7AA |
:1047A000F0E080E480838081807E816080838FEE72 |
:1047B00080937A000895CF92DF92EF92FF920F9349 |
:1047C0001F93F8946091191E70911A1E80E090E07A |
:1047D0000E94B1CF20917306309174064091750606 |
:1047E000509176060E94ADCE2FE832EC45E75FE3AC |
:1047F0000E94D6D06B017C010E9480CFDC01CB01EE |
:1048000060E470E00E94C7D18C0190937C06809395 |
:104810007B06C701B60120E030E040E85CE30E947F |
:10482000D6D0609373067093740680937506909348 |
:1048300076060032110574F020E030E040E85FE3D6 |
:104840000E94ADCE609373067093740680937506D4 |
:104850009093760660917306709174068091750648 |
:104860009091760620E030E040EF50E40E9418CFAF |
:1048700060937706709378068093790690937A0612 |
:104880000E9485CF7093800660937F0610927E060B |
:1048900010927D0680E090E0A0E0B0E08093730687 |
:1048A00090937406A0937506B093760678941F9142 |
:1048B0000F91FF90EF90DF90CF9008951F920F928D |
:1048C0000FB60F920BB60F9211242F933F934F9375 |
:1048D0005F936F937F938F939F93AF93BF93EF9368 |
:1048E000FF93609178007091790080E090E00E94E1 |
:1048F000B1CF9B01AC0160917306709174068091F9 |
:104900007506909176060E94ADCE60937306709303 |
:104910007406809375069093760680917D0690913B |
:104920007E06019690937E0680937D06805090418E |
:104930000CF041DFFF91EF91BF91AF919F918F916B |
:104940007F916F915F914F913F912F910F900BBE8F |
:104950000F900FBE0F901F901895EF92FF920F933C |
:104960001F93CF93DF9388E660E047E127E001E003 |
:104970001CD489E661E045E125E000E016D486E636 |
:1049800060E041E027E057D287E662E041E023E0C3 |
:1049900001E00BD460917F067091800681E0613464 |
:1049A0007807E0F180911B1E882359F40F2EF0E068 |
:1049B000EF2EF0E0FF2EF0E90F2FF0E41F2FF02D87 |
:1049C0000AC00F2EFAE9EF2EF9E9FF2EF9E90F2FB1 |
:1049D000F0E41F2FF02D6054714080E090E00E94C1 |
:1049E000B1CFA80197010E9418CF0E9480CFEB01A0 |
:1049F00077FD14C0653171051CF0C4E1D0E002C040 |
:104A0000672B71F04C2F8EE78C1B62E0415023E046 |
:104A100001E011D2C431D10551F002C0C0E0D0E0B4 |
:104A200043E14C1B8AE662E023E000E004D2CDB70C |
:104A3000DEB7E6E00C943FD22C988EBD0DB407FE95 |
:104A4000FDCF2C9A08951F93182FF8942B9881E886 |
:104A5000F3DF812FF1DF2B9A78941F910895FF9255 |
:104A60000F931F938C01F62E2B98880F892F881F88 |
:104A7000990B91958055E0DF809181060F77080FA3 |
:104A8000802F82958F70805FD7DF802F8F70D4DF6B |
:104A90002B9A8F2DD1DF1F910F91FF900895CF9307 |
:104AA000DF93C0E0D0E0FE01E05AF64E8081FE01C7 |
:104AB000E052F74E80836081CE01D1DF219683E002 |
:104AC000C038D80781F7FE01E052F74E1082CE01C0 |
:104AD00060E0C5DF219684E0C030D807A1F7DF9100 |
:104AE000CF910895982F87FD39C06034B8F5862F8F |
:104AF000869586958695282F30E03695322F222789 |
:104B000037952795290F311D677081E090E0582F68 |
:104B100001C0550F6A95EAF7652F3370413061F097 |
:104B2000413018F04230A1F40DC0F901E052F74EC7 |
:104B30008081609586230BC0F901E052F74E808199 |
:104B4000852B05C0F901E052F74E808185278083CF |
:104B5000F901E052F74E6081C90181CF08950F93AA |
:104B60001F93CF93DF93082F10E01695102F002787 |
:104B7000179507951370C0E0D0E0CE01800F911F0C |
:104B8000DC01A052B74E2C91FE01EE51F34E208372 |
:104B90006C9165DF2196CE37D10579F7DF91CF9102 |
:104BA0001F910F9108950F931F93CF93DF93082FB9 |
:104BB00010E01695102F0027179507951370C0E089 |
:104BC000D0E0CE01800F911FFE01EE51F34E208107 |
:104BD000FC01E052F74E2083608141DF2196CE3701 |
:104BE000D10579F7DF91CF911F910F9108958091B1 |
:104BF000941D8F5F8093941D883020F050DF87E0F4 |
:104C00008093941D0895A9E0B0E0E9E0F6EA0C94E1 |
:104C100017D28E83A62E0F83482E5524662E772416 |
:104C2000842F90E09D838C8384199509E22FF0E016 |
:104C3000FB83EA83AF014619570997FF04C022247A |
:104C40002A94322C06C09C01009711F021E030E03C |
:104C5000190157FF04C088248A94982C07C09A0130 |
:104C60004115510511F021E030E049016C0197FF39 |
:104C700004C0D094C194D108D3947A0157FF04C0E2 |
:104C8000F094E194F108F3948E816A2D4F812ADF2C |
:104C9000EC14FD049CF5C60162E070E00E94C7D1EF |
:104CA00022273327261B370B39872887A22CB22CC3 |
:104CB000B40C00E010E03E81432E552416C0488518 |
:104CC00059854E0D5F1D598748871416150634F413 |
:104CD000680C791C4C195D09598748878B2D662D06 |
:104CE0004F8100DF020D131DBA0CC801840D951D04 |
:104CF000EC81FD818E179F0711F732C0C70162E07A |
:104D000070E00E94C7D122273327261B370B398733 |
:104D100028878982B82CB60C00E010E06A2C772432 |
:104D200017C0488559854C0D5D1D59874887141655 |
:104D3000150634F4420C531C4E195F0959874887F5 |
:104D4000842D6B2D4F81CEDE080D191D5981B50EB6 |
:104D5000C801860D971DEA81FB818E179F0709F711 |
:104D60002996E2E10C9433D2AF92BF92CF92DF92B8 |
:104D7000EF92FF920F931F93A62E322F102FFC2C31 |
:104D8000802F8550823008F04CC0053019F4BB24C8 |
:104D900010E003C0BB24B39412E0C42EDD2483E0F2 |
:104DA000CC0CDD1C8A95E1F78F2D992787FD909516 |
:104DB0008C0D9D1D18161906ECF46A2D70E02C2D33 |
:104DC00021502F0DFB01EE0FFF1FE60FF71FEE0F17 |
:104DD000FF1F4E2D4150630F711DCB01880F991F8E |
:104DE000860F971F880F991F480F8E2D8E0F622FE9 |
:104DF0000B2D09DF86E0A89EA00111248E2D992796 |
:104E000087FD9095840F951F1816190654F46F2D81 |
:104E10006C0D41504E0D262F295F842F61500B2DB4 |
:104E2000F2DE812F1F910F91FF90EF90DF90CF90D6 |
:104E3000BF90AF900895A0E0B0E0E1E2F7EA0C94F3 |
:104E400020D2182FB02E87FD1FE7603408F06FE3E3 |
:104E5000C42EDD24862F90E0820F911DC10ED11C3F |
:104E600021E8C216D10418F020E8C22ED12C7C0112 |
:104E700081E4E816F10418F090E4E92EF12CC62F35 |
:104E8000D0E007C0812F6C2F4C2D2C2F0B2DBBDEBB |
:104E90002196EC16FD06B0F7CDB7DEB7E9E00C942D |
:104EA0003CD2A0E0B0E0E7E5F7EA0C941DD2F82E82 |
:104EB000D22EB02E8E2D4F3DE1F0403E38F4463DCF |
:104EC00081F04C3D91F0443CA9F407C0463F59F0B5 |
:104ED0004C3F69F0443E71F402C041E00BC042E037 |
:104EE00009C043E007C044E005C045E003C046E018 |
:104EF00001C04EE17FE7E72EE4221F2D110F1F0DA9 |
:104F0000110F1B0DC62FD0E09E01220F220F220F82 |
:104F1000622F982E920E882309F049C032E0D316F2 |
:104F200019F084E0D81629F4812F45E027E001E04C |
:104F300082DFD695DC2FCC27D795C79586E0F89EE3 |
:104F400090011124C20FD31F2B2D332727FD30953D |
:104F5000C20FD31FD370E89EC00111248C01085EDC |
:104F6000144C4EE1E42E4BE3F42EE80EF91EF8014A |
:104F70009491F2E0DF1619F024E0D21639F4FE0124 |
:104F8000E052F74E80818927808304C0FE01E05201 |
:104F9000F74E9083FE01E052F74E6081CE015FDD57 |
:104FA0000F5F1F4F21960E151F0509F73EC086E0C3 |
:104FB000E89EC0011124EC01C85ED44C882483947F |
:104FC0003EE1A32E3BE3B32EA80EB91EFE01C49012 |
:104FD000092DFF24EE24E3948E2D8C2169F0F2E05C |
:104FE000DF1619F440E050E011C040E024E0D21692 |
:104FF00009F441E048250AC032E0D31629F040E028 |
:1050000050E084E0D81611F441E050E0812F602F89 |
:1050100069DDF3940F5FE8E0FE1611F0EE0CDCCFD3 |
:1050200021961F5FCA15DB0589F6CDB7DEB7ECE028 |
:105030000C9439D2EF920F93982F362F042FE22E33 |
:105040008A30A9F08D3019F41092931D11C080910F |
:10505000931D6091941D492F232F23DF8091931D71 |
:105060008F5F8093931D863118F01092931DBFDDE2 |
:105070000F91EF900895A0E0B0E0E1E4F8EA0C941D |
:1050800020D2182FB62EEA01C22ED02EFE2C08C038 |
:105090002196812F6B2D2C2D0D2DEF2C02DF1F5F04 |
:1050A00048814423A9F7CDB7DEB7E9E00C943CD2A0 |
:1050B000EF920F9300E0EE24DEDF0F91EF90089562 |
:1050C000EF920F9300E0EE24ECDE0F91EF90089545 |
:1050D0007F928F929F92AF92BF92CF92DF92EF9288 |
:1050E000FF920F931F93B82EC62E942E822EA02EC1 |
:1050F0001E2DA1DE1130A1F1123009F04AC07B2C27 |
:105100007394DD24AA2011F4DD24D394872D6C2D13 |
:105110004D2DE8DC1C2D1F5F8B2D612F4D2DE2DC0A |
:10512000092D0B0DE02EEA948E2D6C2D4D2DDADC21 |
:10513000802F612F4D2DD6DC182D1C0DF12EFA94E9 |
:105140008B2D6F2D4D2DCEDC872D612F4D2DCADC83 |
:105150008E2D612F4D2DC6DC802F6F2D4D2DC2DC85 |
:10516000FF24AA2011F4FF24F3948B2D6C2D4F2DD6 |
:10517000B9DC092D0B0D802F6C2D4F2DB3DC182DB4 |
:105180001C0D8B2D612F4F2DADDC802F612F4F2DEE |
:10519000A9DC1F910F91FF90EF90DF90CF90BF900F |
:1051A000AF909F908F907F900895A0E0B0E0EBEDDE |
:1051B000F8EA0C9422D2F82ED02E87FF02C0EFE737 |
:1051C000FE2E162F603408F01FE350E0C12FD0E010 |
:1051D000C20FD11D4F0D511D4138510510F040E84F |
:1051E00050E0E42E8F2D612F212F0D2D0CDDC134C9 |
:1051F000D10510F0C0E4D0E08E2D612F4E2D2C2F64 |
:105200000D2D01DD8E2D6C2F4F2D2C2FFCDC8F2DC5 |
:105210006C2F4F2D212FF7DCCDB7DEB7E7E00C94D4 |
:105220003ED27F928F929F92AF92BF92CF92DF92A7 |
:10523000EF92FF920F931F93A82EB62E842E722EFC |
:10524000C02E1E2DB2DF113009F444C0123009F017 |
:105250005AC09A2C9394DD24CC2011F4DD24D394ED |
:10526000892D6B2D4D2D3EDC1B2D1F5F8A2D612F4F |
:105270004D2D38DC892D612F4C2D34DCE82CEA0CC7 |
:10528000FE2CFA948F2D6B2D4D2D2CDC8E2D612F45 |
:105290004D2D28DC8F2D612F4C2D24DC172D1B0D5F |
:1052A000012F01508A2D602F4D2D1CDC892D612F7F |
:1052B0004D2D18DC892D602F4C2D14DC8F2D612F86 |
:1052C0004D2D10DC8E2D602F4D2D0CDC8F2D602F81 |
:1052D0004C2D08DCFF24CC2011F4FF24F3948A2DFC |
:1052E0006B2D4F2DFFDB082D0A0D802F6B2D4F2DC1 |
:1052F000F9DB172D1B0D8A2D612F4F2DF3DB802F2E |
:10530000612F4F2DEFDB1F910F91FF90EF90DF90FA |
:10531000CF90BF90AF909F908F907F900895FF9215 |
:105320000F931F93CF93DF93F82E14E620E0C0E095 |
:10533000D0E08F2D612F0E94A7D1082F882321F460 |
:10534000222311F4113069F48091931D8F5F8093B3 |
:10535000931D8150402F405D6091941D20E0B0DE90 |
:1053600021E0812F6AE00E94A7D1982F2196C330B7 |
:10537000D10531F0019F802D1124F81A192FD9CFB2 |
:10538000CDB7DEB7E5E00C9440D28093931D6093D7 |
:10539000941D842FC4CFA1E0B0E0E1EDF9EA0C94B4 |
:1053A00024D2982F990F990F990F1FE3191B612F82 |
:1053B000685080E04FE723E000E099833CDD0CEF8C |
:1053C000F02EF10E99819C5F80E06F2D4FE7292F21 |
:1053D00001E031DD80E06F2D40E084DB8FE76F2D51 |
:1053E00040E080DB135081E06F2D40E0212F00E092 |
:1053F0000ADC8EE76F2D4FE7212F05DC2196E5E0D3 |
:105400000C9440D20F93F82FA62F742FE22F842FE5 |
:105410008550823018F040E050E01BC040E012168A |
:105420000CF441E050E0202F332727FD30958F2FDB |
:1054300090E063E0880F991F6A95E1F7280F391F04 |
:10544000121613060CF451E0753011F072E001C031 |
:1054500070E03E2F341B6F2F660F660F660F600FD4 |
:10546000651B86E0A89FC0011124480F252F295FE6 |
:1054700000E0711101E0832FDEDC0F9108950F939E |
:10548000962F042F65E1492F20E0BCDF0F9108958E |
:10549000CF92EF92FF920F931F93CF93DF93EC0184 |
:1054A000062FF42E122F0E94C5D4282F81E0609180 |
:1054B000931D4091941DEF2CC12E56DC082F05C082 |
:1054C000602F4F2D212FB6DD2196FE018491882378 |
:1054D000B9F7DF91CF911F910F91FF90EF90CF908F |
:1054E0000895EF920F938093931D6093941DCA01CA |
:1054F000622F402F2E2DCCDF0F91EF9008958093D7 |
:10550000931D6093941DCA01622F40E020E0C0CF3C |
:1055100040E020E0BDCFCF92DF92EF92FF920F9359 |
:105520001F93D82E7B01C42E122FCB010E94C5D40D |
:105530009C01220F331F280F391F121B105C109281 |
:10554000931DD092941DC7016C2D412F202FA0DFF9 |
:105550001F910F91FF90EF90DF90CF900895FF92F1 |
:105560000F931F93CF93DF93F82E8A0185E28093E8 |
:10557000951D222319F4C6E9DDE105C080E380937F |
:10558000961DC7E9DDE1CB016CE87DE14AE050E022 |
:105590000E9443D5ECE8FDE102C0899331968081F9 |
:1055A0008823D9F7101611068CF48EE28993C8016E |
:1055B0006CE87DE14AE050E00E9443D5ECE8FDE173 |
:1055C00002C08993319680818823D9F7F8821982A5 |
:1055D000CDB7DEB7E5E00C9440D2AFE0B0E0E3EF4A |
:1055E000FAEA0C9417D28C876D876A0159010B87F0 |
:1055F000449F9001459F300D549F300D11243E83F0 |
:105600002D83AA9CC001AB9C900DBA9C900D1124D7 |
:105610009C838B83A29E4001A39E900CB29E900C13 |
:105620001124BC0168197909882777FD8095982F86 |
:105630000E94B3CF7B018C01ED81FE81BF018827E1 |
:1056400077FD8095982F0E94B3CF20E030E040E8AE |
:105650005EE30E94D6D09B01AC01C801B7010E9455 |
:10566000ADCE0E9439D10E9480CF2B019401220F30 |
:10567000331F3A8329836D856A0D8C854B8532DA19 |
:105680006D856A198C854B852DDA8C858C0D6D8521 |
:105690004B8528DA8C858C196D854B8523DA8D81B5 |
:1056A0009E81880F991FEE27FF27E81BF90BFA87C9 |
:1056B000E9872B813C81220F331F38872F834901D3 |
:1056C0008B819C81880E991E2224332466247724A2 |
:1056D00034C057FC0DC00894A108B108E981FA81D3 |
:1056E00029853A85E20FF31FFA83E9834E1A5F0A90 |
:1056F0000894611C711C8F819885280E391E480CF6 |
:10570000591C0C85060DED84EA0C802F6E2D4B85FF |
:10571000E9D9FD84FA18802F6F2D4B85E3D91C85BC |
:105720001619812F6E2D4B85DDD9812F6F2D4B855D |
:10573000D9D9EF81F8858E0E9F1E29813A812216D4 |
:1057400033063CF2B301882777FD8095982F0E949D |
:10575000B3CF20E030E040E05FE30E94ADCE7B01BC |
:105760008C0165010894C108D108EB81FC81BF015F |
:10577000882777FD8095982F0E94B3CFA8019701C5 |
:105780000E94D6D0A80197010E94D6D07B018C013F |
:10579000CC9CC001CD9C900DDC9C900D11242D81E2 |
:1057A0003E81829FB001839F700D929F700D1124E6 |
:1057B000882777FD8095982F0E94B3CF9B01AC017D |
:1057C000C801B7010E94ADCE7B018C018B819C8109 |
:1057D000ED81FE818E9FB0018F9F700D9E9F700D99 |
:1057E0001124882777FD8095982F0E94B3CF9B01C5 |
:1057F000AC01C801B7010E94ACCE0E9439D10E9411 |
:1058000080CF5B01460149845A84509441945108E9 |
:1058100053942D813E8189859A85281B390BE98116 |
:10582000FA812E1B3F0B3F872E8732C01A141B04B0 |
:105830004CF00894611C711C2F813885220E331E98 |
:10584000A20CB31C8E859F85A80EB91ED62C0C8584 |
:10585000060DED84E80C802F6E2D4B8543D9FD8419 |
:10586000F818802F6F2D4B853DD91C851619812F77 |
:105870006E2D4B8537D9812F6F2D4B8533D90894E9 |
:1058800081089108EE85FF85E40DF51DFF87EE8701 |
:10589000C4010196181619064CF22F96E2E10C94F9 |
:1058A00033D2A1E0B0E0E7E5FCEA0C9417D2382E41 |
:1058B000262E4983422EB8010E2D80E090E00E94F2 |
:1058C000B1CF2BED3FE049E450E40E94D6D020E078 |
:1058D00030E044E353E40E9418CF6B017C01C70120 |
:1058E0000E9415CF4B015C01552466247724C30127 |
:1058F000B2010E94B1CF9B01AC01C501B4010E946D |
:10590000D6D00E9439D14B015C01B601C7010E947B |
:1059100059D16B017C012981622F70E080E090E019 |
:105920000E94B1CF9B01AC01C701B6010E94D6D045 |
:105930000E9439D10E9485CF132D160FC501B401E5 |
:105940000E9485CF222D261B832D622D412F5BD9EE |
:105950002196E2E10C9433D2CF92EF92FF920F9313 |
:105960001F93CF93DF93EC01062FF42E122FDC014F |
:105970000D900020E9F71197A81BB90B80E060910A |
:10598000931D4091941D2A2FEF2CC12EEDD9082F85 |
:1059900005C0602F4F2D212F4DDB21968881882354 |
:1059A000C1F7DF91CF911F910F91FF90EF90CF90B2 |
:1059B0000895EF920F938093931D6093941DCA01F5 |
:1059C000622F402F2E2DC8DF0F91EF900895A0E099 |
:1059D000B0E0EDEEFCEA0C9423D2FE013E96B2E676 |
:1059E000EB2EBDE1FB2E81E0B70148E22C853D8521 |
:1059F0008F010E94F24489858093931D8A8580934C |
:105A0000941DC7016B8540E020E0A6DF2096E6E00C |
:105A10000C943FD2A0E0B0E0E0E1FDEA0C9423D288 |
:105A2000FE01709602E6E02E0DE1F02E81E0B70156 |
:105A300048E22E853F858F010E94F244898580933C |
:105A4000931D8A858093941DC7016B854C852D8598 |
:105A500083DF2096E6E00C943FD2A0E0B0E0E3E3E1 |
:105A6000FDEA0C9423D2FE01709682E6E82E8DE1C9 |
:105A7000F82E80E0B70148E22E853F858F010E9415 |
:105A8000F24489858093931D8A858093941DC70174 |
:105A90006B854C852D8560DF2096E6E00C943FD227 |
:105AA000AF92BF92CF92EF920F93EDB7FEB73B97B5 |
:105AB000FEBFEDBFADB6BEB60894A11CB11C81837C |
:105AC000F50161830283E382C4828EE391E09683D1 |
:105AD0008583CA01B90124E630E040E050E00E942D |
:105AE000FCD1F5012783308741875287B6DF8DB718 |
:105AF0009EB70B969EBF8DBF0F91EF90CF90BF903A |
:105B0000AF9008954F925F926F927F928F92AF9273 |
:105B1000BF92CF92DF92EF92FF920F931F93DF938A |
:105B2000CF93CDB7DEB76097DEBFCDBFD82EB62EF0 |
:105B3000E986FA860B871C872D873E874F87588B0F |
:105B4000CE010196BE01675F7F4F0E94D743498017 |
:105B50005A806B807C808DB79EB70D979EBF8DBF9E |
:105B60000DB71EB70F5F1F4FEDB7FEB7D182F8011B |
:105B7000B182C282A382848284E591E09683858388 |
:105B8000C301B20120E13EE040E050E00E94DAD1E2 |
:105B9000F80127831086C301B2012CE330E040E016 |
:105BA00050E00E94DAD1CA01B9012CE330E040E0B4 |
:105BB00050E00E94DAD1F80161871286C301B20178 |
:105BC0002CE330E040E050E00E94DAD1F801638736 |
:105BD000148643DF8DB79EB70D969EBF8DBF60962E |
:105BE000DEBFCDBFCF91DF911F910F91FF90EF905E |
:105BF000DF90CF90BF90AF908F907F906F905F902D |
:105C00004F900895CF92DF92EF920F931F93382F0A |
:105C1000162F6A01CA016CE370E00E94B3D18DB700 |
:105C20009EB70B979EBF8DBFEDB7FEB73196ADB750 |
:105C3000BEB711963C93118322830383E48283E6EB |
:105C400091E096838583708767838CE390E09C0165 |
:105C5000629FC001639F900D729F900D1124C81A1E |
:105C6000D90AD286C186F9DE8DB79EB70B969EBF44 |
:105C70008DBF1F910F91EF90DF90CF9008958F927D |
:105C80009F92AF92CF92EF92FF920F931F93182F94 |
:105C9000F62E4A012150304084E6B90141E050E03F |
:105CA000202F5DDC8DB79EB709979EBF8DBFEDB7E6 |
:105CB000FEB73196ADB7BEB711961C93F182E28262 |
:105CC000C382A48285E99DE196838583908687823D |
:105CD000C4DE8DB79EB709969EBF8DBF1F910F91F1 |
:105CE000FF90EF90CF90AF909F908F9008958F92FC |
:105CF0009F92AF92CF92EF92FF920F931F93182F24 |
:105D0000F62E4A012250304085E7B90142E050E0CA |
:105D1000202F25DC8DB79EB709979EBF8DBFEDB7AD |
:105D2000FEB73196ADB7BEB711961C93F182E282F1 |
:105D3000C382A48285E99DE19683858390868782CC |
:105D40008CDE8DB79EB709969EBF8DBF1F910F91B8 |
:105D5000FF90EF90CF90AF909F908F9008958F928B |
:105D60009F92AF92CF92EF92FF920F931F93182FB3 |
:105D7000F62E4A012150304085E7B90141E050E05C |
:105D8000202FEDDB8DB79EB709979EBF8DBFEDB776 |
:105D9000FEB73196ADB7BEB711961C93F182E28281 |
:105DA000C382A48285E99DE196838583908687825C |
:105DB00054DE8DB79EB709969EBF8DBF1F910F9180 |
:105DC000FF90EF90CF90AF909F908F900895AF92FB |
:105DD000CF92EF920F93CC24AA24C1DF0F91EF90C2 |
:105DE000CF90AF9008958F929F92AF92CF92EF9203 |
:105DF000FF920F931F93182FF62E4A0184E6B901E4 |
:105E000040E050E0202FABDB8DB79EB709979EBFD7 |
:105E10008DBFEDB7FEB73196ADB7BEB711961C93E7 |
:105E2000F182E282C382A48285E99DE19683858323 |
:105E30009086878212DE8DB79EB709969EBF8DBF72 |
:105E40001F910F91FF90EF90CF90AF909F908F9008 |
:105E50000895AF92CF92EF920F93CC24AA24C3DF80 |
:105E60000F91EF90CF90AF9008958F929F92AF9245 |
:105E7000CF92EF92FF920F931F93182FF62E4A01A5 |
:105E800085E7B90140E050E0202F69DB8DB79EB770 |
:105E900009979EBF8DBFEDB7FEB73196ADB7BEB7C0 |
:105EA00011961C93F182E282C382A48285E99DE16E |
:105EB0009683858390868782D0DD8DB79EB70996BD |
:105EC0009EBF8DBF1F910F91FF90EF90CF90AF902D |
:105ED0009F908F900895AF92CF92EF920F93CC2422 |
:105EE000AA24C3DF0F91EF90CF90AF9008958093D5 |
:105EF000931D6093941DCA01622F40E020E02CCDD9 |
:105F00000F93972FE62FF72FDF010D900020E9F771 |
:105F10001197AE1BBF0BFD01EE0FFF1FEA0FFB1F1A |
:105F20002E1B205C1092931D8093941D862F642F4E |
:105F3000422F202F11DD0F910895A0E0B0E0E3EA99 |
:105F4000FFEA0C9423D2FE013F9642E6E42E4DE197 |
:105F5000F42E81E0B70148E22D853E858F010E9435 |
:105F6000F2448985B7014A852B850C85C9DF2096C7 |
:105F7000E6E00C943FD21F93CF93DF9320EE38E1FD |
:105F8000C90160E070E040E054E00E940DD510EBE4 |
:105F90002B98812F0E941CA580E10E941CA5809156 |
:105FA00081060E941CA52B9AC0E0D0E080E00E94F0 |
:105FB0001CA52196C038D105C9F71F5F183B41F7D2 |
:105FC0001092931D1092941DDF91CF911F9108950F |
:105FD0000F93CF93DF93EC01662309F0CCDF80E0D1 |
:105FE00060E04FE727E001E00E941BA7CE010E947E |
:105FF000C5D4419710F40E94ADA481E060E0AE01E9 |
:1060000022E07DDADF91CF910F9108950F93B3DFF6 |
:1060100088E068E04FE620E301E0C7D882E062E074 |
:1060200048E35EE322E06BDA82E064E047E25EE3AD |
:1060300020E065DA82E065E048E15EE320E05FDAD7 |
:1060400080E10E94844C882379F480E20E94844C91 |
:10605000882351F480E40E94844C882329F480E84A |
:106060000E94844C882361F30F9108951F93182F89 |
:106070001092931D1092941D8CE58CBD81E08DBD16 |
:106080002C9A2A9880E090E028E833E1F9013197D2 |
:10609000F1F701968A309105C9F72A9A2C982B9826 |
:1060A00080E40E941CA581EA0E941CA580EC0E944D |
:1060B0001CA586EA0E941CA582EA0E941CA58FE20C |
:1060C0000E941CA588EF0E941CA580E00E941CA5D0 |
:1060D00087E20E941CA5113031F481E80E941CA5C2 |
:1060E0008091BD1D04C081E80E941CA586E10E942C |
:1060F0001CA58CEA0E941CA580E00E941CA58FEACA |
:106100000E941CA538DF1F910895A0E0B0E0EBE8E5 |
:10611000F0EB0C941BD2FC01EB01DA01648075807A |
:1061200086809780E080F180028113811496AD9083 |
:10613000BD90CD90DC9017976D917D918D919C9144 |
:106140002AE530E040E050E00E9488D128EE33E0BC |
:1061500040E050E00E94FCD12E0D3F1D401F511F1A |
:10616000288339834A835B83C801B70120E836E975 |
:1061700048E950E00E94FCD1DA01C901F0D3182FA0 |
:10618000092FC601B50120ED32E040E050E00E9449 |
:1061900088D16B017C01212F302FC90197FF03C0EB |
:1061A000909581959F4F9C01442737FD4095542F32 |
:1061B000C701B6010E94FCD1620E731E841E951E9B |
:1061C0006C827D828E829F8281E0CDB7DEB7EEE069 |
:1061D0000C9437D2ADE0B0E0E0EFF0EB0C9417D2C6 |
:1061E000C982DA82EB82FC820D831E832F8338877B |
:1061F00049875A876B877C878D87ED80FE800F816A |
:1062000018856EA07FA088A499A4C401B3016E0D67 |
:106210007F1D801F911F20E03DE241E351E00E947D |
:10622000FCD1DA01C9019BD3AA2797FDA095BA2F0B |
:10623000BC01CD012FE435E040E050E00E9488D160 |
:106240005B016C0169817A818B819C812AA13BA1D0 |
:106250004CA15DA1621B730B840B950B2AE030E00F |
:1062600040E050E00E94FCD1C601B5010E9488D1F7 |
:1062700020E836E948E950E00E94FCD1522E432E36 |
:10628000342E252EE618F70808091909C801B701A8 |
:1062900029E534E040E050E00E9488D120E137E277 |
:1062A00040E050E00E94FCD1D22EC32EB42EA52E89 |
:1062B000652D742D832D922D59D34C01652D742D90 |
:1062C000832D922D252D342D432D522D0E9488D1C2 |
:1062D0007B018C016D2D7C2D8B2D9A2D2D2D3C2D30 |
:1062E0004B2D5A2D0E9488D1E60EF71E081F191F4C |
:1062F000C801B701ADD29C01C401845E9D4F68E620 |
:1063000071E00E94C7D1B9012D96E2E10C9433D21D |
:106310000F938BEE9DE161E041E00E944E6410928C |
:106320009F04809192088823A9F487E066E040E00A |
:10633000A6D887E890E00E947CCEBC0187E046E0CA |
:1063400020E000E0E8D888E592E06FEF7FEF40E0E2 |
:106350000E94D24C8EE790E00E947CCEBC0182E08D |
:1063600040E020E001E0D7D880E00E94DC6210929B |
:1063700092080E943FC20E94A8C90E948F5E0F919E |
:1063800008950F931F930E94B950882369F48FE7F3 |
:1063900090E00E947CCE61E048EE53E021E001E015 |
:1063A0000E94A8668EEF3DC08BEE9DE161E041E06A |
:1063B0000E944E648DE790E00E947CCEBC0182E09A |
:1063C00040E020E001E0A7D880E00E94DC620E946B |
:1063D00090C98091C61D882311F00E942BC08FEFB9 |
:1063E0009DE10E9484C2182F0E9486488BE395E2AB |
:1063F0000E948FC481E080939F040E948F5E1123CE |
:1064000011F081E00EC00E94A8C981E890E00E94CE |
:106410007CCE61E048EE53E021E001E00E94A866F6 |
:1064200080E01F910F910895A0E0B0E0EAE1F2EB67 |
:106430000C9417D2D82EEB01A4DF18160CF0FCC177 |
:1064400084EF91E090938316809382160E94C74C4C |
:1064500081E0809392088BEE9DE161E041E00E9433 |
:106460004E6421E0D216F1F486E061E041ED5EE396 |
:1064700020E00FEFF2E0EF2E34D88FE061E04FEC38 |
:106480005EE320E00EEF2DD880E06DE042E22DE0EB |
:1064900001E00E9403A68CE56DE04FE72DE00E942D |
:1064A00003A612C032E0D31679F481E062E041E045 |
:1064B0000E943FAA8CE790E00E947CCEBC0181E064 |
:1064C00042E020E001E027D880E062E04AEC5EE3B1 |
:1064D00020E000E0E3E0EE2E04D880E063E045EC4D |
:1064E0005EE320E00E9471AA80E064E040EC5EE39D |
:1064F00020E00E9471AA8BE062E04AEB5EE320E0BC |
:106500000E9471AA8BE063E044EB5EE320E00E940E |
:1065100071AA8BE064E04EEA5EE320E00E9471AA7B |
:1065200085E062E044E00E943FAA89E090E00E949A |
:106530007CCEAC0181E065E022E074E0E72E0E94B1 |
:1065400071AA80E06BE24FE724E101E00E94D5A848 |
:10655000FF2454E0B52E40E1842E32E0932E24E94E |
:10656000422E2EE3522E94EA292E9EE3392E88EAFB |
:10657000682E8EE3782E0E94ADA40E94E1C5882388 |
:1065800009F43BC181E0D81649F487E061E047E2B5 |
:1065900058E220E00EEF12E0E12E0BDA9091232872 |
:1065A000913009F44EC1913000F1923019F09630DB |
:1065B00039F403C024E83EE31AC029E83EE317C0DB |
:1065C000ADB7BEB71997BEBFADBFEDB7FEB7319639 |
:1065D0001196BC929182128281E0838383E084834E |
:1065E000768265829783108618C02AE73EE38DB7CE |
:1065F0009EB709979EBF8DBFEDB7FEB73196ADB779 |
:10660000BEB71196BC929182128281E0838383E0AF |
:1066100084833682258230872783FCD92DB73EB705 |
:10662000275F3F4F3EBF2DBF80912328815082308E |
:1066300020F488E062E04FE103C088E062E040E2DD |
:1066400020E002E0B3E0EB2E0E9451A74091242805 |
:1066500086E063E050E022E030E000E0EE24CC246D |
:10666000C394A3E0AA2E01DC80912428863020F078 |
:1066700088E063E04FE103C088E063E040E220E0AF |
:1066800002E0F3E0EF2E0E9451A78DB79EB7099765 |
:106690009EBF8DBFEDB7FEB73196ADB7BEB71196B1 |
:1066A000BC92B182128281E0838313E014832EE9CD |
:1066B0003EE336832583809121289091222890877C |
:1066C0008783A8D900D0EDB7FEB73196ADB7BEB776 |
:1066D00011968C92918212821382148329E93EE3EF |
:1066E000368325838091152890911628A09117282C |
:1066F000B091182887839087A187B2878BD98DB7EF |
:106700009EB70B969EBF8DBF0E94E6C5ADB7BEB7C4 |
:106710001B97BEBFADBFEDB7FEB7319611968C92F9 |
:106720001183128213821483568245826783708795 |
:10673000818792876FD940912528509126282DB7BF |
:106740003EB7255F3F4F3EBF2DBF4115510559F460 |
:1067500080E164E04EE85EE320E000E0E3E0EE2E5E |
:106760000E9471AA0AC080E164E024E030E000E009 |
:10677000EE24CC2473E0A72EF2DA20911928309170 |
:106780001A2840911B2850911C2881E066E000E007 |
:10679000EE2496E0C92E84D920911D2830911E2820 |
:1067A00040911F28509120288CE066E079D92097ED |
:1067B00021F1809123288150823040F4FF2009F09C |
:1067C00043C0D0937F16C0937E160BC031E0F31602 |
:1067D000A1F480917E1690917F16FF24892B69F495 |
:1067E0000AC080917E1690917F16892B21F40E9419 |
:1067F000C74C82E021C0FF24F394809182169091CF |
:106800008316892B11F41092920880E20E94844C26 |
:10681000882329F480919208882309F0ACCECD2BEF |
:1068200021F080919208882309F472DD0E94C74C00 |
:1068300080919208811181E0CDB7DEB7E2E10C943E |
:1068400033D22FE73EE3D3CE81E0F816B1F6C9CFBD |
:10685000A0E0B0E0EEE2F4EB0C941FD25B016C011F |
:10686000C0E0D0E800E010E89FE0E92EF12C16959A |
:1068700007959E0140E050E0CA01B9010E9488D10D |
:10688000A616B706C806D90618F4C01BD10B02C05D |
:10689000C00FD11F0894E108F108E114F10439F7A1 |
:1068A0009E0140E050E0CA01B9010E9488D1A616BD |
:1068B000B706C806D90608F42197CE01CDB7DEB7D2 |
:1068C000EAE00C943BD297FD02C041E004C09095F1 |
:1068D00081959F4F4FEF68E671E00E94C7D1FC01A0 |
:1068E0008B3591055CF08B5590408A35910540F4CD |
:1068F00084EB90E09C012E1B3F0BF90121E010C0BE |
:10690000CF01855B90408A35910518F4E45BF04037 |
:1069100006C088E691E0BC016E1B7F0BFB012FEFE8 |
:10692000EE0FFF1FEB51FC4F332727FD3095808181 |
:106930009181B901689F9001699F300D789F300D5A |
:106940001124842F992787FD9095A901489F9001D4 |
:10695000499F300D589F300D1124C90108952AE533 |
:1069600030E0A901481B590BCA01ADCFAAE0B0E045 |
:10697000ECEBF4EB0C9417D269837A838B839C83C2 |
:1069800019012A0129813A8137FF03C030952195E9 |
:106990003F4FB901882777FD8095982F0E94B3CF8C |
:1069A0005B016C0120E030E040E050E00E9411CF3C |
:1069B000882341F457FC03C060E070E0DAC064EB68 |
:1069C00070E0D7C0910137FE03C0309521953F4F4D |
:1069D000B901882777FD8095982F0E94B3CF7B015E |
:1069E0008C01A60195010E9411CF88238CF4C80167 |
:1069F000B701A60195010E9418CF0F2EF0E0EF2EEF |
:106A0000F0E0FF2EF0E80F2FFFE31F2FF02D0CC05A |
:106A1000C601B501A80197010E9418CF7B018C0126 |
:106A200060E070E080E89FE320E038ED4BED56E455 |
:106A30000E94D6D00E9480CF3B014C01C801B70113 |
:106A400020E038ED4BED56E40E94D6D00E9480CF76 |
:106A50009B01AC0103ED1EE31E830D83EE24FF2496 |
:106A6000870160E070E064015301062E04C0D694F3 |
:106A7000C794B794A7940A94D2F7AF82B886C98610 |
:106A8000DA8659016A01062E04C0D594C794B794DA |
:106A9000A7940A94D2F757FF17C06A187B088C088E |
:106AA0009D088F819885A985BA85280F391F4A1FAF |
:106AB0005B1FED81FE8185919491A0E0B0E0E81A22 |
:106AC000F90A0A0B1B0B16C06A0C7B1C8C1C9D1C44 |
:106AD000AF80B884C984DA842A193B094C095D095E |
:106AE000ED81FE8185919491A0E0B0E0E80EF91E61 |
:106AF0000A1F1B1F6F5F7F4F8D819E8102969E83B1 |
:106B00008D836F30710509F0AECFC801B7010E94C7 |
:106B1000B1CF21EE3EE245E652E40E94D6D020E01D |
:106B200038ED4BED56E40E9418CF0E9485CF9B01B3 |
:106B3000AC01121413041404150464F084EB90E007 |
:106B4000A0E0B0E05C016D01A21AB30AC40AD50A44 |
:106B5000A6019501C980DA80EB80FC801C141D041D |
:106B60001E041F042CF066277727621B730B01C0DD |
:106B7000B901CB012A96E2E10C9433D2AAE3B0E04A |
:106B8000E4ECF5EB0C9417D282E064EF71E04CDC9E |
:106B900018160CF056C389E190E06FEF7FEF40E0EC |
:106BA0000E94D24CE8D90E94ED498FE4809394086A |
:106BB00019822DB73EB7275030403EBF2DBFEDB7ED |
:106BC000FEB7319684E6ADB7BEB711968C931182AD |
:106BD000DD24D394D2828E010F5F1F4F1483038371 |
:106BE00021E0E22EF12CF682E5820E942D4A8AE015 |
:106BF0008983EDB7FEB731968FE6ADB7BEB7119674 |
:106C00008C9382E08183D28214830383F682E582AF |
:106C10000E942D4A80E991E09093951680939416F6 |
:106C200088EC90E090938116809380162DB73EB744 |
:106C3000295F3F4F3EBF2DBF0E943268109215283A |
:106C4000109216281092172810921828FF24F394F7 |
:106C50001DAA1EAA1FAA18AE18AA19AA1AAA1BAA08 |
:106C60001CAABB24B39409AF1AAFE3C2FF2009F4F6 |
:106C700060C081D900D000D000D0EDB7FEB731960A |
:106C8000ADB7BEB711961C921182B28213822FE06B |
:106C90003FE33583248351D94DB75EB74A5F5F4FD9 |
:106CA0005EBF4DBF81E260E041E22BE001E00E9467 |
:106CB00003A68FE560E04FE52BE00E9403A680E08D |
:106CC0006CE04FE72CE00E9403A682E467E24FE706 |
:106CD00027E20E9403A680E061E242E42CE0EE2479 |
:106CE000E3940E9411A98DB79EB707979EBF8DBFF1 |
:106CF000EDB7FEB7319623E0ADB7BEB711962C9332 |
:106D000032E03183128223834FEF44838BE09FE391 |
:106D1000968385830E940AADADB7BEB71796BEBFF6 |
:106D2000ADBF81E062E10E94A36981E062E30E945D |
:106D30002E6B8091E505882311F4FF2450C10E9439 |
:106D40009249E091E705F091E805F093E817E093A8 |
:106D5000E717109282066580768087809084218074 |
:106D60003280438054806092131E7092141E809271 |
:106D7000151E9092161E81819281A381B481809309 |
:106D80000F1E9093101EA093111EB093121E0E940E |
:106D9000376880E060E040E020E00E94167180E00B |
:106DA00069E04EE121E002E00E94416EE091E717C8 |
:106DB000F091E81747A950AD80E160E020E0EE24B3 |
:106DC0000E9402AEE091E717F091E81723A934A9D9 |
:106DD000442737FD4095542F87E062E000E093E0C0 |
:106DE000E92ECC24CA940E94E073E091E717F09159 |
:106DF000E81742A9463010F496E001C095E081E121 |
:106E000065E050E022E030E000E0E92E13E0C12E22 |
:106E1000B7EEAB2E2AD887E76FE00E94387083E088 |
:106E200064E0A4019301ADEFEA2EF8EFCF2E0E94AB |
:106E300050AD8CE064E0A2019101EE24E3940E9445 |
:106E400050AD2DB73EB72D5030403EBF2DBFE09125 |
:106E5000E717F091E817ADB7BEB7119631968DE000 |
:106E600001900D928150E1F7CA88DB88EC88FD889B |
:106E70000E891F89288D398D4A8D5B8D6C8D7D8D96 |
:106E80008E8DA8D90F900F900F900F90EDB7FEB791 |
:106E90003196ADB7BEB711961C9214E011831282E1 |
:106EA00087E08383148321E03FE336832583CB018E |
:106EB0006AE070E00E94B3D1708767830E940AADD8 |
:106EC00000D0EDB7FEB73196ADB7BEB711961C93A3 |
:106ED00085E0818312828DEF8383B8E0B48329EF4C |
:106EE0003EE33683258389A99AA9ABA9BCA98783E8 |
:106EF0009087A187B2870E940AAD20912328822F14 |
:106F00008150ADB7BEB71B96BEBFADBF823010F08B |
:106F100096E001C095E04DB75EB7495050405EBF66 |
:106F20004DBFEDB7FEB731963EE0ADB7BEB7119697 |
:106F30003C9385E081839283B38248E0448383EF6E |
:106F40009EE396838583278310860E940AAD409135 |
:106F50002428ADB7BEB71996BEBFADBF463010F4FA |
:106F600096E001C095E088A9811181E0F82E81E1C9 |
:106F700065E050E022E030E000E0E92EE4E0CE2ED3 |
:106F800078E0A72E0E9435AF88E760E30E94387052 |
:106F90002091192830911A2840911B2850911C28C3 |
:106FA00083E067E09DEFE92ECC24C3940E9450ADAE |
:106FB00020911D2830911E2840911F285091202893 |
:106FC0008CE067E0EE24E3940E9450AD1092E5055A |
:106FD00020E931E0309395162093941618AA0E9468 |
:106FE000E1C5882309F4D0C080919208882309F470 |
:106FF000CBC08091152890911628A0911728B091A8 |
:1070000018282DA93EA94FA958AD281739074A07B6 |
:107010005B0708F0B9C080912428863008F4A8C026 |
:10702000809123288150823008F0A2C08091281ED0 |
:107030009091291EAA2797FDA095BA2F8093AA1D8B |
:107040009093AB1DA093AC1DB093AD1D8091401EDD |
:107050009091411EAA2797FDA095BA2F8093AE1D4F |
:107060009093AF1DA093B01DB093B11D80911928CE |
:1070700090911A28A0911B28B0911C288A839B8389 |
:10708000AC83BD8380911D2890911E28A0911F285C |
:10709000B09120288E839F83A887B987CE0102965E |
:1070A000BE01665F7F4F4AEA5DE12FD8BE8E8E85B6 |
:1070B0009F85A889B9898A8B9B8BAC8BBD8B8A8570 |
:1070C0009B85AC85BD858E8B9F8BA88FB98F81E00A |
:1070D00090E0A0E0B0E08A8F9B8FAC8FBD8FAFEFC8 |
:1070E000BFEFB8A3AF8F80912C1E89A3BCE3BAA3D6 |
:1070F000BBA2BCA21DA226E42AA73FE43BA74CE406 |
:107100004CA74DA784E68EA31FA280912A1E88A7B4 |
:107110005FEF59A71EA61FA68DB79EB707979EBF04 |
:107120008DBFEDB7FEB7319683E7ADB7BEB7119609 |
:107130008C93B2E0B183B28222E130E02C0F3D1F8C |
:10714000348323838EE190E0968385830E942D4AC9 |
:1071500089A99AA9ABA9BCA90196A11DB11D89ABAA |
:107160009AABABABBCABADB7BEB71796BEBFADBFAE |
:1071700020911528309116284091172850911828F1 |
:107180002DAB3EAB4FAB58AF80E20E94844C8823BE |
:1071900009F054C080E10E94844C882309F04EC05D |
:1071A0008091801690918116892B49F58AE0898318 |
:1071B0004DB75EB7475050405EBF4DBFEDB7FEB70D |
:1071C00031968FE6ADB7BEB711968C93B2E0B1831E |
:1071D000B28229AD23833AAD348381E090E0968377 |
:1071E00085830E942D4A88EC90E0909381168093CD |
:1071F00080164DB75EB7495F5F4F5EBF4DBF809150 |
:10720000941690919516892BA1F458A9552311F441 |
:107210000E94DE7A88EC90E060E870E040E00E9436 |
:10722000D24C80E991E0909395168093941691E06A |
:1072300098AB80919208882309F018CD10926306CC |
:1072400067D8EA96E2E10C9433D281E08093C41DC2 |
:10725000089581E08093C51D089581E08093C61D47 |
:1072600008951092C61D089560930F1E7093101E0E |
:107270008093111E9093121E2093131E3093141EA0 |
:107280004093151E5093161E089587E00E94AFA5E7 |
:1072900080E067E043EC5FE320E00E947FAA82EB9E |
:1072A0009DE160E070E049E857E00E94E3D587E0A7 |
:1072B0000C94D3A51F93CF93DF930E94BBAF80E0C4 |
:1072C00060E04DEA5FE322E00E947FAA80E061E097 |
:1072D00042EA5FE320E00E947FAA80E062E04CE89F |
:1072E0005FE320E00E947FAA89E88093B31D10929B |
:1072F000B41D85E08093B51D41E04093B61D82E446 |
:107300008093B71D4093B81D80E18093B91D80E440 |
:107310008093BA1D4093BB1D1092BC1D84E18093E5 |
:10732000BD1D84E68093BE1D1092BF1D1092C01D2E |
:107330001092C11D1092C21D1092C31D1092C41D47 |
:107340001092C51D4093C61D1092171E1092181E54 |
:1073500040931C1E10921D1E40931E1E10921F1E55 |
:1073600088E28093201E4093211E4093221E20E13C |
:1073700037E23093241E2093231E1092251E109274 |
:10738000261E84E08093271E40931B1E30931A1EF6 |
:107390002093191E1092291E1092281E1092411E31 |
:1073A0001092401E8EE190E090932B1E80932A1E37 |
:1073B00085E090E090932D1E80932C1E10922E1E3F |
:1073C00040932F1E1092301E10920F1E1092101E0E |
:1073D0001092111E1092121E1092131E1092141E63 |
:1073E0001092151E1092161E87EC9DE167E87FE350 |
:1073F0000E94BED48CEC9DE163E87FE30E94BED482 |
:107400008CED9DE164E77FE30E94BED487ED9DE1B2 |
:107410006FE67FE30E94BED4EBEEFDE111922DE119 |
:10742000EF3FF207D9F780E381939EE1ED30F90752 |
:10743000D9F7108211828BE88093B21D10923F1E03 |
:107440001092311E8DE290E09093331E8093321E95 |
:107450009093351E8093341E2FE730E03093371E13 |
:107460002093361E1092381E90933A1E8093391E38 |
:1074700090933C1E80933B1E30933E1E20933D1EF6 |
:1074800084E18093421E1092431E81E08093441E4B |
:107490008AE08093451E1092461E1092471E88E295 |
:1074A0008093481E1092491EEF01ED96EEE5FEE135 |
:1074B0008EE190E09983888380519F4F9B878A87D4 |
:1074C00010821582128622968F5E90403196AEE130 |
:1074D000C435DA0779F711E0198F1A8F1B8FCE01A7 |
:1074E0004D966BE67FE30E94BED4CE01889662E69D |
:1074F0007FE30E94BED41BAA1C8E8FEF9FEFAFEFDD |
:10750000BFEF8CAB9DABAEABBFAB18AE19AE1AAF35 |
:107510001BAFCE01CC9662E57FE30E94BED480EA29 |
:107520009EE162E47FE30E94BED480EB9EE162E3D1 |
:107530007FE30E94BED41092C01E1092C11E8AE04A |
:10754000E2ECFEE1DF011D928A95E9F78CEC9EE109 |
:1075500061E37FE30E94BED487ED9EE160E37FE3B9 |
:107560000E94BED40E946A680E9457680E945F68A9 |
:107570008CDE80E066E04BE15FE320E00E947FAAC2 |
:1075800082E167E048E15FE320E00E947FAA78940F |
:1075900088EC90E060E870E040E00E94D24C80E12E |
:1075A0000E94844C8823D9F32998DF91CF911F91B1 |
:1075B00008950F9382EB9DE160E070E049E857E0A9 |
:1075C0000E94D3D5D7D78091B51D823019F481E0C0 |
:1075D0008093B51D8091B21D8B3809F4BDC08757CB |
:1075E000853110F067DEB8C00E946A680E94576853 |
:1075F0000E945F688091B21D813828F481E08093F9 |
:107600006E1E80936F1E8091B21D823818F481E047 |
:107610008093BB1D8091B21D833880F41092701E40 |
:107620001092871E81E79EE163E470E40E94BED45D |
:107630008CE79EE16AE370E40E94BED48091B21DA3 |
:10764000843860F48FEF9FEFAFEFBFEF8093881E19 |
:107650009093891EA0938A1EB0938B1E8091B21DB9 |
:107660008538D8F410928C1E10928D1E81E0809384 |
:107670008E1E80938F1E80E99EE16AE270E40E9474 |
:10768000BED480EA9EE16AE170E40E94BED480EB41 |
:107690009EE16AE070E40E94BED48091B21D8738FA |
:1076A00010F41092C01E8091B21D883850F41092D0 |
:1076B000C11E82EC9EE16FEF70E04AE050E00E9454 |
:1076C0000DD58091B21D893860F48CEC9EE169E0A3 |
:1076D00070E40E94BED487ED9EE168E070E40E94F1 |
:1076E000BED48091B21D8A3838F48AE0E2ECFEE123 |
:1076F000DF011D928A95E9F78091B21D8B3810F455 |
:107700001092BC1D8BE88093B21D78940E94BBAF91 |
:1077100082E066EF7FE340E020E000E00E948BAA79 |
:1077200083E06AEE7FE340E020E00E948BAA85E0E0 |
:107730006AED7FE340E020E00E948BAA82E167E0EF |
:1077400047ED5FE320E00E947FAA80E10E94844C25 |
:107750008823D9F39ADDF8940F9108958091C61D7E |
:10776000813019F410928306089581E08093830696 |
:107770000895CF93DF93EC01FC0120E080818D32EE |
:1077800041F483818D3229F4422F445020E030E0CF |
:1077900006C02F5F3196283289F740E0F7CFFE010F |
:1077A000E20FF31FE40FF11D80818D3209F44F5F6A |
:1077B000FB01E20FF31FDE01A20FB31FA40FB11DE7 |
:1077C0008C9180832F5F3F4F2E30310541F7DF9141 |
:1077D000CF910895CF93DF93EC0140E020E030E0BB |
:1077E000FE01E20FF31FE40FF11D80818D3209F4D9 |
:1077F0004F5FFB01E20FF31FDE01A20FB31FA40FC7 |
:10780000B11D8C9180832F5F3F4F2E30310541F7A2 |
:10781000DF91CF910895CF93DF93EC01A0E0B0E02A |
:107820009881903239F48981803221F4A60FB71FF4 |
:107830001C9209C0FB01EA0FFB1F90831196219651 |
:10784000A231B10569F7DF91CF910895CF93DF930E |
:1078500044C00E942748EC0121E08030920711F0DB |
:107860000E948A49CE01807022E08030920729F17F |
:1078700022E08130920720F48050914049F533C0D6 |
:1078800024E08030920771F08050984009F5809193 |
:1078900085068F5F80938506809186068F5F809333 |
:1078A000860621C0809185068F5F80938506809132 |
:1078B00087068F5F8093870616C0809185068F5F4D |
:1078C00080938506809188068F5F809388060BC021 |
:1078D0008BE395E26C2F0E946EC48BE395E2CED7CA |
:1078E000882309F4B6CFDF91CF910895A0E5B0E0E9 |
:1078F000ECE7FCEB0C9423D200E010E088E8E82EE3 |
:1079000083E1F82E04C0A2DFC7010197F1F70F5FF2 |
:107910001F4F87E0023D180709F43DC08BE395E255 |
:107920006DE470E40E940DC5882369F334C08BE3D5 |
:1079300095E2B801AFD70F5F1F4F8BE395E26AE482 |
:1079400070E40E940AC5882391F3F80110828981AE |
:10795000882329F1CE01019663ED72E03BDF20E040 |
:1079600030E0F901EE54F24ED901AD52BD4F8C9189 |
:1079700082A72F5F3F4F2D30310599F78BE395E2BA |
:1079800067E470E40E940DC5882381F08BE395E2E3 |
:10799000C6D781E00CC07E010894E11CF11C8BE38A |
:1079A00095E263D7882311F48701C7CF80E0C05BDD |
:1079B000DF4FE6E00C943FD2A1E0B0E0E2EEFCEB5A |
:1079C0000C941AD2562E7C01CC244E010894811CB2 |
:1079D000911C28E8A22E23E1B22E94EF692E91E0AB |
:1079E000792E4EC0F70180810E944348892B09F40B |
:1079F0004AC08091E202882341F484EF91E0F301D0 |
:107A00003197F1F70197D9F732C0813021F4C501E0 |
:107A10000197F1F72CC0823051F5809183068130B7 |
:107A200031F505C013DFC5010197F1F703C0DD246F |
:107A300000E010E00F5F1F4F0A3C110529F08BE3B7 |
:107A400095E213D7882371F78BE395E2B40122D72F |
:107A5000F70190818981891779F0892F0E94434825 |
:107A6000892B89F0D394F3E0DF1611F705C0EEDE21 |
:107A70008BE395E2B4010ED7C3940894E11CF11C8A |
:107A8000C51408F4AFCF2196EFE00C9436D2A4E1F0 |
:107A9000B0E0EDE4FDEB0C941FD2B82EF62EE72EED |
:107AA000E82FF0E0EB31F10508F0B7C1EA57FF4FDE |
:107AB0008E010F5F1F4F0C944ED2C8016BEF70E424 |
:107AC0000E94BED4E7ECFDE1DE01159681918D9315 |
:107AD0008DE1EB3CF807D1F719868DC0C80166EF40 |
:107AE00070E46AC0C80161EF70E483C0C8016CEE45 |
:107AF00070E47FC0C80167EE70E47BC0C80162EE2D |
:107B000070E477C0C8016DED70E473C0C80168ED22 |
:107B100070E46FC0C80163ED70E46BC0C8016EEC27 |
:107B200070E467C0C80169EC70E463C0C80164EC2C |
:107B300070E45FC0C8016FEB70E45BC0C8016CEB20 |
:107B400070E457C0C80168EB70E453C0C80164EB2F |
:107B500070E44FC0C8016FEA70E44BC0C8016AEA24 |
:107B600070E40E94BED4F80101900020E9F731973B |
:107B70008F2D9E2D9C01D9018CE00D90019281509A |
:107B8000E1F7198A38C0C80165EA70E432C0C8015B |
:107B900060EA70E403C0C8016BE970E40E94BED4DF |
:107BA0006CEAE62E6DE0F62E29C0C80166E970E4A5 |
:107BB00003C0C80161E970E40E94BED458EBE52E11 |
:107BC0005BE0F52E1BC0C8016CE870E40E94BED4D7 |
:107BD000C8016CEC7DE10E9414D50DC0C80167E8B6 |
:107BE00070E407C0C80162E870E403C0C8016DE733 |
:107BF00070E40E94BED444E6E42EF12C8E010F5FA7 |
:107C00001F4FC8016BE770E40E94B3D41FDE8BE303 |
:107C100095E285D6F80101900020E9F73197E01B45 |
:107C2000F10BC8016E2FC8DE80918306882339F0DE |
:107C300000E010E038E8C32E33E1D32EE7C0B701EF |
:107C400080E090E00E94B1CF5B016C0120E030E465 |
:107C50004CE955E40E94D6D07B018C0120E030E055 |
:107C600040E85FE30E9411CF88231CF461E070E0DC |
:107C700024C0C801B70120E03FEF4FE757E40E945E |
:107C8000D2D01816B4F4C601B50120E030E040E2CD |
:107C900051E40E94D6D00E9485CF84EF91E005C0C8 |
:107CA000FC013197F1F76150704061157105C1F722 |
:107CB00007C0C801B7010E9485CFCB010197F1F73A |
:107CC0000E9486488BE395E22AD693C0BFDD85E00B |
:107CD000B81609F045C0B70180E090E00E94B1CF2E |
:107CE0005B016C0120E030E44CE955E40E94D6D001 |
:107CF0007B018C0120E030E040E85FE30E9411CF7F |
:107D000088231CF461E070E024C0C801B70120E0C2 |
:107D10003FEF4FE757E40E94D2D01816B4F4C601E3 |
:107D2000B50120E030E040E251E40E94D6D00E944C |
:107D300085CF84EF91E005C0FC013197F1F76150E8 |
:107D4000704061157105C1F707C0C801B7010E94F5 |
:107D500085CFCB010197F1F7C9DD282F30E05FC057 |
:107D60008BE395E266E770E458D6882309F444C0B3 |
:107D7000B70180E090E00E94B1CF5B016C0120E090 |
:107D800030E44CE955E40E94D6D07B018C0120E020 |
:107D900030E040E85FE30E9411CF88231CF461E0EB |
:107DA00070E024C0C801B70120E03FEF4FE757E47F |
:107DB0000E94D2D01816B4F4C601B50120E030E01C |
:107DC00040E251E40E94D6D00E9485CF84EF91E03A |
:107DD00005C0FC013197F1F76150704061157105E4 |
:107DE000C1F707C0C801B7010E9485CFCB01019739 |
:107DF000F1F721E030E013C08BE395E26EE670E42A |
:107E00000CD6882351F4C6010197F1F70F5F1F4F7D |
:107E100081E0063F180709F059CF20E030E0C901A2 |
:107E20006496EAE00C943BD2EF92FF920F93DF93BB |
:107E3000CF9300D0CDB7DEB7082F82E060E041E0FD |
:107E40000E943FAA85E190E00E947CCE9A838983BC |
:107E5000802F0E94EDCA2DB73EB72C5030403EBF58 |
:107E60002DBFEDB7FEB73196A2E0FA2EADB7BEB783 |
:107E70001196FC9211821282EE24E394E38226EBA7 |
:107E800031E43583248329813A81378326836087CF |
:107E90007187828793870E949DAFF092E2028DB72F |
:107EA0009EB70C969EBF8DBF802F0E9400480E94F7 |
:107EB00086488BE395E233D51092830680E060E03C |
:107EC00070E0E5DD85E160E070E0E1DD86E160E045 |
:107ED00070E0DDDDE092830680E060E070E0D7DDF9 |
:107EE000892B09F400E0802F0F900F90CF91DF9144 |
:107EF0000F91FF90EF90089592E09093E20210921C |
:107F00008306882351F086E160E070E0C0DD85E102 |
:107F100060E070E0BCDD80E005C084E160E070E01E |
:107F2000B6DD83E160E070E0B2CD0F931F93182FB0 |
:107F30008091E302181791F1112399F482E0809364 |
:107F4000E20281E0D9DF89E060E070E0A0DD892B0A |
:107F500001F11092E30287E060E070E098DD81D9E2 |
:107F600018C01130B1F402E00093E20281E0C4DFF6 |
:107F700088E060E070E08BDD892B59F01093E3021C |
:107F80008AE060E070E083DD68D980E0B5DF0093CF |
:107F9000E20290E08091E302181709F491E0892F42 |
:107FA0001F910F9108951F93CF93DF93182F82E0B5 |
:107FB0008093E2020E9486488BE395E2B0D481E090 |
:107FC0009BDF80E060E070E062DD892B19F420E047 |
:107FD00030E03CC01330B9F0143028F4113079F09F |
:107FE000123039F40EC0163041F0173021F0153040 |
:107FF00061F081E10BC08CE009C08DE007C08EE02C |
:1080000005C08FE003C080E101C082E160E070E064 |
:108010003EDDEC01812F0E9400480E9486488BE3E0 |
:1080200095E27DD480E060E070E031DD80E060E0EA |
:1080300070E02DDD892B11F410E004C010E0CD2B91 |
:1080400009F011E080E058DF212F30E0C901DF9115 |
:10805000CF911F9108951F938BE395E269E876E035 |
:1080600040E052E0F8D388EE93E024EF31E0F901EC |
:108070003197F1F70197D9F7E9DB8BE395E24FD41C |
:108080008091C61D813059F412E01093E20234DF72 |
:1080900080E04BDF882331F01093E20203C082E0DE |
:1080A0008093E2021F910895A0E0B2E0EAE5F0ECCF |
:1080B0000C9423D2109285068BE395E269E876E072 |
:1080C00040E052E0C8D30E94864884E060E040E08F |
:1080D00020E00E9419630E94EA6380E026DF882383 |
:1080E00009F46BC00E94EA6381E0809383068AE111 |
:1080F00060E070E0CCDC892B71F400D00F92EDB71A |
:10810000FEB7118280E690E4938382830E945B43F2 |
:108110000F900F900F900E94EA6304E610E0B0E128 |
:10812000EB2EB7E2FB2E92DBC7010197F1F78BE351 |
:1081300095E2A4D3882379F000D00F92EDB7FEB773 |
:10814000118283E590E4938382830E945B430F90C6 |
:108150000F900F9003C00150104029F70E94EA636E |
:108160007E010894E11CF11C1EC08BE395E2B8016E |
:1081700091D30F5F1F4F8BE395E260E570E44AD423 |
:10818000882399F3F80110828981882361F000D057 |
:108190000F92EDB7FEB71182F382E2820E9449434B |
:1081A0000F900F900F908BE395E25FD3882311F42B |
:1081B0008701E1CF0E94C74C81E0C050DE4FE6E06E |
:1081C0000C943FD2AFEFB0E0E8EEF0EC0C941FD28D |
:1081D00080E560E040E020E00E9419630E94EA63CD |
:1081E00080E0A3DE882309F4C7C00E94EA6380E030 |
:1081F00060E070E04CDC8BE395E291D386E060E0D8 |
:1082000070E045DC892B09F4B7C00E94EA6300E105 |
:1082100017E268E8E62E63E1F62E18DB0150104005 |
:10822000C7010197F1F78BE395E228D3882391F4F6 |
:1082300080E20E948E4C882319F0FF24F3940BC037 |
:108240000115110539F08BE395E26BE171E4E5D39B |
:10825000882319F3FF248BE395E261D301151105FF |
:1082600009F43CC0FF20D1F58CE49DE19093A11668 |
:108270008093A01610E050E1E52E57E2F52E21C0C4 |
:108280008091A0169091A11664E670E00E94B3D18F |
:10829000812F90E06817790759F08091A01690918E |
:1082A000A11664E670E00E94B3D1162F0E94EA6323 |
:1082B000CDDAC7010197F1F780E20E948E4C882346 |
:1082C00069F48091A0169091A116892B39F08BE367 |
:1082D00095E26FE071E471D3882391F20E94EA6322 |
:1082E000EE24FF245E010894A11CB11C44E0C42EBE |
:1082F000D12CCC0EDD1E35C08BE395E2B801CAD27D |
:108300000F5F1F4F8BE395E26CE071E483D388230A |
:1083100099F3F80110828981882319F1C50160E081 |
:1083200071E40E94DDD4892B19F58701000F111F1C |
:10833000C70125E0880F991F2A95E1F7080F191F3B |
:10834000B80167527A4DC60166DA03511A4DC6016B |
:10835000B8010FDA0894E11CF11CF0E1EF16F1040A |
:1083600039F08BE395E281D2882311F48501CACFDD |
:108370000E94C74C8E2D01C080E0C150DF4FEAE063 |
:108380000C943BD21FDF8093840608950F931F93B4 |
:10839000CF93DF93C0E0D0E008E813E104C056DAE1 |
:1083A000C8010197F1F72196C433D105C1F78BE3DA |
:1083B00095E26BE571E431D3DF91CF911F910F917D |
:1083C0000895EF92FF920F931F93CF93DF938C0149 |
:1083D000C0E0D0E078E8E72E73E1F72E45C036DA4A |
:1083E0008BE395E260E571E418D3882349F0CEDF92 |
:1083F00082E08093E20280E060E070E048DB38C019 |
:108400008BE395E268E471E408D3882359F088EEA1 |
:1084100093E024EF31E0F9013197F1F70197D9F7B3 |
:1084200081E00DC08BE395E26EE271E4F6D2882321 |
:1084300049F0ACDF80E060E070E029DB82E080930F |
:10844000E20218C098010115110511F021E030E099 |
:1084500080E090E005C0C7010197F1F781E090E06E |
:1084600082179307C4F321960C171D0708F0B7CFA6 |
:108470008091E202CDB7DEB7E6E00C943FD2A1E0F6 |
:10848000B0E0E5E4F2EC0C9421D210E00BE26E01D6 |
:108490000894C11CD11CE4EFEE2EE1E0FE2E09830E |
:1084A000C60161E089DA80E197E2F7013197F1F7DF |
:1084B0000197D9F71F5F143091F782E08093E202B1 |
:1084C00081E0809383060E9486488BE395E227D261 |
:1084D00082E060E070E0DBDA80ED97E072DF82300E |
:1084E00011F010E004C08BE395E219D211E083E0B3 |
:1084F00060E070E0CCDA892B09F410E0812F90E085 |
:108500002196E8E00C943DD2CF93DF93EC018BE30E |
:1085100095E269E876E040E052E09DD197D98BE39F |
:1085200095E2FDD180E090E04CDF8130E9F080E021 |
:1085300060E070E0ACDA84E0BE01A9DA892BA1F03A |
:1085400080E060E070E0A3DA81E060E070E09FDA54 |
:10855000892B51F080E29EE434DF20E030E081306E |
:1085600029F421E030E002C020E030E0C901DF91D1 |
:10857000CF910895ECECFDE101C031968081882314 |
:1085800019F08E32D1F706C08091CC1D803211F0E7 |
:10859000882331F48CEC9DE162EC71E40E94BED43E |
:1085A0008CEC9DE10C94D944FF920F931F93E2DF72 |
:1085B00083E066EA71E440E050E02EE531E48901B1 |
:1085C0000E94AA65882309F442C1C80161E041E024 |
:1085D0000E94476485E190E00E947CCE8C018091EE |
:1085E000B716FBD72DB73EB72C5030403EBF2DBF3E |
:1085F000EDB7FEB7319622E0ADB7BEB711962C931A |
:108600001182128221E023832AE931E43583248315 |
:108610001783068360877187828793870E949DAF47 |
:108620002DB73EB7245F3F4F3EBF2DBF82E048D6F7 |
:10863000109283068BE395E269E876E040E052E031 |
:108640000AD18091B716F0DB882309F0B3C087E028 |
:10865000EBDB882309F0AEC081E0E6DB882309F07C |
:10866000A9C082E0E1DB882309F0A4C083E0DCDB61 |
:10867000882309F09FC084E0D7DB882309F09AC0E3 |
:1086800085E0D2DB882309F095C088E891E461E0B9 |
:1086900040ED57E021E001E00E94A8661092C31D62 |
:1086A0008091271E0E94004881E00AD60E94C74C94 |
:1086B000CEC083E060E070E0EAD9892BD1F0A8C099 |
:1086C0008AE060E070E0E3D9892B09F0A6C012E0EF |
:1086D0002EC08BE060E070E0DAD9892B09F0A2C0EF |
:1086E00013E025C088E160E070E0D1D9892B51F01A |
:1086F0009EC011E01CC089E160E070E0C8D9892B00 |
:1087000061F09AC014E013C085E060E070E0BFD96A |
:10871000892B09F096C016E00AC015E008C08091C8 |
:10872000271E41DC892B11F418E001C010E0809174 |
:10873000271E0E9400481123F1F480E0DDDB11E0E8 |
:108740001093E3020E9429B90E942DB98091C61DA1 |
:10875000813019F41092830602C01093830681E0E1 |
:10876000AFD588E791E460E048EE53E021E001E016 |
:108770000E94A8666CC081E0A3D500D000D0EDB700 |
:10878000FEB731968AE691E4ADB7BEB712969C93D8 |
:108790008E931197128313820E9472470F900F904D |
:1087A0000F900F9061E040ED57E021E001E00E9462 |
:1087B000B8664DC00EE511E4C80161E041E00E94D9 |
:1087C000476400D000D000D0EDB7FEB73196A2E0EC |
:1087D000FA2EADB7BEB71196FC921182128281E0DB |
:1087E0008383158304830E949DAF2DB73EB72A5F14 |
:1087F0003F4F3EBF2DBF87E060E040E020E00E9499 |
:108800001963F092E2020E94EA63882309F051CFD3 |
:108810000E94EA63882309F053CF0E94EA63882309 |
:1088200009F057CF0E94EA63882309F05BCF0E94CA |
:10883000EA63882309F05FCF0E94EA63882309F086 |
:1088400063CF0E94EA63882309F470CF68CF1F9139 |
:108850000F91FF900895FC015583448377836683CD |
:1088600013821282118210820895FC0120E080811F |
:108870009181892B09F421E0822F0895FC0140E0C9 |
:1088800024813581808191812817390709F441E0DD |
:10889000842F0895CF93DF93EC01DB0188819981C8 |
:1088A000892B11F480E017C0EE81FF818A819B81C2 |
:1088B000E80FF91F80818C938A819B816C817D8177 |
:1088C00001960E94B3D19B838A8388819981019705 |
:1088D0009983888381E0DF91CF910895CF93DF93CF |
:1088E000EC01462F6C817D812881398162177307E5 |
:1088F00011F480E011C08A819B81820F931F0E9436 |
:10890000B3D1EE81FF81E80FF91F408388819981FF |
:1089100001969983888381E0DF91CF910895FC01CE |
:10892000118210821382128281E00895A0E0B0E0EB |
:10893000ECE9F4EC0C9421D2EC016B017A0100E03B |
:1089400010E019C0C8018E0D9F1D8217930778F59E |
:108950008A819B818E0D9F1D6C817D81800F911F6F |
:108960000E94B3D1EE81FF81E80FF91F8081841747 |
:10897000F1F40F5F1F4FF601E00FF11F44912881C2 |
:1089800039814423F9F68A819B818E0D9F1D6C816C |
:108990007D81800F911F0E94B3D19B838A832E1902 |
:1089A0003F09201B310B3983288381E001C080E01F |
:1089B000CDB7DEB7E8E00C943DD20F931F93CF9371 |
:1089C000DF93EC018B0120E030E016C088819981B3 |
:1089D00028173907C8F48A819B816C817D81820FB9 |
:1089E000931F0E94B3D1EE81FF81E80FF91F8081B0 |
:1089F000841751F42F5F3F4FF801E20FF31F4491AA |
:108A0000442321F781E001C080E0DF91CF911F91E5 |
:108A10000F91089540E050E089CFEF92FF920F93BD |
:108A20001F93CF93DF938C017B01C0E0D0E009C09E |
:108A3000C801B701AE017ADF882311F081E008C0D8 |
:108A40002196F80180819181C817D90788F380E0C9 |
:108A5000CDB7DEB7E6E00C943FD2BC0140E020E0A9 |
:108A600030E09AE3E42FF0E0EB5EF74DDB01A20F7C |
:108A7000B31F8C91828B4F5F213011F0233031F482 |
:108A8000E42FF0E0EB5EF74D928B4F5F2F5F3F4F8F |
:108A90002630310539F7E42FF0E0EB5EF74D128A0E |
:108AA0000895A0E0B0E0E7E5F5EC0C9417D22C01B6 |
:108AB00033242224EE24FF2487010F2EF0E06F2EB2 |
:108AC000F0E07F2EF0E08F2EF0E09F2EF02D0F2EA5 |
:108AD000FDECAF2EFCECBF2EFCECCF2EFDE3DF2E29 |
:108AE000F02D45C06D3219F42224239440C06B321E |
:108AF000F1F16E3219F43324339439C0C62FD0E02B |
:108B00003320A9F4C801B7012AE030E040E050E08A |
:108B10000E9488D17B018C01E097CE01AA2797FDA6 |
:108B2000A095BA2FE80EF91E0A1F1B1F20C0E09760 |
:108B3000BE01882777FD8095982F0E94B3CF9B01B7 |
:108B4000AC01C601B5010E94D6D09B01AC01C401A5 |
:108B5000B3010E94ADCE3B014C01C601B50120E03E |
:108B600030E040E251E40E9418CF5B016C01F20159 |
:108B700061912F01662309F0B5CFC801B7010E94AA |
:108B8000B3CF9B01AC01C401B3010E94ADCE211053 |
:108B90009058CDB7DEB7E2E10C9433D2DC01FB0193 |
:108BA00002C0119631962C91222329F080818823CE |
:108BB00011F02817B1F330E08081281B3109C90179 |
:108BC000089580918908811181E0089520918E088F |
:108BD00030918F084091900850919108B901CA01D5 |
:108BE0000895A2E0B0E0E7EFF5EC0C941CD25A0136 |
:108BF0006B01BE016F5F7F4F4AE050E00E9459D386 |
:108C00007B018C01E981FA8180818E3249F54F0127 |
:108C10001AC0C801B7012AE030E040E050E00E94ED |
:108C200088D17B018C01872D90E0C097AA2797FD02 |
:108C3000A095BA2FE80EF91E0A1F1B1F0894A10861 |
:108C4000B108C108D108F4017180772049F0089477 |
:108C5000811C911CA114B104C104D104D1F616C029 |
:108C6000A114B104C104D10489F0F501EE0FFF1F76 |
:108C7000EE0FFF1FEA53FE4B25913591459154911C |
:108C8000C801B7010E9488D17B018C01B701C801DE |
:108C90002296EDE00C9438D28F929F92AF92BF92C1 |
:108CA000CF92DF92EF92FF920F931F93FC014B0143 |
:108CB0008281482F50E04051524080812AE0829FBB |
:108CC000C00111242181820F911D805E91409C0181 |
:108CD00073E0220F331F7A95E1F7880F991F280F51 |
:108CE000391F420F531F5A01CC24B7FCC094DC2C0F |
:108CF000CF01039646E050E060E070E072DF7B0158 |
:108D00008C01C601B50120E836E948E950E00E942F |
:108D100088D15B016C01C801B70126E030E040E07A |
:108D200050E00E94FCD12A0D3B1D4C1D5D1DF4013D |
:108D30008081873539F450954095309521953F4F86 |
:108D40004F4F5F4FB901CA011F910F91FF90EF90F4 |
:108D5000DF90CF90BF90AF909F908F9008958F92AB |
:108D60009F92AF92BF92CF92DF92EF92FF920F93BA |
:108D70001F93FC014B0190818AE0989F9001112480 |
:108D80008181280F311D205132405901CC24B7FC7C |
:108D9000C094DC2CCF01029646E050E060E070E029 |
:108DA00020DF7B018C01C601B50120E836E948E9E6 |
:108DB00050E00E9488D15B016C01C801B70126E038 |
:108DC00030E040E050E00E94FCD12A0D3B1D4C1DDC |
:108DD0005D1DF4018081833539F4509540953095BF |
:108DE00021953F4F4F4F5F4FB901CA011F910F911E |
:108DF000FF90EF90DF90CF90BF90AF909F908F90BB |
:108E0000089580910628282F30E0813420F0A901B0 |
:108E10004753504003C0A901405350408091072858 |
:108E2000282F30E0813418F02753304002C02053FF |
:108E3000304074E0440F551F7A95E1F7240F351F39 |
:108E40008091FA279091FB2782179307B1F0109237 |
:108E5000890880918E0890918F08A0919008B09118 |
:108E600091080196A11DB11D80938E0890938F08E3 |
:108E7000A0939008B0939108089582E398E26DE67C |
:108E800071E08CDE892B09F00AC18CE898E20E941F |
:108E900085D4809363298093232880918C28803304 |
:108EA00081F41092702910927129109272291092F7 |
:108EB00073291092742910927529109276291092B4 |
:108EC000772980914128282F30E0813418F02753EA |
:108ED000304002C02053304080914228482F50E05B |
:108EE000813418F04753504002C040535040C901EC |
:108EF00063E0880F991F6A95E1F7220F331F820FF5 |
:108F0000931F840F8093302880914328282F30E0CE |
:108F1000813418F02753304002C0205330408091F4 |
:108F20004428482F50E0813418F04753504002C085 |
:108F300040535040C901B3E0880F991FBA95E1F73B |
:108F4000220F331F820F931F840F80937829809103 |
:108F50004528282F30E0813418F02753304002C0D4 |
:108F60002053304080914628482F50E0813418F03B |
:108F70004753504002C040535040C901F3E0880FAE |
:108F8000991FFA95E1F7220F331F820F931F840F69 |
:108F900080937E2981E498E260DD80E598E26FE5C8 |
:108FA00078E2DDDE609370297093712980937229D5 |
:108FB000909373298EE698E26DE778E26DDE609318 |
:108FC000742970937529809376299093772920913D |
:108FD0007029309171294091722950917329209301 |
:108FE000192830931A2840931B2850931C2860930B |
:108FF0001D2870931E2880931F28909320288BE9AA |
:1090000098E24FDD7DD7609364298BE998E20E9456 |
:1090100085D4809324288AEA98E241E050E060E019 |
:1090200070E0DFDD70937A296093792970932628A8 |
:109030006093252889EB98E241E050E060E070E021 |
:10904000D0DD709362296093612970932228609328 |
:10905000212881E08093890880918A0890918B086B |
:10906000A0918C08B0918D080196A11DB11D80932F |
:109070008A0890938B08A0938C08B0938D088091F8 |
:109080008A0890918B08A0918C08B0918D088093EC |
:10909000152890931628A0931728B09318280895A0 |
:1090A000682F843209F581E08093032810923128DB |
:1090B0001092FB271092FA2710927F2910926029B4 |
:1090C00010925F2910926F2910926E291092052834 |
:1090D000109204281092FE271092FD2784EF91E051 |
:1090E000909383168093821680910328882309F435 |
:1090F00094C06D3011F06A3061F540916E29509145 |
:109100006F292091FD273091FE27809104289091AE |
:109110000528281B390BFA01B4E0EE0FFF1FBA95A2 |
:10912000E1F7E41BF50BE20FF31FEE5CF74D108245 |
:109130004F5F5F4F50936F2940936E29E0915F29F5 |
:10914000F0916029EA5FF74D10821092032859DEF2 |
:1091500064C080913128882321F06A3211F4109282 |
:10916000312880913128882361F0262F30E08091CA |
:10917000FA279091FB27822793279093FB278093D0 |
:10918000FA27643219F481E08093312880917F2995 |
:10919000882369F080915F2990916029FC01EA5F42 |
:1091A000F74D608301969093602980935F296A321E |
:1091B00019F481E080937F2920916E2930916F29E5 |
:1091C0004091FD275091FE27809104289091052819 |
:1091D000FA01E81BF90BCF01F901A4E0EE0FFF1F24 |
:1091E000AA95E1F7E21BF30BE80FF91FEE5CF74DD0 |
:1091F00060836C3261F410822F5F3F4F30936F2990 |
:1092000020936E29509305284093042806C04F5F91 |
:109210005F4F5093FE274093FD2780916E2908955C |
:1092200011B814B817B81AB880B18093852983B1E2 |
:109230008093872986B18093842989B1809381297D |
:1092400017B84798379B02C081E001C082E0809345 |
:1092500086294098309B03C01092832903C081E087 |
:1092600080938329409A089580918629813009F45A |
:10927000479880918629823009F45F9808958091FB |
:109280008629813009F4479A80918629823009F431 |
:109290005F9A089580918629813009F45F9A8091C0 |
:1092A0008629823009F4479A089580918629813071 |
:1092B00009F45F9880918629823009F447980895CF |
:1092C000982F80918329882319F088B1847F88B9E9 |
:1092D000923051F0933001F1913009F5809183295A |
:1092E000439A10929308089580918329882311F05E |
:1092F000409A01C0439880919308882381F481E0CB |
:109300008093930880E395E724EF31E0F9013197EA |
:10931000F1F70197D9F70895419A109293080895AB |
:1093200080919308882399F480918329882311F0F0 |
:1093300082E0C6CF439881E08093930880E395E76D |
:1093400024EF31E0F9013197F1F70197D9F708954A |
:1093500080918329882311F081E0B2CF439A109243 |
:1093600093080895CF92DF92EF92FF920F931F938D |
:1093700057DF80918329882331F087B1836087B9D3 |
:1093800088B1847F88B982B1806F82B90C9880914E |
:109390006D1E813009F409988FEF84B988B1806916 |
:1093A00088B987B18C6E87B93C988091832943989E |
:1093B0008AB1806F8AB95E9A6DDF299A0E945E4CED |
:1093C00081E080937D1684E190E00E94324984E13F |
:1093D00090E00E94E04780E00E9436B00E94D9BA37 |
:1093E00078948091271E8093B7160E940048809140 |
:1093F000271E0E94634981E00E9436B080E991E017 |
:1094000060E870E040E00E94D24C109263060E9437 |
:10941000CDA38091B51D8330A8F081E08093B51D68 |
:1094200082E090E060E070E041E050E02CE001E09C |
:10943000EE24FF24CC24DD240E94D0568093B51D59 |
:109440000E9445B980E00E94556480918329882359 |
:1094500011F41BE009C082E062E172E440E020E028 |
:1094600007E00E948BAA15E080918629813039F4AB |
:1094700060E072E440E020E0012F0E948BAA80911E |
:109480008629823041F481E06EEE71E440E020E014 |
:10949000012F0E948BAA86E090E02ED4BC0185E0CB |
:1094A00040E020E002E00E948BAA87E090E024D414 |
:1094B000BC0186E040E020E004E00E948BAA80E04E |
:1094C00066E24FE728E101E092E0E92E0E9411A94F |
:1094D00088E293E224EF31E0F9013197F1F7019747 |
:1094E000D9F70799299888EE93E024EF31E0F90144 |
:1094F0003197F1F70197D9F780E991E060E870E0E2 |
:1095000040E00E94D24C80EF0E94844C21DF809189 |
:10951000C21D813031F48091C41D882311F40E9452 |
:10952000AB338091C31D813029F48091C51D882300 |
:1095300009F43AD80E94AEBB8091C21D813031F44B |
:109540008091C01D882311F485E001C086E007D01A |
:109550001F910F91FF90EF90DF90CF900895429A66 |
:10956000289A469A459A823051F0833018F488231D |
:10957000F9F008958530C1F0863071F517C08091FB |
:10958000C9008F7E8093C9008091C900877F809336 |
:10959000C9008091C9008F778093C9005298539871 |
:1095A0005A985B984298089545980895469808956A |
:1095B0008091C9008F7E8093C9008091C900877F08 |
:1095C0008093C9008091C9008F778093C900529819 |
:1095D00053985A985B9828980895843049F18530BB |
:1095E00038F48230D9F08330F0F4813059F511C06D |
:1095F000863029F0863008F1873021F505C020EC4F |
:1096000032E140E050E022C020E639E040E050E0A6 |
:109610001DC020E835E240E050E018C020E03BE407 |
:1096200040E050E013C020E036E940E050E00EC0DA |
:1096300020E031EE40E050E009C020E032EC41E0B3 |
:1096400050E004C020E030E040E050E0B901CA0141 |
:1096500008950E94BBAF80E060E040EB52E422E05E |
:109660000E947FAA80E061E04AE952E422E00E9481 |
:109670007FAA80E063E044E852E420E00E947FAAF1 |
:1096800080E064E04EE652E420E00E947FAA80E0A1 |
:1096900065E048E552E420E00E947FAA80E066E0B1 |
:1096A00042E452E420E00E947FAA8DE190E024D3BE |
:1096B000AC018CE067E020E00E947FAA82E000DE3F |
:1096C00082E04DDF80E20E94844C8823D9F38091B0 |
:1096D000C01D813011F486E001C085E040DF81E0EB |
:1096E000EFCD02C00E94ADA480E20E94844C88238A |
:1096F000C9F384E190E00E94E0478091271E0E9418 |
:1097000000488091C01D813011F486E001C085E0E1 |
:1097100026DF81E0D5CDEF92FF920F931F93CF9379 |
:10972000DF937C01EB01142F0E94BBAF80E060E06F |
:109730004FE727E001E00E941BA781E060E0A7015E |
:1097400022E00E947FAA0E94ADA482E0B70140E01F |
:1097500020E00DEF0E948BAA80E690E0CDD2BC0104 |
:1097600083E040E020E001E00E948BAA112321F475 |
:109770008881882371F41DC0CE010E94C5D4892B35 |
:10978000C1F085E0BE0140E020E001E00E948BAA2C |
:1097900007C085E0BE0140E020E001E00E9480AF0C |
:1097A00080E065E24FE72EE001E092E0E92E0E94C2 |
:1097B00011A98DE090E0A0D2AC018CE067E020E040 |
:1097C0000E947FAACDB7DEB7E6E059C68CE192E4ED |
:1097D00064E771E040E09FDF80E0C1DE82CF81E39B |
:1097E00092E46AE272E441E096DF82E0B8DE79CF8B |
:1097F00086EC92E464E771E040E08DDF83E060DDB9 |
:1098000080E0ADDE6ECF84EE92E463ED72E441E081 |
:1098100082DF83E055DD80E0A2DE63CF8CE791E05C |
:1098200067ED7DE10E941FD582EF92E465E771E06C |
:1098300040E071DF82E044DD88E99AE324EF31E023 |
:10984000F9013197F1F70197D9F78091C61D882367 |
:1098500019F481E00E9495BF80E081DE42CF8EEF57 |
:1098600092E464E771E040E056DF82E029DD80E1C8 |
:1098700097E224EF31E0F9013197F1F70197D9F739 |
:109880008091C61D882319F481E00E9495BF82E073 |
:1098900066DE27CF67DD10929508109296080E9429 |
:1098A0009C9380910F1E9091101EA091111EB0915B |
:1098B000121E181619061A061B067CF48091131E38 |
:1098C0009091141EA091151EB091161E181619061F |
:1098D0001A061B0614F40E94098181E00E94D599A2 |
:1098E00022D1FECF0F931F9387E061E040E050E06C |
:1098F0002EE033E400E213E40E94A73D0E94343DD1 |
:1099000082E062E04BE75FE72EE233E408E313E432 |
:109910000E94A73D0E94343D01E413E488E062E028 |
:109920004EEC5DEC98010E94A73D0DE413E489E044 |
:1099300062E040E050E098010E94A73D07E513E493 |
:109940008AE061E042E95EE598010E94A73D1F912F |
:109950000F9108950F931F938CE061E040E050E079 |
:1099600020E633E489010E94A73D8091C618882330 |
:1099700049F084E062E042E155E928E633E48901F8 |
:109980000E94A73D83E061E043EE5FE924E733E412 |
:1099900089010E94A73D85E061E049E05EE92FE78B |
:1099A00033E408E813E40E94A73D0E94343D8BE0B5 |
:1099B00062E044E35EEC20E933E40CE913E40E9446 |
:1099C000A73D88E062E04EEC5DEC21E433E48901E0 |
:1099D0000E94A73D89E062E040E050E02DE433E4DE |
:1099E00089010E94A73D8AE061E042E95EE527E542 |
:1099F00033E489010E94A73D1F910F9108950F93B1 |
:109A00001F9381E061E045EF5BE728EA33E48901D9 |
:109A10000E94A73D82E062E04BE75FE72EE233E47D |
:109A200008E313E40E94A73D0E94343D8CE061E00E |
:109A300040E050E020E633E489010E94A73D809198 |
:109A4000C618882349F084E062E042E155E928E63F |
:109A500033E489010E94A73D83E061E043EE5FE9C2 |
:109A600024E733E489010E94A73D85E061E049E0F5 |
:109A70005EE92FE733E408E813E40E94A73D86E09F |
:109A800061E047E551EA2CEA33E489010E94A73DF1 |
:109A90000E94343D8BE062E044E35EEC20E933E475 |
:109AA0000CE913E40E94A73D88E062E04EEC5DEC17 |
:109AB00021E433E489010E94A73D89E062E040E0AF |
:109AC00050E02DE433E489010E94A73D8AE061E083 |
:109AD00042E95EE527E533E489010E94A73D1F9135 |
:109AE0000F9108950E94C73D84EE92E00E941837BE |
:109AF0008CE190E001D1BC0185E046EE56E60E9483 |
:109B0000713688E660E070E04EEC5DEC0E947136E4 |
:109B10008091B118882309F072CF8091B018882302 |
:109B200009F018CFDFCE1F931091C31D0E94864805 |
:109B30000E94CDA380EF0E94844C89EE92E061E800 |
:109B400071E04CE050E090D781E00E942F35CADFF1 |
:109B500080E00E94783E0E943136873021F481E017 |
:109B60000E94D59907C08C3059F480E090E061E004 |
:109B70000E94229B882341F00E94A637B3DF04C0D5 |
:109B8000893011F40E94E9578091C31D181701F321 |
:109B90000E94A637A7DF1091C31DDACF0F931F9342 |
:109BA0000E94C73D88E094E40E9431378091C31D34 |
:109BB000882369F08091C21D882349F088E261E022 |
:109BC0004EE05CEC28EB33E489010E94A73D8091D4 |
:109BD0008329882369F08091C11E882349F08DE292 |
:109BE00061E048EF5BEC26EC33E489010E94A73D7D |
:109BF00080918329882369F08091701E882349F021 |
:109C00008CE261E043E05CEC24ED33E489010E94E6 |
:109C1000A73D8091C21D882349F089E261E046EEAC |
:109C20005BEC22EE33E489010E94A73D8AE261E009 |
:109C30004FEE5BEC20EF33E489010E94A73D809159 |
:109C4000C31D882349F08BE261E04FE25CEC2CEF0E |
:109C500033E489010E94A73D80E00E94783E0E9483 |
:109C6000A6371F910F9108950F931F930E94C73D30 |
:109C70000E944B378091C31D8823B1F08091B118A9 |
:109C8000882391F08FE361E04EEB55EB24E134E45F |
:109C900089010E94A73D80E461E041EE5BE52EE191 |
:109CA00034E489010E94A73D0E94343D8CE361E0C9 |
:109CB00047E65EEC2EE234E489010E94A73D80E095 |
:109CC0000E94783E0E94A6371F910F9108958091BF |
:109CD0009508882319F481E00E94D7878091950820 |
:109CE000813019F482E00E94D787809195088230F4 |
:109CF00011F40C94D78708959C018091B51D823092 |
:109D000018F081E08093B51D8091B51D813051F42C |
:109D1000F901EE0FFF1FEE0FFF1FE35CFB4B859178 |
:109D200094910895F901EE0FFF1FEE0FFF1FE55C00 |
:109D3000FB4B859194910895282F8091B51D823019 |
:109D400018F081E08093B51D8091B51D813011F030 |
:109D5000622F792FCB0108955058BB27AA270ED028 |
:109D6000D6C19FD130F0A4D120F031F49F3F11F43F |
:109D70001EF45EC10EF4E095E7FB54C1E92FE9D172 |
:109D800080F3BA17620773078407950718F071F418 |
:109D90009EF501C20EF4E0950B2EBA2FA02D0B01FB |
:109DA000B90190010C01CA01A0011124FF27591B20 |
:109DB00099F0593F50F4503E68F11A16F040A22F26 |
:109DC000232F342F4427585FF3CF46953795279597 |
:109DD000A795F0405395C9F77EF41F16BA0B620B96 |
:109DE000730B840BBAF09150A1F0FF0FBB1F661FDD |
:109DF000771F881FC2F70EC0BA0F621F731F841F20 |
:109E000048F4879577956795B795F7959E3F08F045 |
:109E1000B3CF9395880F08F09927EE0F9795879504 |
:109E20000895DCD008F481E008954AD1E39580C11B |
:109E30000CD06DC13DD140F034D130F021F45F3F02 |
:109E400019F0F0C05111A8C1F3C083D198F3992340 |
:109E5000C9F35523B1F3951B550BBB27AA276217EE |
:109E60007307840738F09F5F5F4F220F331F441F33 |
:109E7000AA1FA9F333D00E2E3AF0E0E830D091506B |
:109E80005040E695001CCAF729D0FE2F27D0660F58 |
:109E9000771F881FBB1F261737074807AB07B0E897 |
:109EA00009F0BB0B802DBF01FF2793585F4F2AF0AD |
:109EB0009E3F510568F0B6C06FC15F3FECF3983E1E |
:109EC000DCF3869577956795B795F7959F5FC9F70A |
:109ED000880F911D9695879597F90895E1E0660F93 |
:109EE000771F881FBB1F621773078407BA0720F00C |
:109EF000621B730B840BBA0BEE1F88F7E095089575 |
:109F000004D06894B11148C108952BD188F09F57AF |
:109F100090F0B92F9927B751A0F0D1F0660F771FB5 |
:109F2000881F991F1AF0BA95C9F712C0B13081F095 |
:109F300032D1B1E008952FC1672F782F8827B85FFD |
:109F400039F0B93FCCF3869577956795B395D9F7F6 |
:109F50003EF490958095709561957F4F8F4F9F4F00 |
:109F60000895E89409C097FB3EF49095809570950C |
:109F700061957F4F8F4F9F4F9923A9F0F92F96E955 |
:109F8000BB279395F695879577956795B795F111CA |
:109F9000F8CFFAF4BB0F11F460FF1BC06F5F7F4F67 |
:109FA0008F4F9F4F16C0882311F096E911C0772379 |
:109FB00021F09EE8872F762F05C0662371F096E882 |
:109FC000862F70E060E02AF09A95660F771F881F51 |
:109FD000DAF7880F9695879597F90895990F0008F5 |
:109FE000550FAA0BE0E8FEEF16161706E807F9076B |
:109FF000C0F012161306E407F50798F0621B730B06 |
:10A00000840B950B39F40A2661F0232B242B252B86 |
:10A0100021F408950A2609F4A140A6958FEF811D29 |
:10A02000811D089597F99F6780E870E060E00895CA |
:10A030009FEF80EC0895DF93CF931F930F93FF92D0 |
:10A04000EF92DF927B018C01689405C0DA2EEF015C |
:10A05000B9D0FE01E894A59125913591459155918E |
:10A06000AEF3EF018BDEFE019701A801DA9479F7D8 |
:10A07000DF90EF90FF900F911F91CF91DF910895A6 |
:10A080009F938F937F936F93FF93EF939B01AC010B |
:10A090008DD0EF91FF91CFDF2F913F914F915F9145 |
:10A0A00085C000240A941616170618060906089596 |
:10A0B00000240A9412161306140605060895B8CF54 |
:10A0C00050D0E8F3E894E0E0BB279F57F0F02AED8A |
:10A0D0003FE049EC06C0EE0FBB0F661F771F881FDD |
:10A0E00028F0B23A62077307840728F0B25A620B6D |
:10A0F000730B840BE3959A9572F7803830F49A9538 |
:10A10000BB0F661F771F881FD2F79048F5C0092E36 |
:10A110000394000C11F4882352F0BB0F40F4BF2BC2 |
:10A1200011F460FF04C06F5F7F4F8F4F9F4F089502 |
:10A13000EF93E0FF06C0A2EA2AED3FE049EC5FEBB7 |
:10A140001DDEE5DF0F90039401FC9058E2E4F1E09E |
:10A1500097CF57FD9058440F551F59F05F3F71F04E |
:10A160004795880F97FB991F61F09F3F79F087957E |
:10A170000895121613061406551FF2CF4695F1DF07 |
:10A1800008C0161617061806991FF1CF8695710597 |
:10A19000610508940895E894BB2766277727CB01CB |
:10A1A00097F908951BDF08F48FEF08950BD0AFCF18 |
:10A1B00078DF28F07DDF18F0952309F033CF38CF12 |
:10A1C0001124EACFC6DFA0F3959FD1F3950F50E09D |
:10A1D000551F629FF001729FBB27F00DB11D639F59 |
:10A1E000AA27F00DB11DAA1F649F6627B00DA11DFF |
:10A1F000661F829F2227B00DA11D621F739FB00DA5 |
:10A20000A11D621F839FA00D611D221F749F332714 |
:10A21000A00D611D231F849F600D211D822F762FAD |
:10A220006A2F11249F5750408AF0E1F088234AF0AA |
:10A23000EE0FFF1FBB1F661F771F881F91505040F6 |
:10A24000A9F79E3F510570F0EDCEA6CF5F3FECF32E |
:10A25000983EDCF3869577956795B795F795E795E2 |
:10A260009F5FC1F7FE2B880F911D9695879597F9F3 |
:10A27000089577DFE0F09E37D8F09639B8F49E382D |
:10A2800048F4672F782F8827985FF9CF86957795C0 |
:10A29000679593959539D0F3B62FB1706B0F711DFB |
:10A2A000811D20F4879577956795939508C022C006 |
:10A2B00073CF9F9305DF0F9007FCEE5F39CF8823A4 |
:10A2C00071F4772321F09850872B762F07C06623EF |
:10A2D00011F499270DC09051862B70E060E02AF0B0 |
:10A2E0009A95660F771F881FDAF7880F96958795DE |
:10A2F00097F908959F3F31F0915020F48795779515 |
:10A300006795B795880F911D9695879597F908954C |
:10A31000629FD001739FF001829FE00DF11D649F49 |
:10A32000E00DF11D929FF00D839FF00D749FF00DD5 |
:10A33000659FF00D9927729FB00DE11DF91F639F76 |
:10A34000B00DE11DF91FBD01CF0111240895991B26 |
:10A3500079E004C0991F961708F0961B881F7A951C |
:10A36000C9F780950895AA1BBB1B51E107C0AA1F1E |
:10A37000BB1FA617B70710F0A61BB70B881F991FA6 |
:10A380005A95A9F780959095BC01CD01089597FB4A |
:10A39000092E07260AD077FD04D0E5DF06D000207D |
:10A3A0001AF4709561957F4F0895F6F79095819511 |
:10A3B0009F4F0895A1E21A2EAA1BBB1BFD010DC0E1 |
:10A3C000AA1FBB1FEE1FFF1FA217B307E407F50765 |
:10A3D00020F0A21BB30BE40BF50B661F771F881F41 |
:10A3E000991F1A9469F760957095809590959B01D7 |
:10A3F000AC01BD01CF01089597FB092E05260ED0B3 |
:10A4000057FD04D0D7DF0AD0001C38F45095409592 |
:10A41000309521953F4F4F4F5F4F0895F6F7909538 |
:10A420008095709561957F4F8F4F9F4F08952F9224 |
:10A430003F924F925F926F927F928F929F92AF92D4 |
:10A44000BF92CF92DF92EF92FF920F931F93CF9321 |
:10A45000DF93CDB7DEB7CA1BDB0B0FB6F894DEBFB8 |
:10A460000FBECDBF09942A88398848885F846E84DE |
:10A470007D848C849B84AA84B984C884DF80EE8028 |
:10A48000FD800C811B81AA81B981CE0FD11D0FB631 |
:10A49000F894DEBF0FBECDBFED010895EE0FFF1F94 |
:10A4A0000590F491E02D0994CF93DF93BC018230A5 |
:10A4B000910510F462E070E0A0918A29B0918B2997 |
:10A4C000ED01E0E0F0E040E050E021C088819981BA |
:10A4D0008617970769F48A819B81309719F09383D7 |
:10A4E000828304C090938B2980938A29FE0134C013 |
:10A4F0006817790738F44115510519F08417950745 |
:10A5000008F4AC01FE018A819B819C01E90120973E |
:10A51000E9F641155105A9F1CA01861B970B04976D |
:10A5200008F4BA01E0E0F0E02AC08D919C91119707 |
:10A5300084179507F9F46417750781F412968D91C5 |
:10A540009C911397309719F09383828304C0909362 |
:10A550008B2980938A29FD0132964FC0CA01861B40 |
:10A56000970BFD01E80FF91F6193719302978D938B |
:10A570009C9343C0FD01828193819C01D901109776 |
:10A58000A1F68091882990918929892B41F48091A5 |
:10A590008A0190918B019093892980938829409119 |
:10A5A0008C0150918D014115510541F44DB75EB7B5 |
:10A5B0008091880190918901481B590B209188292D |
:10A5C0003091892924173507B0F4CA01821B930BF7 |
:10A5D0008617970780F0AB014E5F5F4F8417950792 |
:10A5E00050F0420F531F5093892940938829F90155 |
:10A5F0006193719302C0E0E0F0E0CF01DF91CF9171 |
:10A600000895CF93DF93009709F450C0EC0122978F |
:10A610001B821A82A0918A29B0918B29109709F187 |
:10A6200040E050E0AC17BD0708F1BB83AA83FE01F0 |
:10A6300021913191E20FF31FAE17BF0779F48D918D |
:10A640009C911197280F391F2E5F3F4F3983288324 |
:10A6500012968D919C9113979B838A834115510586 |
:10A6600071F4D0938B29C0938A2920C012968D91C2 |
:10A670009C911397AD01009711F0DC01D3CFFA0143 |
:10A68000D383C28321913191E20FF31FCE17DF07ED |
:10A6900069F488819981280F391F2E5F3F4FFA0195 |
:10A6A000318320838A819B8193838283DF91CF9141 |
:10A6B00008952F923F925F926F927F928F929F9216 |
:10A6C000AF92BF92CF92DF92EF92FF920F931F93C0 |
:10A6D000CF93DF938C011B01EA016115710519F01D |
:10A6E000FB0191838083209749F0CE0102978397E5 |
:10A6F00028F020E030E040E050E0F3C0F801A19005 |
:10A700008F018A2D90E01ED1892BC1F7FDE2AF1693 |
:10A7100031F4F801A1908F015524539407C0FBE256 |
:10A72000AF1619F4F801A1908F015524209719F064 |
:10A73000C031D105C1F4F0E3AF1679F4F80180819E |
:10A74000883711F0883549F4F801A1800E5F1F4F5A |
:10A75000F2E05F2AC0E1D0E006C0209721F480E358 |
:10A76000A816E9F427C0C830D10531F1C930D105A8 |
:10A7700024F4C230D10531F50CC0CA30D10589F0BE |
:10A78000C031D105F9F4C12CD12CE12CB8E0FB2E5D |
:10A7900027C0C12CD12CE12CA0E4FA2E21C0CAE0A4 |
:10A7A000D0E0FCECCF2EFCECDF2EFCECEF2EFCE03E |
:10A7B000FF2E16C0C8E0D0E0C12CD12CE12CE0E186 |
:10A7C000FE2E0EC09E01442737FD4095542F60E0B9 |
:10A7D00070E080E090E8EEDDC901DA016C017D01F6 |
:10A7E00020E030E040E050E060E03E01882477FC6B |
:10A7F0008094982C70EDB72EBA0CE9E0EB1570F44C |
:10A800008A2D81548A3118F499ECB92E06C08A2D0C |
:10A8100081568A3148F589EAB82EBA0C8B2D90E022 |
:10A820008C179D070CF56F3FD9F0C216D306E406CE |
:10A83000F506A8F0CA01B901A401930169DD9B01E5 |
:10A84000AC012B0D311D411D511D2130F0E03F07A2 |
:10A85000F0E04F07F0E85F0710F461E001C06FEF30 |
:10A86000F801A1908F01C6CF2114310481F0662335 |
:10A8700031F001501040F1011183008308C051FEF6 |
:10A880001AC002501040F1011183008314C067FF09 |
:10A8900012C050FC05C02FEF3FEF4FEF5FE704C041 |
:10A8A00020E030E040E050E882E290E090938D2993 |
:10A8B00080938C2916C050FE08C050954095309565 |
:10A8C00021953F4F4F4F5F4F0CC057FF0AC082E2A8 |
:10A8D00090E090938D2980938C292FEF3FEF4FEFDD |
:10A8E0005FE7B901CA01DF91CF911F910F91FF90EE |
:10A8F000EF90DF90CF90BF90AF909F908F907F9020 |
:10A900006F905F903F902F900895FC0188279927C2 |
:10A91000E89421912032E9F3293010F02E30C8F369 |
:10A920002B3239F02D3231F4689403C0FDD0820F00 |
:10A93000911D219120532A30C8F31EF490958195E2 |
:10A940009F4F0895911160C1803219F08950855050 |
:10A95000D0F70895FB01DC0102C005900D92415033 |
:10A960005040D8F70895FB01DC010D900020E9F775 |
:10A97000119705900D920020E1F70895FB01DC018D |
:10A9800005900D920020E1F70895FC01059000204C |
:10A99000E9F7809590958E0F9F1F0895FB01DC01CC |
:10A9A0004150504048F005900D920020C9F701C079 |
:10A9B0001D9241505040E0F70895FB0155915523F9 |
:10A9C000A9F0BF01DC014D9145174111E1F759F4A0 |
:10A9D000CD010590002049F04D9140154111C9F37A |
:10A9E000FB014111EFCF81E090E001970895FB0159 |
:10A9F000DC0104C08D910190801921F44150504038 |
:10AA0000C8F7881B990B0895FB01DC0102C0019077 |
:10AA10000D9241505040D8F70895DC0101C06D936C |
:10AA200041505040E0F70895FB01DC010D900020FB |
:10AA3000E9F7119701900D920020E1F70895FB01CD |
:10AA4000DC0101900D920020E1F70895FB01DC018B |
:10AA50004150504030F08D910190801919F4002040 |
:10AA6000B9F7881B990B0895FB01DC014150504058 |
:10AA700048F001900D920020C9F701C01D9241508D |
:10AA80005040E0F70895FB019F01E8944230BCF08C |
:10AA90004532ACF44A3029F497FB1EF49095819529 |
:10AAA0009F4F642F77275FDC805D8A330CF0895DD0 |
:10AAB0008193CB010097A9F716F45DE251931082C0 |
:10AAC000C901A5C0FA01CF93FF93EF9322303CF167 |
:10AAD00025322CF5C22FE894CA3049F497FB3EF496 |
:10AAE00090958095709561957F4F8F4F9F4F2C2F3C |
:10AAF000332744275527FF93EF935CDCEF91FF91B9 |
:10AB0000605D6A330CF0695D6193B901CA01605000 |
:10AB100070408040904059F716F4CDE2C193108206 |
:10AB20008F919F91CF9173C07AE0979F902D879FCF |
:10AB3000802D910D11240895FA01CF93FF93EF9387 |
:10AB40002230C4F02532B4F4C22F2C2F33274427EF |
:10AB50005527FF93EF932EDCEF91FF91605D6A33F1 |
:10AB60000CF0695D6193B901CA016050704080408A |
:10AB7000904059F710828F919F91CF9148C0FB016F |
:10AB80009F0142306CF045325CF4642F7727EBDB99 |
:10AB9000805D8A330CF0895D8193CB010097A9F722 |
:10ABA0001082C90134C0DC01CB01FC01F999FECF50 |
:10ABB00006C0F2BDE1BDF89A319600B40D92415045 |
:10ABC0005040B8F70895DC01A40FB51F4150504024 |
:10ABD00040F0CB01840F951F2E9105D0415050407D |
:10ABE000D8F70895262FF999FECF92BD81BDF89A26 |
:10ABF000019700B4021639F01FBA20BD0FB6F894C1 |
:10AC0000FA9AF99A0FBE0895992788270895DC01CA |
:10AC1000FC01672F71917723E1F7329704C07C9193 |
:10AC20006D9370836291AE17BF07C8F30895F894CF |
:02AC3000FFCF54 |
:10AC32004254204465766963657300332E396D0092 |
:10AC42003F3F3F3F00332E3978002044656C00506F |
:10AC52004B543A202D2D656D7074792D2D002531C0 |
:10AC6200643A20257300504B543A20257300253353 |
:10AC72002E356C6400253032752E253032752E2526 |
:10AC820030347500253032753A253032753A253028 |
:10AC9200327500253032753A25303275002447501E |
:10ACA200474741000052452D49443A203030303068 |
:10ACB20000332E38355F680020008E290000FF0126 |
:10ACC2000A000F00FFFF01FF760000AC1FAC1FACB3 |
:10ACD2001FAC1F300000B01FB01FB41FB91F3101DD |
:10ACE20001BC1FC01FC41FC71F50FFF8CB1FCB1FC3 |
:10ACF200D01FD01F430110D81FD81FDD1FE61F53DE |
:10AD0200111CF11FF11FF61F04204100001420182E |
:10AD120020142018204900001C201C202020202064 |
:10AD2200571D1D282028202C202C20551E1E352082 |
:10AD32003520392039204D1F1F4120412045204B0D |
:10AD42002058202052205220562056204F1E1E14DA |
:10AD5200201820142018204E1F1F5E2062205E2023 |
:10AD62006220461E1E662066206A206A20481F1F37 |
:10AD72006F206F207320732051202084208420882C |
:10AD82002088204400009B209B209F209F2054F776 |
:10AD9200F6A820A820AE20AE204B0000B620BA2094 |
:10ADA200BE20CD20560101D720DB20DF20ED20522E |
:10ADB2000202FB20FF20032110214703031D212152 |
:10ADC2002125213421FF00000000000000000000C6 |
:10ADD2006AF437690837681E36673635664E346559 |
:10ADE2004634646833628E3261B63160E2305FE2CB |
:10ADF200305EDA305DD2305CD2305BCA305ACA3053 |
:10AE0200FF0000313233343536373839303132339E |
:10AE120034000201504B5420762020202020202094 |
:10AE2200202020202020202020001F1C1F1E1F1E4B |
:10AE32001F1F1E1F1E1FACADAAABAEAFB0B1B2B387 |
:10AE4200B4B5FE5ADFFECBE2FFE6EBE9EACBECED6E |
:10AE5200E8FF0D0E0FFE57FFD2D3D4D5FE313233A9 |
:10AE62003738FFE6C8C9E7CAFE0608040A070B76A8 |
:10AE7200050C095EFE626377FF2324EEE3BEC02564 |
:10AE82002660FE2728EFBF292A61FE2B2C2EF02FE9 |
:10AE9200F130F2FE70717273FFEB47485150494C2A |
:10AEA2004A4D4B4E4FFE525374545556CEDDFE5C06 |
:10AEB200F4FE797A6FFE75FF4041D6D7E1466D6E9A |
:10AEC200FE4243D9D8FE4445FF10111364F359CF13 |
:10AED200FE1A6665FE1CDE1B5D5FCDFED0DCE0FF68 |
:10AEE2001415161718FE3BEDFE1239FE1E3A195BB9 |
:10AEF200FF1F2021223C3D3E3FFFEC343536FFE66A |
:10AF0200060C62FEEB47CE5CFE59CFFFF36E95F462 |
:10AF12009195FF000000008F001E01AD013B02CAA7 |
:10AF2200025803E603740402058F051B06A70633C5 |
:10AF320007BE074808D2085B09E3096B0AF20A78E0 |
:10AF42000BFD0B810C040D860D070E870E060F8478 |
:10AF52000F00107B10F5106E11E5115B12CF12423B |
:10AF620013B31323149214FE146A15D3153B16A1BE |
:10AF72001605176717C81727188318DE1837198E92 |
:10AF820019E319361A871AD61A231B6E1BB61BFD34 |
:10AF92001B411C831CC31C001D3C1D751DAB1DE009 |
:10AFA2001D121E421E6F1E9A1EC31EE91E0D1F2E6B |
:10AFB2001F4D1F691F841F9B1FB01FC31FD31FE19B |
:0CAFC2001FEC1FF51FFB1FFF1F002000ED |
:00000001FF |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/Font8x8.c |
---|
0,0 → 1,273 |
/* |
* font8x8.c |
* LCD-OSD |
* |
* Created by Peter Mack on 26.12.09. |
* Copyright 2009 SCS GmbH & Co. KG. All rights reserved. |
* |
*/ |
#include <avr/pgmspace.h> |
const uint8_t Font8x8[256][8]PROGMEM= |
{ |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x00 |
{0x7E,0x81,0x95,0xB1,0xB1,0x95,0x81,0x7E}, // 0x01 |
{0x7E,0xFF,0xEB,0xCF,0xCF,0xEB,0xFF,0x7E}, // 0x02 |
{0x0E,0x1F,0x3F,0x7E,0x3F,0x1F,0x0E,0x00}, // 0x03 |
{0x08,0x1C,0x3E,0x7F,0x3E,0x1C,0x08,0x00}, // 0x04 |
{0x38,0x3A,0x9F,0xFF,0x9F,0x3A,0x38,0x00}, // 0x05 |
{0x10,0x38,0xBC,0xFF,0xBC,0x38,0x10,0x00}, // 0x06 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x07 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x08 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x09 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0A |
{0x70,0xF8,0x88,0x88,0xFD,0x7F,0x07,0x0F}, // 0x0B |
{0x00,0x4E,0x5F,0xF1,0xF1,0x5F,0x4E,0x00}, // 0x0C |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0D |
{0xC0,0xFF,0x7F,0x05,0x05,0x65,0x7F,0x3F}, // 0x0E |
{0x99,0x5A,0x3C,0xE7,0xE7,0x3C,0x5A,0x99}, // 0x0F |
{0x7F,0x3E,0x3E,0x1C,0x1C,0x08,0x08,0x00}, // 0x10 |
{0x08,0x08,0x1C,0x1C,0x3E,0x3E,0x7F,0x00}, // 0x11 |
{0x00,0x24,0x66,0xFF,0xFF,0x66,0x24,0x00}, // 0x12 |
{0x00,0x5F,0x5F,0x00,0x00,0x5F,0x5F,0x00}, // 0x13 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0x14 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0x15 |
{0x00,0x70,0x70,0x70,0x70,0x70,0x70,0x00}, // 0x16 |
{0x80,0x94,0xB6,0xFF,0xFF,0xB6,0x94,0x80}, // 0x17 |
{0x00,0x04,0x06,0x7F,0x7F,0x06,0x04,0x00}, // 0x18 |
{0x00,0x10,0x30,0x7F,0x7F,0x30,0x10,0x00}, // 0x19 |
{0x08,0x08,0x08,0x2A,0x3E,0x1C,0x08,0x00}, // 0x1A |
{0x08,0x1C,0x3E,0x2A,0x08,0x08,0x08,0x00}, // 0x1B |
{0x3C,0x3C,0x20,0x20,0x20,0x20,0x20,0x00}, // 0x1C |
{0x08,0x1C,0x3E,0x08,0x08,0x3E,0x1C,0x08}, // 0x1D |
{0x30,0x38,0x3C,0x3E,0x3E,0x3C,0x38,0x30}, // 0x1E |
{0x06,0x0E,0x1E,0x3E,0x3E,0x1E,0x0E,0x06}, // 0x1F |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x20 |
{0x00,0x06,0x5F,0x5F,0x06,0x00,0x00,0x00}, // 0x21 |
{0x00,0x07,0x07,0x00,0x07,0x07,0x00,0x00}, // 0x22 |
{0x14,0x7F,0x7F,0x14,0x7F,0x7F,0x14,0x00}, // 0x23 |
{0x24,0x2E,0x6B,0x6B,0x3A,0x12,0x00,0x00}, // 0x24 |
{0x46,0x66,0x30,0x18,0x0C,0x66,0x62,0x00}, // 0x25 |
{0x30,0x7A,0x4F,0x5D,0x37,0x7A,0x48,0x00}, // 0x26 |
{0x04,0x07,0x03,0x00,0x00,0x00,0x00,0x00}, // 0x27 |
{0x00,0x1C,0x3E,0x63,0x41,0x00,0x00,0x00}, // 0x28 |
{0x00,0x41,0x63,0x3E,0x1C,0x00,0x00,0x00}, // 0x29 |
{0x08,0x2A,0x3E,0x1C,0x1C,0x3E,0x2A,0x08}, // 0x2A |
{0x08,0x08,0x3E,0x3E,0x08,0x08,0x00,0x00}, // 0x2B |
{0x00,0xA0,0xE0,0x60,0x00,0x00,0x00,0x00}, // 0x2C |
{0x08,0x08,0x08,0x08,0x08,0x08,0x00,0x00}, // 0x2D |
{0x00,0x00,0x60,0x60,0x00,0x00,0x00,0x00}, // 0x2E |
{0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00}, // 0x2F |
{0x3E,0x7F,0x59,0x4D,0x7F,0x3E,0x00,0x00}, // 0x30 |
{0x42,0x42,0x7F,0x7F,0x40,0x40,0x00,0x00}, // 0x31 |
{0x62,0x73,0x59,0x49,0x6F,0x66,0x00,0x00}, // 0x32 |
{0x22,0x63,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x33 |
{0x18,0x1C,0x16,0x13,0x7F,0x7F,0x10,0x00}, // 0x34 |
{0x27,0x67,0x45,0x45,0x7D,0x39,0x00,0x00}, // 0x35 |
{0x3C,0x7E,0x4B,0x49,0x79,0x30,0x00,0x00}, // 0x36 |
{0x03,0x63,0x71,0x19,0x0F,0x07,0x00,0x00}, // 0x37 |
{0x36,0x7F,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x38 |
{0x06,0x4F,0x49,0x69,0x3F,0x1E,0x00,0x00}, // 0x39 |
{0x00,0x00,0x6C,0x6C,0x00,0x00,0x00,0x00}, // 0x3A |
{0x00,0xA0,0xEC,0x6C,0x00,0x00,0x00,0x00}, // 0x3B |
{0x08,0x1C,0x36,0x63,0x41,0x00,0x00,0x00}, // 0x3C |
{0x14,0x14,0x14,0x14,0x14,0x14,0x00,0x00}, // 0x3D |
{0x00,0x41,0x63,0x36,0x1C,0x08,0x00,0x00}, // 0x3E |
{0x02,0x03,0x51,0x59,0x0F,0x06,0x00,0x00}, // 0x3F |
{0x3E,0x7F,0x41,0x5D,0x5D,0x1F,0x1E,0x00}, // 0x40 |
{0x7C,0x7E,0x13,0x13,0x7E,0x7C,0x00,0x00}, // 0x41 |
{0x41,0x7F,0x7F,0x49,0x49,0x7F,0x36,0x00}, // 0x42 |
{0x1C,0x3E,0x63,0x41,0x41,0x63,0x22,0x00}, // 0x43 |
{0x41,0x7F,0x7F,0x41,0x63,0x7F,0x1C,0x00}, // 0x44 |
{0x41,0x7F,0x7F,0x49,0x5D,0x41,0x63,0x00}, // 0x45 |
{0x41,0x7F,0x7F,0x49,0x1D,0x01,0x03,0x00}, // 0x46 |
{0x1C,0x3E,0x63,0x41,0x51,0x73,0x72,0x00}, // 0x47 |
{0x7F,0x7F,0x08,0x08,0x7F,0x7F,0x00,0x00}, // 0x48 |
{0x00,0x41,0x7F,0x7F,0x41,0x00,0x00,0x00}, // 0x49 |
{0x30,0x70,0x40,0x41,0x7F,0x3F,0x01,0x00}, // 0x4A |
{0x41,0x7F,0x7F,0x08,0x1C,0x77,0x63,0x00}, // 0x4B |
{0x41,0x7F,0x7F,0x41,0x40,0x60,0x70,0x00}, // 0x4C |
{0x7F,0x7F,0x06,0x0C,0x06,0x7F,0x7F,0x00}, // 0x4D |
{0x7F,0x7F,0x06,0x0C,0x18,0x7F,0x7F,0x00}, // 0x4E |
{0x1C,0x3E,0x63,0x41,0x63,0x3E,0x1C,0x00}, // 0x4F |
{0x41,0x7F,0x7F,0x49,0x09,0x0F,0x06,0x00}, // 0x50 |
{0x1E,0x3F,0x21,0x71,0x7F,0x5E,0x00,0x00}, // 0x51 |
{0x41,0x7F,0x7F,0x19,0x39,0x6F,0x46,0x00}, // 0x52 |
{0x26,0x67,0x4D,0x59,0x7B,0x32,0x00,0x00}, // 0x53 |
{0x03,0x41,0x7F,0x7F,0x41,0x03,0x00,0x00}, // 0x54 |
{0x7F,0x7F,0x40,0x40,0x7F,0x7F,0x00,0x00}, // 0x55 |
{0x1F,0x3F,0x60,0x60,0x3F,0x1F,0x00,0x00}, // 0x56 |
{0x7F,0x7F,0x30,0x18,0x30,0x7F,0x7F,0x00}, // 0x57 |
{0x63,0x77,0x1C,0x08,0x1C,0x77,0x63,0x00}, // 0x58 |
{0x07,0x4F,0x78,0x78,0x4F,0x07,0x00,0x00}, // 0x59 |
{0x67,0x73,0x59,0x4D,0x47,0x63,0x71,0x00}, // 0x5A |
{0x00,0x7F,0x7F,0x41,0x41,0x00,0x00,0x00}, // 0x5B |
{0x01,0x03,0x06,0x0C,0x18,0x30,0x60,0x00}, // 0x5C |
{0x00,0x41,0x41,0x7F,0x7F,0x00,0x00,0x00}, // 0x5D |
{0x08,0x0C,0x06,0x03,0x06,0x0C,0x08,0x00}, // 0x5E |
{0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80}, // 0x5F |
{0x00,0x00,0x03,0x07,0x04,0x00,0x00,0x00}, // 0x60 |
{0x20,0x74,0x54,0x54,0x3C,0x78,0x40,0x00}, // 0x61 |
{0x41,0x3F,0x7F,0x44,0x44,0x7C,0x38,0x00}, // 0x62 |
{0x38,0x7C,0x44,0x44,0x6C,0x28,0x00,0x00}, // 0x63 |
{0x30,0x78,0x48,0x49,0x3F,0x7F,0x40,0x00}, // 0x64 |
{0x38,0x7C,0x54,0x54,0x5C,0x18,0x00,0x00}, // 0x65 |
{0x48,0x7E,0x7F,0x49,0x03,0x02,0x00,0x00}, // 0x66 |
{0x98,0xBC,0xA4,0xA4,0xF8,0x7C,0x04,0x00}, // 0x67 |
{0x41,0x7F,0x7F,0x08,0x04,0x7C,0x78,0x00}, // 0x68 |
{0x00,0x44,0x7D,0x7D,0x40,0x00,0x00,0x00}, // 0x69 |
{0x40,0xC4,0x84,0xFD,0x7D,0x00,0x00,0x00}, // 0x6A |
{0x41,0x7F,0x7F,0x10,0x38,0x6C,0x44,0x00}, // 0x6B |
{0x00,0x41,0x7F,0x7F,0x40,0x00,0x00,0x00}, // 0x6C |
{0x7C,0x7C,0x0C,0x18,0x0C,0x7C,0x78,0x00}, // 0x6D |
{0x7C,0x7C,0x04,0x04,0x7C,0x78,0x00,0x00}, // 0x6E |
{0x38,0x7C,0x44,0x44,0x7C,0x38,0x00,0x00}, // 0x6F |
{0x84,0xFC,0xF8,0xA4,0x24,0x3C,0x18,0x00}, // 0x70 |
{0x18,0x3C,0x24,0xA4,0xF8,0xFC,0x84,0x00}, // 0x71 |
{0x44,0x7C,0x78,0x44,0x1C,0x18,0x00,0x00}, // 0x72 |
{0x48,0x5C,0x54,0x54,0x74,0x24,0x00,0x00}, // 0x73 |
{0x00,0x04,0x3E,0x7F,0x44,0x24,0x00,0x00}, // 0x74 |
{0x3C,0x7C,0x40,0x40,0x3C,0x7C,0x40,0x00}, // 0x75 |
{0x1C,0x3C,0x60,0x60,0x3C,0x1C,0x00,0x00}, // 0x76 |
{0x3C,0x7C,0x60,0x30,0x60,0x7C,0x3C,0x00}, // 0x77 |
{0x44,0x6C,0x38,0x10,0x38,0x6C,0x44,0x00}, // 0x78 |
{0x9C,0xBC,0xA0,0xA0,0xFC,0x7C,0x00,0x00}, // 0x79 |
{0x4C,0x64,0x74,0x5C,0x4C,0x64,0x00,0x00}, // 0x7A |
{0x08,0x08,0x3E,0x77,0x41,0x41,0x00,0x00}, // 0x7B |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0x7C |
{0x41,0x41,0x77,0x3E,0x08,0x08,0x00,0x00}, // 0x7D |
{0x02,0x03,0x01,0x03,0x02,0x03,0x01,0x00}, // 0x7E |
{0x78,0x7C,0x46,0x43,0x46,0x7C,0x78,0x00}, // 0x7F |
{0x1E,0xBF,0xE1,0x61,0x33,0x12,0x00,0x00}, // 0x80 |
{0x3A,0x7A,0x40,0x40,0x7A,0x7A,0x40,0x00}, // 0x81 |
{0x38,0x7C,0x56,0x57,0x5D,0x18,0x00,0x00}, // 0x82 |
{0x02,0x23,0x75,0x55,0x55,0x7D,0x7B,0x42}, // 0x83 |
{0x21,0x75,0x54,0x54,0x7D,0x79,0x40,0x00}, // 0x84 |
{0x20,0x75,0x57,0x56,0x7C,0x78,0x40,0x00}, // 0x85 |
{0x00,0x22,0x77,0x55,0x55,0x7F,0x7A,0x40}, // 0x86 |
{0x1C,0xBE,0xE2,0x62,0x36,0x14,0x00,0x00}, // 0x87 |
{0x02,0x3B,0x7D,0x55,0x55,0x5D,0x1B,0x02}, // 0x88 |
{0x39,0x7D,0x54,0x54,0x5D,0x19,0x00,0x00}, // 0x89 |
{0x38,0x7D,0x57,0x56,0x5C,0x18,0x00,0x00}, // 0x8A |
{0x01,0x45,0x7C,0x7C,0x41,0x01,0x00,0x00}, // 0x8B |
{0x02,0x03,0x45,0x7D,0x7D,0x43,0x02,0x00}, // 0x8C |
{0x00,0x45,0x7F,0x7E,0x40,0x00,0x00,0x00}, // 0x8D |
{0x79,0x7D,0x26,0x26,0x7D,0x79,0x00,0x00}, // 0x8E |
{0x70,0x7A,0x2D,0x2D,0x7A,0x70,0x00,0x00}, // 0x8F |
{0x44,0x7C,0x7E,0x57,0x55,0x44,0x00,0x00}, // 0x90 |
{0x20,0x74,0x54,0x54,0x7C,0x7C,0x54,0x54}, // 0x91 |
{0x7C,0x7E,0x0B,0x09,0x7F,0x7F,0x49,0x00}, // 0x92 |
{0x32,0x7B,0x49,0x49,0x7B,0x32,0x00,0x00}, // 0x93 |
{0x32,0x7A,0x48,0x48,0x7A,0x32,0x00,0x00}, // 0x94 |
{0x30,0x79,0x4B,0x4A,0x78,0x30,0x00,0x00}, // 0x95 |
{0x3A,0x7B,0x41,0x41,0x7B,0x7A,0x40,0x00}, // 0x96 |
{0x38,0x79,0x43,0x42,0x78,0x78,0x40,0x00}, // 0x97 |
{0xBA,0xBA,0xA0,0xA0,0xFA,0x7A,0x00,0x00}, // 0x98 |
{0x39,0x7D,0x44,0x44,0x44,0x7D,0x39,0x00}, // 0x99 |
{0x3D,0x7D,0x40,0x40,0x7D,0x3D,0x00,0x00}, // 0x9A |
{0x38,0x7C,0x64,0x54,0x4C,0x7C,0x38,0x00}, // 0x9B |
{0x68,0x7E,0x7F,0x49,0x43,0x66,0x20,0x00}, // 0x9C |
{0x5C,0x3E,0x73,0x49,0x67,0x3E,0x1D,0x00}, // 0x9D |
{0x44,0x6C,0x38,0x38,0x6C,0x44,0x00,0x00}, // 0x9E |
{0x40,0xC8,0x88,0xFE,0x7F,0x09,0x0B,0x02}, // 0x9F |
{0x20,0x74,0x56,0x57,0x7D,0x78,0x40,0x00}, // 0xA0 |
{0x00,0x44,0x7E,0x7F,0x41,0x00,0x00,0x00}, // 0xA1 |
{0x30,0x78,0x48,0x4A,0x7B,0x31,0x00,0x00}, // 0xA2 |
{0x38,0x78,0x40,0x42,0x7B,0x79,0x40,0x00}, // 0xA3 |
{0x7A,0x7B,0x09,0x0B,0x7A,0x73,0x01,0x00}, // 0xA4 |
{0x7A,0x7B,0x19,0x33,0x7A,0x7B,0x01,0x00}, // 0xA5 |
{0x00,0x26,0x2F,0x29,0x2F,0x2F,0x28,0x00}, // 0xA6 |
{0x00,0x26,0x2F,0x29,0x29,0x2F,0x26,0x00}, // 0xA7 |
{0x30,0x78,0x4D,0x45,0x60,0x20,0x00,0x00}, // 0xA8 |
{0x1C,0x22,0x7D,0x4B,0x5B,0x65,0x22,0x1C}, // 0xA9 |
{0x08,0x08,0x08,0x08,0x38,0x38,0x00,0x00}, // 0xAA |
{0x61,0x3F,0x1F,0xCC,0xEE,0xAB,0xB9,0x90}, // 0xAB |
{0x61,0x3F,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xAC |
{0x00,0x00,0x60,0xFA,0xFA,0x60,0x00,0x00}, // 0xAD |
{0x08,0x1C,0x36,0x22,0x08,0x1C,0x36,0x22}, // 0xAE |
{0x22,0x36,0x1C,0x08,0x22,0x36,0x1C,0x08}, // 0xAF |
{0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00}, // 0xB0 |
{0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55}, // 0xB1 |
{0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF}, // 0xB2 |
{0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00}, // 0xB3 |
{0x10,0x10,0x10,0xFF,0xFF,0x00,0x00,0x00}, // 0xB4 |
{0x70,0x78,0x2C,0x2E,0x7B,0x71,0x00,0x00}, // 0xB5 |
{0x72,0x79,0x2D,0x2D,0x79,0x72,0x00,0x00}, // 0xB6 |
{0x71,0x7B,0x2E,0x2C,0x78,0x70,0x00,0x00}, // 0xB7 |
{0x1C,0x22,0x5D,0x55,0x55,0x41,0x22,0x1C}, // 0xB8 |
{0x14,0x14,0xF7,0xF7,0x00,0xFF,0xFF,0x00}, // 0xB9 |
{0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0x00}, // 0xBA |
{0x14,0x14,0xF4,0xF4,0x04,0xFC,0xFC,0x00}, // 0xBB |
{0x14,0x14,0x17,0x17,0x10,0x1F,0x1F,0x00}, // 0xBC |
{0x18,0x3C,0x24,0xE7,0xE7,0x24,0x24,0x00}, // 0xBD |
{0x2B,0x2F,0xFC,0xFC,0x2F,0x2B,0x00,0x00}, // 0xBE |
{0x10,0x10,0x10,0xF0,0xF0,0x00,0x00,0x00}, // 0xBF |
{0x00,0x00,0x00,0x1F,0x1F,0x10,0x10,0x10}, // 0xC0 |
{0x10,0x10,0x10,0x1F,0x1F,0x10,0x10,0x10}, // 0xC1 |
{0x10,0x10,0x10,0xF0,0xF0,0x10,0x10,0x10}, // 0xC2 |
{0x00,0x00,0x00,0xFF,0xFF,0x10,0x10,0x10}, // 0xC3 |
{0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10}, // 0xC4 |
{0x10,0x10,0x10,0xFF,0xFF,0x10,0x10,0x10}, // 0xC5 |
{0x22,0x77,0x55,0x57,0x7E,0x7B,0x41,0x00}, // 0xC6 |
{0x72,0x7B,0x2D,0x2F,0x7A,0x73,0x01,0x00}, // 0xC7 |
{0x00,0x00,0x1F,0x1F,0x10,0x17,0x17,0x14}, // 0xC8 |
{0x00,0x00,0xFC,0xFC,0x04,0xF4,0xF4,0x14}, // 0xC9 |
{0x14,0x14,0x17,0x17,0x10,0x17,0x17,0x14}, // 0xCA |
{0x14,0x14,0xF4,0xF4,0x04,0xF4,0xF4,0x14}, // 0xCB |
{0x00,0x00,0xFF,0xFF,0x00,0xF7,0xF7,0x14}, // 0xCC |
{0x14,0x14,0x14,0x14,0x14,0x14,0x14,0x14}, // 0xCD |
{0x14,0x14,0xF7,0xF7,0x00,0xF7,0xF7,0x14}, // 0xCE |
{0x66,0x3C,0x3C,0x24,0x3C,0x3C,0x66,0x00}, // 0xCF |
{0x05,0x27,0x72,0x57,0x7D,0x38,0x00,0x00}, // 0xD0 |
{0x49,0x7F,0x7F,0x49,0x63,0x7F,0x1C,0x00}, // 0xD1 |
{0x46,0x7D,0x7D,0x55,0x55,0x46,0x00,0x00}, // 0xD2 |
{0x45,0x7D,0x7C,0x54,0x55,0x45,0x00,0x00}, // 0xD3 |
{0x44,0x7D,0x7F,0x56,0x54,0x44,0x00,0x00}, // 0xD4 |
{0x0A,0x0E,0x08,0x00,0x00,0x00,0x00,0x00}, // 0xD5 |
{0x00,0x44,0x7E,0x7F,0x45,0x00,0x00,0x00}, // 0xD6 |
{0x02,0x45,0x7D,0x7D,0x45,0x02,0x00,0x00}, // 0xD7 |
{0x01,0x45,0x7C,0x7C,0x45,0x01,0x00,0x00}, // 0xD8 |
{0x10,0x10,0x10,0x1F,0x1F,0x00,0x00,0x00}, // 0xD9 |
{0x00,0x00,0x00,0xF0,0xF0,0x10,0x10,0x10}, // 0xDA |
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, // 0xDB |
{0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0}, // 0xDC |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0xDD |
{0x00,0x45,0x7F,0x7E,0x44,0x00,0x00,0x00}, // 0xDE |
{0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}, // 0xDF |
{0x38,0x7C,0x46,0x47,0x45,0x7C,0x38,0x00}, // 0xE0 |
{0xFC,0xFE,0x2A,0x2A,0x3E,0x14,0x00,0x00}, // 0xE1 |
{0x3A,0x7D,0x45,0x45,0x45,0x7D,0x3A,0x00}, // 0xE2 |
{0x38,0x7C,0x45,0x47,0x46,0x7C,0x38,0x00}, // 0xE3 |
{0x32,0x7B,0x49,0x4B,0x7A,0x33,0x01,0x00}, // 0xE4 |
{0x3A,0x7F,0x45,0x47,0x46,0x7F,0x39,0x00}, // 0xE5 |
{0x80,0xFE,0x7E,0x20,0x20,0x3E,0x1E,0x00}, // 0xE6 |
{0x42,0x7E,0x7E,0x54,0x1C,0x08,0x00,0x00}, // 0xE7 |
{0x41,0x7F,0x7F,0x55,0x14,0x1C,0x08,0x00}, // 0xE8 |
{0x3C,0x7C,0x42,0x43,0x7D,0x3C,0x00,0x00}, // 0xE9 |
{0x3A,0x79,0x41,0x41,0x79,0x3A,0x00,0x00}, // 0xEA |
{0x3C,0x7D,0x43,0x42,0x7C,0x3C,0x00,0x00}, // 0xEB |
{0xB8,0xB8,0xA2,0xA3,0xF9,0x78,0x00,0x00}, // 0xEC |
{0x0C,0x5C,0x72,0x73,0x5D,0x0C,0x00,0x00}, // 0xED |
{0x02,0x02,0x02,0x02,0x02,0x02,0x00,0x00}, // 0xEE |
{0x00,0x00,0x02,0x03,0x01,0x00,0x00,0x00}, // 0xEF |
{0x10,0x10,0x10,0x10,0x10,0x10,0x00,0x00}, // 0xF0 |
{0x44,0x44,0x5F,0x5F,0x44,0x44,0x00,0x00}, // 0xF1 |
{0x28,0x28,0x28,0x28,0x28,0x28,0x00,0x00}, // 0xF2 |
{0x71,0x35,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xF3 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0xF4 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0xF5 |
{0x08,0x08,0x6B,0x6B,0x08,0x08,0x00,0x00}, // 0xF6 |
{0x00,0x80,0xC0,0x40,0x00,0x00,0x00,0x00}, // 0xF7 |
{0x00,0x06,0x0F,0x09,0x0F,0x06,0x00,0x00}, // 0xF8 |
{0x02,0x02,0x00,0x00,0x02,0x02,0x00,0x00}, // 0xF9 |
{0x00,0x00,0x00,0x10,0x10,0x00,0x00,0x00}, // 0xFA |
{0x00,0x12,0x13,0x1F,0x1F,0x10,0x10,0x00}, // 0xFB |
{0x00,0x11,0x15,0x15,0x1F,0x1F,0x0A,0x00}, // 0xFC |
{0x00,0x19,0x1D,0x15,0x17,0x12,0x00,0x00}, // 0xFD |
{0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00}, // 0xFE |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00} // 0xFF |
}; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/Font8x8.h |
---|
0,0 → 1,31 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X8_H |
#define _FONT8X8_H |
#include <avr/pgmspace.h> |
extern const uint8_t Font8x8[256][8]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/font8x6.c |
---|
0,0 → 1,232 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY font8x6.c |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK (ehemals 'Antenne' Ascii 31) |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: ASC 11 (0x0B) SYMBOL_SMALLDEGREE |
//# - add: ASC 16 (0x10) SYMBOL_RCQUALITY |
//############################################################################ |
#include <avr/pgmspace.h> |
// one byte is a column |
// bit 7 is the bottom |
// |
// 123456 |
// L 1 | XXX | |
// O 2 |X X | |
// W 4 |X X | |
// 8 | XXX | |
// H 1 |X X | |
// I 2 |X X | |
// G 4 | XXX | |
// H 8 | | |
// |
// 0x36,0x49,0x49,0x49,0x36,0x00 |
// |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | X| |
// 8 | X | |
// H 1 | X | |
// I 2 |X X | |
// G 4 | X | |
// H 8 | | |
// 0x20,0x40,0x20,0x10,0x08,0x04 |
// 123456 |
// L 1 | | |
// O 2 | X| |
// W 4 | X | |
// 8 | X | |
// H 1 |X X | |
// I 2 | X | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x10,0x08,0x04,0x02 |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | XX| |
// 8 | XX | |
// H 1 |X XX | |
// I 2 | XX | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x30,0x18,0x0c,0x04 |
//---------------------------------------------------------------- |
const uint8_t font8x6[128][6] PROGMEM = |
{ |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 0 00 (not useable) |
{ 0x78,0x15,0x14,0x15,0x78,0x00 }, // ASCII - 1 01 'Ä' |
{ 0x20,0x55,0x54,0x55,0x78,0x00 }, // ASCII - 2 02 'ä' |
{ 0x38,0x45,0x44,0x45,0x38,0x00 }, // ASCII - 3 03 'Ö' |
{ 0x30,0x49,0x48,0x49,0x30,0x00 }, // ASCII - 4 04 'ö' |
{ 0x3c,0x41,0x40,0x41,0x3c,0x00 }, // ASCII - 5 05 'Ü' |
{ 0x38,0x41,0x40,0x21,0x78,0x00 }, // ASCII - 6 06 'ü' |
{ 0x7e,0x15,0x15,0x15,0x0a,0x00 }, // ASCII - 7 07 'ß' |
{ 0x22,0x17,0x0F,0x17,0x22,0x00 }, // ASCII - 8 08 SAT Symbol |
{ 0x00,0x84,0x82,0xFF,0x82,0x84 }, // ASCII - 9 09 Altitude Symbol |
{ 0x1c,0x14,0x1c,0x00,0x00,0x00 }, // ASCII - 10 0A (not useable) SYMBOL_AVG |
{ 0x00,0x07,0x05,0x07,0x00,0x00 }, // ASCII - 11 0B small degree SYMBOL_SMALLDEGREE |
{ 0x10,0x38,0x54,0x10,0x10,0x1e }, // ASCII - 12 0C Enter Symbol |
{ 0x18,0x0c,0x18,0x00,0x00,0x00 }, // ASCII - 13 0D (not useable) SYMBOL_MAX |
{ 0x10,0x10,0x10,0x10,0x10,0x10 }, // ASCII - 14 0E hor. line |
{ 0x10,0x10,0x10,0x7c,0x10,0x10 }, // ASCII - 15 0F hor. line with tick mark |
{ 0x08,0x10,0x08,0x00,0x00,0x00 }, // ASCII - 16 10 rc quality SYMBOL_MIN |
{ 0x08,0x14,0x00,0x00,0x14,0x08 }, // ASCII - 17 11 <> Change |
{ 0x10,0x08,0x04,0x04,0x08,0x10 }, // ASCII - 18 12 /\ Up |
{ 0x08,0x10,0x20,0x20,0x10,0x08 }, // ASCII - 19 13 \/ Down |
{ 0x00,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 20 14 < Left |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 21 15 > Right |
{ 0x04,0x02,0x7f,0x02,0x04,0x00 }, // ASCII - 22 16 /|\ Arrow up |
{ 0x10,0x20,0x7f,0x20,0x10,0x00 }, // ASCII - 23 17 \|/ Arrow down |
{ 0x10,0x38,0x54,0x10,0x10,0x10 }, // ASCII - 24 18 <- Arrow left |
{ 0x10,0x10,0x10,0x54,0x38,0x10 }, // ASCII - 25 19 -> Arrow right |
{ 0x10,0x18,0x1c,0x1c,0x18,0x10 }, // ASCII - 26 1A ^ Triangle up |
{ 0x08,0x18,0x38,0x38,0x18,0x08 }, // ASCII - 27 1B v Triangle down |
{ 0x00,0x08,0x1c,0x3e,0x7f,0x00 }, // ASCII - 28 1C < Triangle left |
{ 0x00,0x7f,0x3e,0x1c,0x08,0x00 }, // ASCII - 29 1D > Triangle right |
{ 0x06,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 30 1E '°' big degree SYMBOL_BIGDEGREE |
// { 0x06,0x49,0x7d,0x49,0x06,0x00 }, // ASCII - 31 1F Antenne (ALT) |
{ 0x10,0x20,0x10,0x08,0x04,0x02 }, // ASCII - 31 1F SYMBOL_CHECK |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 32 20 ' ' |
{ 0x00,0x00,0x2f,0x00,0x00,0x00 }, // ASCII - 33 21 '!' |
{ 0x00,0x07,0x00,0x07,0x00,0x00 }, // ASCII - 34 22 '"' |
{ 0x14,0x7f,0x14,0x7f,0x14,0x00 }, // ASCII - 35 23 '#' |
{ 0x24,0x2a,0x6b,0x2a,0x12,0x00 }, // ASCII - 36 24 '$' |
{ 0x23,0x13,0x08,0x64,0x62,0x00 }, // ASCII - 37 25 '%' |
{ 0x36,0x49,0x55,0x22,0x50,0x00 }, // ASCII - 38 26 '&' |
{ 0x00,0x05,0x03,0x00,0x00,0x00 }, // ASCII - 39 27 ''' |
{ 0x00,0x1c,0x22,0x41,0x00,0x00 }, // ASCII - 40 28 '(' |
{ 0x00,0x41,0x22,0x1c,0x00,0x00 }, // ASCII - 41 29 ')' |
{ 0x14,0x08,0x3e,0x08,0x14,0x00 }, // ASCII - 42 2a '*' |
{ 0x08,0x08,0x3e,0x08,0x08,0x00 }, // ASCII - 43 2b '+' |
{ 0x00,0x50,0x30,0x00,0x00,0x00 }, // ASCII - 44 2c ',' |
{ 0x08,0x08,0x08,0x08,0x08,0x00 }, // ASCII - 45 2d '-' |
{ 0x00,0x60,0x60,0x00,0x00,0x00 }, // ASCII - 46 2e '.' |
{ 0x20,0x10,0x08,0x04,0x02,0x00 }, // ASCII - 47 2f '/' |
{ 0x3e,0x51,0x49,0x45,0x3e,0x00 }, // ASCII - 48 30 '0' |
{ 0x00,0x42,0x7f,0x40,0x00,0x00 }, // ASCII - 49 31 '1' |
{ 0x42,0x61,0x51,0x49,0x46,0x00 }, // ASCII - 50 32 '2' |
{ 0x21,0x41,0x45,0x4b,0x31,0x00 }, // ASCII - 51 33 '3' |
{ 0x18,0x14,0x12,0x7f,0x10,0x00 }, // ASCII - 52 34 '4' |
{ 0x27,0x45,0x45,0x45,0x39,0x00 }, // ASCII - 53 35 '5' |
{ 0x3c,0x4a,0x49,0x49,0x30,0x00 }, // ASCII - 54 36 '6' |
{ 0x03,0x01,0x71,0x09,0x07,0x00 }, // ASCII - 55 37 '7' |
{ 0x36,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 56 38 '8' |
{ 0x06,0x49,0x49,0x29,0x1e,0x00 }, // ASCII - 57 39 '9' |
{ 0x00,0x36,0x36,0x00,0x00,0x00 }, // ASCII - 58 3a ':' |
{ 0x00,0x56,0x36,0x00,0x00,0x00 }, // ASCII - 59 3b ';' |
{ 0x08,0x14,0x22,0x41,0x00,0x00 }, // ASCII - 60 3c '<' |
{ 0x14,0x14,0x14,0x14,0x14,0x00 }, // ASCII - 61 3d '=' |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 62 3e '>' |
{ 0x02,0x01,0x51,0x09,0x06,0x00 }, // ASCII - 63 3f '?' |
{ 0x32,0x49,0x79,0x41,0x3e,0x00 }, // ASCII - 64 40 '@' |
{ 0x7e,0x11,0x11,0x11,0x7e,0x00 }, // ASCII - 65 41 'A' |
{ 0x7f,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 66 42 'B' |
{ 0x3e,0x41,0x41,0x41,0x22,0x00 }, // ASCII - 67 43 'C' |
{ 0x7f,0x41,0x41,0x22,0x1c,0x00 }, // ASCII - 68 44 'D' |
{ 0x7f,0x49,0x49,0x49,0x41,0x00 }, // ASCII - 69 45 'E' |
{ 0x7f,0x09,0x09,0x09,0x01,0x00 }, // ASCII - 70 46 'F' |
{ 0x3e,0x41,0x49,0x49,0x7a,0x00 }, // ASCII - 71 47 'G' |
{ 0x7f,0x08,0x08,0x08,0x7f,0x00 }, // ASCII - 72 48 'H' |
{ 0x00,0x41,0x7f,0x41,0x00,0x00 }, // ASCII - 73 49 'I' |
{ 0x20,0x40,0x41,0x3f,0x01,0x00 }, // ASCII - 74 4a 'J' |
{ 0x7f,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 75 4b 'K' |
{ 0x7f,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 76 4c 'L' |
{ 0x7f,0x02,0x0c,0x02,0x7f,0x00 }, // ASCII - 77 4d 'M' |
{ 0x7f,0x04,0x08,0x10,0x7f,0x00 }, // ASCII - 78 4e 'N' |
{ 0x3e,0x41,0x41,0x41,0x3e,0x00 }, // ASCII - 79 4f 'O' |
{ 0x7f,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 80 50 'P' |
{ 0x3e,0x41,0x51,0x21,0x5e,0x00 }, // ASCII - 81 51 'Q' |
{ 0x7f,0x09,0x19,0x29,0x46,0x00 }, // ASCII - 82 52 'R' |
{ 0x46,0x49,0x49,0x49,0x31,0x00 }, // ASCII - 83 53 'S' |
{ 0x01,0x01,0x7f,0x01,0x01,0x00 }, // ASCII - 84 54 'T' |
{ 0x3f,0x40,0x40,0x40,0x3f,0x00 }, // ASCII - 85 55 'U' |
{ 0x1f,0x20,0x40,0x20,0x1f,0x00 }, // ASCII - 86 56 'V' |
{ 0x3f,0x40,0x38,0x40,0x3f,0x00 }, // ASCII - 87 57 'W' |
{ 0x63,0x14,0x08,0x14,0x63,0x00 }, // ASCII - 88 58 'X' |
{ 0x07,0x08,0x70,0x08,0x07,0x00 }, // ASCII - 89 59 'Y' |
{ 0x61,0x51,0x49,0x45,0x43,0x00 }, // ASCII - 90 5a 'Z' |
{ 0x7f,0x41,0x41,0x00,0x00,0x00 }, // ASCII - 91 5b '[' |
{ 0x02,0x04,0x08,0x10,0x20,0x00 }, // ASCII - 92 5c '\' |
{ 0x00,0x41,0x41,0x7f,0x00,0x00 }, // ASCII - 93 5d ']' |
{ 0x04,0x02,0x01,0x02,0x04,0x00 }, // ASCII - 94 5e '^' |
{ 0x40,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 95 5f '_' |
{ 0x00,0x01,0x02,0x04,0x00,0x00 }, // ASCII - 96 60 '`' |
{ 0x20,0x54,0x54,0x54,0x78,0x00 }, // ASCII - 97 61 'a' |
{ 0x7f,0x48,0x44,0x44,0x38,0x00 }, // ASCII - 98 62 'b' |
{ 0x38,0x44,0x44,0x44,0x20,0x00 }, // ASCII - 99 63 'c' |
{ 0x38,0x44,0x44,0x48,0x7f,0x00 }, // ASCII - 100 64 'd' |
{ 0x38,0x54,0x54,0x54,0x18,0x00 }, // ASCII - 101 65 'e' |
{ 0x08,0x7e,0x09,0x01,0x02,0x00 }, // ASCII - 102 66 'f' |
{ 0x0c,0x52,0x52,0x52,0x3e,0x00 }, // ASCII - 103 67 'g' |
{ 0x7f,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 104 68 'h' |
{ 0x00,0x44,0x7d,0x40,0x00,0x00 }, // ASCII - 105 69 'i' |
{ 0x20,0x40,0x44,0x3d,0x00,0x00 }, // ASCII - 106 6a 'j' |
{ 0x7f,0x10,0x28,0x44,0x00,0x00 }, // ASCII - 107 6b 'k' |
{ 0x00,0x41,0x7f,0x40,0x00,0x00 }, // ASCII - 108 6c 'l' |
{ 0x7c,0x04,0x18,0x04,0x78,0x00 }, // ASCII - 109 6d 'm' |
{ 0x7c,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 110 6e 'n' |
{ 0x38,0x44,0x44,0x44,0x38,0x00 }, // ASCII - 111 6f 'o' |
{ 0x7c,0x14,0x14,0x14,0x08,0x00 }, // ASCII - 112 70 'p' |
{ 0x08,0x14,0x14,0x18,0x7c,0x00 }, // ASCII - 113 71 'q' |
{ 0x7c,0x08,0x04,0x04,0x08,0x00 }, // ASCII - 114 72 'r' |
{ 0x48,0x54,0x54,0x54,0x20,0x00 }, // ASCII - 115 73 's' |
{ 0x04,0x3f,0x44,0x40,0x20,0x00 }, // ASCII - 116 74 't' |
{ 0x3c,0x40,0x40,0x20,0x7c,0x00 }, // ASCII - 117 75 'u' |
{ 0x1c,0x20,0x40,0x20,0x1c,0x00 }, // ASCII - 118 76 'v' |
{ 0x3c,0x40,0x38,0x40,0x3c,0x00 }, // ASCII - 119 77 'w' |
{ 0x44,0x28,0x10,0x28,0x44,0x00 }, // ASCII - 120 78 'x' |
{ 0x0c,0x50,0x50,0x50,0x3c,0x00 }, // ASCII - 121 79 'y' |
{ 0x44,0x64,0x54,0x4c,0x44,0x00 }, // ASCII - 122 7a 'z' |
{ 0x00,0x08,0x36,0x41,0x00,0x00 }, // ASCII - 123 7b '{' |
{ 0x00,0x00,0x7f,0x00,0x00,0x00 }, // ASCII - 124 7c '|' |
{ 0x00,0x41,0x36,0x08,0x00,0x00 }, // ASCII - 125 7d '}' |
{ 0x08,0x08,0x2a,0x1c,0x08,0x00 }, // ASCII - 126 7e -> |
{ 0x08,0x1c,0x2a,0x08,0x08,0x00 }, // ASCII - 127 7f <- |
}; |
/* |
{ 0x02,0x0a,0x2a,0x0a,0x02,0x00 }, // ASCII - 16 10 rc quality SYMBOL_RCQUALITY (wieder entfernt weil der Platz gebraucht wurde) |
*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/font8x6.h |
---|
0,0 → 1,30 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X6_H |
#define _FONT8X6_H |
#include <avr/pgmspace.h> |
//extern prog_uint8_t font8x6[128][6]; |
extern const uint8_t font8x6[128][6]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/lcd.c |
---|
0,0 → 1,1990 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.c |
//# |
//# 10.11.2014 cebra |
//# - fix: lcd_init() weiter Helligkeitseinstellungen korrigiert (OCR2A) |
//# |
//# 28.06.2014 OG |
//# - chg: lcdx_cls_rowwidth() kompatibel zu mode MINVERSX und MNORMALX |
//# - chg: lcdx_cls_row() auf lcdx_cls_rowwidth() umgestellt |
//# - fix: lcdx_printp() umgestellt auf strlen_P() statt strlen() |
//# |
//# 27.06.2014 OG |
//# - add: die lcd-print Funktionen wurden um MINVERSX und MNORMALX erweitert |
//# um einen zusaetzlichen Rand links und oben als Linie zu zeichnen |
//# (wird z.B. in followme.c verwendet) |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# - add: include <avr/interrupt.h> |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 26.05.2014 OG |
//# - chg: LCD_Init() - LCD-Modus nur noch normal (kein Invers mehr) |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 10.04.2014 OG |
//# - add: include: lipo.h |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# - chg: PRINTF_BUFFER_SIZE von 80 auf 40 |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: lcd_frect_round() |
//# - chg: lcd_rect_round() um R2 ergaenzt |
//# |
//# 12.02.2014 OG |
//# - add: lcd_rect_round() |
//# |
//# 04.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Aufbau der Formatmaske |
//# |
//# 03.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Berechnung Formatstring korrigiert |
//# - fix: writex_ndigit_number_u_100th() Parameter 'mode' fehlte |
//# |
//# 16.06.2013 OG |
//# - chg: LCD_ORIENTATION festgesetzt auf 0 (Normal) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - chg: writex_datetime_time(), writex_datetime_date() Lokalzeitberechnung |
//# via UTCdatetime2local() |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer GPS-Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 29.04.2013 OG |
//# - chg: lcd_cls() geaendert auf memset |
//# |
//# 28.04.2013 OG |
//# - chg: high-level Funktionen wie writex_ndigit... auf xprintf umgebaut |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# siehe dazu: Doku in utils/xstring.h fuer xprintf |
//# |
//# 27.03.2013 OG |
//# - add: writex_datetime_time() |
//# - add: writex_datetime_date() |
//# - add: Show_PKTError_NoRAM() |
//# |
//# 22.03.2013 OG |
//# - add: writex_gpspos() |
//# |
//# 11.03.2013 OG |
//# - fix: writex_time() Ausgabe Doppelpunkt ":" ergaenzt um mode,xoffs,yoffs |
//# |
//# 07.03.2013 OG |
//# - del: Kompatibilitaetsfunktion lcd_printpj_at() entfernt |
//# |
//# 06.03.2013 OG |
//# - lcdx_putc() wurde erweitert mit Unterstuetzung des 8x8 Font (alte Jeti |
//# Funktionen) und Pixel-Verschiebung (xoffs,yoffs) |
//# - etliche Char basierte Funktionen wurden erweitert um Parameter xoffs,yoffs |
//# um die Ausgabe um +/- Pixel zu verschieben. Fuer Pixelgenaue Positionierung |
//# von OSD-Screen Elementen. |
//# Die neuen Funktionen mit Pixel-Verschiebung beginnen mit lcdx_, writex_ ... |
//# - add: Kompatibilitaet gewaehrleistet durch Mapper-Funktionen |
//# - add: defines fuer Char-Drawmode's (MNORMAL, MINVERS, MBIG, MBIGINVERS) |
//# - del: Jeti-Funktionen (teilweise ersetzt durch Kompatibilitaetsfunktionen) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <stdarg.h> |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "lcd.h" |
#include "../main.h" |
#include "../HAL_HW3_9.h" |
#include "../utils/xutils.h" // xprintf, UTCdatetime2local |
#include "../lipo/lipo.h" // show_Lipo() |
#include "font8x6.h" |
#ifdef USE_FONT_BIG |
#include "Font8x8.h" |
#endif |
#define DISP_W 128 |
#define DISP_H 64 |
#define DISP_BUFFER ((DISP_H * DISP_W) / 8) |
#define LINE_BUFFER (((DISP_H/8) * DISP_W) / 8) |
#define Jeti 1 // Jeti Routinen |
volatile uint8_t display_buffer[DISP_BUFFER]; // Display-Puffer, weil nicht zurückgelesen werden kann |
volatile uint8_t line_buffer[LINE_BUFFER]; // Zeilen-Puffer, weil nicht zurückgelesen werden kann |
volatile uint16_t display_buffer_pointer; // Pointer auf das aktuell übertragene Byte |
volatile uint8_t display_buffer_counter; // Hilfszähler zur Selektierung der Page |
volatile uint8_t display_page_counter; // aktuelle Page-Nummer |
volatile uint8_t display_mode; // Modus für State-Machine |
volatile uint8_t LCD_ORIENTATION = 0; // 0=Normal / 1=reversed |
// DOG: 128 x 64 with 6x8 Font => 21 x 8 |
// MAX7456: 30 x 16 |
uint8_t lcd_xpos; |
uint8_t lcd_ypos; |
char s[7]; |
#define PRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer lcdx_printf_at() / lcdx_printf_at_P() |
char printf_buffer[PRINTF_BUFFER_SIZE]; |
char format_buffer[20]; |
//----------------------------------------------------------- |
void send_byte( uint8_t data ) |
{ |
clr_cs (); |
SPDR = data; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
// * Writes one command byte |
// * cmd - the command byte |
// |
void lcd_command( uint8_t cmd ) |
{ |
// LCD_SELECT(); |
// LCD_CMD(); |
// spi_write(cmd); |
// LCD_UNSELECT(); |
clr_cs (); |
SPDR = cmd; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_set_contrast( uint8_t value ) |
{ |
cli(); |
clr_A0(); |
send_byte( 0x81 ); |
send_byte( value ); // Daten zum LCD senden |
set_A0(); |
sei(); |
} |
//----------------------------------------------------------- |
void lcd_cls( void ) |
{ |
uint16_t i, j; |
memset( (void *)display_buffer, 0, 1024); |
// for (i = 0; i < DISP_BUFFER; i++) |
// display_buffer[i] = 0x00; |
for (i = 0; i < 8; i++) |
{ |
clr_A0 (); |
send_byte (0xB0 + i); //1011xxxx |
send_byte (0x10); //00010000 |
// send_byte(0x04); //00000100 gedreht plus 4 Byte |
// send_byte(0x00); //00000000 |
send_byte (LCD_ORIENTATION); //00000000 |
set_A0 (); |
for (j = 0; j < 128; j++) |
send_byte (0x00); |
} |
lcd_xpos = 0; |
lcd_ypos = 0; |
} |
//----------------------------------------------------------- |
void set_adress (uint16_t adress, uint8_t data) |
{ |
uint8_t page; |
uint8_t column; |
page = adress >> 7; |
clr_A0 (); |
send_byte (0xB0 + page); |
column = (adress & 0x7F) + LCD_ORIENTATION; |
send_byte (0x10 + (column >> 4)); |
send_byte (column & 0x0F); |
set_A0 (); |
send_byte (data); |
} |
//----------------------------------------------------------- |
void scroll (void) |
{ |
uint16_t adress; |
for (adress = 0; adress < 896; adress++) |
{ |
display_buffer[adress] = display_buffer[adress + 128]; |
set_adress (adress, display_buffer[adress]); |
} |
for (adress = 896; adress < 1024; adress++) |
{ |
display_buffer[adress] = 0; |
set_adress (adress, 0); |
} |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
// + Plot (set one Pixel) |
//----------------------------------------------------------- |
// mode: |
// 0=Clear, 1=Set, 2=XOR |
void lcd_plot (uint8_t xpos, uint8_t ypos, uint8_t mode) |
{ |
uint16_t adress; |
uint8_t mask; |
if ((xpos < DISP_W) && (ypos < DISP_H)) |
{ |
adress = (ypos / 8) * DISP_W + xpos; // adress = 0/8 * 128 + 0 = 0 |
mask = 1 << (ypos & 0x07); // mask = 1<<0 = 1 |
adress &= DISP_BUFFER - 1; |
switch (mode) |
{ |
case 0: |
display_buffer[adress] &= ~mask; |
break; |
case 1: |
display_buffer[adress] |= mask; |
break; |
case 2: |
display_buffer[adress] ^= mask; |
break; |
} |
set_adress (adress, display_buffer[adress]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t ch; |
uint16_t adress; |
uint8_t x1,y1; |
uint8_t x0,y0; |
uint8_t xw; |
uint8_t mask; |
uint8_t bit; |
uint8_t *font; |
//------------------------ |
// char translate |
//------------------------ |
switch (c) |
{ // ISO 8859-1 |
case 0xc4: c = 0x01; break; // Ä |
case 0xe4: c = 0x02; break; // ä |
case 0xd6: c = 0x03; break; // Ö |
case 0xf6: c = 0x04; break; // ö |
case 0xdc: c = 0x05; break; // Ü |
case 0xfc: c = 0x06; break; // ü |
case 0xdf: c = 0x1e; break; // ß c = 0x07; ° (used by Jeti) |
} |
c &= 0x7f; |
//------------------------ |
// Font Parameter setzen |
//------------------------ |
#ifdef USE_FONT_BIG |
if( mode <=2 ) // normaler font (8x6) |
{ |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
} |
else // grosser font (8x8) |
{ |
font = (uint8_t *) &Font8x8[0][0]; |
xw = 8; |
} |
#else |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
#endif |
//------------------------ |
//------------------------ |
x0 = (x*xw) + xoffs; |
y0 = (y*8) + yoffs; |
if( yoffs == 0) |
{ |
//---------------------------------------------------------- |
// orginaler Character Algo |
// |
// funktioniert auch mit x Verschiebung aber nicht |
// mit y Verschiebung. |
// |
// Da 8 Bit aufeinmal gesetzt werden ist dieser Algo |
// bzgl. Geschwindigkeit effektiver als der Y-Algo. |
//---------------------------------------------------------- |
if( mode==MINVERS || mode==MBIGINVERS ) |
lcd_frect( (x*xw)+xoffs, (y*8), xw-1, 7, 1); // invertierte Darstellung |
adress = y * 128 + x * xw + xoffs; |
adress &= 0x3FF; |
for( x1 = 0; x1 < xw; x1++) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
if( mode==MINVERS || mode==MBIGINVERS ) |
display_buffer[adress + x1] ^= ch; |
else |
display_buffer[adress + x1] = ch; |
set_adress (adress + x1, display_buffer[adress + x1]); |
} |
} |
else |
{ |
//---------------------------------------------------------- |
// Y-Algo |
// neuer Character Algo (nur wenn Pixel y-Verschiebung) |
//---------------------------------------------------------- |
for( x1 = 0; x1 < xw; x1++ ) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
mask = 1; |
for( y1 = 0; y1 < 8; y1++ ) |
{ |
bit = (ch & mask); |
if( bit ) |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 0 : 1) ); |
else |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 1 : 0) ); |
mask = (mask << 1); |
} |
} |
} |
} |
//----------------------------------------------------------- |
//--- Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode ) |
{ |
lcdx_putc( x, y, c, mode, 0,0 ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w) |
{ |
uint8_t lcd_width; |
uint8_t lcd_zpos; |
uint8_t i; |
uint8_t max = 21; |
lcd_width = w; |
lcd_xpos = x; |
lcd_ypos = y; |
if ((lcd_xpos + lcd_width) > max) |
lcd_width = max - lcd_xpos; |
lcd_zpos = lcd_xpos + lcd_width; |
for (i = lcd_xpos; i < lcd_zpos; i++) |
lcd_putc (i, lcd_ypos, 0x20, 0); |
} |
//----------------------------------------------------------- |
void wait_1ms (void) |
{ |
_delay_ms (1); |
} |
//----------------------------------------------------------- |
void wait_ms (uint16_t time) |
{ |
uint16_t i; |
for (i = 0; i < time; i++) |
wait_1ms (); |
} |
//----------------------------------------------------------- |
void LCD_Init( uint8_t LCD_Mode ) // LCD_Mode 0= Default Mode 1= EEPROM-Parameter) |
{ |
lcd_xpos = 0; |
lcd_ypos = 0; |
// DDRB = 0xFF; |
// SPI max. speed |
// the DOGM128 lcd controller can work at 20 MHz |
SPCR = (1 << SPE) | (1 << MSTR) | (1 << CPHA) | (1 << CPOL); |
SPSR = (1 << SPI2X); |
set_cs(); |
clr_reset(); |
wait_ms(10); |
set_reset(); |
clr_cs(); |
clr_A0(); |
send_byte( 0x40 ); //Display start line = 0 |
//------------------------------------------------------------ |
// 10.11.2014 cebra |
// Umschaltung der Displayansicht wird nicht mehr benötigt |
// if (LCD_Mode == 1) |
// { |
// if (LCD_ORIENTATION == 0) |
// { |
// send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
// else |
// { |
// send_byte( 0xA0 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC8 ); // C0 normal C8 reverse(original) |
// } |
// } |
// else |
// { |
send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
/* |
//------------------------------------------------------------ |
// 26.05.2014 OG: einstellen von Normal/Invers durch den User |
// nicht mehr unterstuetzt, da der Modus Invers beim PKT |
// auf verschiedenen Display's nicht gut aussieht. |
// Ab jetzt nur noch LC-Modus Normal. |
//------------------------------------------------------------ |
if (LCD_Mode == 1) |
{ |
if (Config.LCD_DisplayMode == 0) |
send_byte (0xA6); //Display normal, not mirrored |
else |
send_byte (0xA7); //Display reverse, not mirrored |
} |
else |
send_byte (0xA6); |
*/ |
send_byte( 0xA6 ); //Display normal, not mirrored |
send_byte( 0xA2 ); //Set bias 1/9 (Duty 1/65) |
send_byte( 0x2F ); //Booster, regulator and follower on |
send_byte( 0xF8 ); //Set internal booster to 4x |
send_byte( 0x00 ); //Set internal booster to 4x |
send_byte( 0x27 ); //resistor ratio set |
if( LCD_Mode == 1 ) |
{ |
send_byte( 0x81 ); //Electronic volume register set |
send_byte( Config.LCD_Kontrast ); //Electronic volume register set |
} |
else |
{ |
send_byte( 0x81 ); |
send_byte( 0x16 ); |
} |
send_byte( 0xAC ); //Cursor |
send_byte( 0x00 ); //No Cursor |
send_byte( 0xAF ); //No indicator |
if( Config.HWSound == 0 ) |
{ |
if( LCD_Mode == 1 ) |
{ |
//------------------------------------------------------------ |
//10.11.2014 cebra, |
//Helligkeitseinstellungen werden nicht mehr genutzt |
// Helligkeit setzen |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
//OCR2A = 255; |
} |
else |
{ |
// OCR2A = 255; |
} |
} |
lcd_cls(); |
} |
//----------------------------------------------------------- |
// sicher eine Zeile für die Statusanzeige |
void copy_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
line_buffer[i] = display_buffer[adress+i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
// holt gesicherte Zeile wieder zurück |
void paste_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
display_buffer[adress+i] =line_buffer[i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
while (*s) |
{ |
lcdx_putc(x, y, *s++, mode, xoffs,yoffs); |
x++; |
} |
}/* lcd_puts */ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode ) |
{ |
lcdx_puts_at( x, y, s, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void new_line (void) |
{ |
lcd_ypos++; |
if (lcd_ypos > 7) |
{ |
scroll (); |
lcd_ypos = 7; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns (const char *text, uint8_t mode) |
{ |
while (pgm_read_byte(text)) |
{ |
switch (pgm_read_byte(text)) |
{ |
case 0x0D: |
lcd_xpos = 0; |
break; |
case 0x0A: |
new_line(); |
break; |
default: |
lcd_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode); |
lcd_xpos++; |
if (lcd_xpos > 21) |
{ |
lcd_xpos = 0; |
// new_line (); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_printpns (text, mode); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// erweitert bei mode MINVERSX und MNORMALX links und oben den |
// Text um jeweils einen Pixel -> der Invers-Modus sieht besser aus |
//-------------------------------------------------------------- |
uint8_t _lcdx_print_modeextend( uint8_t progmem, uint8_t x, uint8_t y, uint8_t textlen, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t draw = 0; |
if( (mode == MNORMALX) || (mode == MINVERSX) ) |
{ |
if( mode == MNORMALX ) mode = MNORMAL; |
else mode = MINVERS; |
if( mode == MINVERS ) |
draw = 1; |
if( (y*8)+yoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs, (y*8)+yoffs-1, (x*6)+xoffs+(textlen*6)-1, (y*8)+yoffs-1, draw); // horizontale Linie ueber dem Text |
if( (x*6)+xoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs-1, (y*8)+yoffs-1, (x*6)+xoffs-1, (y*8)+yoffs+7, draw); // vertikale Linie links neben dem Text |
} |
return mode; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void _lcdx_print_outchar( unsigned char c, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc( lcd_xpos, lcd_ypos, c, mode, xoffs,yoffs ); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_modeextend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
_lcdx_print_outchar( *text, mode, xoffs,yoffs); |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
char c; |
mode = _lcdx_print_modeextend( true, lcd_xpos, lcd_ypos, strlen_P( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( (c = pgm_read_byte(text)) ) |
{ |
_lcdx_print_outchar( c, mode, xoffs,yoffs); |
text++; |
} |
} |
/* |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
switch( *text ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, *text, mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( true, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( pgm_read_byte(text) ) |
{ |
switch( pgm_read_byte(text) ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
*/ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp (const char *text, uint8_t mode) |
{ |
lcdx_printp ( text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_printp (text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcdx_printp_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//--- lcd_print: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print (uint8_t *text, uint8_t mode ) |
{ |
lcdx_print (text, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_print (text, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen((const char *)text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_print_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen_P(text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_printp_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center_P( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//--- lcd_print_at: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode ) |
{ |
lcdx_print_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
void print_display (uint8_t *text) |
{ |
while (*text) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, *text, 0); |
lcd_xpos++; |
if (lcd_xpos >= 20) |
{ |
lcd_xpos = 0; |
new_line (); |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
print_display (text); |
} |
//----------------------------------------------------------- |
// + Line (draws a line from x1,y1 to x2,y2 |
// + Based on Bresenham line-Algorithm |
// + found in the internet, modified by thkais 2007 |
//----------------------------------------------------------- |
void lcd_line( unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode) |
{ |
int x, y, count, xs, ys, xm, ym; |
x = (int) x1; |
y = (int) y1; |
xs = (int) x2 - (int) x1; |
ys = (int) y2 - (int) y1; |
if (xs < 0) |
xm = -1; |
else |
if (xs > 0) |
xm = 1; |
else |
xm = 0; |
if (ys < 0) |
ym = -1; |
else |
if (ys > 0) |
ym = 1; |
else |
ym = 0; |
if (xs < 0) |
xs = -xs; |
if (ys < 0) |
ys = -ys; |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
if (xs > ys) // Flat Line <45 degrees |
{ |
count = -(xs / 2); |
while (x != x2) |
{ |
count = count + ys; |
x = x + xm; |
if (count > 0) |
{ |
y = y + ym; |
count = count - xs; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
else // Line >=45 degrees |
{ |
count =- (ys / 2); |
while (y != y2) |
{ |
count = count + xs; |
y = y + ym; |
if (count > 0) |
{ |
x = x + xm; |
count = count - ys; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
} |
//----------------------------------------------------------- |
// + Filled rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_frect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
uint16_t i; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
for (i = y1; i <= y2; i++) |
{ |
lcd_line (x1, i, x2, i, mode); |
} |
} |
//----------------------------------------------------------- |
// ausgefuelltes Rechteck mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_frect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_rect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
lcd_line (x1, y1, x2, y1, mode); |
lcd_line (x2, y1, x2, y2, mode); |
lcd_line (x2, y2, x1, y2, mode); |
lcd_line (x1, y2, x1, y1, mode); |
} |
//----------------------------------------------------------- |
// Rechteck mit mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_rect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+1 , y1+1 , mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1+1 , mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+1 , y1+widthy-1, mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
lcd_plot( x1+widthx-1, y1+widthy-1, mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of a circle |
// + Based on Bresenham-algorithm found in wikipedia |
// + modified by thkais (2007) |
//----------------------------------------------------------- |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_plot (x0, y0 + radius, mode); |
lcd_plot (x0, y0 - radius, mode); |
lcd_plot (x0 + radius, y0, mode); |
lcd_plot (x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y --; |
ddF_y += 2; |
f += ddF_y; |
} |
x ++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
lcd_plot (x0 + y, y0 + x, mode); |
lcd_plot (x0 - y, y0 + x, mode); |
lcd_plot (x0 + y, y0 - x, mode); |
lcd_plot (x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// + filled Circle |
// + modified circle-algorithm thkais (2007) |
//----------------------------------------------------------- |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius,uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_line (x0, y0 + radius, x0, y0 - radius, mode); |
lcd_line (x0 + radius, y0, x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y--; |
ddF_y += 2; |
f += ddF_y; |
} |
x++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_line (x0 + x, y0 + y, x0 - x, y0 + y, mode); |
lcd_line (x0 + x, y0 - y, x0 - x, y0 - y, mode); |
lcd_line (x0 + y, y0 + x, x0 - y, y0 + x, mode); |
lcd_line (x0 + y, y0 - x, x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) r); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) r); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) ry); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) rx); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode) |
{ |
const int16_t rx2 = rx * rx; |
const int16_t ry2 = ry * ry; |
int16_t F = round (ry2 - rx2 * ry + 0.25 * rx2); |
int16_t ddF_x = 0; |
int16_t ddF_y = 2 * rx2 * ry; |
int16_t x = 0; |
int16_t y = ry; |
lcd_plot (x0, y0 + ry, mode); |
lcd_plot (x0, y0 - ry, mode); |
lcd_plot (x0 + rx, y0, mode); |
lcd_plot (x0 - rx, y0, mode); |
// while ( 2*ry2*x < 2*rx2*y ) { we can use ddF_x and ddF_y |
while (ddF_x < ddF_y) |
{ |
if(F >= 0) |
{ |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F -= ddF_y; |
} |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x + ry2; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
F = round (ry2 * (x + 0.5) * (x + 0.5) + rx2 * (y - 1) * (y - 1) - rx2 * ry2); |
while(y > 0) |
{ |
if(F <= 0) |
{ |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x; |
} |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F += rx2 - ddF_y; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
lcd_ellipse (x0, y0, radius + 3, radius, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
lcd_ellipse_line(x, y, r + 3, r, deg, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_view_font (uint8_t page) |
{ |
int x; |
int y; |
lcd_cls (); |
lcd_printp (PSTR(" 0123456789ABCDEF\r\n"), 0); |
lcd_printpns_at (0, 7, PSTR(" \x1a \x1b Exit"), 0); |
lcd_ypos = 2; |
for (y = page * 4 ; y < (page * 4 + 4); y++) |
{ |
if (y < 10) |
{ |
lcd_putc (0, lcd_ypos, '0' + y, 0); |
} |
else |
{ |
lcd_putc (0, lcd_ypos, 'A' + y - 10, 0); |
} |
lcd_xpos = 2; |
for (x = 0; x < 16; x++) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, y * 16 + x, 0); |
lcd_xpos++; |
} |
lcd_ypos++; |
} |
} |
//----------------------------------------------------------- |
uint8_t hdigit (uint8_t d) |
{ |
if (d < 10) |
{ |
return '0' + d; |
} |
else |
{ |
return 'A' + d - 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos, lcd_ypos, hdigit (h & 0x0f), mode); |
} |
//----------------------------------------------------------- |
void lcd_print_hex (uint8_t h, uint8_t mode) |
{ |
// lcd_xpos = x; |
// lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h & 0x0f), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, ' ', mode); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//----------------------------------------------------------- |
// lcd_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
void lcd_write_number_u (uint8_t number) |
{ |
uint8_t num = 100; |
uint8_t started = 0; |
while (num > 0) |
{ |
uint8_t b = number / num; |
if (b > 0 || started || num == 1) |
{ |
lcd_putc (lcd_xpos++, lcd_ypos, '0' + b, 0); |
started = 1; |
} |
number -= b * num; |
num /= 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_write_number_u (number); |
} |
//----------------------------------------------------------- |
// numtype: 'd' oder 'u' |
// |
// Ergebnis: ein String in format_buffer |
// -> "%4d" "%4.2d" "%03.1d" "%4u" usw... |
//----------------------------------------------------------- |
void make_number_format( char numtype, int16_t length, int16_t decimals, uint8_t pad ) |
{ |
register char *p; |
register char *psrc; |
p = format_buffer; |
*p = '%'; p++; // start '%' |
if(pad) { *p = '0'; p++; } // pad '0' |
itoa( length, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // vorkomma |
if( decimals > 0 ) |
{ |
*p = '.'; p++; // punkt '.' |
itoa( decimals, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // nachkomma |
} |
*p = numtype; p++; // 'd' oder 'u' |
*p = 0; |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s (x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u_10th( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-2, 2, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad) |
{ |
writex_ndigit_number_u_100th( x, y, number, length, pad, MNORMAL, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s_10th ( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// write <seconds> as human readable time at <x>/<y> to MAX7456 display mem |
// |
void writex_time( uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
uint16_t min = seconds / 60; |
seconds -= min * 60; |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u", min, seconds); |
/* |
writex_ndigit_number_u (x, y, min, 2, 0,mode, xoffs,yoffs); |
lcdx_putc (x + 2, y, ':', mode, xoffs,yoffs); |
writex_ndigit_number_u (x + 3, y, seconds, 2, 1,mode, xoffs,yoffs); |
*/ |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_time( uint8_t x, uint8_t y, uint16_t seconds) |
{ |
writex_time( x, y, seconds, 0, 0,0); |
} |
//-------------------------------------------------------------- |
// writex_datetime_time() |
// |
// Anzeigeformat: 'hh:mm:ss' |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u:%02u", (uint8_t)(dtlocal.seconds/3600), (uint8_t)((dtlocal.seconds/60)%60), (uint8_t)(dtlocal.seconds%60)); |
} |
//-------------------------------------------------------------- |
// writex_datetime_date() |
// |
// Anzeigeformat: 'dd.mm.yyyy' (keine Unterstuetzung von anderen Formaten aus aller Welt) |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
if( datetime.year > 0 ) |
{ |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u.%02u.%04u", dtlocal.day, dtlocal.month, dtlocal.year); |
} |
else |
{ |
// keine UTCZeit -> keine Datumsanzeige |
lcdx_printp_at( x, y, PSTR(" . . "), mode, xoffs,yoffs); |
} |
} |
//----------------------------------------------------------- |
// writes a single gps-pos |
//----------------------------------------------------------- |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%3.5ld", GPSpos/100); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Show_PKTError_NoRAM(void) |
{ |
lcd_cls(); |
lcd_rect( 8, 8, 127-16, 6*8, 1); |
// 123456789012345678901 |
lcd_printp_at( 2,2 , PSTR("** OUT OF RAM **"), MINVERS); |
lcd_printp_at( 2,4 , PSTR("this function is"), MNORMAL); |
lcd_printp_at( 2,5 , PSTR("not available!"), MNORMAL); |
while ( !( (get_key_press (1 << KEY_ENTER)) || (get_key_press (1 << KEY_ESC)) || (get_key_press (1 << KEY_PLUS)) || (get_key_press (1 << KEY_MINUS)) ) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ShowTitle_P( const char *title, uint8_t clearscreen ) |
{ |
if( clearscreen ) |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( strlen_P(title) < 17 ) |
show_Lipo(); |
lcd_printp_at ( 1, 0, title , MINVERS); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void Popup_Draw( uint8_t heigthC ) |
{ |
uint8_t y, h; |
h = heigthC*8; |
y = 63-h; |
//lcd_frect( 0, (hC*8)-6, 127, 63-(hC*8)+6, 1); // Box white |
lcd_frect( 0, y-8, 127, 3, 0); // Box clear |
lcd_frect( 0, y-4, 127, h+4, 1); // Box fill white |
lcd_plot ( 0, y-4, 0); // Ecke links oben 1 |
lcd_plot ( 127, y-4, 0); // Ecke rechts oben 1 |
lcd_line ( 1, y-4, 0, y-3, 0); // Ecke links oben 2 |
lcd_line ( 127-1, y-4, 127, y-3, 0); // Ecke rechts oben 2 |
} |
//----------------------------------------------------------- |
// lcdx_cls_rowwidth( y, width, mode, xoffs,yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
// |
// width: in Zeichen |
//----------------------------------------------------------- |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
int8_t xadd = 0; |
int8_t yadd = 0; |
if( mode == MNORMALX || mode == MINVERSX ) |
{ |
if( xoffs > 0 ) xadd = 1; |
if( (y*8)+yoffs > 0 ) yadd = 1; |
if( mode == MNORMALX ) |
mode = MNORMAL; |
else |
mode = MINVERS; |
} |
lcd_frect( xoffs-xadd, (y*8)+yoffs-yadd, (width*6)+xadd, 7+yadd, (mode == MNORMAL ? 0 : 1) ); // Zeile loeschen |
} |
//----------------------------------------------------------- |
// lcdx_cls_row( y, mode, yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
//----------------------------------------------------------- |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_rowwidth( y, 21, mode, 0, yoffs ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_setpos( uint8_t x, uint8_t y ) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_print_char( uint8_t c, uint8_t mode ) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, c, mode, 0,0); |
lcd_xpos++; |
if( lcd_xpos > 20 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//---------------------------------------------------- |
// gibt einen Linefeed aus (CR+LF) |
//---------------------------------------------------- |
void lcd_print_LF( void ) |
{ |
lcd_print_char( 0x0D, MNORMAL ); |
lcd_print_char( 0x0A, MNORMAL ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd/lcd.h |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* Thanks to Oliver Schwaneberg for adding several functions to this library!* |
* * |
* Author: Jan Michel (jan at mueschelsoft dot de) * |
* License: GNU General Public License, version 3 * |
* Version: v0.93 September 2010 * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.h |
//# |
//# 27.06.2014 OG |
//# - add: define MINVERSX, MNORMALX |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: R# define's fuer lcd_rect_round() |
//# |
//# 12.02.2014 OG |
//# - add: lcd_frect_round() |
//# - add: lcd_rect_round() |
//# |
//# 03.02.2014 OG |
//# - fix: bei writex_ndigit_number_u_100th() fehlte der Parameter 'mode' |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: define SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 28.04.2013 OG |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# - del: write_gps_pos() |
//# |
//# 22.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 11.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 07.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 06.03.2013 OG |
//# - add: extended Funktionen lcdx_... / writex... |
//############################################################################ |
#ifndef _LCD_H |
#define _LCD_H |
#include <stdarg.h> // Notwendig! (OG) |
#include "../timer/timer.h" |
#define MNORMAL 0 // Zeichendarstellung: Normal |
#define MINVERS 2 // Zeichendarstellung: Invers |
#define MBIG 3 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Normal |
#define MBIGINVERS 4 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Invers |
#define MNORMALX 5 // Zeichendarstellung: Normal - oben und Links wird eine 1 Pixel Linie gezogen |
#define MINVERSX 6 // Zeichendarstellung: Invers - oben und Links wird eine 1 Pixel Linie gezogen |
// fuer lcd_rect_round() |
#define R0 0 // Radius 0 |
#define R1 1 // Radius 1 |
#define R2 2 // Radius 2 |
#define SYMBOL_AVG 10 |
#define SYMBOL_MAX 13 |
#define SYMBOL_MIN 16 |
#define SYMBOL_SMALLDEGREE 11 |
#define SYMBOL_BIGDEGREE 30 |
#define SYMBOL_CHECK 31 |
//------------------------------------------------------------------------------------ |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
//------------------------------------------------------------------------------------ |
// X-tended |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcdx_print (uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp (const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_puts_at(uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_time (uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs); |
//void writex_gpspos( uint8_t x, uint8_t y, uint32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void Show_PKTError_NoRAM(void); |
void ShowTitle_P( const char *title, uint8_t clearscreen ); |
//------------------------------------------------------------------------------------ |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ); |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcd_setpos( uint8_t x, uint8_t y ); |
void lcd_print_char( uint8_t c, uint8_t mode ); |
void lcd_print_LF( void ); |
void Popup_Draw( uint8_t heigthC ); |
/* |
//----------------------------------------------------------------------------- |
// Command Codes |
//----------------------------------------------------------------------------- |
//1: Display on/off |
#define LCD_DISPLAY_ON 0xAF //switch display on |
#define LCD_DISPLAY_OFF 0xAE //switch display off |
//2: display start line set (lower 6 bits select first line on lcd from 64 lines in memory) |
#define LCD_START_LINE 0x40 |
//3: Page address set (lower 4 bits select one of 8 pages) |
#define LCD_PAGE_ADDRESS 0xB0 |
//4: column address (lower 4 bits are upper / lower nibble of column address) |
#define LCD_COL_ADDRESS_MSB 0x10 |
#define LCD_COL_ADDRESS_LSB 0x00 //second part of column address |
//8: select orientation (black side of the display should be further away from viewer) |
#define LCD_BOTTOMVIEW 0xA1 //6 o'clock view |
#define LCD_TOPVIEW 0xA0 //12 o'clock view |
//9: select normal (white background, black pixels) or reverse (black background, white pixels) mode |
#define LCD_DISPLAY_POSITIVE 0xA6 //not inverted mode |
#define LCD_DISPLAY_INVERTED 0xA7 //inverted display |
//10: show memory content or switch all pixels on |
#define LCD_SHOW_NORMAL 0xA4 //show dram content |
#define LCD_SHOW_ALL_POINTS 0xA5 //show all points |
//11: lcd bias set |
#define LCD_BIAS_1_9 0xA2 |
#define LCD_BIAS_1_7 0xA3 |
//14: Reset Controller |
#define LCD_RESET_CMD 0xE2 |
//15: output mode select (turns display upside-down) |
#define LCD_SCAN_DIR_NORMAL 0xC0 //normal scan direction |
#define LCD_SCAN_DIR_REVERSE 0xC8 //reversed scan direction |
//16: power control set (lower 3 bits select operating mode) |
//Bit 0: Voltage follower on/off - Bit 1: Voltage regulator on/off - Bit 2: Booster circuit on/off |
#define LCD_POWER_CONTROL 0x28 //base command |
#define LCD_POWER_LOW_POWER 0x2F |
#define LCD_POWER_WIDE_RANGE 0x2F |
#define LCD_POWER_LOW_VOLTAGE 0x2B |
//17: voltage regulator resistor ratio set (lower 3 bits select ratio) |
//selects lcd voltage - 000 is low (~ -2V), 111 is high (~ - 10V), also depending on volume mode. Datasheet suggests 011 |
#define LCD_VOLTAGE 0x20 |
//18: Volume mode set (2-byte command, lower 6 bits in second word select value, datasheet suggests 0x1F) |
#define LCD_VOLUME_MODE_1 0x81 |
#define LCD_VOLUME_MODE_2 0x00 |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
//19: static indicator (2-byte command), first on/off, then blinking mode |
#define LCD_INDICATOR_ON 0xAD //static indicator on |
#define LCD_INDICATOR_OFF 0xAC //static indicator off |
#define LCD_INDICATOR_MODE_OFF 0x00 |
#define LCD_INDICATOR_MODE_1HZ 0x01 |
#define LCD_INDICATOR_MODE_2HZ 0x10 |
#define LCD_INDICATOR_MODE_ON 0x11 |
//20: booster ratio set (2-byte command) |
#define LCD_BOOSTER_SET 0xF8 //set booster ratio |
#define LCD_BOOSTER_234 0x00 //2x-4x |
#define LCD_BOOSTER_5 0x01 //5x |
#define LCD_BOOSTER_6 0x03 //6x |
//#endif |
//22: NOP command |
#define LCD_NOP 0xE3 |
//#if DISPLAY_TYPE == 102 |
////25: advanced program control |
//#define LCD_ADV_PROG_CTRL 0xFA |
//#define LCD_ADV_PROG_CTRL2 0x10 |
//#endif |
//----------------------------------------------------------------------------- |
// Makros to execute commands |
//----------------------------------------------------------------------------- |
#define LCD_SWITCH_ON() lcd_command(LCD_DISPLAY_ON) |
#define LCD_SWITCH_OFF() lcd_command(LCD_DISPLAY_OFF) |
#define LCD_SET_FIRST_LINE(i) lcd_command(LCD_START_LINE | ((i) & 0x3F)) |
#define LCD_SET_PAGE_ADDR(i) lcd_command(LCD_PAGE_ADDRESS | ((i) & 0x0F)) |
#define LCD_SET_COLUMN_ADDR(i) lcd_command(LCD_COL_ADDRESS_MSB | ((i>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((i) & 0x0F)) |
#define LCD_GOTO_ADDRESS(page,col); lcd_command(LCD_PAGE_ADDRESS | ((page) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_MSB | ((col>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((col) & 0x0F)); |
#define LCD_SET_BOTTOM_VIEW() lcd_command(LCD_BOTTOMVIEW) |
#define LCD_SET_TOP_VIEW() lcd_command(LCD_TOPVIEW) |
#define LCD_SET_MODE_POSITIVE() lcd_command(LCD_DISPLAY_POSITIVE) |
#define LCD_SET_MODE_INVERTED() lcd_command(LCD_DISPLAY_INVERTED) |
#define LCD_SHOW_ALL_PIXELS_ON() lcd_command(LCD_SHOW_ALL_POINTS) |
#define LCD_SHOW_ALL_PIXELS_OFF() lcd_command(LCD_SHOW_NORMAL) |
#define LCD_SET_BIAS_RATIO_1_7() lcd_command(LCD_BIAS_1_7) |
#define LCD_SET_BIAS_RATIO_1_9() lcd_command(LCD_BIAS_1_9) |
#define LCD_SEND_RESET() lcd_command(LCD_RESET_CMD) |
#define LCD_ORIENTATION_NORMAL() lcd_command(LCD_SCAN_DIR_NORMAL) |
#define LCD_ORIENTATION_UPSIDEDOWN() lcd_command(LCD_SCAN_DIR_REVERSE) |
#define LCD_SET_POWER_CONTROL(i) lcd_command(LCD_POWER_CONTROL | ((i) & 0x07)) |
#define LCD_SET_LOW_POWER() lcd_command(LCD_POWER_LOW_POWER) |
#define LCD_SET_WIDE_RANGE() lcd_command(LCD_POWER_WIDE_RANGE) |
#define LCD_SET_LOW_VOLTAGE() lcd_command(LCD_POWER_LOW_VOLTAGE) |
#define LCD_SET_BIAS_VOLTAGE(i) lcd_command(LCD_VOLTAGE | ((i) & 0x07)) |
#define LCD_SET_VOLUME_MODE(i) lcd_command(LCD_VOLUME_MODE_1); \ |
lcd_command(LCD_VOLUME_MODE_2 | ((i) & 0x3F)) |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
#define LCD_SET_INDICATOR_OFF() lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_INDICATOR_MODE_OFF) |
#define LCD_SET_INDICATOR_STATIC() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_ON) |
#define LCD_SET_INDICATOR_1HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_1HZ) |
#define LCD_SET_INDICATOR_2HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_2HZ) |
#define LCD_SET_INDICATOR(i,j) lcd_command(LCD_INDICATOR_OFF | ((i) & 1)); \ |
lcd_command(((j) & 2)) |
#define LCD_SLEEP_MODE lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_DISPLAY_OFF); \ |
lcd_command(LCD_SHOW_ALL_POINTS) |
//#endif |
//#if DISPLAY_TYPE == 102 |
//#define LCD_TEMPCOMP_HIGH 0x80 |
//#define LCD_COLWRAP 0x02 |
//#define LCD_PAGEWRAP 0x01 |
//#define LCD_SET_ADV_PROG_CTRL(i) lcd_command(LCD_ADV_PROG_CTRL); |
// lcd_command(LCD_ADV_PROG_CTRL2 & i) |
//#endif |
*/ |
extern volatile uint8_t LCD_ORIENTATION; |
//#define LCD_LINES 8 |
//#define LCD_COLS 21 |
extern uint8_t lcd_xpos; |
extern uint8_t lcd_ypos; |
void lcd_set_contrast( uint8_t value ); |
void lcd_command(uint8_t cmd); |
void send_byte (uint8_t data); |
void LCD_Init (uint8_t LCD_Mode); |
void new_line (void); |
void lcd_puts_at(uint8_t x, uint8_t y,const char *s, uint8_t mode ); |
void lcd_putc (uint8_t x, uint8_t y, uint8_t c, uint8_t mode); |
void send_byte (uint8_t data); |
void lcd_print (uint8_t *text, uint8_t mode); |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode); |
void lcd_printp (const char *text, uint8_t mode); |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_printpns (const char *text, uint8_t mode); |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_cls (void); |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w); |
void print_display (uint8_t *text); |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text); |
void copy_line (uint8_t y); |
void paste_line (uint8_t y); |
void lcd_plot (uint8_t x, uint8_t y, uint8_t mode); |
void lcd_line (unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode); |
void lcd_rect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_frect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode); |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode); |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_view_font (uint8_t page); |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode); |
void lcd_write_number_u (uint8_t number); |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number); |
void lcd_print_hex (uint8_t h, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad,uint8_t mode); |
/** |
* Write only some digits of a signed <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /100th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_100th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad); |
/** |
* Write only some digits of a signed <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* write <seconds> as human readable time at <x>/<y> |
*/ |
void write_time (uint8_t x, uint8_t y, uint16_t seconds); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lcd |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lipo/lipo.c |
---|
0,0 → 1,178 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lipo.c |
//# |
//# 15.06.2014 OG |
//# - fix; show_Lipo() - wenn die Spannung unter 3.20 Volt ging, wurde wieder |
//# ein voller Akkubalken angezeigt aufgrund einer Subtraktion - fixed |
//# |
//# 21.02.2014 OG |
//# - chg: show_Lipo() neu geschrieben - etwas smarter, ein paar Bytes kleiner |
//# |
//# 20.02.2014 OG |
//# - chg: Codeformattierungen |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "lipo.h" |
#include "../eeprom/eeprom.h" |
// Global variables |
double accumulator = 0; //!< Accumulated 10-bit samples |
double Vin = 0; //!< 16-bit float number result |
short temp = 0; //!< Temporary variable |
short samples = 0; //!< Number of conversions |
uint16_t volt_avg = 0; |
//-------------------------------------------------------------- |
//! ADC interrupt routine |
//-------------------------------------------------------------- |
ISR (ADC_vect) |
{ |
accumulator += ADCW; |
samples++; |
if( samples>4095 ) |
{ |
oversampled(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ADC_Init (void) |
{ |
DDRA &= ~(1<<DDA1); // MartinR |
PORTA &= ~(1<<PORTA1); // MartinR |
ADMUX = (0<<REFS1) | (1<<REFS0); // externe 5V Referenzspannung nutzen |
ADMUX = (ADMUX & ~(0x1F)) | (1 & 0x1F); // ADC1 verwenden |
ADCSRA = (1<<ADEN)|(1<<ADIE)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // Prescaler 128, Freilaufend, Interrupte frei |
} |
//-------------------------------------------------------------- |
// Error compensation, Scaling 16-bit result, Rounding up, |
// Calculate 16-bit result, Resets variables |
// |
// Quelle AVR121: Enhancing ADC resolution by versampling |
//-------------------------------------------------------------- |
void oversampled(void) |
{ |
cli(); |
accumulator += Config.Lipo_UOffset; //5150 Offset error compensation |
// accumulator *= 0.9993; // Gain error compensation |
accumulator *= 0.9600; //0.9800 Gain error compensation |
temp=(int)accumulator%64; |
accumulator/=64; // Scaling the answer |
if(temp>=32) |
{ |
accumulator += 1; // Round up |
} |
// Vin = (accumulator/65536)*4.910; // Calculating 16-bit result |
Vin =accumulator/7.5; |
volt_avg = Vin; |
// write_ndigit_number_u(0, 3, Vin, 5, 0,0); |
// write_ndigit_number_u(0, 4, volt_avg, 5, 0,0); |
samples = 0; |
accumulator = 0; |
sei(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void show_Lipo( void ) |
{ |
int16_t w = 0; |
//-------------------------------------------- |
// Batterie Symbol zeichnen |
//-------------------------------------------- |
lcd_rect(104, 0, 23, 7, 1); // Rahmen aussen |
lcd_rect(105, 1, 21, 5, 0); // Rahmen innen |
lcd_frect(102, 0, 1, 7, 0); // ganz links etwas loeschen |
lcd_rect(103,2,1,3,1); // Batterie "Kappe" |
//-------------------------------------------- |
// Batterie Balken berechnen / zeichnen |
//-------------------------------------------- |
// |
// Config.PKT_Accutyp |
// true = LiPO Akku mit max. 4.2 Volt (420) |
// false = LiON Akku mit max. 4.1 Volt (410) |
if( volt_avg > 320 ) |
{ |
w = (volt_avg-320)/(Config.PKT_Accutyp ? 4.8 : 4.5); // die Faktoren 4.8 (kann bis 5.0) und 4.5 fuer LiPo / LiON wurden per Test ermittelt |
//lcdx_printf_at_P( 12, 1, MNORMAL, 0,0, PSTR("v=%3d"), w ); // Debug Anzeige um Faktoren zu ermitteln |
} |
if(w<0) w = 0; |
if(w>20) w = 20; // nicht mehr als 20 Pixel Breite |
if( w>0 ) |
lcd_frect( 106+(20-w), 2, w-1, 3, 1); // Batterie Balken: fill |
if( w<20 ) |
lcd_frect( 106, 2, 19-w, 3, 0); // Batterie Balken: clear |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/lipo/lipo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _LIPO_H |
#define _LIPO_H |
short samples; //!< Number of conversions |
double Vin; |
double accumulator; |
uint16_t volt_avg; |
void ADC_Init (void); |
void oversampled(void); |
void show_Lipo(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/main.c |
---|
0,0 → 1,202 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mk/mksettings.h" geaendert auf "mksettings/mksettings.h" |
//# |
//# 29.03.2014 OG |
//# - chg: MK_Show_LastGPS_Position() ersetzt durch OSDDATA_ShowLastGPSPosition() |
//# |
//# 28.03.2014 OG |
//# - chg: mk_last_position() ersetzt durch MK_Show_LastGPS_Position() (mkbase.c) |
//# - del: mk_last_position() |
//# |
//# 21.02.2014 OG |
//# - chg: mk_show_lastposition() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 19.02.2014 OG |
//# - chg: main() hinzugefuegt: MKSettings_TMP_Init0() |
//# - add: #include "mk/mksettings.h" |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 11.02.2014 OG |
//# - chg: main() Aufruf von searchMK() bzgl. Parametern angepasst |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# |
//# 02.02.2014 OG |
//# - chg: Screen-Layout von der Anzeige der letzten MK-Position und |
//# Ausgliederung davon in mk_show_lastposition() |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# - chg: verschoben: MK_load_setting() und searchMK() nach mk/mkbase.c |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: searchMK(): kuerzerer Fehlerbeep bei falscher FC-Revision und |
//# automatisches beenden des Fehlerscreens nach 4 Sekunden |
//# |
//# 24.01.2014 OG |
//# - fix: searchMK(): wenn MK_load_setting() keine Daten liefert wird keine |
//# falsche RC-Settings-Revisionsnummer mehr angezeigt sondern "ERR!" |
//# - chg: searchMK(): Bestaetigung Fehleranzeige bei falscher FC-Revision |
//# geaendert von KEY_ESC nach KEY_ENTER - Grund: das PKT behandelt |
//# dieses Problem nun flexibler und z.B. OSD (und anderes) geht noch |
//# - chg: searchMK(): beschleunigte Erkennung inkompatibler FC-Settings-Revisionen |
//# - chg: timings in MK_load_setting() |
//# |
//# 07.1.2016 Cebra |
//# - chg: setzen globale Variable für Abfrage WrongFCVersion etwas verändert |
//# |
//# 26.06.2013 OG |
//# - del: searchMK() Layout und Code-Struktur |
//# - fix: searchMK() laden der MK-Settings |
//# - add: MK_load_setting() - wird ggf. spaeter zu einer bessern Stelle verschoben |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 20.05.2013 OG |
//# - chg: searchMK() Code fuer KEY_ESC geaendert |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Aufruf von main_menu() geaendert zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 16.05.2013 Cebra |
//# - chg: Startinfo wird nur noch komplett angezeigt wenn im Setup auf JA |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 07.05.2013 Cebra |
//# - chg: wenn NC vorhanden wird die Version der NC in NCversion gespeichert |
//# sonst die FC-Version in FCversion |
//# |
//# 27.04.2013 OG |
//# - chg: GPS-Positionsausgabe umgestellt auf writex_gpspos() |
//# - chg: einige clear-lines auf lcd_frect() umgestellt um Speicher zu sparen |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
//************************************************************************************ |
// Watchdog integrieren und abschalten, wird für Bootloader benötigt |
// !!muss hier stehen bleiben!! |
//-------------------------------------------------------------- |
void wdt_init(void) __attribute__((naked)) __attribute__((section(".init1"))); |
//-------------------------------------------------------------- |
void wdt_init(void) |
{ |
MCUSR = 0; |
wdt_disable(); |
return; |
} |
//************************************************************************************ |
// erst ab hier weitere #includes |
#include "main.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "mk-data-structs.h" |
#include "menu.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "wi232/Wi232.h" |
#include "motortest/twimaster.h" |
#include "messages.h" |
#include "utils/scrollbox.h" |
#include "pkt/pkt.h" |
#include "lipo/lipo.h" |
#include "mk/mkbase.h" |
#include "mksettings/mksettings.h" |
#include "osd/osddata.h" |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
volatile uint8_t mode = 0; |
uint8_t hardware = 0; |
uint8_t current_hardware = 0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int main(void) |
{ |
InitHWPorts(); // Hardwareanhängige Ports konfigurieren |
// dafür wird je nach Hardware die HAL_HWxxx verwendet |
// Define dazu in der main.h |
hardware = NO; |
current_hardware = 0; |
MKSettings_TMP_Init0(); // TMP-Setting initialisieren |
if( Config.LastLongitude>0x00000000 && Config.LastLatitude>0x00000000 ) |
{ |
OSDDATA_ShowLastGPSPosition(); // letzte MK GPS-Position zeigen |
} |
searchMK(true); // MK suchen (true = MK_Info() anzeigen) |
while(true) |
{ |
Menu_Main(); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/main.h |
---|
0,0 → 1,448 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.h |
//# |
//# 14.10.2015 Starter |
//# - Offset for FollowMe jetzt activ in Version 3.85h |
//# - FollowMeStep2 wird nicht mehr benötigt, nur zu Debugzwecken |
//# |
//# 20.09.2015 Starter |
//# - FollowMeStep2 wird jetzt nurnoch von USE_FOLLOWME_STEP2 aktiviert |
//# |
//# 19.09.2015 Cebra |
//# - add: Ordner 10DOF hinzugefügt für die Bearbeitung des GY-87 Sensorboard |
//# Kompass, Gyro, ACC, Luftdruck |
//# |
//# 03.08.2015 Cebra |
//# - add: Define für GPS Debug hinzugefügt für GPS Berechnung FollowMe |
//# - add: #define USE_WAYPOINTS , Code sparen |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85e wegen einigen Änderungen in GPS-Maus Bereich, CRC Erkennung und Weitergabe |
//# |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85d wegen Änderungen in gpsmouse.c |
//# |
//# |
//# 16.07.2015 Cebra |
//# - chg: neue Version 3.85a wegen FC2.11a |
//# |
//# 09.04.2015 Cebra |
//# - chg: neue Version 3.84a wegen FC2.09j |
//# |
//# 05.04.2015 Cebra (PKT383b) |
//# - chg: Änderungen wegen Fehlfunktion mit NC 2.09h |
//# SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) |
//# ergänzt um 2. Parameter |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - chg: Anpassung an FC209d |
//# |
//# 27.01.2015 Cebra (PKT382a) |
//# - chg: Anpassung an FC209a |
//# |
//# 26.09.2014 Cebra (PKT381b) |
//# |
//# |
//# 26.09.2014 Cebra (PKT381a) |
//# |
//# 01.07.2014 OG (PKT380e) |
//# |
//# 28.06.2014 OG (PKT380dX4) |
//# |
//# 27.06.2014 OG (PKT380dX3) |
//# - chg: USE_FOLLOWME eingeschaltet |
//# - chg: USE_TRACKING abgeschaltet |
//# |
//# 22.06.2014 OG (PKT380dX3) |
//# |
//# 20.06.2014 OG (PKT380dX2) |
//# |
//# 18.06.2014 OG (PKT380dX1) |
//# |
//# 18.06.2014 OG (PKT380d) |
//# |
//# 05.06.2014 OG (PKT380cX5) |
//# |
//# 02.06.2014 OG (PKT380cX4) |
//# Update auf Windows Atmel AVR Studio 6.2.1153 (Release) |
//# (von ehemals Atmel AVR Studio 6.2 Beta) |
//# |
//# 27.05.2014 OG (PKT380cX3) |
//# |
//# 20.05.2014 OG (PKT380cX2) |
//# |
//# 19.05.2014 OG (PKT380cX1) |
//# - add: USE_OSD_SCREEN_WAYPOINTS (keine Freigabe! In Entwicklung!) |
//# |
//# 18.05.2014 OG (PKT380cX1) |
//# |
//# 15.05.2014 OG (PKT380c) |
//# |
//# 15.05.2014 OG (PKT380bX7) |
//# - chg: USE_OSD_SCREEN_3DLAGE eingeschaltet (war ungewollt abgeschaltet) |
//# |
//# 14.05.2014 OG (PKT380bX7) |
//# |
//# 13.05.2014 OG (PKT380bX6) |
//# - add: USE_MAINMENU_SEPARATOR (aus menu.h hierhin verschoben) |
//# - del: MKINFO_AUTO_REFRESH |
//# |
//# 12.05.2014 OG (PKT380bX5b) |
//# - chg: keine Code-Changes - dafuer Compiler jetzt Atmel Studio 6.2 Beta |
//# (statt bisher Atmel Studio 5.1) |
//# |
//# 07.05.2014 OG (PKT380bX4) |
//# |
//# 07.05.2014 OG (PKT380bX3) |
//# - fix: EEpromversion erhoeht auf 138 (siehe eeprom.c) |
//# |
//# 02.05.2014 OG (PKT380bX2) |
//# |
//# 28.04.2014 OG |
//# - chg: USE_OSD_SCREEN_OLD abgeschaltet |
//# |
//# 20.04.2014 OG (PKT380bX1) |
//# |
//# 18.04.2014 OG (PKT380b) |
//# |
//# 17.04.2014 OG (PKT380aX1) |
//# - chg: PKT Versionsanzeige geaendert auf "3.80a" statt wie frueher "3.8.0a" |
//# |
//# 16.04.2014 OG (PKT380a) |
//# |
//# 14.04.2014 OG (PKT374aX8) |
//# |
//# 02.04.2014 OG |
//# - chg: define fuer Baud_2400 von 0 auf 7 geaendert |
//# (0/false wird benoetigt um zu erkennen ob keine Baud-Rate erkannt wurde) |
//# |
//# 01.04.2014 OG |
//# - add: USE_SV2MODULE_BLE (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 31.03.2014 OG (PKT374aX7) |
//# |
//# 30.03.2014 OG (PKT374aX6) |
//# Sprachunterstuetzung fuer Hollaendisch vollstaendig entfernt |
//# |
//# 30.03.2014 OG (PKT374aX5) |
//# Source-Schnipp PKT374aX5 |
//# |
//# 01.03.2014 OG (PKT374aX5) |
//# |
//# 26.02.2014 OG (PKT374aX4) |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 22.02.2014 OG (PKT374aX4) |
//# |
//# 17.02.2014 OG (PKT374aX3) |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 14.02.2014 OG (PKT374aX2) |
//# - add: MKINFO_AUTO_REFRESH |
//# - del: veraltete PKT defines HWVERSION... entfernt |
//# |
//# 12.02.2014 OG (PKT374aX2) |
//# - del: defines zu MKVERSIONnnn entfernt |
//# |
//# 10.02.2014 OG (PKT374aX1) |
//# - add: ... |
//# |
//# 05.02.2014 OG (PKT374aX1) |
//# - add: DEBUG_NEW_PARAMETERS - nur fuer Entwicklung! |
//# |
//# 30.01.2014 OG (PKT373cX4) |
//# - add: USE_BLUETOOTH |
//# |
//# 29.01.2014 OG (3.7.3cX3) |
//# - del: extern uint8_t searchMK(void) ist jetzt in mk/mkbase.c |
//# |
//# 24.01.2014 OG (3.7.3cX1) |
//# - add: DEBUG_FC_COMMUNICATION (Auswirkung in usart.c) |
//# |
//# 06.01.2014 CB |
//# - add: Abschaltung MK Parameter aendern bei nicht passender FC Software |
//# |
//# 02.12.2013 CB |
//# - add: Anpassung Parameter an FC 2.01f |
//# |
//# 21.10.2013 CB |
//# - add: Anpassung Parameter an FC 2.01a |
//# |
//# 16.07.2013 CB |
//# - chg: neuen Versionsnummer wegen Wiflypatch |
//# |
//# 07.07.2013 OG |
//# - add: ABO_TIMEOUT und von 3 sec auf 2 sec gesetzt (war vorher in timer.h) |
//# - del: define USE_OSD_SCREEN_ELECTRIC_N |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# - add: DEBUG_SV2_EXTENSION um Debug-Ausgaben fuer den SV2-Patch an-/auszuschalten |
//# |
//# 05.07.2013 CB |
//# - add: USE_WLAN |
//# |
//# 30.06.2013 OG |
//# - add: USE_OSD_PKTHOOK |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - add: USE_PKTTOOLS_SV2 - zeigt SV2-Verbindungsmenuepunkte in PKT-Tools an |
//# bzw. nicht an. Am PKT-SV2 liegen +5 Volt für die FC/NC an. |
//# Anmerkung OG: bei meinen Kompilaten wird das erstmal ausgeschaltet |
//# sein bis 100% klar ist ob das keine Probleme macht |
//# wenn der Kopter mit FC/NC nicht bereits anderweitig |
//# (Netzteil/Lipo) mit Strom versorgt wird. |
//# |
//# 22.06.2013 Cebra |
//# - chg: Fehler bei der Lipooffset Einstellung beseitigt, neue Version 3.7.0c |
//# |
//# 20.06.2013 Cebra |
//# - chg: falsche Versionsnummer korrigiert, neu jetzt 3.7.0b |
//# |
//# 13.06.2013 OG |
//# - del: USE_PCFASTCONNECT (nicht mehr benoetigt) |
//# |
//# 12.06.2013 Cebra |
//# - chg: Versionswechsel auf 3.7.0a, Zielrelease für FC-Software 0.91 (bzw.1.00??) |
//# |
//# 10.06.2013 Cebra |
//# - add: #define MKVERSION091f für FC-Version 0.91f |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a für FC-Version 0.91a |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Kommentare zum Module-Support |
//# |
//# 19.05.2013 OG |
//# - add: USE_MODULEINFO |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: USE_MKGPSINFO |
//# |
//# 15.05.2013 OG |
//# - add: define USE_PKT_STARTINFO |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 13.05.2013 Cebra |
//# - add: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//# 05.05.2013 Cebra |
//# - chg: #define USE_FOLLOWME |
//# |
//# 03.05.2013 OG |
//# - del: USE_XPRINTF_LONG - spart keinen Platz mehr und ist nun dauerhaft notwendig |
//# |
//# 28.04.2013 OG |
//# - add: define USE_PCFASTCONNECT |
//# - add: define USE_FONT_BIG |
//# - add: define USE_XPRINTF_LONG |
//# - chg: Anordnung der Feature- & Debug-defines um einfacher via Copy&Paste |
//# die Einstellungen notieren bzw. posten zu koennen |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: USE_OSD_... defines |
//############################################################################ |
#ifndef _MAIN_H |
#define _MAIN_H |
// Softwareversion des PKT |
#define PKTSWVersion "3.85_h" // PKT Version |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
#define USE_MAINMENU_SEPARATOR // Menuetrennlinien im PKT-Hauptmenues anzeigen |
//######################################################################### |
//# MODULE SUPPORT |
//# ein-/ausgeschalten von Modulen des PKT um ggf. Speicherplatz zu sparen |
//# |
//# Hinweis: |
//# wenn neue USE_* hinzugefuegt werden bitte die in PKT_Info()(pkt.c) |
//# ergaenzen fuer eine aktualisierte Modul-Info |
//######################################################################### |
//--------------------------------------------- |
//- Module: Hauptfunktionen und allgemeine |
//--------------------------------------------- |
#define USE_OSDDATA // zeigt die OSD-Statistikdaten an (ca. 3.5 KByte) |
#define USE_MKSETTINGS // lesen, aktivieren und bearbeiten der MK-Settings 1-5 (ca. ?? KByte) |
#define USE_MKDEBUGDATA // Anzeige MK-Debug-Data (ca. 1.7 KByte) |
#define USE_MKDISPLAY // Anzeige MK-Display (ca. 1 KByte) |
#define USE_MKGPSINFO // Anzeige MK-GPS-Daten (ca. 2 KByte) |
//#define USE_SOUND // PKT Sounderweiterung, benoetigt entsprechende Hardware (ca. 2.5 Kbyte) |
#define USE_MODULEINFO // Anzeige installierter Module in PKT_Info() (ca. 1.8 KByte) |
#define USE_BLUETOOTH // Bluetooth Unterstuetzung des PKT (ca. 13 KByte) |
// Achtung! Das schaltet USE_TRACKING, USE_FOLLOWME aus! |
//#define USE_WAYPOINTS // Waypoint mit dem PKT |
//Entwicklung: |
//#define USE_ACCCALIBRATION // schaltet ACC Calibration im Menü ein/aus |
//#define USE_KOMPASS // Tilt kompensierter Kompass für FollowMe Funktionen |
//--------------------------------------------- |
//- Module: OSD-Screens |
//--------------------------------------------- |
//#define USE_OSD_SCREEN_OLD // OSD Screens OSD0, OSD1, OSD2 (ca. 6.6 KByte) |
#define USE_OSD_SCREEN_NAVIGATION // OSD-Screen: Navigation |
#define USE_OSD_SCREEN_WAYPOINTS // OSD-Screen: Waypoints |
#define USE_OSD_SCREEN_ELECTRIC // OSD-Screen: Electric |
#define USE_OSD_SCREEN_MKSTATUS // OSD-Screen: MK-Status |
#define USE_OSD_SCREEN_USERGPS // OSD-Screen: UserGPS |
#define USE_OSD_SCREEN_3DLAGE // OSD-Screen: 3D-Lage (ca. 900 Bytes) |
#define USE_OSD_SCREEN_STATISTIC // OSD-Screen: Statistic |
//--------------------------------------------- |
//- Module fuer spezielle Benutzergruppen |
//--------------------------------------------- |
#define USE_FOLLOWME // FollowMe Funktionen (ca. 3 Kbyte) |
//#define USE_FOLLOWME_STEP2 // FollowMe Funktionen Abstand und Winkel einstellbar |
//#define USE_JOYSTICK // Joystick Support, benoetigt spezielle Hardware (ca. 4.1 KByte) |
#define USE_WLAN // WLAN WiFly Modul an SV2 (ca. 6 KByte)) |
#define USE_SV2MODULE_BLE // Bluetooth 4 Low Energy Modul - externes Modul an SV2 (RedBearLab BLE Mini) (ca. 200 Bytes) |
//--------------------------------------------- |
//- Module mit geringer Relevanz |
//--------------------------------------------- |
//#define USE_I2CMOTORTEST // I2C Funktionen für Motortest (ca. 3.8 KByte) |
//#define USE_FONT_BIG // grosser 8x8 Font (ca. 2.2 KByte, bei Verwendung von USE_OSD_SCREEN_OLD ca. 2.4 KByte) |
//--------------------------------------------- |
//- Module unfertig bzw. noch in der Entwicklung |
//--------------------------------------------- |
//#define USE_TRACKING // Antennentracking, benoetigt spezielle Hardware (ca. 6 KByte) |
//--------------------------------------------- |
//- zusaetzliche Optionen |
//--------------------------------------------- |
//#define USE_OSD_PKTHOOK |
#define USE_PKTTOOLS_SV2 // im Menue von PKT-Tools erscheinen Punkte zur Verbindung zum MK via |
// 10-Pol-Kabel am PKT-SV2. Der PKT-SV2-Anschluss liefert zur FC/NC +5 Volt! |
//######################################################################### |
//# Debug Module & Einstellungen |
//# Nur fuer die Entwicklung - fuer Veroeffentlichung alles abschalten! |
//######################################################################### |
//#define IgnoreFCVersion // keine FC-Revisions Pruefung |
//#define DEBUG_SV2_EXTENSION // ... |
//#define USE_OSD_DEMO // zeigt Demo-Daten in den OSD-Screens (sofern vom Screen unterstuetzt) (fuer Fotos) (nicht fuer die Oeffentlichkeit) |
//#define USE_OSD_SCREEN_DEBUG // zusaetzliche Debug-Screen's aktivieren (ca. 1 KByte) (nicht fuer die Oeffentlichkeit) |
//#define DEBUG // ?? Funktion unbekannt! |
//#define DEBUG_FC_COMMUNICATION // in usart.c Debugausgaben auf dem PKT bzgl. Datenempfang vom Kopter |
//#define DEBUG_PARAMSET // nur fuer Entwicklung! Fuer Release ABSCHALTEN! |
//#define DEBUG_GPS // Entwicklung der GPS Berechnung für FollowMe (wird im Moment nicth genutzt) |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
//#define ABO_TIMEOUT 300 // 3 sec |
#define ABO_TIMEOUT 200 // 2 sec |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ ggf. abhaengige USE-defines deaktivieren |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef USE_BLUETOOTH |
#undef USE_TRACKING |
#undef USE_FOLLOWME |
#endif |
// Fusebits fü½r Hardware 1.2 D7 DC FC |
// Fusebits für Hardware 1.3 D7 DC FC |
// Fusebits für Hardware 3.x D7 DC FC |
// avrdude -pm1284p -cavr109 -P/dev/ttyUSB1 -b115200 -V -Uflash:w:Dateiname.hex:a |
// hier die entsprechende Hardwareversion der Leiterplatte einstellen |
#define HWVERSION3_9 // Hardware Cebra Oktober 2011 ATmega1284P |
#ifdef HWVERSION3_9 |
#include "HAL_HW3_9.h" |
#endif |
#define NO 0 |
#define NC 1 |
#define FC 2 |
#define MK3MAG 3 |
#define MKGPS 4 |
#define Wi232 5 |
// CB #define ENABLE_PWM |
// Baud Rate |
#define Baud_9600 1 |
#define Baud_19200 2 |
#define Baud_38400 3 |
#define Baud_57600 4 |
#define Baud_115200 5 |
#define Baud_4800 6 |
#define Baud_2400 7 |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
extern volatile uint8_t mode; |
extern uint8_t hardware; |
extern uint8_t current_hardware; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/menu.c |
---|
0,0 → 1,846 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.c |
//# 14.10.2015 Starter |
//# fix: -FollowMeStep2 Debug wird nur noch bei FollowMeStep2 angezeigt |
//# |
//# 03.08.2015 Cebra |
//# - add: Menü _DefMenu_Main_NO erweitert für GPS Test , mit #define schaltbar |
//# |
//# 09.04.2015 Cebra |
//# - add: Vorbereitung für ACC Kalibrartion aus dem Menü |
//# #ifdef USE_ACCCALIBRATION |
//# |
//# 27.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Tracking etwas nach oben verschoben |
//# |
//# 26.06.2014 OG |
//# - chg: Menu_PKTSpezial() - ergaenzt um Setup_FollowMe() |
//# |
//# 18.06.2014 OG |
//# - chg: Menu_PKTSpezial() umorganisiert |
//# |
//# 14.06.2014 OG |
//# - chg: _DefMenu_Main_NO() - "PKT Spezial" wird nicht mehr angezeigt wenn |
//# kein Kopter gefunden wurde; aktuell sind dort nur Funktionen |
//# enthalten die einen Kopter benoetigen - wenn sich das mal irgendwann |
//# aendert muss man das anpassen |
//# |
//# 01.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Aufruf von Tracking und Followme ergaenzt |
//# um Abfrage MKVersion.isNC |
//# - chg: _ConfigMenu_Main() umgestellt auf MKVersion.isNC/isFC |
//# |
//# 14.05.2014 OG |
//# - chg: Aufruf von gps() geaendert zu MK_Gps_Info() |
//# - chg: include "gps/gps.h" geaendert zu "mk/mkgpsinfo.h" |
//# |
//# 13.05.2014 OG |
//# - del: USE_MAINMENU_SEPARATOR (verschoben nach main.h) |
//# - chg: Menu_Main() - del unused variable 'event' |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_PKTSpezial() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 08.04.2014 OG |
//# - chg: die Menuetrennlinien werden jetzt via define USE_MAINMENU_SEPARATOR |
//# angezeigt (das define ist hier im Code) und sind erstmal wieder |
//# abgeschaltet |
//# |
//# 08.04.2014 OG |
//# - add: _DefMenu_Main_NC() - Menuetrennlinien hinzugefuegt |
//# - add: _DefMenu_Main_FC(), _DefMenu_Main_NO() - Menuetrennlinien hinzugefuegt |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# - add: PKT-Connect(): "BLE Extender" (ext. Bluetooth 4 Low Energy Modul an SV2) |
//# - fix: Menu_PKTConnect(): "WLAN Extender" nur sichtar wenn Config.UseWL gesetzt ist |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 24.03.2014 OG |
//# - chg: "MK Info" vor "MK Settings" verschoben |
//# |
//# 28.02.2014 OG |
//# - chg: ID_MKSETTINGS in den Menues fuer NC/FC weiter nach oben geschoben |
//# |
//# 27.02.2014 OG |
//# - chg: Menuetexte fuer Menu_PKTConnect() |
//# |
//# 26.02.2014 OG |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 18.02.2014 OG |
//# - chg: Aufruf von MK_Parameters() ist jetzt MK_Settings() und geht auf |
//# den Source mk/mksettings.c |
//# |
//# 17.02.2014 OG |
//# - chg: aus "MK Parameters" ist "MK Settings" geworden! Entsprechende |
//# defines zu ID's, USE und Sprachen haben sich geaendert |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# - chg: auf mk/mkparameters.c/h umgestellt -> MK_Parameters() |
//# - chg: auf 'MKVersion.paramsetOK' umgestellt |
//# |
//# 05.02.2014 OG |
//# - add: DEBUG_NEW_PARAMETERS in Hauptmenues fuer Entwicklung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - chg: Umstrukturierung vom Hauptmenue |
//# chg: "PKT Tools" -> "PKT Connect" (nur noch Connect-Funktionen drin) |
//# add: "PKT Spezial" -> MK-Motortest und Spezial-Funktionen |
//# wie Joystick, Tracking, FollowMe |
//# - chg: call: display_debug() -> MK_DebugData() |
//# - chg: call: display_data() -> MK_Display() |
//# - chg: #include "debug.h" -> "mk/mkdebugdata.h" |
//# - chg: #include "display.h" -> #include "mk/mkdisplay.h" |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: MK-Display und MK-DebugData auch bei falscher FC-Revision wieder |
//# aktiviert |
//# |
//# 07.01.2014 Cebra |
//# - chg: Abfrage WrongFCVersion etwas verändert |
//# |
//# 06.01.2014 Cebra |
//# - add: Alle Menüpunkte die FC versionabhängig werden bei falscher FC Version ausgeblendet |
//# |
//# 04.10.2013 Cebra |
//# - add: Motortest ohne FC = I2C-Anschluss PKT, mit FC/NC Motortest über FC |
//# |
//# 26.06.2013 Cebra |
//# - add: Menüpunkt Wlan Rangeextender |
//# |
//# 24.06 2013 OG |
//# - chg: Menu_PKTTools(): Menuepunkte fuer PKT-SV2-Verbindungen werden |
//# durch define USE_PKTTOOLS_SV2 (main.h) an-/ausgeschaltet |
//# - chg: Menu_PKTTools(): Menuepunkte werden je nach aktiviertem Wi232 und |
//# Bluetooth ein-/ausgeblendet |
//# - chg: Menuetexte fuer ID_USB2SV2 und ID_BT2SV2 geaendert |
//# - chg: defines fuer Menuetexte von PKT-Tools geaendert |
//# - del: verschiedene Exec_* Funktionen |
//# |
//# 24.06 2013 Cebra |
//# - add: Menuepunkt MKUSB in PKT-Tools |
//# |
//# 13.06.2013 OG |
//# - chg: Fastconnect auf Menu_PKTTools() umgelegt |
//# - chg: Menueeintraege von Menu_PKTTools() modifiziert und reduziert |
//# - chg: in allen Hauptmenues ist PKT-Setup und PKT-Info drin |
//# |
//# 11.06.2013 OG |
//# - chg: Code zu Menu_OSDData() ausgegliedert nach osddata.c |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 21.05.2013 OG |
//# - chg: Menu_Main() umgestellt auf MENUCTRL_EVENT |
//# - fix: Menu_Main() nach Menu-Redraw searchMK() |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# - del: include utils/menuold.h |
//# |
//# 20.05.2013 OG |
//# - chg: Layout Menutitel |
//# |
//# 19.05.2013 OG |
//# - chg: Menuepunkte Tracking und FollowMe werden nur angezeigt wenn BTM222 |
//# installiert ist (Config.UseBT == true) |
//# - add: Menu_PKTTools() (ehemals in tools.c) |
//# - del: PC_Fast_Connect() verschoben nach pkt.c |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.c verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - chg: PKT_Info() erweitert um optionale Module-Info (via USE_MODULEINFO) |
//# und Credits chronoligisch umgedreht |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# - chg: redundante Menue-Strings sind untereinander verpointert um Platz zu |
//# sparen |
//# - chg: Umstellung auf neue menuctrl.c und damit starke Strukturaenderung |
//# Hinweis: autom. PKT-Update via Hauptmenue geht noch nicht - wird |
//# wieder eingebaut (wenn m�glich direkt in menuctrl.c dann geht |
//# das in allen Menues) |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//# |
//# 16.05.2013 OG |
//# - add: define USE_MKGPSINFO fuer gps() |
//# |
//# 05.05.2013 Cebra |
//# - chg: Fehler im Menü bei nicht verfügbaren Funktionen |
//# |
//# 02.05.2013 OG |
//# - chg: Menuetext: osddata_menuitems: "MK Fehlerliste" -> "MK Fehler" |
//# - chg: Menuetext: "OSD Anzeige" wieder zurueck zu "OSD" |
//# |
//# 28.04.2013 OG |
//# - chg: Show_Version() auf ScrollBox umgestellt |
//# - chg: Menuetext "Debug Data" zu "MK Debug Data" (kam frueher zu Verwirrungen bei mir) |
//# - chg: main_menu() bzgl. Menu_OSDDATA() |
//# - add: Menu_OSDDATA() (aus ehemaligem osd/osd_tools.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: OSD-Tools im Menue integriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 14.04.2013 OG |
//# - WIP: Anzeige "OSD-Daten" in param_menuitems_nc, param_menuitems_no |
//# |
//# 04.04.2013 Cebra |
//# - chg:Texttuning |
//# not_possible, für Menüpunkte nicht nicht wählbar sind |
//# |
//# 03.04.2013 OG |
//# - fix: Anzeigefehler wenn (hardware == NC) und nicht gesetzt define 'USE_MKDEBUGDATA' (bzw. 'analognames') |
//# - chg: Layout von Anzeige wenn USE_MKDEBUGDATA nicht verfuegbar (jetzt Invers) |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# |
//# 30.03.2013 CB |
//# - add: automatischer Start der PKT-Updatefunktion im Mainmenüe für das Updatetool |
//# |
//# 28.03.2013 CB |
//# - chg: Hinweis wenn Debug Data nicht möglich ist |
//# |
//# 10.03.2013 Cebra |
//# - add: menu_select, gemeinsame Routine für alle Setupmenüs |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> |
#include "main.h" |
#include "lcd/lcd.h" |
#include "menu.h" |
#include "mk/mkdisplay.h" |
#include "mk/mkdebugdata.h" |
#include "timer/timer.h" |
#include "osd/osd.h" |
#include "osd/osddata.h" |
#include "motortest/motortest.h" |
#include "mk/mkgpsinfo.h" |
#include "eeprom/eeprom.h" |
#include "setup/setup.h" |
#include "uart/uart1.h" |
#include "mk-data-structs.h" |
#include "wi232/Wi232.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "messages.h" |
#include "bluetooth/bluetooth.h" |
#include "followme/followme.h" |
#include "tracking/ng_servo.h" |
#include "tracking/tracking.h" |
#include "stick/stick.h" |
#include "utils/menuctrl.h" |
#include "pkt/pkt.h" |
#include "mk/mkbase.h" |
#include "mksettings/paramset.h" |
#include "mksettings/mksettings.h" |
//############################################################################ |
char titlemain[] = "PKT v "; // buffer fuer PKT-Versionsinfo |
//123456789012345678901 |
// nur fuer Entwicklung... |
static const char DEBUG_PARAMSET_Text[] PROGMEM = "DEBUG PARAMSET"; |
//############################################################################ |
//----------------------- |
// main_menu |
//----------------------- |
#define ID_OSD 1 |
#define ID_OSDDATA 2 |
#define ID_MKDISPLAY 3 |
#define ID_MKSETTINGS 4 |
#define ID_MKDEBUGDATA 5 |
#define ID_MKGPSINFO 6 |
#define ID_SEARCHMK 7 |
#define ID_PKTCONNECT 8 |
#define ID_PKTSETUP 9 |
#define ID_PKTINFO 10 |
#define ID_PKTSPEZIAL 11 |
#define ID_MKINFO 12 |
#define ID_MKACCCAL 13 |
#define ID_DEBUG_GPS 14 |
static const char SEARCHMK_de[] PROGMEM = "Suche Mikrokopter"; |
static const char SEARCHMK_en[] PROGMEM = "search Kopter"; |
static const char OSD_de[] PROGMEM = "OSD"; |
#define OSD_en OSD_de |
static const char OSDDATA_de[] PROGMEM = "OSD Daten"; |
static const char OSDDATA_en[] PROGMEM = "OSD Data"; |
static const char MKINFO_de[] PROGMEM = "MK Info"; |
#define MKINFO_en MKINFO_de |
static const char MKDISPLAY_de[] PROGMEM = "MK Display"; |
#define MKDISPLAY_en MKDISPLAY_de |
static const char MKACCCAL_de[] PROGMEM = "MK ACC Kalibr."; |
static const char MKACCCAL_en[] PROGMEM = "MK ACC Calibr."; |
static const char MKSETTINGS_de[] PROGMEM = "MK Settings"; |
#define MKSETTINGS_en MKSETTINGS_de |
static const char MKDEBUGDATA_de[] PROGMEM = "MK Daten"; |
static const char MKDEBUGDATA_en[] PROGMEM = "MK Data"; |
static const char MKGPSINFO_de[] PROGMEM = "MK GPS Info"; |
#define MKGPSINFO_en MKGPSINFO_de |
static const char PKTCONNECT_de[] PROGMEM = "PKT Connect"; |
#define PKTCONNECT_en PKTCONNECT_de |
static const char PKTSPEZIAL_de[] PROGMEM = "PKT Spezial"; |
static const char PKTSPEZIAL_en[] PROGMEM = "PKT Special"; |
static const char PKTSETUP_de[] PROGMEM = "PKT Setup"; |
#define PKTSETUP_en PKTSETUP_de |
static const char PKTINFO_de[] PROGMEM = "PKT Info"; |
#define PKTINFO_en PKTINFO_de |
#ifdef USE_FOLLOWME_STEP2 |
static const char PKTGPS_de[] PROGMEM = "PKT GPS Debug"; |
#define PKTGPS_en PKTGPS_de |
#endif |
//############################################################################ |
//----------------------------- |
// Menu_PKTSpezial() |
//----------------------------- |
#define ID_BLTESTER 60 |
#define ID_TRACKING 61 |
#define ID_JOYSTICK 62 |
#define ID_FOLLOWME 63 |
#define ID_FOLLOWMESETUP 64 |
static const char BLTESTER_de[] PROGMEM = "MK Motortest"; |
#define BLTESTER_en BLTESTER_de |
static const char TRACKING_de[] PROGMEM = "Tracking"; |
#define TRACKING_en TRACKING_de |
static const char JOYSTICK_de[] PROGMEM = "Joystick"; |
#define JOYSTICK_en JOYSTICK_de |
static const char FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define FOLLOWME_en FOLLOWME_de |
static const char FOLLOWMESETUP_de[] PROGMEM = "Follow Me Setup"; |
#define FOLLOWMESETUP_en FOLLOWMESETUP_de |
//############################################################################ |
//----------------------------- |
// Menu_PKTConnect() |
//----------------------------- |
#define ID_BT2WI232 40 |
#define ID_USB2WI232 41 |
#define ID_USB2SV2 42 |
#define ID_BT2SV2 43 |
#define ID_WIFLY2WI232 44 |
#define ID_BLE2WI232 45 |
static const char BT2WI232_de[] PROGMEM = "BT Extender"; |
#define BT2WI232_en BT2WI232_de |
static const char BLE2WI232_de[] PROGMEM = "BLE Extender"; |
#define BLE2WI232_en BLE2WI232_de |
static const char WIFLY2WI232_de[] PROGMEM = "WLAN Extender"; |
#define WIFLY2WI232_en WIFLY2WI232_de |
static const char USB2WI232_de[] PROGMEM = "USB -> Wi232"; |
#define USB2WI232_en USB2WI232_de |
#ifdef USE_PKTTOOLS_SV2 |
static const char USB2SV2_de[] PROGMEM = "USB -> SV2"; |
#define USB2SV2_en USB2SV2_de |
#endif |
#ifdef USE_PKTTOOLS_SV2 |
static const char BT2SV2_de[] PROGMEM = "BT -> SV2"; |
#define BT2SV2_en BT2SV2_de |
#endif |
//############################################################################################# |
//# Hilfsfunktionen & Verschiedenes |
//############################################################################################# |
//-------------------------------------------------------------- |
// wird von Menu_PKTTools() verwendet |
//-------------------------------------------------------------- |
void Exec_BLTESTER(void) |
{ |
if(hardware == NO) motor_test( I2C_Mode ); |
if(hardware == NC) motor_test( FC_Mode ); |
if(hardware == FC) motor_test( FC_Mode ); |
} |
//############################################################################################# |
//# Menu: Spezial |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer 'PKTSpezial' |
//-------------------------------------------------------------- |
void Menu_PKTSpezial( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Spezial" |
//MenuCtrl_SetTitle_P( PSTR("PKT Spezial") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems: PKTSpezial |
//--------------- |
#ifdef USE_FOLLOWME |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_FOLLOWME , MENU_ITEM, &FollowMe , FOLLOWME_de , FOLLOWME_en ); |
MenuCtrl_PushML2_P( ID_FOLLOWMESETUP, MENU_ITEM, &Setup_FollowMe, FOLLOWMESETUP_de, FOLLOWMESETUP_en ); |
} |
#endif |
#ifdef USE_TRACKING |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_TRACKING , MENU_ITEM, &PKT_tracking , TRACKING_de , TRACKING_en ); |
} |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( ID_JOYSTICK , MENU_ITEM, &joystick , JOYSTICK_de , JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_BLTESTER , MENU_ITEM, &Exec_BLTESTER , BLTESTER_de , BLTESTER_en ); |
// {"Servo Tester ","servo test ","Servo Tester "}, |
// if((val+offset) == 2 ) servo_test(); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: PKT-Connect |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer; PKT-Connect |
// |
// verbindet das PKT mit anderen Devices wie Tablet usw. |
//-------------------------------------------------------------- |
void Menu_PKTConnect( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle_P( PSTR("PKT Connect") ); // kann nicht auf MenuCtrl_SetTitleFromParentItem() umgestellt |
// werden da PKT-Connect auch durch einen Hot-Key aufgerufen |
// werden kann |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_BLUETOOTH |
if( Config.UseBT && Config.UseWi ) |
MenuCtrl_PushML2_P( ID_BT2WI232 , MENU_ITEM, &Port_BT2Wi , BT2WI232_de , BT2WI232_en ); |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch && Config.UseBLE ) |
MenuCtrl_PushML2_P( ID_BLE2WI232 , MENU_ITEM, &Port_BLE2Wi , BLE2WI232_de , BLE2WI232_en ); |
#endif |
if( PCB_SV2RxTxPatch && Config.UseWL ) |
MenuCtrl_PushML2_P( ID_WIFLY2WI232 , MENU_ITEM, &Port_WiFly2Wi , WIFLY2WI232_de , WIFLY2WI232_en ); |
if( Config.UseWi ) |
MenuCtrl_PushML2_P( ID_USB2WI232 , MENU_ITEM, &Port_USB2Wi , USB2WI232_de , USB2WI232_en ); |
#ifdef USE_PKTTOOLS_SV2 // am PKT-SV2 liegen +5 Volt an... |
MenuCtrl_PushML2_P( ID_USB2SV2 , MENU_ITEM, &Port_USB2FC , USB2SV2_de , USB2SV2_en ); |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
MenuCtrl_PushML2_P( ID_BT2SV2 , MENU_ITEM, &Port_BT2FC , BT2SV2_de , BT2SV2_en ); |
#endif |
#endif // end: #ifdef USE_PKTTOOLS_SV2 |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: Menu_Main |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NC( void ) |
{ |
MenuCtrl_PushML2_P( ID_OSD , MENU_ITEM, &osd , OSD_de , OSD_en ); |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MKGPSINFO |
MenuCtrl_PushML2_P( ID_MKGPSINFO , MENU_ITEM, &MK_Gps_Info , MKGPSINFO_de , MKGPSINFO_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_FC( void ) |
{ |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NO( void ) |
{ |
#ifdef USE_FOLLOWME_STEP2 |
MenuCtrl_PushML2_P( ID_DEBUG_GPS , MENU_ITEM, &Debug_GPS , PKTGPS_de , PKTGPS_en ); |
#endif |
MenuCtrl_PushML2_P( ID_SEARCHMK , MENU_ITEM, NOFUNC , SEARCHMK_de , SEARCHMK_en ); |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
//MenuCtrl_PushML2_P( ID_PKTSPEZIAL, MENU_SUB , &Menu_PKTSpezial, PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT, MENU_SUB , &Menu_PKTConnect, PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM, &PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _ConfigMenu_Main( void ) |
{ |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( titlemain ); |
//MenuCtrl_SetShowBatt( true ); |
//----------- |
// Tasten |
//----------- |
MenuCtrl_SetKey( KEY_ESC , strGet(OFF), &PKT_SwitchOff ); |
MenuCtrl_SetKey( KEY_ENTER_LONG , NOTEXT, &Menu_PKTConnect ); |
if ( MKVersion.isNC ) _DefMenu_Main_NC(); |
else if( MKVersion.isFC ) _DefMenu_Main_FC(); |
else _DefMenu_Main_NO(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Main( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT = Config.UseBT; // merkt sich Bluetooth Einstellung falls durch Setup geaendert |
uart1_flush(); |
ADC_Init(); // ADC für Lipomessung // MartinR hierher verschoben |
get_key_press(KEY_ALL); |
strncpy( &titlemain[5], PKTSWVersion, 12); // baue Titel mit PKT-Versionsnummer zusammen |
MenuCtrl_ShowLevel( true ); // zeige Menuelevel in der Titelzeile aller Menues |
_ConfigMenu_Main(); // initialisiert das Menu je nach Hardware (NO,FC,NC) verschieden |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); // wartet nicht auf Event, springt direkt zurueck (wegen evtl. BT-Aenderung) |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------- |
// ID_SEARCHMK |
//-------------- |
if( itemid == ID_SEARCHMK ) // neuer Hardware Modus? |
{ |
if( searchMK(true) ) // true = zeige MK_Info() an |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_MKINFO |
//-------------- |
if( itemid == ID_MKINFO ) |
{ |
if( MK_Info( 0, true) ) // neuer Hardware Modus? (in MK_Info() kann man eine Neusuche des MK starten) (true = mit Settings-Refresh) |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_PKTSETUP |
//-------------- |
if( itemid == ID_PKTSETUP ) |
{ |
Setup_MAIN(); |
} |
//-------------- |
// Bluetooth geaendert? |
//-------------- |
if( UseBT != Config.UseBT ) // falls sich im PKT-Setup die Einstellung bzgl. installiertem Bluetooth Modul |
{ // geaendert hat werden ggf. andere Menuepunkte angezeigt (Tracking, FollowMe) |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
UseBT = Config.UseBT; |
} |
} |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/menu.h |
---|
0,0 → 1,57 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.h |
//# |
//# 19.05.2013 OG |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.h verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//############################################################################ |
#ifndef MENU_H |
#define MENU_H |
void Menu_Main(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/messages.c |
---|
0,0 → 1,829 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.c |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 28.06.2014 OG |
//# - chg: Text von ERROR_NODATA statt "Datenverlust!" jetzt "MK Datenverlust!" |
//# - chg: Text von OSD_ERROR statt "FEHLER: Datenverlust!" jetzt "MK Datenverlust!" |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 11.05.2014 OG |
//# - chg: STR_SAVE_de - jetzt "Speichern" statt "speichern" |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# - chg: OSD_ERROR_de,OSD_ERROR_en Space am Ende entfernt / Ausrufungszeichen hinzugefuegt |
//# |
//# 09.04.2014 OG |
//# - chg: WL1 von "WiFly vorh.:" zu "WiFly Modus:" |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 02.04.2014 OG |
//# - chg: Text WL1_de gekuerzt |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: Text zu WL1 geaendert von "Wlan eingebaut" zu "WiFly vorhanden" |
//# -> das ist ein Aufsteckmodul und wird nicht eingebaut |
//# - chg: strGet() angepasst auf 2 Sprachen (Deutsch, Englisch) |
//# - del: Hollaendisch vollstaendig geloescht |
//# |
//# 29.03.2014 OG |
//# - chg: Test geaendert von START_LASTPOS2 zu "löschen Nein" |
//# - chg: einige Texte bzgl. OSD-Data gekuerzt wegen Umstellung auf lcdx_printp_center() |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - chg: Texte von BOOT1, BOOT2, TESTSTRING |
//# - add: STR_VON, STR_NACH, STR_PKT |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: STARTMSG2 |
//# - add: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - chg: START_LASTPOS |
//# - chg: START_LASTPOS1 |
//# - chg: START_LASTPOS3 |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - del: etliche ":" aus Bezeichnungen geloescht (bei verschiedenen Einstellungen) |
//# -> woher kamen die ueberhaupt? In der Historie sehe ich dazu nichts! |
//# - add: MSG_WARNUNG, UPDATE3 |
//# - chg: UPDATE1, UPDATE2 (PKT Update) |
//# - chg: SHUTDOWN (Space's entfernt) |
//# - chg: START_VERSIONCHECK3: angepasst auf neue flexibilitaet des PKT wenn |
//# FC-Revision nicht unterstuetzt wird |
//# - chg: OK_de von "Ok" nach "OK" |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: Texte geaendert von START_VERSIONCHECK... |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting (fuer Setup: ein-/ausschalten der MK-Settingsanzeige) |
//# - del: DISPLAY8 (ehemals LCD-Orientierung, nicht mehr benoetigt) |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 OG |
//# - chg: kleine Textaenderung an STARTMSG2 |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 CB |
//# - add: Texte für OSD Statistik erweitert |
//# |
//# 25.05.2013 OG |
//# - chg: redundante Strings durch define ersetzt um Speicherplatz zu sparen |
//# |
//# 05.05.2013 Cebra |
//# - add: Erweiterung PKT Zeitsetup |
//# |
//# 25.04.2013 Cebra |
//# - chg: LIPO entfernt, Text doppelt |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "eeprom/eeprom.h" |
#include "messages.h" |
typedef enum |
{ |
GERMAN, |
ENGLISH, |
NETHERLAND, |
LAST_LANGUAGE, |
} LANGUAGE; |
static const char KEYLINE1_de[] PROGMEM = " \x1a \x1b Ende OK"; |
static const char KEYLINE1_en[] PROGMEM = " \x1a \x1b end OK"; |
static const char KEYLINE2_de[] PROGMEM = " \x18 \x19 Ende OK"; |
static const char KEYLINE2_en[] PROGMEM = " \x18 \x19 end OK"; |
static const char KEYLINE3_de[] PROGMEM = " \x18 \x19 Ende "; |
static const char KEYLINE3_en[] PROGMEM = " \x18 \x19 end "; |
static const char KEYLINE4_de[] PROGMEM = "Ende OK"; |
static const char KEYLINE4_en[] PROGMEM = "end OK"; |
static const char KEYLINE5_de[] PROGMEM = " \x18 \x19 Abbr. OK"; |
static const char KEYLINE5_en[] PROGMEM = " \x18 \x19 cancel OK"; |
static const char KEYCANCEL_de[] PROGMEM = " Abbr."; |
static const char KEYCANCEL_en[] PROGMEM = "cancel"; |
static const char BOOT1_de[] PROGMEM = "Taste 1 Sekunde"; |
static const char BOOT1_en[] PROGMEM = "press button"; |
static const char BOOT2_de[] PROGMEM = "festhalten!"; |
static const char BOOT2_en[] PROGMEM = "for 1 second!"; |
static const char START_LASTPOS_de[] PROGMEM = "Letzte Position"; |
static const char START_LASTPOS_en[] PROGMEM = "last position"; |
static const char START_LASTPOS1_de[] PROGMEM = "Breitengr. Längengr."; |
static const char START_LASTPOS1_en[] PROGMEM = "latitude longitude"; |
static const char START_LASTPOS2_de[] PROGMEM = "löschen Nein"; |
static const char START_LASTPOS2_en[] PROGMEM = "delete No"; |
static const char START_LASTPOS3_de[] PROGMEM = "Google Eingabe:"; |
static const char START_LASTPOS3_en[] PROGMEM = "Google Input:"; |
static const char START_SEARCHFC_de[] PROGMEM = "suche Mikrokopter..."; |
static const char START_SEARCHFC_en[] PROGMEM = "searching Kopter..."; |
static const char ENDE_de[] PROGMEM = "Ende"; |
static const char ENDE_en[] PROGMEM = "end"; |
static const char OK_de[] PROGMEM = "OK"; |
static const char OK_en[] PROGMEM = "ok"; |
static const char FEHLER_de[] PROGMEM = "Fehler"; |
static const char FEHLER_en[] PROGMEM = "error"; |
static const char AKTIV_de[] PROGMEM = "aktiv"; |
static const char AKTIV_en[] PROGMEM = "activ"; |
static const char STR_SAVE_de[] PROGMEM = "Speichern"; |
static const char STR_SAVE_en[] PROGMEM = "save"; |
static const char STR_SAVING_de[] PROGMEM = "wird gespeichert..."; |
static const char STR_SAVING_en[] PROGMEM = "saving..."; |
static const char STR_DISCARD_de[] PROGMEM = "Verwerfen"; |
static const char STR_DISCARD_en[] PROGMEM = "discard"; |
static const char STR_COPY_de[] PROGMEM = "Kopieren"; |
static const char STR_COPY_en[] PROGMEM = "copy"; |
static const char STR_SEARCH_de[] PROGMEM = "suche"; |
static const char STR_SEARCH_en[] PROGMEM = "search"; |
static const char STR_FOUND_de[] PROGMEM = "gefunden"; |
static const char STR_FOUND_en[] PROGMEM = "found"; |
static const char STR_SET_de[] PROGMEM = "setze"; |
static const char STR_SET_en[] PROGMEM = "set"; |
static const char STR_WITH_de[] PROGMEM = "mit"; |
static const char STR_WITH_en[] PROGMEM = "with"; |
static const char STR_WITHOUT_de[] PROGMEM = "ohne"; |
static const char STR_WITHOUT_en[] PROGMEM = "without"; |
static const char STR_SWITCHMOTOROFF_de[] PROGMEM = "Motoren ausschalten!"; |
static const char STR_SWITCHMOTOROFF_en[] PROGMEM = "switch motors off!"; |
static const char ON_de[] PROGMEM = "Ein "; |
static const char ON_en[] PROGMEM = "On "; |
static const char OFF_de[] PROGMEM = "Aus "; |
static const char OFF_en[] PROGMEM = "off "; |
static const char ESC_de[] PROGMEM = "ESC"; |
#define ESC_en ESC_de |
static const char SHUTDOWN_de[] PROGMEM = "PKT ausschalten?"; |
static const char SHUTDOWN_en[] PROGMEM = "shutdown PKT?"; |
static const char YESNO_de[] PROGMEM = "Ja Nein"; |
static const char YESNO_en[] PROGMEM = "yes no"; |
static const char NOYES_de[] PROGMEM = "Nein Ja"; |
static const char NOYES_en[] PROGMEM = "no yes"; |
static const char DELETEDATA_de[] PROGMEM = "Daten löschen?"; |
static const char DELETEDATA_en[] PROGMEM = "delete data?"; |
static const char UPDATE1_de[] PROGMEM = "Verbinde PKT mit PC!"; |
static const char UPDATE1_en[] PROGMEM = "Connect PKT to PC"; |
static const char UPDATE2_de[] PROGMEM = "Drücke 'Start' für"; |
static const char UPDATE2_en[] PROGMEM = "Press 'Start' for"; |
static const char UPDATE3_de[] PROGMEM = "min. 2 Sekunden"; |
static const char UPDATE3_en[] PROGMEM = "min. 2 seconds"; |
static const char ENDSTART_de[] PROGMEM = "Ende Start"; |
static const char ENDSTART_en[] PROGMEM = "End Start"; |
static const char CONNECT13_de[] PROGMEM = "Modul eingebaut"; |
static const char CONNECT13_en[] PROGMEM = "Modul built in"; |
static const char KABEL_de[] PROGMEM = "Kabel"; |
static const char KABEL_en[] PROGMEM = "cable"; |
static const char SLAVE_de[] PROGMEM = "Slave"; |
#define SLAVE_en SLAVE_de |
static const char NORMAL_de[] PROGMEM = "Normal"; |
#define NORMAL_en NORMAL_de |
static const char REVERSE_de[] PROGMEM = "Reverse"; |
static const char REVERSE_en[] PROGMEM = "inverse"; |
static const char ENDOK_de[] PROGMEM = "Ende OK"; |
static const char ENDOK_en[] PROGMEM = "End OK"; |
static const char EEPROM1_de[] PROGMEM = "PKT-EEProm"; |
static const char EEPROM1_en[] PROGMEM = "Realy delete"; |
static const char EEPROM2_de[] PROGMEM = "wirklich löschen?"; |
static const char EEPROM2_en[] PROGMEM = "PKT-EEProm?"; |
static const char DEUTSCH_de[] PROGMEM = "deutsch"; |
static const char DEUTSCH_en[] PROGMEM = "german"; |
static const char ENGLISCH_de[] PROGMEM = "englisch"; |
static const char ENGLISCH_en[] PROGMEM = "english"; |
static const char YES_de[] PROGMEM = "Ja "; |
static const char YES_en[] PROGMEM = "yes "; |
static const char NOO_de[] PROGMEM = "Nein"; |
static const char NOO_en[] PROGMEM = "no "; |
static const char OSD_3D_V_de[] PROGMEM = "V"; |
static const char OSD_3D_V_en[] PROGMEM = "F"; |
static const char OSD_3D_H_de[] PROGMEM = "H"; |
static const char OSD_3D_H_en[] PROGMEM = "B"; |
static const char OSD_3D_L_de[] PROGMEM = "L"; |
#define OSD_3D_L_en OSD_3D_L_de |
static const char OSD_3D_R_de[] PROGMEM = "R"; |
#define OSD_3D_R_en OSD_3D_R_de |
static const char OSD_3D_NICK_de[] PROGMEM = "Ni"; |
#define OSD_3D_NICK_en OSD_3D_NICK_de |
static const char OSD_3D_ROLL_de[] PROGMEM = "Ro"; |
#define OSD_3D_ROLL_en OSD_3D_ROLL_de |
static const char OSD_ERROR_de[] PROGMEM = "MK Datenverlust!"; |
static const char OSD_ERROR_en[] PROGMEM = "MK Data lost!"; |
static const char PARA_AKTIVI_de[] PROGMEM = "Aktivieren"; |
static const char PARA_AKTIVI_en[] PROGMEM = "activate"; |
static const char PARA_COPY_de[] PROGMEM = "Kopiere Setting"; |
static const char PARA_COPY_en[] PROGMEM = "copy settings"; |
static const char PARA_COPYQ_de[] PROGMEM = "Wirklich kopieren?"; |
static const char PARA_COPYQ_en[] PROGMEM = "really copy?"; |
static const char GPS2_de[] PROGMEM = "gewähltes GPS Gerät "; |
static const char GPS2_en[] PROGMEM = "selected GPS device "; |
static const char STATS_ITEM_0_de[] PROGMEM = "max Höhe :"; |
static const char STATS_ITEM_0_en[] PROGMEM = "max altitude:"; |
static const char STATS_ITEM_1_de[] PROGMEM = "max Geschw. :"; |
static const char STATS_ITEM_1_en[] PROGMEM = "max speed :"; |
static const char STATS_ITEM_2_de[] PROGMEM = "max Entfern.:"; |
static const char STATS_ITEM_2_en[] PROGMEM = "max distance:"; |
static const char STATS_ITEM_3_de[] PROGMEM = "min Spannung:"; |
static const char STATS_ITEM_3_en[] PROGMEM = "min voltage :"; |
static const char STATS_ITEM_4_de[] PROGMEM = "max Zeit :"; |
static const char STATS_ITEM_4_en[] PROGMEM = "max time :"; |
static const char STATS_ITEM_5_de[] PROGMEM = "max Strom :"; |
static const char STATS_ITEM_5_en[] PROGMEM = "max current :"; |
static const char STATS_ITEM_6_de[] PROGMEM = "Ent.Kapazit.:"; |
static const char STATS_ITEM_6_en[] PROGMEM = "UsedCapacity:"; |
static const char POTI_de[] PROGMEM = "Poti "; |
static const char POTI_en[] PROGMEM = "poti "; |
static const char TASTER_de[] PROGMEM = "Taster"; |
static const char TASTER_en[] PROGMEM = "switch"; |
static const char STAT_OSDBL_de[] PROGMEM = "OSD- und BL-Daten"; |
static const char STAT_OSDBL_en[] PROGMEM = "OSD- and BL-Data"; |
static const char STAT_ERROR_de[] PROGMEM = "MK Fehlerliste"; |
static const char STAT_ERROR_en[] PROGMEM = "MK errorlist"; |
static const char STAT_GPS_de[] PROGMEM = "User GPS-Daten"; |
static const char STAT_GPS_en[] PROGMEM = "User GPS-data"; |
static const char STAT_POS_de[] PROGMEM = "letzte Position"; |
static const char STAT_POS_en[] PROGMEM = "last position"; |
static const char STAT_ALL_de[] PROGMEM = "ALLE Daten"; |
static const char STAT_ALL_en[] PROGMEM = "ALL data"; |
static const char DELETE_de[] PROGMEM = "löschen?"; |
static const char DELETE_en[] PROGMEM = "delete?"; |
static const char STR_OSDSCREEN_GENERAL_de[] PROGMEM = "General"; |
#define STR_OSDSCREEN_GENERAL_en STR_OSDSCREEN_GENERAL_de |
static const char STR_OSDSCREEN_NAVIGATION_de[] PROGMEM = "Navigation"; |
#define STR_OSDSCREEN_NAVIGATION_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_WAYPOINTS_de[] PROGMEM = "Waypoints"; |
#define STR_OSDSCREEN_WAYPOINTS_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_ELECTRIC_de[] PROGMEM = "Electric"; |
#define STR_OSDSCREEN_ELECTRIC_en STR_OSDSCREEN_ELECTRIC_de |
static const char STR_OSDSCREEN_MKSTATUS_de[] PROGMEM = "MK-Status"; |
#define STR_OSDSCREEN_MKSTATUS_en STR_OSDSCREEN_MKSTATUS_de |
static const char STR_OSDSCREEN_USERGPS_de[] PROGMEM = "User GPS"; |
#define STR_OSDSCREEN_USERGPS_en STR_OSDSCREEN_USERGPS_de |
static const char STR_OSDSCREEN_3DLAGE_de[] PROGMEM = "3D Lage"; |
#define STR_OSDSCREEN_3DLAGE_en STR_OSDSCREEN_3DLAGE_de |
static const char STR_OSDSCREEN_STATISTIC_de[] PROGMEM = "Statistics"; |
#define STR_OSDSCREEN_STATISTIC_en STR_OSDSCREEN_STATISTIC_de |
static const char STR_OSDSCREEN_OSD0_de[] PROGMEM = "OSD0"; |
#define STR_OSDSCREEN_OSD0_en STR_OSDSCREEN_OSD0_de |
static const char STR_OSDSCREEN_OSD1_de[] PROGMEM = "OSD1"; |
#define STR_OSDSCREEN_OSD1_en STR_OSDSCREEN_OSD1_de |
static const char STR_OSDSCREEN_OSD2_de[] PROGMEM = "OSD2"; |
#define STR_OSDSCREEN_OSD2_en STR_OSDSCREEN_OSD2_de |
static const char NOMKFOUND_de[] PROGMEM = "keinen MK gefunden!"; |
static const char NOMKFOUND_en[] PROGMEM = "no MK found!"; |
static const char STR_ERROR_de[] PROGMEM = "FEHLER"; |
static const char STR_ERROR_en[] PROGMEM = "ERROR"; |
static const char ERROR_NODATA_de[] PROGMEM = "MK Datenverlust!"; |
static const char ERROR_NODATA_en[] PROGMEM = "MK Data lost!"; |
static const char MSG_LOADSETTINGS_de[] PROGMEM = "lade Settings..."; |
static const char MSG_LOADSETTINGS_en[] PROGMEM = "loading settings..."; |
static const char MSG_ACTIVATESETTING_de[] PROGMEM = "aktiviere Setting..."; |
static const char MSG_ACTIVATESETTING_en[] PROGMEM = "activate setting..."; |
static const char EDIT_SETTING_de[] PROGMEM = "Bearbeiten"; |
static const char EDIT_SETTING_en[] PROGMEM = "edit"; |
static const char STR_VON_de[] PROGMEM = "von"; |
static const char STR_VON_en[] PROGMEM = "from"; |
static const char STR_NACH_de[] PROGMEM = "nach"; |
static const char STR_NACH_en[] PROGMEM = "to"; |
static const char STR_PKT_de[] PROGMEM = "PKT"; |
#define STR_PKT_en STR_PKT_de |
static const char STR_ACTIVE_de[] PROGMEM = "AKTIV!"; |
static const char STR_ACTIVE_en[] PROGMEM = "ACTIVE!"; |
static const char MODULE_EXIST_de[] PROGMEM = "Modul vorhanden"; |
static const char MODULE_EXIST_en[] PROGMEM = "Module exist"; |
static const char STR_INITIALIZE_de[] PROGMEM = "Initialisieren"; |
static const char STR_INITIALIZE_en[] PROGMEM = "Initialize"; |
static const char STR_LONGPRESS_de[] PROGMEM = "langer Tastendruck:"; |
static const char STR_LONGPRESS_en[] PROGMEM = "long press:"; |
static const char STR_FAV_ADD_de[] PROGMEM = "Fav hinzugefügt!"; |
static const char STR_FAV_ADD_en[] PROGMEM = "fav added!"; |
static const char STR_FAV_FULL_de[] PROGMEM = "* Fav ist voll *"; |
static const char STR_FAV_FULL_en[] PROGMEM = "* fav is full *"; |
static const char STR_FAV_EXIST_de[] PROGMEM = "* Fav vorhanden *"; |
static const char STR_FAV_EXIST_en[] PROGMEM = "* fav exists*"; |
static const char STR_FAV_NOTEXIST_de[] PROGMEM = "kein Fav vorhanden!"; |
static const char STR_FAV_NOTEXIST_en[] PROGMEM = "no fav exist!"; |
static const char STR_MENUCTRL_DELITEM_de[] PROGMEM = "Eintrag entfernt!"; |
static const char STR_MENUCTRL_DELITEM_en[] PROGMEM = "item deleted!"; |
static const char STR_MENUCTRL_DELASK_de[] PROGMEM = "Eintrag entfernen?"; |
static const char STR_MENUCTRL_DELASK_en[] PROGMEM = "delete item?"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_de[] PROGMEM = "nicht möglich!"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_en[] PROGMEM = "not possible"; |
static const char STR_HELP_PKTOFFTIME1_de[] PROGMEM = "Minuten (0=Aus)"; |
static const char STR_HELP_PKTOFFTIME1_en[] PROGMEM = "minutes (0=off)"; |
static const char STR_HELP_LOWBAT1_de[] PROGMEM = "<4.20 Einzelzelle "; |
static const char STR_HELP_LOWBAT1_en[] PROGMEM = "<4.20 single cell "; |
static const char STR_HELP_LOWBAT2_de[] PROGMEM = ">4.20 Gesamtspannu."; |
static const char STR_HELP_LOWBAT2_en[] PROGMEM = ">4.20 total voltage"; |
static const char STR_HELP_LIPOOFFSET1_de[] PROGMEM = "verstellen, bis die"; |
static const char STR_HELP_LIPOOFFSET1_en[] PROGMEM = "adjust offset until"; |
static const char STR_HELP_LIPOOFFSET2_de[] PROGMEM = "Spannung passt!"; |
static const char STR_HELP_LIPOOFFSET2_en[] PROGMEM = "voltage fits!"; |
static const char STR_DATA_de[] PROGMEM = "Daten"; |
static const char STR_DATA_en[] PROGMEM = "data"; |
static const char STR_READING_de[] PROGMEM = "lese"; |
static const char STR_READING_en[] PROGMEM = "reading"; |
static const char STR_LABELS_de[] PROGMEM = "Labels"; |
static const char STR_LABELS_en[] PROGMEM = "labels"; |
static const char STR_EXTSV2MODULE_de[] PROGMEM = "externes SV2-Modul"; |
static const char STR_EXTSV2MODULE_en[] PROGMEM = "external SV2 module"; |
static const char STR_ATTENTION_de[] PROGMEM = "* ACHTUNG *"; |
static const char STR_ATTENTION_en[] PROGMEM = "* ATTENTION *"; |
static const char STR_SWITCHOFFMK_de[] PROGMEM = "Kopter ausschalten!"; |
static const char STR_SWITCHOFFMK_en[] PROGMEM = "switch off kopter!"; |
static const char STR_WI232CONFIG_de[] PROGMEM = "Wi.232 Konfig."; |
static const char STR_WI232CONFIG_en[] PROGMEM = "Wi.232 Config"; |
static const char STR_USBCONNECTED_de[] PROGMEM = "mit USB verbunden"; |
static const char STR_USBCONNECTED_en[] PROGMEM = "connected with USB"; |
static const char STR_SEEMKWIKI_de[] PROGMEM = "siehe MK-Wiki:"; |
static const char STR_SEEMKWIKI_en[] PROGMEM = "see MK-Wiki:"; |
static const char STR_PKT_SWITCHOFF_NOW_de[] PROGMEM = "jetzt ausschalten?"; |
static const char STR_PKT_SWITCHOFF_NOW_en[] PROGMEM = "switch off now?"; |
static const char STR_PKT_RESTART_NOW_de[] PROGMEM = "PKT neu starten!"; |
static const char STR_PKT_RESTART_NOW_en[] PROGMEM = "restart PKT!"; |
static const char STR_WI232_ACTIVATE_de[] PROGMEM = "aktiviere Wi.232!"; |
static const char STR_WI232_ACTIVATE_en[] PROGMEM = "activate Wi.232!"; |
static const char STR_WAITSATFIX_de[] PROGMEM = "* warte auf Satfix *"; |
static const char STR_WAITSATFIX_en[] PROGMEM = "* waiting satfix *"; |
static const char STR_GPSMOUSECONNECT_de[] PROGMEM = "verbinde GPS-Maus"; |
static const char STR_GPSMOUSECONNECT_en[] PROGMEM = "search gps-mouse"; |
static const char STR_GPSMOUSEDISCONNECT_de[] PROGMEM = "trenne GPS-Maus"; |
static const char STR_GPSMOUSEDISCONNECT_en[] PROGMEM = "disjoin gps-mouse"; |
static const char STR_NOBTM222_de[] PROGMEM = "kein BTM222 vorh."; |
static const char STR_NOBTM222_en[] PROGMEM = "no BTM222 installed"; |
static const char STR_NOGPSMOUSE_de[] PROGMEM = "keine GPS-Maus!"; |
static const char STR_NOGPSMOUSE_en[] PROGMEM = "no gps-mouse!"; |
static const char STR_GPSMOUSECONNECTION_de[] PROGMEM = "GPS-Maus Verbindung"; |
static const char STR_GPSMOUSECONNECTION_en[] PROGMEM = "gps-mouse connection"; |
static const char STR_SEARCH_BT_ASK_de[] PROGMEM = "BT Geräte suchen?"; |
static const char STR_SEARCH_BT_ASK_en[] PROGMEM = "search BT devices?"; |
static const char STR_SEARCH_BT_de[] PROGMEM = "suche BT Geräte"; |
static const char STR_SEARCH_BT_en[] PROGMEM = "search BT devices"; |
static const char STR_NO_BT_FOUND_de[] PROGMEM = "kein Gerät gefunden"; |
static const char STR_NO_BT_FOUND_en[] PROGMEM = "no device found"; |
static const char STR_BT_DEVICES_de[] PROGMEM = "BT Geräte"; |
static const char STR_BT_DEVICES_en[] PROGMEM = "BT devices"; |
static const char STR_BT_SEARCHTIME_de[] PROGMEM = "(ca. 2 Minuten)"; |
static const char STR_BT_SEARCHTIME_en[] PROGMEM = "(ca. 2 minutes)"; |
static const char STR_BT_LOSTDATA_de[] PROGMEM = "BT Datenverlust"; |
static const char STR_BT_LOSTDATA_en[] PROGMEM = "BT data loss"; |
static const char STR_METERS_de[] PROGMEM = "Meter"; |
static const char STR_METERS_en[] PROGMEM = "meters"; |
static const char STR_WAITNMEA_de[] PROGMEM = "Warte auf GPS Daten"; |
static const char STR_WAITNMEA_en[] PROGMEM = "waiting for GPS Data"; |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
static const char LAST_STR_de[] PROGMEM = "Lastring"; |
#define LAST_STR_en LAST_STR_de |
//****************************************************************** |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * const strings[] PROGMEM = |
{ |
KEYLINE1_de, KEYLINE1_en, |
KEYLINE2_de, KEYLINE2_en, |
KEYLINE3_de, KEYLINE3_en, |
KEYLINE4_de, KEYLINE4_en, |
KEYLINE5_de, KEYLINE5_en, |
KEYCANCEL_de, KEYCANCEL_en, |
BOOT1_de, BOOT1_en, |
BOOT2_de, BOOT2_en, |
START_LASTPOS_de, START_LASTPOS_en, |
START_LASTPOS1_de, START_LASTPOS1_en, |
START_LASTPOS2_de, START_LASTPOS2_en, |
START_LASTPOS3_de, START_LASTPOS3_en, |
START_SEARCHFC_de, START_SEARCHFC_en, |
ENDE_de, ENDE_en, |
OK_de, OK_en, |
FEHLER_de, FEHLER_en, |
AKTIV_de, AKTIV_en, |
STR_SAVE_de, STR_SAVE_en, |
STR_SAVING_de, STR_SAVING_en, |
STR_DISCARD_de, STR_DISCARD_en, |
STR_COPY_de, STR_COPY_en, |
STR_SEARCH_de, STR_SEARCH_en, |
STR_FOUND_de, STR_FOUND_en, |
STR_SET_de, STR_SET_en, |
STR_WITH_de, STR_WITH_en, |
STR_WITHOUT_de, STR_WITHOUT_en, |
STR_SWITCHMOTOROFF_de, STR_SWITCHMOTOROFF_en, |
ON_de, ON_en, |
OFF_de, OFF_en, |
ESC_de, ESC_en, |
SHUTDOWN_de, SHUTDOWN_en, |
YESNO_de, YESNO_en, |
NOYES_de, NOYES_en, |
DELETEDATA_de, DELETEDATA_en, |
UPDATE1_de, UPDATE1_en, |
UPDATE2_de, UPDATE2_en, |
UPDATE3_de, UPDATE3_en, |
ENDSTART_de, ENDSTART_en, |
CONNECT13_de, CONNECT13_en, |
KABEL_de, KABEL_en, |
SLAVE_de, SLAVE_en, |
NORMAL_de, NORMAL_en, |
REVERSE_de, REVERSE_en, |
ENDOK_de, ENDOK_en, |
EEPROM1_de, EEPROM1_en, |
EEPROM2_de, EEPROM2_en, |
DEUTSCH_de, DEUTSCH_en, |
ENGLISCH_de, ENGLISCH_en, |
YES_de, YES_en, |
NOO_de, NOO_en, |
OSD_3D_V_de, OSD_3D_V_en, |
OSD_3D_H_de, OSD_3D_H_en, |
OSD_3D_L_de, OSD_3D_L_en, |
OSD_3D_R_de, OSD_3D_R_en, |
OSD_3D_NICK_de, OSD_3D_NICK_en, |
OSD_3D_ROLL_de, OSD_3D_ROLL_en, |
OSD_ERROR_de, OSD_ERROR_en, |
PARA_AKTIVI_de, PARA_AKTIVI_en, |
PARA_COPY_de, PARA_COPY_en, |
PARA_COPYQ_de, PARA_COPYQ_en, |
GPS2_de, GPS2_en, |
STATS_ITEM_0_de, STATS_ITEM_0_en, |
STATS_ITEM_1_de, STATS_ITEM_1_en, |
STATS_ITEM_2_de, STATS_ITEM_2_en, |
STATS_ITEM_3_de, STATS_ITEM_3_en, |
STATS_ITEM_4_de, STATS_ITEM_4_en, |
STATS_ITEM_5_de, STATS_ITEM_5_en, |
STATS_ITEM_6_de, STATS_ITEM_6_en, |
POTI_de, POTI_en, |
TASTER_de, TASTER_en, |
STAT_OSDBL_de, STAT_OSDBL_en, |
STAT_ERROR_de, STAT_ERROR_en, |
STAT_GPS_de, STAT_GPS_en, |
STAT_POS_de, STAT_POS_en, |
STAT_ALL_de, STAT_ALL_en, |
DELETE_de, DELETE_en, |
STR_OSDSCREEN_GENERAL_de, STR_OSDSCREEN_GENERAL_en, |
STR_OSDSCREEN_NAVIGATION_de, STR_OSDSCREEN_NAVIGATION_en, |
STR_OSDSCREEN_WAYPOINTS_de, STR_OSDSCREEN_WAYPOINTS_en, |
STR_OSDSCREEN_ELECTRIC_de, STR_OSDSCREEN_ELECTRIC_en, |
STR_OSDSCREEN_MKSTATUS_de, STR_OSDSCREEN_MKSTATUS_en, |
STR_OSDSCREEN_USERGPS_de, STR_OSDSCREEN_USERGPS_en, |
STR_OSDSCREEN_3DLAGE_de, STR_OSDSCREEN_3DLAGE_en, |
STR_OSDSCREEN_STATISTIC_de, STR_OSDSCREEN_STATISTIC_en, |
STR_OSDSCREEN_OSD0_de, STR_OSDSCREEN_OSD0_en, |
STR_OSDSCREEN_OSD1_de, STR_OSDSCREEN_OSD1_en, |
STR_OSDSCREEN_OSD2_de, STR_OSDSCREEN_OSD2_en, |
NOMKFOUND_de, NOMKFOUND_en, |
STR_ERROR_de, STR_ERROR_en, |
ERROR_NODATA_de, ERROR_NODATA_en, |
MSG_LOADSETTINGS_de, MSG_LOADSETTINGS_en, |
MSG_ACTIVATESETTING_de, MSG_ACTIVATESETTING_en, |
EDIT_SETTING_de, EDIT_SETTING_en, |
STR_VON_de, STR_VON_en, |
STR_NACH_de, STR_NACH_en, |
STR_PKT_de, STR_PKT_en, |
STR_ACTIVE_de, STR_ACTIVE_en, |
MODULE_EXIST_de, MODULE_EXIST_en, |
STR_INITIALIZE_de, STR_INITIALIZE_en, |
STR_LONGPRESS_de, STR_LONGPRESS_en, |
STR_FAV_ADD_de, STR_FAV_ADD_en, |
STR_FAV_FULL_de, STR_FAV_FULL_en, |
STR_FAV_EXIST_de, STR_FAV_EXIST_en, |
STR_FAV_NOTEXIST_de, STR_FAV_NOTEXIST_en, |
STR_MENUCTRL_DELITEM_de, STR_MENUCTRL_DELITEM_en, |
STR_MENUCTRL_DELASK_de, STR_MENUCTRL_DELASK_en, |
STR_MENUCTRL_NOTPOSSIBLE_de, STR_MENUCTRL_NOTPOSSIBLE_en, |
STR_HELP_PKTOFFTIME1_de, STR_HELP_PKTOFFTIME1_en, |
STR_HELP_LOWBAT1_de, STR_HELP_LOWBAT1_en, |
STR_HELP_LOWBAT2_de, STR_HELP_LOWBAT2_en, |
STR_HELP_LIPOOFFSET1_de, STR_HELP_LIPOOFFSET1_en, |
STR_HELP_LIPOOFFSET2_de, STR_HELP_LIPOOFFSET2_en, |
STR_DATA_de, STR_DATA_en, |
STR_READING_de, STR_READING_en, |
STR_LABELS_de, STR_LABELS_en, |
STR_EXTSV2MODULE_de, STR_EXTSV2MODULE_en, |
STR_ATTENTION_de, STR_ATTENTION_en, |
STR_SWITCHOFFMK_de, STR_SWITCHOFFMK_en, |
STR_WI232CONFIG_de, STR_WI232CONFIG_en, |
STR_USBCONNECTED_de, STR_USBCONNECTED_en, |
STR_SEEMKWIKI_de, STR_SEEMKWIKI_en, |
STR_PKT_SWITCHOFF_NOW_de, STR_PKT_SWITCHOFF_NOW_en, |
STR_PKT_RESTART_NOW_de, STR_PKT_RESTART_NOW_en, |
STR_WI232_ACTIVATE_de, STR_WI232_ACTIVATE_en, |
STR_WAITSATFIX_de, STR_WAITSATFIX_en, |
STR_GPSMOUSECONNECT_de, STR_GPSMOUSECONNECT_en, |
STR_GPSMOUSEDISCONNECT_de, STR_GPSMOUSEDISCONNECT_en, |
STR_NOBTM222_de, STR_NOBTM222_en, |
STR_NOGPSMOUSE_de, STR_NOGPSMOUSE_en, |
STR_GPSMOUSECONNECTION_de, STR_GPSMOUSECONNECTION_en, |
STR_SEARCH_BT_ASK_de, STR_SEARCH_BT_ASK_en, |
STR_SEARCH_BT_de, STR_SEARCH_BT_en, |
STR_NO_BT_FOUND_de, STR_NO_BT_FOUND_de, |
STR_BT_DEVICES_de, STR_BT_DEVICES_en, |
STR_BT_SEARCHTIME_de, STR_BT_SEARCHTIME_en, |
STR_BT_LOSTDATA_de, STR_BT_LOSTDATA_en, |
STR_METERS_de, STR_METERS_en, |
STR_WAITNMEA_de, STR_WAITNMEA_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
LAST_STR_de, LAST_STR_en, |
}; // end: strings |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache den entsprechenden String |
// aus der obigen Stringtabelle zureuck |
//-------------------------------------------------------------- |
const char * strGet( int str_no) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings[(str_no*2)+1] ); // Englisch (=1) |
return (const char*) pgm_read_word( &strings[str_no*2] ); // Deutsch (=0) |
} |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache Parameter str_de oder |
// Parameter str_en zurueck |
// |
// wird z.B. in Verbindung mit de/en-Menuetexten verwendet |
//-------------------------------------------------------------- |
const char * strGetLanguage( char const *str_de, char const *str_en) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return str_en; // Englisch (=1) |
return str_de; // Deutsch (=0) |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/messages.h |
---|
0,0 → 1,205 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.h |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: NUM_LANG von 3 auf 2 reduziert (Hollaendisch vollstaendig geloescht) |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - add: STR_VON, STR_NACH |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: START_MSG2 |
//# - add:: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - add: MSG_WARNUNG, UPDATE3 |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting |
//# |
//# 13.06.2013 CB |
//# - add: ENUM Texte für OSD Statistik erweitert |
//# |
//############################################################################ |
#ifndef MESSAGES_H |
#define MESSAGES_H |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings, LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
KEYLINE1, KEYLINE2, KEYLINE3, KEYLINE4, KEYLINE5, KEYCANCEL, BOOT1, BOOT2, |
START_LASTPOS, START_LASTPOS1, START_LASTPOS2, START_LASTPOS3, |
START_SEARCHFC, ENDE, OK, FEHLER, AKTIV, STR_SAVE, STR_SAVING, STR_DISCARD, STR_COPY, STR_SEARCH, |
STR_FOUND, STR_SET, STR_WITH, STR_WITHOUT, STR_SWITCHMOTOROFF, |
ON, OFF, ESC, SHUTDOWN, YESNO, NOYES,DELETEDATA, UPDATE1, UPDATE2, UPDATE3, ENDSTART, CONNECT13, |
KABEL, SLAVE, NORMAL, REVERSE, ENDOK, EEPROM1, EEPROM2, DEUTSCH, ENGLISCH, |
YES, NOO, OSD_3D_V, OSD_3D_H, OSD_3D_L, |
OSD_3D_R, OSD_3D_NICK, OSD_3D_ROLL, OSD_ERROR, PARA_AKTIVI, PARA_COPY, PARA_COPYQ, |
GPS2, STATS_ITEM_0, STATS_ITEM_1, STATS_ITEM_2, STATS_ITEM_3, STATS_ITEM_4, STATS_ITEM_5, STATS_ITEM_6, |
POTI, TASTER, STAT_OSDBL, STAT_ERROR, STAT_GPS, STAT_POS, STAT_ALL, DELETE, |
STR_OSDSCREEN_GENERAL, STR_OSDSCREEN_NAVIGATION, STR_OSDSCREEN_WAYPOINTS, STR_OSDSCREEN_ELECTRIC, STR_OSDSCREEN_MKSTATUS, STR_OSDSCREEN_USERGPS, STR_OSDSCREEN_3DLAGE, |
STR_OSDSCREEN_STATISTIC, STR_OSDSCREEN_OSD0, STR_OSDSCREEN_OSD1, STR_OSDSCREEN_OSD2, NOMKFOUND, |
STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING, EDIT_SETTING, |
STR_VON, STR_NACH, STR_PKT, STR_ACTIVE, MODULE_EXIST, STR_INITIALIZE, STR_LONGPRESS, |
STR_FAV_ADD, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST, STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE, |
STR_HELP_PKTOFFTIME1, STR_HELP_LOWBAT1, STR_HELP_LOWBAT2, STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2, |
STR_DATA, STR_READING, STR_LABELS, STR_EXTSV2MODULE, STR_ATTENTION, STR_SWITCHOFFMK, STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI, |
STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE, |
STR_WAITSATFIX, STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT, STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION, STR_SEARCH_BT_ASK, |
STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES, STR_BT_SEARCHTIME, STR_BT_LOSTDATA, STR_METERS,STR_WAITNMEA, |
LAST_STR |
} STR; |
#define NUM_LANG 2 // German, English |
const char * strGet( int str_no); |
const char * strGetLanguage( char const *str_de, char const *str_en); |
void Test_Language (void); // bleibt für Tests |
#endif /* _MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkbase.c |
---|
0,0 → 1,825 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.c |
//# |
//# 21.06.2014 OG |
//# - chg: searchMK() schaltet am Ende wieder auf die NC um sofern diese |
//# vorhanden ist |
//# |
//# 19.05.2014 OG |
//# - chg: MKVersion_Setting_print() umgestellt auf strGet(STR_ERROR) fuer |
//# Multi-Sprachenunterstuetzung |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 13.05.2014 OG |
//# - chg: MK_Info() - del: MKINFO_AUTO_REFRESH (nicht mehr unterstuetzt) |
//# - chg: MK_Setting_write() - del: unused variable 'lsetting' |
//# |
//# 17.04.2014 OG |
//# - chg: MK_Setting_load - Ansprechverhalten verbessert wenn Windows |
//# Mikrokoptertool aktiv ist |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 24.03.2014 OG |
//# - chg: searchMK(): doppeltes MK_SwitchToNC() um evtl. vorhandene NC |
//# zuverlaessiger zu erkennen |
//# - fix: searchMK(): RetVal initialisiert |
//# - chg: Default-Anzeigezeit von MK_Info() in searchMK() auf 5 Sek. reduziert |
//# |
//# 21.02.2014 OG |
//# - fix: MKVersion_Setting_print() Anzeige der Settings-Nummer gekuerzt |
//# |
//# 20.02.2014 OG |
//#- chg: searchMK() auf aktuelle Version von PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: MKVersion_Setting_print() um ein paar Bytes zu sparen |
//# - chg: MK_Info() um ein paar Bytes zu sparen |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() (Anleitung bei der Funktion beachten!) |
//# - del: WrongFCVersion |
//# |
//# 12.02.2014 OG |
//# - chg: verschiedene Verbesserungen an MK_Info(), MK_load_settings(), searckMK() |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# |
//# 08.02.2014 OG |
//# - chg: searckMK() Anpassung an MKVersion |
//# - chg: MK_load_setting() Anpassung an MKVersion und Parameter geaendert |
//# - add: MKVersion_Init(), MKVersion_print_at() |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# - chg: kleine Aenderung in der Anzeige "für FC..." in searchMK() |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c: MK_load_setting(), searchMK() |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include <avr/wdt.h> |
#include <avr/eeprom.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "mkbase.h" |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
MKVersion_t MKVersion; |
uint8_t cmd; |
//############################################################################################# |
//# 1. MKVersion |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKVersion_Init() |
// |
// initialisiert MKVersion auf 0 |
//-------------------------------------------------------------- |
void MKVersion_Init( void ) |
{ |
memset( &MKVersion, 0, sizeof(MKVersion_t) ); |
} |
//-------------------------------------------------------------- |
// value = MKVersion_Cmp( FC/NCver, SWMajor,SWMinor,SWPatch ) |
// |
// Vergleicht eine uebergebene FC/NC-Version mit der vom MK |
// ermittelten Version |
// |
// Parameter: |
// |
// FC/NCver: MKVersion.FCVer oder MKVersion.NCVer |
// |
// SWMajor,SWMinor,SWPatch: siehe nachfolgendes... |
// |
//--- |
// Beispiel aus osd.c/OSD_MK_GetData(): |
// |
// v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version "0.30g" |
// |
// if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
// { ... |
// |
// Sowas geht auch: |
// |
// if( v && (v == GIVEN_VERSION) ) // nur wenn aktuelle NC-Version == "0.30g"... |
// |
// oder: |
// |
// if( v && (v < GIVEN_VERSION) ) // nur wenn aktuelle NC-Version kleiner als "0.30g"... |
// |
//--- |
// Warum "if( v && ..." ? |
// |
// Wenn das PKT keine FC/NC-Version ermitteln konnte, und somit kein |
// Vergleich moeglich ist, gibt MKVersion_Cmp() false (=0) zurueck! |
//-------------------------------------------------------------- |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ) |
{ |
SWPatch = SWPatch -'a'; |
if( ver.SWMajor > 0 || ver.SWMinor > 0 ) // NC/FC Version erkannt? |
{ |
if( (ver.SWMajor == SWMajor) && (ver.SWMinor == SWMinor) && (ver.SWPatch == SWPatch) ) |
return VERSION_EQUAL; // ==2 |
if( ver.SWMajor != SWMajor ) |
{ |
if( ver.SWMajor < SWMajor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWMinor != SWMinor ) |
{ |
if( ver.SWMinor < SWMinor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWPatch < SWPatch ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
return VERSION_NO; // ==0 |
} |
//-------------------------------------------------------------- |
// MKVersion_print_at() |
// |
// zeigt die aktuelle FC, NC Version auf dem LCD an |
// |
// Parameter: |
// what: NC, FC, |
//-------------------------------------------------------------- |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ) |
{ |
const char *mask = PSTR("%1u.%02u%c"); |
const char *nostr = PSTR("-----"); |
switch(what) |
{ |
case FC: if( MKVersion.FCVer.SWMajor > 0 || MKVersion.FCVer.SWMinor > 0 ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, mask, MKVersion.FCVer.SWMajor, MKVersion.FCVer.SWMinor, MKVersion.FCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
case NC: if( (MKVersion.NCVer.SWMajor != 0) || (MKVersion.NCVer.SWMinor != 0) ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, PSTR("%1u.%02u%c"), MKVersion.NCVer.SWMajor, MKVersion.NCVer.SWMinor, MKVersion.NCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
} |
} |
//-------------------------------------------------------------- |
// MKVersion_Setting_print( y, drawmode, xoffs,yoffs) |
// |
// Zeigt das aktuelle MK-Setting aus MKVersion zentriert in der |
// Zeile y auf dem PKT-Screen an |
//-------------------------------------------------------------- |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ) |
{ |
char buffer[18]; // String Buffer fuer eine Zeile |
if( MKVersion.mksetting == 0 ) |
xsnprintf_P( buffer, 18, PSTR("* %S *"), strGet(STR_ERROR) ); // FC Setting konnte NICHT gelesen werden |
else if( MKVersion.mksetting != 0 && MKVersion.paramsetOK ) // FC-Setting wurde gelesen... |
xsnprintf_P( buffer, 18, PSTR("%u %s"), MKVersion.mksetting, MKVersion.mksettingName ); // ... passende FC-Reversion -> Nummer und Name anzeigen |
else |
xsnprintf_P( buffer, 18, PSTR("%u"), MKVersion.mksetting ); // ... aber FALSCHE FC-Version -> nicht den Namen anzeigen (nur die Nummer) |
lcdx_print_center( y, (unsigned char *)buffer, drawmode, xoffs,yoffs); // Ausgabe Setting-Name (zentriert) |
} |
//############################################################################################# |
//# 2. Sonstiges - MK-Suche, Settings laden, MK-Info |
//############################################################################################# |
#define SWITCH_DELAY 25 // alt 50 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToNC( void ) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToFC( void ) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToMAG( void ) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToGPS( void ) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
// ret = MK_Setting_load( lsetting, &mk_param_struct, showerror) |
// |
// holt MK-Settings - es wird auf die FC umgeschaltet! |
//-------------------------------------------------------------- |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ) |
{ |
// timeout = 15 bis 20 ist meist ein guter Wert |
//MK_SwitchToFC(); |
mode = 'Q'; // Settings Request |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout ) |
{ |
MK_SwitchToFC(); |
SendOutData( 'q', ADDRESS_FC, 1, &lsetting, 1); // lsetting == 0xff -> aktuelles MK-Setting laden |
timer2 = 25; // timer ist ggf. belegt von MK_Info() -> versuchen wir es mit timer2.... |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
} |
mode = 0; |
// Initialisierungen |
paramsetInit(0); |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
paramsetInit( (unsigned char *) pRxData ); // fuellt u.a. auch MKVersion.paramsetRevision |
} |
return MKVersion.mksetting; // wird von paramsetInit() gesetzt/initialisert |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout) |
{ |
//uint8_t timeout = 50; |
uint8_t setting; |
if( !MKVersion.paramsetOK ) |
{ |
return 0; // Error |
} |
//MK_SwitchToFC(); // ? |
setting = 0; |
mode = 'S'; // Settings |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout && !setting) |
{ |
SendOutData( 's', ADDRESS_FC, 2, &wsetting, 1, (pRxData+1), paramsetSize() ); // pRxData+1 = die mk_datastruct beginnt bei +1; bei 0 ist die Settingsnummer |
timer2 = 70; |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
setting = *pRxData; |
if( !setting ) |
rxd_buffer_locked = FALSE; |
} |
} |
setting = MK_Setting_load( 0xff, 15); // damit pRxData & MKVersion initialisiert sind |
return setting; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_change( uint8_t setting ) |
{ |
uint8_t lsetting = 0; |
uint8_t wsetting = 0; |
lsetting = MK_Setting_load( setting, 20); |
if( lsetting == setting ) |
{ |
wsetting = MK_Setting_write( setting, 50); |
} |
/* |
//-------------------- |
// DEBUG... |
//-------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("load : %1u (req:%1u)"), lsetting, setting ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("write: %1u "), wsetting ); |
if( (setting != lsetting) || (setting != wsetting) ) |
set_beep( 1000, 0x0f0f, BeepNormal ); // Error |
timer2 = 500; while( timer2 > 0 ); |
*/ |
return wsetting; // 0 = Fehler; oder 1..5 = geschriebenes Setting |
} |
//-------------------------------------------------------------- |
// ok = searchMK( showMKInfo ) |
// |
// Parameter: |
// showMKInfo: true/false |
//-------------------------------------------------------------- |
uint8_t searchMK( uint8_t showMKInfo ) |
{ |
uint8_t timeout; |
//uint8_t setting; // aktuelles FC-Setting (1..5) |
uint8_t RetVal = false; |
uint8_t redraw = true; |
uint8_t searchbar_y; |
uint8_t searchbar_w = 0; |
MKVersion_Init(); |
//---------------- |
// Switch to NC |
//---------------- |
MK_SwitchToNC(); |
_delay_ms(50); // 24.03.2014 OG: manchmal wurde nicht auf eine vorhandene NC umgeschaltet |
MK_SwitchToNC(); // evtl. wird das durch doppeltes Senden verbessert |
//--------------------------------------- |
// MK-Suchscreen - Versionsabfrage NC |
//--------------------------------------- |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 50; |
//searchbar_y = 5*8-1; |
searchbar_y = 5*8+2; |
while( !rxd_buffer_locked ) |
{ |
//------------------------ |
// MK Suchscreen anzeigen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (mit Lipo-Anzeige) |
lcdx_printp_center( 2, strGet(STR_PKT), MNORMAL, 0,2); // "PKT" |
lcdx_printp_at( 0, 3, strGet(START_SEARCHFC), MNORMAL, 0,8); // "suche Mikrokopter..." |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
lcd_rect ( 0, searchbar_y , 126, 8, 1); // Rahmen fuer Bar-Anzeige |
searchbar_w = 2; |
redraw = false; |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
//------------------------ |
// MK Datenanfrage |
//------------------------ |
SendOutData( 'v', ADDRESS_ANY, 0); // Versions-Anfrage an MK senden |
timer = 16; // kurze Verögerung fuer Datenanfrage und Fortschrittsbalken |
while( timer > 0 ); |
//------------------------ |
// Fortschrittsbalken |
//------------------------ |
lcd_frect( 2, searchbar_y+2, searchbar_w, 4, 1); // Fortschrittsbalken zeichnen... |
searchbar_w += 2; |
if( searchbar_w >= 125 ) |
{ |
searchbar_w = 0; |
lcd_frect( 2, searchbar_y+2, 123, 4, 0); |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
hardware = NO; |
return false; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
} |
//-------------------------- |
// MK gefunden? |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
//--- |
// TODO OG: testen mit entsprechendem Hardware Setup des MK's! |
//--- |
if( (rxd_buffer[1] - 'a') == ADDRESS_FC ) |
{ |
hardware = FC; |
current_hardware = hardware; |
MKVersion.isFC = true; |
RetVal = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
if( (rxd_buffer[1] - 'a') == ADDRESS_NC ) |
{ |
hardware = NC; |
current_hardware = hardware; |
MKVersion.isNC = true; |
RetVal = true; |
memcpy( &MKVersion.NCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
//-------------------------- |
// jetzt: Version der FC abfragen |
//-------------------------- |
if( hardware == NC ) |
{ |
MK_SwitchToFC(); |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 40; |
while( !rxd_buffer_locked && timeout) |
{ |
SendOutData( 'v', ADDRESS_FC, 0); |
timer = 20; |
while( timer > 0 ); |
timeout--; |
} |
//-------------------------- |
// FC gefunden! |
// -> Version kopieren |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
MKVersion.isFC = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
} |
} // end: if( rxd_buffer_locked ) |
//--------------------------------------- |
// FC EEprom Version / Struktur pruefen |
//--------------------------------------- |
MK_Setting_load( 0xff, 15 ); // 0xff == aktuelles Parameterset holen; 15 == timeout; Ergebnisse in MKVersion |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( MKVersion.isNC ) |
{ |
MK_SwitchToNC(); // wieder auf NC umschalten als default |
} |
if( MKVersion.paramsetOK ) // FC Revision OK? |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep (FC-Revision ok) |
if( showMKInfo ) MK_Info( 500, true ); // Anzeige von MK_Info() max. 5 Sekunden (mit Settings-Refresh) |
} |
else // FC Revision nicht unterstuetzt... |
{ |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep (FC-Revision nicht unterstuetzt) |
if( showMKInfo ) MK_Info( 1600, true ); // Anzeige von MK_Info() max. 16 Sekunden (mit Settings-Refresh) |
} |
//timer = 50; |
//while (timer > 0); |
get_key_press(KEY_ALL); |
return RetVal; |
} // searchMK() |
//-------------------------------------------------------------- |
// chg = MK_Info( displaytimeout, refreshSettings ) |
// |
// Parameter: |
// displaytimeout : 0 = kein autom. Timeout |
// n = Anzeige verschwindet automatisch nach n Zeit |
// (bei z.B. n=600 nach 6 Sekunden) |
// |
// refreshSettings: true/false |
// true = in bestimmten Zeitintervallen (RELOAD_SETTING_TIME) |
// wird MK_Setting_load() durchgeführt um ggf. einen Kopterwechsel |
// automatisch zu erkennen. |
// Dazu muss MKINFO_AUTO_REFRESH in main.h eingeschaltet sein! |
// |
// Rueckgabe: |
// hardwarechange: true/false |
//-------------------------------------------------------------- |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ) |
{ |
uint8_t redraw; |
uint8_t old_isFC; |
uint8_t old_isNC; |
old_isFC = MKVersion.isFC; |
old_isNC = MKVersion.isNC; |
//displaytimeout = 0; |
if( displaytimeout ) |
timer = displaytimeout; |
#define RELOAD_SETTING_TIME 350 // alle 0.35 sec neue Settings vom MK laden |
timer_get_tidata = RELOAD_SETTING_TIME; // ich brauche weitere timer... und bediene mich jetzt mal bei einem osd-timer |
// (timer2 ist belegt von MK_Setting_load) |
//get_key_press(KEY_ALL); |
redraw = true; |
while( true ) |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw != 2 ) |
lcd_cls(); |
show_Lipo(); // LiPo direkt anzeigen damit es nicht flackert |
lcd_frect( 0, 0, 101, 8, 1); // Headline: Box fill Black mit Platz fuer PKT-LiPo rechts |
lcdx_printp_at( 1, 0, PSTR("MK Info"), MINVERS, 0,0); // Titel |
if( MKVersion.isFC || MKVersion.isNC ) // MK gefunden... |
{ |
lcd_frect_round( 0, 30, 127, 22, 1, R2); // Fill: Anzeigebereich der Settings in Schwarz |
//-------------------- |
// FC-Version |
//-------------------- |
lcdx_printp_at( 0, 2, PSTR("FC:") , MNORMAL, 4,-5); |
MKVersion_print_at( 4, 2, FC , MNORMAL, 2,-5); |
//-------------------- |
// FC-Revision |
//-------------------- |
lcdx_printp_at( 12, 2, PSTR("Rev:"), MNORMAL, 5,-5); |
lcdx_printf_at_P( 17, 2, MNORMAL, 2,-5, PSTR("%03u"), MKVersion.paramsetRevision ); |
if( !MKVersion.paramsetOK ) |
lcdx_printp_at( 11, 3, PSTR(" Rev-ERR!"), MINVERS, 5,-4); // Fehler FC-Revision (nicht vom PKT unterstuetzt) |
else |
lcd_frect( 11*6+5, 3*8-4, 9*6, 8, 0); // ggf. Fehleranzeige loeschen (nach refreshSettings) |
//-------------------- |
// NC-Version |
//-------------------- |
lcdx_printp_at( 0, 3, PSTR("NC:") , MNORMAL, 4,-3); |
MKVersion_print_at( 4, 3, NC , MNORMAL, 2,-3); |
//-------------------- |
// aktuelles Setting |
//-------------------- |
lcdx_printp_center( 4, PSTR("MK Setting"), MINVERS, 0,1); // "MK Setting" (zentriert) |
//lcdx_printp_at( 5, 4, PSTR("MK Setting"), MINVERS, 4,1); |
MKVersion_Setting_print( 5, MINVERS, 0,3); |
} |
else // else: if( MKVersion.isFC || MKVersion.isNC ) // KEINEN MK gefunden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(NOMKFOUND), false, 0, true, false ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
} // end: if( MKVersion.isFC || MKVersion.isNC ) |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
if( displaytimeout == 0 ) |
lcd_printp_at( 0, 7, PSTR("Refresh"), MNORMAL); // Keyline: Refresh |
redraw = false; |
} // end: if( redraw ) |
//------------------------------------------ |
// PKT-LiPo Anzeige |
//------------------------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_MINUS) && (displaytimeout == 0) ) |
{ |
searchMK( false ); // false = zeige nicht MK_Info() an |
redraw = true; |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
break; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
//------------------------------------------ |
// displaytimeout? |
//------------------------------------------ |
if( displaytimeout && !timer ) |
{ |
break; |
} |
} // end: while( true ) |
get_key_press(KEY_ALL); |
return( (old_isFC != MKVersion.isFC) || (old_isNC != MKVersion.isNC) ); |
} // MK_Info() |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkbase.h |
---|
0,0 → 1,143 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.h |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() |
//# - add: defines zu VERSION... fuer MKVersion_Cmp() |
//# - del: WrongFCVersion |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# - add: MKVersion_print_at |
//# |
//# 08.02.2014 OG |
//# - chg: MK_load_setting() Parameter geaendert |
//# - add: extern MKVersion_t MKVersion |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c |
//############################################################################ |
#ifndef _MKBASE_H |
#define _MKBASE_H |
//#include "../mksettings/paramset.h" |
#include "../mk-data-structs.h" |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char isFC; // true / false - FC vorhanden? -> wird gesetzt durch searchMK() |
unsigned char isNC; // true / false - NC vorhanden? -> wird gesetzt durch searchMK() |
Version_t FCVer; // -> wird gesetzt durch searchMK() |
Version_t NCVer; // -> wird gesetzt durch searchMK() |
unsigned char paramsetOK; // true wenn Revision in paramset.c vorhanden und initialisiert -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char paramsetRevision; // Revision FC-Parameterset -> wird gesetzt druch paramsetInit()/paramset.c |
uint8_t mksetting; // -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char mksettingName[13]; // -> wird gesetzt druch paramsetInit()/paramset.c |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
extern MKVersion_t MKVersion; |
//------------------------------------------------------ |
// defines fuer den Versionsvergleich von FC/NC |
// siehe: MKVersion_Cmp()... (mkbase.c) |
//------------------------------------------------------ |
#define GIVEN_VERSION 2 // das macht die Sache leserlicher fuer resultierenden Ergebnisse! => siehe Anmerkungen: MKVersion_Cmp() ! |
#define VERSION_NO 0 |
#define VERSION_LOWER 1 |
#define VERSION_EQUAL 2 |
#define VERSION_GREATER 3 |
void MKVersion_Init( void ); |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ); |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ); |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ); |
void MK_SwitchToNC( void ); |
void MK_SwitchToFC( void ); |
void MK_SwitchToMAG( void ); |
void MK_SwitchToGPS( void ); |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ); |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout); |
uint8_t MK_Setting_change( uint8_t setting ); |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ); |
uint8_t searchMK( uint8_t showMKInfo ); |
#endif // end: #ifndef _MKBASE_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkdebugdata.c |
---|
0,0 → 1,416 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.c |
//# |
//# 04.06.2014 OG |
//# komplett neues Layout |
//# - chg: MK_DebugData(), GetAnalogNames() umgestellt auf PKT_Message_Datenverlust() |
//# - chg: GetAnalogNames() Timeout reduziert |
//# - add: #include "../lipo/lipo.h" |
//# - add: #include "../pkt/pkt.h" |
//# - add: #include <stdbool.h> |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.c' jetzt 'mk/mkdebugdata.c' |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: Benutzer kann GetAnalogNames() via KEY_ESC abbrechen |
//# - chg: Screen-Layout GetAnalogNames() |
//# - chg: SwitchToNC/FC Reihenfolge |
//# |
//# 27.03.2013 OG |
//# - chg: auf malloc umgestellt |
//# - fix: diverse Anzeigefehler |
//# - chg: teilweise redunten Code entfernt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <string.h> |
#include "../main.h" |
#ifdef USE_MKDEBUGDATA |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../uart/usart.h" |
#include "mkdebugdata.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
//-------------------------------------------------------------- |
#define PAGELINES 5 // Anzahl der Zeilen pro Anzeigescreens |
#define XOFFS 4 |
#define YOFFS 12 |
#define TIMEOUT 200 // 2 sec |
#define ANALOGTIME 20 // 200 ms |
#define MKDD_ANALOGNAME_SIZE 2*32*17 // MALLOC: 32 names, 16 characters + 1 0x00 |
// WARNING: this work for NC & FC only |
// if current_hardware == MK3MAG or MKGPS the access is outside of the array... |
//uint8_t AnalogNames[2][32][16 + 1]; // 32 names, 16 characters + 1 0x00 |
uint8_t *AnalogNames; // MALLOC: MKDD_ANALOGNAME_SIZE - 32 names, 16 characters + 1 0x00 |
uint8_t AnalogNamesRead[2] = {0,0}; |
static const char strFC[] PROGMEM = "FC"; |
static const char strNC[] PROGMEM = "NC"; |
//-------------------------------------------------------------- |
// Speicher wieder freigeben |
//-------------------------------------------------------------- |
void MKDD_MemFree( void ) |
{ |
free( AnalogNames ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showTitle( void ) |
{ |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0, PSTR(" %2S %16S"), (current_hardware==FC ? strFC : strNC), strGet(STR_DATA) ); // Titelzeile |
show_Lipo(); |
} |
//-------------------------------------------------------------- |
// lOk = GetAnalogNames() |
// |
// Return: |
// true = OK |
// false = User-Abort (KEY_ESC) |
//-------------------------------------------------------------- |
uint8_t GetAnalogNames( void ) |
{ |
uint8_t i = AnalogNamesRead[current_hardware - 1]; |
uint8_t t = 0; |
uint8_t lOk = TRUE; |
int8_t yoffs = -2; // yoffs der Progress-Bar |
lcd_cls (); |
showTitle(); // Titel |
lcdx_printf_center_P( 3, MNORMAL, 0,0, PSTR("%S %S %S"), strGet(STR_READING), (current_hardware==FC ? strFC : strNC), strGet(STR_LABELS) ); |
lcd_rect_round( 13, 40+yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
mode = 'A'; // read Names |
_delay_ms(200); |
rxd_buffer_locked = FALSE; |
timer = ANALOGTIME; |
while( (i < 32) && lOk ) |
{ |
SendOutData ('a', ADDRESS_ANY, 1, &i, 1); |
while( !rxd_buffer_locked && timer && lOk) |
lOk = !get_key_press (1 << KEY_ESC); |
if( timer ) |
{ |
Decode64 (); |
if( i == *pRxData ) |
{ |
lcd_frect( 16, 42+yoffs, (i+1)*3, 2, 1); // Progress |
memcpy( (uint8_t *) (AnalogNames+((current_hardware-1)*32*17)+(i*17)), (uint8_t *) pRxData + 1, 16); |
*(AnalogNames+((current_hardware-1)*32*17)+(i*17)+16) = 0; |
i++; |
t = 0; |
} |
else |
{ |
_delay_ms(100); |
} |
timer = ANALOGTIME; |
rxd_buffer_locked = FALSE; |
} |
else if( lOk ) |
{ // timeout occured |
t++; |
timer = ANALOGTIME; |
if( t >= 15 ) // timeout? |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
break; |
} |
} |
else |
{ |
lOk = !get_key_press( 1 << KEY_ESC ); |
} |
} |
if(lOk) AnalogNamesRead[current_hardware - 1] = i; |
//_delay_ms(4000); // DEBUG |
return lOk; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showLabels( uint8_t page) |
{ |
uint8_t i = 0; |
for( i = 0; i < PAGELINES; i++ ) |
{ |
if( (i + (page * PAGELINES)) >= 32 ) |
{ |
//lcdx_cls_rowwidth( y, width, mode, xoffs, yoffs ) |
lcdx_cls_rowwidth( i, 20, MNORMAL, XOFFS,YOFFS ); |
} |
else |
{ |
lcdx_print_at( 0, i, AnalogNames+((current_hardware-1)*32*17)+(i + page * PAGELINES)*17, MNORMAL, XOFFS,YOFFS); |
} |
} |
} |
//-------------------------------------------------------------- |
// |
//-------------------------------------------------------------- |
void MK_DebugData(void) |
{ |
uint8_t tmp_dat; |
uint8_t i = 0; |
uint8_t page = 0; |
uint8_t pagemax = (32/PAGELINES); |
uint8_t redraw = 2; |
DebugData_t *DebugData; |
// alloc ram |
AnalogNames = malloc( MKDD_ANALOGNAME_SIZE ); |
if( !AnalogNames ) |
{ |
Show_PKTError_NoRAM(); |
return; |
} |
memset( AnalogNames, 0, MKDD_ANALOGNAME_SIZE ); // init: AnalogNames |
AnalogNamesRead[0] = 0; |
AnalogNamesRead[1] = 0; |
SwitchToFC(); |
timer = TIMEOUT; |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
if( !timer ) |
{ |
MKDD_MemFree(); |
return; |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
timer1 = 0; |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//------------------------------------------ |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw==2 ) lcd_cls(); |
showTitle(); // Titelzeile |
lcd_rect_round( 0, (1*8)+1, 127, (6*8)-4, 1, R2); // Rahmen fuer 5 Zeilen Display |
showLabels( page ); |
lcd_printp_at( 0, 7, strGet(KEYLINE3), 0); |
lcd_write_number_u_at (5, 7, page + 1); |
lcd_printp_at( (current_hardware==FC ? 3 : 19), 7, strFC, 0); |
lcd_printp_at( (current_hardware==FC ? 19 : 3), 7, strNC, 0); |
redraw = false; |
} |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
DebugData = (DebugData_t *) pRxData; |
if( !timer1 ) |
{ |
for( i = 0; i < PAGELINES && (i + (page * PAGELINES)) < 32; i++) |
{ |
writex_ndigit_number_s( 21-6, i, DebugData->Analog[i + page * PAGELINES], 5, 0, MNORMAL, XOFFS,YOFFS); |
} |
timer1 = 25; // Anzeigeverzoegerung damit es nicht flackert |
} |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} |
if( !abo_timer ) |
{ // renew abo every ... sec |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//------------------------------------------ |
// PKT LiPo anzeigen |
//------------------------------------------ |
show_Lipo(); |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = 2; |
timer = TIMEOUT; |
} |
//------------------------------------------ |
// Tasten |
//------------------------------------------ |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
page--; |
if( page > pagemax ) page = pagemax; |
redraw = true; |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
page++; |
if( page > pagemax ) page = 0; |
redraw = true; |
} |
if( (hardware == NC) && get_key_press (1 << KEY_ENTER) ) |
{ |
if( current_hardware == NC ) SwitchToFC(); |
else SwitchToNC(); |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
tmp_dat = 10; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//page = 0; |
redraw = true; |
} |
} |
while (!get_key_press(1 << KEY_ESC) && timer); // Taste: Ende oder Timeout |
clear_key_all(); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); // Request Debugdata abschalten |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
//SwitchToNC(); |
// free ram |
MKDD_MemFree(); |
} |
#endif // end: #ifdef USE_MKDEBUGDATA |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkdebugdata.h |
---|
0,0 → 1,51 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.h' jetzt 'mk/mkdebugdata.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _MKDEBUGDATA_H |
#define _MKDEBUGDATA_H |
extern uint8_t AnalogNamesRead[2]; |
void MK_DebugData(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkdisplay.c |
---|
0,0 → 1,267 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.c |
//# |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Display() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - chg: MK_Display() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 28.04.2014 OG |
//# - chg: MK_Display() - leichte Optimierung der Display-Ausgabe um ein paar |
//# Bytes Code zu sparen |
//# |
//# 09.04.2014 OG - vervollstaendigte Steuerung des MK-Display's |
//# - chg: die Tasten fuer 'Ende' und NC/FC-Umschaltung muessen jetzt lange |
//# gedruekckt werden (get_key_long) - keine Beschriftung mehr dafuer |
//# vorhanden da die Beschriftung jetzt hoch/runter zeigt! |
//# (aber 'Ende' ist da wo es immer im PKT ist) |
//# - add: Unterstuetzung der hoch/runter Tasten |
//# - chg: diverse Aenderungen bei der Kommunkation und Einbindung aktueller |
//# Funktionen von mkbase.c |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.c' jetzt 'mk/mkdisplay.c' |
//# |
//# 25.01.2014 OG |
//# - optisches Facelift |
//# - PKT_CtrlHook() eingeklinkt |
//# - Versionshistorie hinzugefuegt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk-data-structs.h" |
#include "mkbase.h" |
#include "mkdisplay.h" |
#define TIMEOUT 500 // 5 sec |
uint8_t ACC_Display=false; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Display(void) |
{ |
uint8_t cmd; |
uint8_t redraw = true; |
uint8_t exit = false; |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
// cmd = 0xfc; // Home = first page |
cmd = 0xff; // aktuelle Seite |
redraw = true; |
MK_SwitchToFC(); // add 31.3.2014 Cebra, definierter Zustand |
if (ACC_Display==true) |
{ |
cmd = 0x06; |
SendOutData( 'l', ADDRESS_ANY, 1, &cmd,1); |
cmd = 0xff; |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
// _delay_ms(250); |
} |
do |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
ShowTitle_P( ( current_hardware == NC ? PSTR("NC") : PSTR("FC") ), true ); |
lcd_printp_at( 4, 0, PSTR("Display"), MINVERS); |
lcd_rect_round( 0, 2*7-3, 127, 5*7+3+3, 1, R2); // Rahmen |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
redraw = false; |
} |
show_Lipo(); |
SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
cmd = 0xff; |
_delay_ms(250); |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
// (Anzeige von unten nach oben) |
//------------------------------------------ |
rxd_buffer[83] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &rxd_buffer[3+60], MNORMAL, 5,2); |
rxd_buffer[63] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &rxd_buffer[3+40], MNORMAL, 5,1); |
rxd_buffer[43] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &rxd_buffer[3+20], MNORMAL, 5,0); |
rxd_buffer[23] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &rxd_buffer[3+ 0], MNORMAL, 5,-1); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) |
{ |
cmd = 0xfe; // key: rechts (next page) |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) |
{ |
cmd = 0xfd; // key: links (previous page) |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
cmd = 0xfb; // key: runter |
} |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
cmd = 0xf7; // key: hoch |
} |
if( (hardware == NC) && get_key_long(1 << KEY_ENTER) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
if( current_hardware == NC ) |
{ |
MK_SwitchToFC(); |
redraw = true; |
} |
else |
{ |
MK_SwitchToNC(); |
MK_SwitchToNC(); |
redraw = true; |
} |
// cmd = 0xff; |
timer2 = 50; |
while( timer2 > 0 ); |
} |
if( get_key_long(1 << KEY_ESC) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
exit = true; |
} |
} |
while( (!exit) && timer ); |
mode = 0; |
rxd_buffer_locked = FALSE; |
//------------------------------------------ |
// Timeout? |
//------------------------------------------ |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkdisplay.h |
---|
0,0 → 1,53 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.h' jetzt 'mk/mkdisplay.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _DISPLAY_H |
#define _DISPLAY_H |
extern uint8_t ACC_Display; |
void MK_Display(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkgpsinfo.c |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.c |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Gps_Info() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 14.05.2014 OG |
//# - chg: Code-Redesign; neues Screen-Layout; Code-Reduzierung; |
//# Anpassungen an neue PKT-Funktionen |
//# - chg: umbenannt von 'gps.c' zu 'mkgpsinfo.c' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "../messages.h" |
//############################################################## |
//############################################################## |
//#define DEBUG_MKGPS // erweiterte Anzeigen zum debuggen |
#define GPSTIMEOUT 300 // 3 Sekunden |
#define UBX_BUFFER_SIZE 100 // 100 Byte Buffer fuer GPS-Daten |
uint8_t ck_a = 0; |
uint8_t ck_b = 0; |
union long_union |
{ |
uint32_t dword; |
uint8_t byte[4]; |
} longUnion; |
union int_union |
{ |
uint16_t dword; |
uint8_t byte[2]; |
} intUnion; |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint32_t join_4_bytes(uint8_t Buffer[]) |
{ |
longUnion.byte[0] = *Buffer; |
longUnion.byte[1] = *(Buffer+1); |
longUnion.byte[2] = *(Buffer+2); |
longUnion.byte[3] = *(Buffer+3); |
return (longUnion.dword); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void checksum(uint8_t data) |
{ |
ck_a += data; |
ck_b += ck_a; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Gps_Info( void ) |
{ |
uint8_t UBX_buffer[UBX_BUFFER_SIZE]; |
uint8_t UBX_payload_counter = 0; |
uint8_t UBX_class = 0; |
uint8_t UBX_id = 0; |
uint8_t UBX_ck_a = 0; |
uint8_t data = 0; |
uint8_t length = 0; |
uint8_t state = 0; |
uint8_t redraw = true; |
uint8_t refresh = false; |
uint8_t yoffs = 1; // Anzeige-Verschiebung: der obere Bereich |
uint8_t yoffs2 = 4; // Anzeige-Verschiebung: GPS-Koordinaten |
int16_t v_16; // tmp-Variable |
int32_t v_32; // tmp-Variable |
#ifdef DEBUG_MKGPS |
uint8_t maxlen = 0; |
#endif |
MK_SwitchToNC(); // Anmerkung OG: warum auch immer es besser ist erst auf die NC |
MK_SwitchToGPS(); // umzuschalten... es laeuft so scheinbar zuverlaessiger |
MK_SwitchToGPS(); |
timer_mk_timeout = GPSTIMEOUT; |
do |
{ |
//------------------------ |
// REDRAW |
//------------------------ |
if( redraw ) |
{ |
ShowTitle_P( PSTR("MK GPS Info"), true); |
lcdx_printp_at( 0,1, PSTR("Fix:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,2, PSTR("Sat:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,3, PSTR("Alt:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 9,1, PSTR("PDOP:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,2, PSTR("Accu:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,3, PSTR("Sped:"), MNORMAL, 3,yoffs ); |
lcd_frect( 0, (4*7)+4+yoffs2, 127, 7, 1); // GPS: Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8+yoffs2); // GPS: "Breitengr Längengr" |
lcd_rect( 0, (3*8)+7+yoffs2, 127, (2*8)+4, 1); // GPS: Rahmen |
lcdx_printp_at(12,7, strGet(ENDE), MNORMAL, 0,1); // KEYLINE |
redraw = false; |
} |
//------------------------ |
// REFRESH |
//------------------------ |
if( refresh ) |
{ |
if((UBX_class == 1) && (UBX_id == 6)) // GPS: SVINFO |
{ |
//-------------- |
// GPS Status |
//-------------- |
switch( UBX_buffer[10] ) |
{ |
case 4: |
case 3: lcdx_printp_at( 5, 1, PSTR("3D"), MNORMAL, 1,yoffs); break; |
case 2: lcdx_printp_at( 5, 1, PSTR("2D"), MNORMAL, 1,yoffs); break; |
default: lcdx_printp_at( 5, 1, PSTR("no"), MNORMAL, 1,yoffs); |
} |
// GPS ok => ein Haken wird angezeigt |
if( (UBX_buffer[11] & 1) == 1 ) lcdx_putc( 7, 1, SYMBOL_CHECK, MNORMAL, 3,yoffs ); |
else lcdx_putc( 7, 1, ' ', MNORMAL, 3,yoffs ); |
//--- |
// Anzeige von "D" - evtl. DGPS (Differential GPS)? |
// aktuell nicht mehr unterstuetzt da kein Platz auf dem Screen |
//--- |
//if( (UBX_buffer[11] & 3) == 3 ) lcd_printp_at (10,0, PSTR("D"), 0); |
//else lcd_printp_at (10,0, PSTR(" "), 0); |
//-------------- |
// Sat |
//-------------- |
lcdx_printf_at_P( 5, 2, MNORMAL, 0,yoffs, PSTR("%2u"), UBX_buffer[47] ); |
//-------------- |
// PDOP |
//-------------- |
v_16 = UBX_buffer[44]+UBX_buffer[45]*255; |
lcdx_printf_at_P( 15, 1, MNORMAL, 0,yoffs, PSTR("%2.2d"), v_16 ); |
//-------------- |
// Accuracy |
//-------------- |
v_32 = (int32_t)join_4_bytes(&UBX_buffer[24]); |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,yoffs, PSTR("%2.2ldm"), v_32 ); |
} |
if((UBX_class == 1) && (UBX_id == 18)) // GPS: VELNED |
{ |
//-------------- |
// Speed |
//-------------- |
v_16 = (int16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
lcdx_printf_at_P( 15, 3, MNORMAL, 0,yoffs, PSTR("%3dkmH"), v_16 ); |
//uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
//write_ndigit_number_u (11, 4, speed, 3, 0,0); |
//lcd_printp_at (15,4, PSTR("km/h"), 0); |
} |
if((UBX_class == 1) && (UBX_id == 2)) // GPS: POSLLH |
{ |
//-------------- |
// Altitude |
//-------------- |
v_16 = (int16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//v_16 = v_16 + 3000; |
lcdx_printf_at_P( 4, 3, MNORMAL, 1,yoffs, PSTR("%4d"), v_16 ); |
//uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//write_ndigit_number_u (11, 7, height, 4, 0,0); |
//lcd_printp_at(16,7, PSTR("m"), 0); |
//-------------- |
// Breitengrad - Lat (51.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[8]); |
writex_gpspos( 1, 4, v_32 , MNORMAL, 0,10+yoffs2); // Anzeige: Breitengrad |
//write_ndigit_number_u ( 0, 7, (uint16_t)(lat/10000000), 2, 0,0); |
//lcd_printp_at ( 2, 7, PSTR("."), 0); |
//write_ndigit_number_u ( 3, 7, (uint16_t)((lat/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u ( 7, 7, (uint16_t)((lat/10) % 100), 2, 1,0); |
//-------------- |
// Laengengrad - Long (7.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[4]); |
writex_gpspos( 12, 4, v_32, MNORMAL, -1,10+yoffs2); // Anzeige: Laengengrad |
//write_ndigit_number_u (10, 7, (uint16_t)(lon/10000000), 2, 0,0); |
//lcd_printp_at (12, 7, PSTR("."), 0); |
//write_ndigit_number_u (13, 7, (uint16_t)((lon/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u (17, 7, (uint16_t)((lon/10) % 100), 2, 1,0); |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
refresh = false; |
} // end: if( refresh ) |
//-------------------------- |
// PROCESS DATA |
//-------------------------- |
if( uart_getc_nb( &data ) ) |
{ |
switch( state ) |
{ |
case 0: if( data == 0xB5 ) // GPS: init 1 |
{ |
UBX_payload_counter = 0; |
UBX_id = 0; |
UBX_class = 0; |
ck_a = 0; |
ck_b = 0; |
state++; |
} |
break; |
case 1: if( data == 0x62 ) // GPS: init 2 |
state++; |
else |
state = 0; |
break; |
case 2: if( data != 1 ) // GPS: class |
state = 0; |
else |
{ |
checksum(data); |
UBX_class = data; |
state++; |
} |
break; |
case 3: if( (data != 48) // GPS: id |
&&(data != 6 ) |
&&(data != 18) |
&&(data != 2 )) |
{ |
state = 0; |
} |
else |
{ |
UBX_id = data; |
checksum(data); |
state++; |
} |
break; |
case 4: if( data >= UBX_BUFFER_SIZE ) // GPS: length lo |
state = 0; |
else |
{ |
checksum(data); |
length = data; |
state++; |
} |
#ifdef DEBUG_MKGPS |
// DEBUG - Anzeige der Paketgroesse |
if( data > maxlen ) |
{ |
// groesstes gemessenes Paket lag bei 181 |
// ob man das Paket braucht ist eine andere Frage |
maxlen = data; |
lcdx_printf_at_P( 1, 7, MNORMAL, 0,1, PSTR("%3u"), maxlen ); |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
} |
#endif |
break; |
case 5: if( data != 0 ) // GPS: length hi |
state = 0; |
else |
{ |
checksum(data); |
state++; |
} |
break; |
case 6: UBX_buffer[UBX_payload_counter] = data; // GPS: data |
checksum(data); |
UBX_payload_counter++; |
length--; |
if( length == 0 ) |
{ |
state++; |
}; |
break; |
case 7: state++; // GPS: check lo |
UBX_ck_a = data; |
break; |
case 8: state = 0; // GPS: check hi |
if((UBX_ck_a == ck_a) && (data == ck_b)) |
{ |
timer_mk_timeout = GPSTIMEOUT; |
refresh = true; |
} |
} // end: switch( state ) |
} // end: if( uart_getc_nb(&data) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
/* |
// funktioniert hier nicht - warum auch immer |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
*/ |
} while( !get_key_press(1 << KEY_ESC) && timer_mk_timeout ); |
//-------------------------- |
// in den Timeout gelaufen? |
//-------------------------- |
if( !timer_mk_timeout ) |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
SwitchToNC(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk/mkgpsinfo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.h |
//# |
//# 14.05.2014 OG |
//# - chg: umbenannt von 'gps.h' zu 'mkgpsinfo.h' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef _GPS_H |
#define _GPS_H |
void MK_Gps_Info( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mk-data-structs.h |
---|
0,0 → 1,501 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mk_data-stucts.h |
//# |
//# 16.07.2015 Cebra |
//# - chg: Receiver Selection |
//# RECEIVER_USER 8 -> RECEIVER_MLINK 8 |
//# - add: Receiver Selection |
//# RECEIVER_USER 9 |
//# |
//# 26.01.2015 Cebra |
//# - chg: #defines für ServoCompInvert an aktuelle Syntax angepasst |
//# SVNick -> SERVO_NICK_INV |
//# SVRoll -> SERVO_ROLL_INV |
//# SVRelMov -> SERVO_RELATIVE |
//# - add: #define CH_DIRECTION_1 0x08 für ComingHome Ausrichtung |
//# #define CH_DIRECTION_2 0x10 |
//# |
//# 19.06.2014 OG |
//# - chg: Aktualisiert: struct Point_t (aus Quelle: NaviCtrl V2.06f/waypoints.h - Revision 546) |
//# |
//# 12.02.2014 OG |
//# - del: die alten defines bzgl. MKVERSIONnnn und mk_param_struct_t entfernt |
//# |
//# 02.12.2013 Cebra |
//# - add: #define MKVERSION201f, Erweiterung für FC201f |
//# |
//# 21.10.2013 Cebra |
//# - add: #define MKVERSION201a, Erweiterung für FC201a |
//# |
//# 26.06.2013 OG |
//# - chg: "0.91f" -> "0.91f/l" |
//# |
//# 13.06.2013 OG |
//# - chg: "0.90j" -> "0.90h/j" |
//# - chg: Code-Layout |
//# |
//# 05.06.2013 Cebra |
//# - chg: Anpassungen an FC091c |
//# |
//# 03.06.2013 Cebra |
//# - chg: #define Fehler beseitigt |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a, Erweiterung für FC091a |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 11.05.2013 OG |
//# - add: DateTime_t |
//# |
//# 20.03.2013 OG |
//# - add: BLData_t (http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct) |
//############################################################################ |
#ifndef _MK_DATA_STRUCTS_H |
#define _MK_DATA_STRUCTS_H |
#include "main.h" |
//################################################################################## |
//################################################################################## |
#define u8 uint8_t |
#define s8 int8_t |
#define u16 uint16_t |
#define s16 int16_t |
#define u32 uint32_t |
#define s32 int32_t |
#define NUMBER_OF_DEBUG_DATAS 32 |
#define ANALOG_NAME_LENGTH 16 |
// Version of supported serial protocol |
#define MIN_VERSION 7 |
#define MAX_VERSION 10 |
// Setting index |
#define SETTING_1 1 |
#define SETTING_2 2 |
#define SETTING_3 3 |
#define SETTING_4 4 |
#define SETTING_5 5 |
#define SETTING_CURRENT 0xff |
// MikroKopter defines |
// taken from |
// FC Software eeprom.h |
// |
//GlobalConfig3 aus FC/eeprom.h |
#define CFG3_NO_SDCARD_NO_START 0x01 |
#define CFG3_DPH_MAX_RADIUS 0x02 |
#define CFG3_VARIO_FAILSAFE 0x04 |
#define CFG3_MOTOR_SWITCH_MODE 0x08 //FC0.88L 7.5.12 |
#define CFG3_NO_GPSFIX_NO_START 0x10 //FC0.88L 7.5.12 |
#define CFG3_USE_NC_FOR_OUT1 0x20 |
#define CFG3_SPEAK_ALL 0x40 |
#define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//GlobalConfig |
#define CFG_HOEHENREGELUNG 0x01 |
#define CFG_HOEHEN_SCHALTER 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_KOMPASS_AKTIV 0x08 |
#define CFG_KOMPASS_FIX 0x10 |
#define CFG_GPS_AKTIV 0x20 |
#define CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define CFG_DREHRATEN_BEGRENZER 0x80 |
//Bitconfig MAsk |
#define CFG_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
#define CFG_MOTOR_BLINK1 0x10 |
#define CFG_MOTOR_OFF_LED1 0x20 |
#define CFG_MOTOR_OFF_LED2 0x40 |
#define CFG_MOTOR_BLINK2 0x80 |
// ServoCompInvert, FC0.89 entfällt und geändert s.u. |
//#define SVNick 0x01 |
//#define SVRoll 0x02 |
//#define SVRelMov 0x04 |
//ab FC 209a (26.1.2015 cebra) |
// bitcoding for EE_Parameter.ServoCompInvert |
#define SERVO_NICK_INV 0x01 |
#define SERVO_ROLL_INV 0x02 |
#define SERVO_RELATIVE 0x04 // direct poti control or relative moving of the servo value |
#define CH_DIRECTION_1 0x08 |
#define CH_DIRECTION_2 0x10 |
//CH Orientation ServoBits 0x10 0x08 |
// --> no change 0 0 |
// --> front to starting point 0 1 |
// --> rear to to starting point 1 0 |
//-> start orientation 1 1 |
// ExtraConfig |
#define CFG2_HEIGHT_LIMIT 0x01 |
#define CFG2_VARIO_BEEP 0x02 |
#define CFG_SENSITIVE_RC 0x04 // 26.1.2015 entfällt ab FC 209a, cebra |
#define CFG_3_3V_REFERENCE 0x08 |
#define CFG_NO_RCOFF_BEEPING 0x10 |
#define CFG_GPS_AID 0x20 |
#define CFG_LEARNABLE_CAREFREE 0x40 |
#define CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
// bit mask for ParamSet.Config0 |
#define CFG0_AIRPRESS_SENSOR 0x01 |
#define CFG0_HEIGHT_SWITCH 0x02 |
#define CFG0_HEADING_HOLD 0x04 |
#define CFG0_COMPASS_ACTIVE 0x08 |
#define CFG0_COMPASS_FIX 0x10 |
#define CFG0_GPS_ACTIVE 0x20 |
#define CFG0_AXIS_COUPLING_ACTIVE 0x40 |
#define CFG0_ROTARY_RATE_LIMITER 0x80 |
// defines for the receiver selection |
#define RECEIVER_PPM 0 |
#define RECEIVER_SPEKTRUM 1 |
#define RECEIVER_SPEKTRUM_HI_RES 2 |
#define RECEIVER_SPEKTRUM_LOW_RES 3 |
#define RECEIVER_JETI 4 |
#define RECEIVER_ACT_DSL 5 |
#define RECEIVER_HOTT 6 |
#define RECEIVER_SBUS 7 |
#define RECEIVER_MLINK 8 //FC2.11 16.7.2015 CB |
#define RECEIVER_USER 9 //FC2.11 16.7.2015 CB |
#define RECEIVER_UNKNOWN 0xFF |
// MikroKopter Flags |
// taken from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.73d%2Ffc.h |
//alt 0.86 |
//#define FCFLAG_MOTOR_RUN 0x01 |
//#define FCFLAG_FLY 0x02 |
//#define FCFLAG_CALIBRATE 0x04 |
//#define FCFLAG_START 0x08 |
//#define FCFLAG_NOTLANDUNG 0x10 |
//#define FCFLAG_LOWBAT 0x20 |
//#define FCFLAG_SPI_RX_ERR 0x40 |
//#define FCFLAG_I2CERR 0x80 |
// FC_StatusFlags 0.88 |
#define FC_STATUS_MOTOR_RUN 0x01 |
#define FC_STATUS_FLY 0x02 |
#define FC_STATUS_CALIBRATE 0x04 |
#define FC_STATUS_START 0x08 |
#define FC_STATUS_EMERGENCY_LANDING 0x10 |
#define FC_STATUS_LOWBAT 0x20 |
#define FC_STATUS_VARIO_TRIM_UP 0x40 |
#define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
// FC_StatusFlags2 |
#define FC_STATUS2_CAREFREE 0x01 |
#define FC_STATUS2_ALTITUDE_CONTROL 0x02 |
#define FC_STATUS2_RC_FAILSAVE_ACTIVE 0x04 |
#define FC_STATUS2_OUT1_ACTIVE 0x08 |
#define FC_STATUS2_OUT2_ACTIVE 0x10 |
// NaviCtrl Flags |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
#define NC_FLAG_FREE 0x01 |
#define NC_FLAG_PH 0x02 |
#define NC_FLAG_CH 0x04 |
#define NC_FLAG_RANGE_LIMIT 0x08 |
#define NC_FLAG_NOSERIALLINK 0x10 |
#define NC_FLAG_TARGET_REACHED 0x20 |
#define NC_FLAG_MANUAL_CONTROL 0x40 |
#define NC_FLAG_GPS_OK 0x80 |
typedef struct |
{ |
unsigned char SWMajor; |
unsigned char SWMinor; |
unsigned char ProtoMajor; |
unsigned char ProtoMinor; |
unsigned char SWPatch; |
unsigned char HardwareError[5]; |
} __attribute__((packed)) Version_t; |
// FC Debug Struct |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.h |
typedef struct // 0.86 |
{ |
uint8_t Digital[2]; |
// NC: unsigned; FC: signed !!!! |
int16_t Analog[32]; // Debugvalues |
} __attribute__((packed)) DebugData_t; |
//****************************************************************** |
// uart1.h NC 0.87, zur Zeit hier nicht verwendet 28.01.2012 CB |
#define AMPEL_FC 0x01 |
#define AMPEL_BL 0x02 |
#define AMPEL_NC 0x04 |
#define AMPEL_COMPASS 0x08 |
typedef struct //0.87 |
{ |
u8 StatusGreen; |
u8 StatusRed; |
u16 Analog[32]; // Debugwerte |
} __attribute__((packed)) DebugOut_t; |
//****************************************************************** |
// NaviCtrl OSD Structs |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
typedef struct //NC uart1.h |
{ |
s16 AngleNick; // in 0.1 deg |
s16 AngleRoll; // in 0.1 deg |
s16 Heading; // in 0.1 deg |
u8 StickNick; |
u8 StickRoll; |
u8 StickYaw; |
u8 StickGas; |
u8 reserve[4]; |
} __attribute__((packed)) Data3D_t; |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
typedef struct |
{ |
u16 Distance; // distance to target in cm |
s16 Bearing; // course to target in deg |
} __attribute__((packed)) GPS_PosDev_t; |
//---------------------------- |
// aus NC waypoint.h |
// |
// AKTUALISIERT: 18.06.2014 OG |
// |
// Anmerkung 18.06.2014 OG |
// "Name" hinzugefuegt - das gibt es |
// seit dem 23.03.2012! |
//---------------------------- |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // Will be transferred to the FC and can be used as Poti value there |
u8 AltitudeRate; // rate to change the setpoint in steps of 0.1m/s |
u8 Speed; // rate to change the Position(0 = max) in steps of 0.1m/s |
u8 CamAngle; // Camera servo angle in degree (255 -> POI-Automatic) |
u8 Name[4]; // Name of that point (ASCII) |
u8 reserve[2]; // reserve |
} __attribute__((packed)) Point_t; |
//---------------------------- |
// aus NC waypoint.h |
// VERALTET! 18.06.2014 OG |
//---------------------------- |
/* |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // |
u8 AltitudeRate; // rate to change the setpoint |
u8 Speed; // rate to change the Position |
u8 CamAngle; // Camera servo angle |
u8 reserve[6]; // reserve |
} __attribute__((packed)) Point_t; |
*/ |
// NaviCtrl struct |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
#define NAVIDATA_VERSION 5 |
typedef struct |
{ |
u8 Version; // version of the data structure |
GPS_Pos_t CurrentPosition; // see ubx.h for details |
GPS_Pos_t TargetPosition; |
GPS_PosDev_t TargetPositionDeviation; |
GPS_Pos_t HomePosition; |
GPS_PosDev_t HomePositionDeviation; |
u8 WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
u8 WaypointNumber; // number of stored waypoints |
u8 SatsInUse; // number of satellites used for position solution |
s16 Altimeter; // hight according to air pressure |
s16 Variometer; // climb(+) and sink(-) rate |
u16 FlyingTime; // in seconds |
u8 UBat; // Battery Voltage in 0.1 Volts |
u16 GroundSpeed; // speed over ground in cm/s (2D) |
s16 Heading; // current flight direction in ° as angle to north |
s16 CompassHeading; // current compass value in |
s8 AngleNick; // current Nick angle in 1 |
s8 AngleRoll; // current Roll angle in 1 |
u8 RC_Quality; // RC_Quality |
u8 FCStatusFlags; // Flags from FC |
u8 NCFlags; // Flags from NC |
u8 Errorcode; // 0 --> okay |
u8 OperatingRadius; // current operation radius around the Home Position in m |
s16 TopSpeed; // velocity in vertical direction in cm/s |
u8 TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
u8 FCStatusFlags2; // StatusFlags2 (since version 5 added) |
s16 SetpointAltitude; // setpoint for altitude |
u8 Gas; // for future use |
u16 Current; // actual current in 0.1A steps |
u16 UsedCapacity; // used capacity in mAh |
} __attribute__((packed)) NaviData_t; |
typedef struct |
{ |
uint8_t Version; // the version of the BL (0 = old) |
uint8_t SetPoint; // written by attitude controller |
uint8_t SetPointLowerBits; // for higher Resolution of new BLs |
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present |
uint8_t ReadMode; // select data to read |
// the following bytes must be exactly in that order! |
uint8_t Current; // in 0.1 A steps, read back from BL |
uint8_t MaxPWM; // read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
int8_t Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in �C |
} __attribute__((packed)) MotorData_t; |
typedef struct |
{ |
uint8_t Revision; // must be BL_REVISION |
uint8_t SetMask; // settings mask |
uint8_t PwmScaling; // maximum value of control pwm, acts like a thrust limit |
uint8_t CurrentLimit; // current limit in A |
uint8_t TempLimit; // in �C |
uint8_t CurrentScaling; // scaling factor for current measurement |
uint8_t BitConfig; // see defines above |
uint8_t crc; // checksum |
} __attribute__((packed)) BLConfig_t; |
//############################################################################## |
//# Datenstruktur von ehemals mk_param_struct_t jetzt via mk/paramset.c ! |
//############################################################################## |
//------------------------------------- |
// 20.03.2013 OG: BLData_t |
// aus: http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct |
//------------------------------------- |
typedef struct |
{ |
unsigned char Index; // address of BL-Ctrl |
unsigned char Current; // in 0.1 A steps, read back from BL |
unsigned char Temperature; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
unsigned char MaxPWM; // EVENTUELL: read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
unsigned char Status; // EVENTUELL: 7 bit for I2C error counter, highest bit indicates if motor is present |
} BLData_t; |
//------------------------------------- |
// 11.05.2013 OG: DateTime_t |
// aus: NC v0.30g timer1.h |
//------------------------------------- |
typedef struct{ |
u16 Year; |
u8 Month; |
u8 Day; |
u8 Hour; |
u8 Min; |
u8 Sec; |
u16 mSec; |
u8 Valid; |
} DateTime_t; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/mkparameters.c |
---|
0,0 → 1,2692 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.c |
//# |
//# 16.07.2015 Cebra |
//# - add: Erweiterung SingleWpControlChannel; (FC2.11a) |
//# MenuKeyChannel; (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: Erweiterung paramEditItemTable und ID_MENU_KAMERA_Items[] um neue Parameter (FC2.09j) |
//# param_ServoNickFailsave, param_ServoRollFailsave, param_Servo3Failsave, param_Servo4Failsave, param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: Comming Home Ausrichtung hinzugefügt, |
//# neue Einstellungen ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: im Menü Höhe -> param_Hoehe_TiltCompensation, FC207d |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Parameters_MenuMain() eine Menue-Trennlinie hinter Favoriten eingefuegt |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_Favoriten() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# |
//# 10.05.2014 OG |
//# - del: editDisableDeclCalc() - wurde ersetzt durch eine Transform-Funktion |
//# in paramset.c und wird jetzt von editGeneric() bearbeitet |
//# |
//# 07.05.2014 OG |
//# - chg: Menu_Favoriten() - uebernimmt den gegebenen Menuetitel vom |
//# uebergeordneten Eintrag aus mkparameters_messages.h (also Multilanguage) |
//# - chg: fav_add() - erweitert um Menue-Separatoren (Trennlinien) aufzunehmen |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: Favoriten-Verwaltung implementiert (Aenderungen an verschiedenen Funktionen) |
//# |
//# 18.04.2014 OG |
//# - fix: im ID_MENU_NAVICTRL fehlten param_NaviStickThreshold ("GPS Stick-Schwelle") |
//# und param_NaviGpsMinSat ("Min. Sat") |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Menu_EditCategory() blendet ggf. doppelt aufeinanderfolgenden |
//# Menue-Separatoren aus |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktioionen del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - add: Unterstuetzung fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 27.03.2014 OG |
//# kompletter neuer Code fuer ein erstes Release |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 20.02.2014 OG |
//# - chg: MK_Parameters_Menu() meldet "nicht verfügbar" |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mkparameters_messages.h" |
//############################################################################################# |
//# Strukturen; Forward-Deklarationen |
//############################################################################################# |
#define DEBUG_PARAMEDIT // schaltet zusaetzliche Debug-Ausgaben ein |
#define EOF 255 // End Of File (bzw. end of Table...) |
#define SEPARATOR 254 // ID fuer einen Separator in einem Menue (Trennlinie) |
#define SCREEN_REFRESH 1 // neuzeichnen/refresh der Anzeige |
#define SCREEN_REDRAW 2 // neuzeichnen/refresh der Anzeige |
//-------------------------------------------- |
// editGenericCode_t |
// deklariert ein einzelnes Code-Zeichen fuer Edit-Generic |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char code; // z.B. '0', '1', 'v', 'P', 'C' ... (einzelnes Zeichen aus paramEditItem_t.format ) |
unsigned char min; |
unsigned char max; |
const char *shortText_de; // |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
//--------------------------------------------- |
//----- globale Modulvariablen |
//--------------------------------------------- |
editGenericCode_t genericCode; // Daten eines einzelnen Code-Zeichen (im RAM) |
paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_SHORT_VALUE_de[] PROGMEM = "%3u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
#define GENERIC_SHORT_VALUE_en GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_de GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_en GENERIC_SHORT_VALUE_de |
// |
//static const char GENERIC_SHORT_VALUE_ACCZ_de[] PROGMEM = "%4u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
//#define GENERIC_SHORT_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_de GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
static const char GENERIC_SHORT_NO_de[] PROGMEM = " N"; |
#define GENERIC_SHORT_NO_en GENERIC_SHORT_NO_de |
static const char GENERIC_LONG_NO_de[] PROGMEM = "Nein"; |
static const char GENERIC_LONG_NO_en[] PROGMEM = "No"; |
static const char GENERIC_SHORT_YES_de[] PROGMEM = " J"; |
static const char GENERIC_SHORT_YES_en[] PROGMEM = " Y"; |
static const char GENERIC_LONG_YES_de[] PROGMEM = "Ja"; |
static const char GENERIC_LONG_YES_en[] PROGMEM = "Yes"; |
static const char GENERIC_SHORT_POTI_de[] PROGMEM = " P%u"; |
#define GENERIC_SHORT_POTI_en GENERIC_SHORT_POTI_de |
static const char GENERIC_LONG_POTI_de[] PROGMEM = "Poti %u"; |
#define GENERIC_LONG_POTI_en GENERIC_LONG_POTI_de |
static const char GENERIC_SHORT_CHANNEL_de[] PROGMEM = "C%2u"; |
#define GENERIC_SHORT_CHANNEL_en GENERIC_SHORT_CHANNEL_de |
static const char GENERIC_LONG_CHANNEL_de[] PROGMEM = "Kanal %u"; |
static const char GENERIC_LONG_CHANNEL_en[] PROGMEM = "Channel %u"; |
static const char GENERIC_SHORT_SERCHANNEL_de[] PROGMEM = "S%2u"; |
#define GENERIC_SHORT_SERCHANNEL_en GENERIC_SHORT_SERCHANNEL_de |
static const char GENERIC_LONG_SERCHANNEL_de[] PROGMEM = "Ser. Kanal %u"; |
static const char GENERIC_LONG_SERCHANNEL_en[] PROGMEM = "Ser. Channel %u"; |
static const char GENERIC_SHORT_AUS_de[] PROGMEM = "Aus"; |
static const char GENERIC_SHORT_AUS_en[] PROGMEM = "Off"; |
#define GENERIC_LONG_AUS_de GENERIC_SHORT_AUS_de |
#define GENERIC_LONG_AUS_en GENERIC_SHORT_AUS_en |
static const char GENERIC_SHORT_INACTIV_de[] PROGMEM = "Ina"; |
#define GENERIC_SHORT_INACTIV_en GENERIC_SHORT_INACTIV_de |
static const char GENERIC_LONG_INACTIV_de[] PROGMEM = "Inaktiv"; |
#define GENERIC_LONG_INACTIV_en GENERIC_LONG_INACTIV_de |
static const char GENERIC_SHORT_WPEVENT_de[] PROGMEM = "WPE"; |
#define GENERIC_SHORT_WPEVENT_en GENERIC_SHORT_WPEVENT_de |
static const char GENERIC_LONG_WPEVENT_de[] PROGMEM = "WP-Event"; |
#define GENERIC_LONG_WPEVENT_en GENERIC_LONG_WPEVENT_de |
static const char GENERIC_SHORT_MINIMUM_de[] PROGMEM = "Min"; |
#define GENERIC_SHORT_MINIMUM_en GENERIC_SHORT_MINIMUM_de |
static const char GENERIC_LONG_MINIMUM_de[] PROGMEM = "Minimum"; |
#define GENERIC_LONG_MINIMUM_en GENERIC_LONG_MINIMUM_de |
static const char GENERIC_SHORT_MIDDLE_de[] PROGMEM = "Mid"; |
#define GENERIC_SHORT_MIDDLE_en GENERIC_SHORT_MIDDLE_de |
static const char GENERIC_LONG_MIDDLE_de[] PROGMEM = "Mitte"; |
static const char GENERIC_LONG_MIDDLE_en[] PROGMEM = "Middle"; |
static const char GENERIC_SHORT_MAXIMUM_de[] PROGMEM = "Max"; |
#define GENERIC_SHORT_MAXIMUM_en GENERIC_SHORT_MAXIMUM_de |
static const char GENERIC_LONG_MAXIMUM_de[] PROGMEM = "Maximum"; |
#define GENERIC_LONG_MAXIMUM_en GENERIC_LONG_MAXIMUM_de |
static const char GENERIC_SHORT_AN_de[] PROGMEM = " An"; |
static const char GENERIC_SHORT_AN_en[] PROGMEM = " On"; |
#define GENERIC_LONG_AN_de GENERIC_SHORT_AN_de |
#define GENERIC_LONG_AN_en GENERIC_SHORT_AN_en |
static const char GENERIC_SHORT_FREE_de[] PROGMEM = "Fre"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_FREE_en GENERIC_SHORT_FREE_de |
static const char GENERIC_LONG_FREE_de[] PROGMEM = "Free"; |
#define GENERIC_LONG_FREE_en GENERIC_LONG_FREE_de |
static const char GENERIC_SHORT_CH_de[] PROGMEM = " CH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_CH_en GENERIC_SHORT_CH_de |
static const char GENERIC_LONG_CH_de[] PROGMEM = "Coming Home (CH)"; |
#define GENERIC_LONG_CH_en GENERIC_LONG_CH_de |
static const char GENERIC_SHORT_PH_de[] PROGMEM = " PH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_PH_en GENERIC_SHORT_PH_de |
static const char GENERIC_LONG_PH_de[] PROGMEM = "Position Hold (PH)"; |
#define GENERIC_LONG_PH_en GENERIC_LONG_PH_de |
static const char GENERIC_SHORT_DISABLED_de[] PROGMEM = "Dis"; // z.b. "Auto Start / Land" |
#define GENERIC_SHORT_DISABLED_en GENERIC_SHORT_DISABLED_de |
static const char GENERIC_LONG_DISABLED_de[] PROGMEM = "Disabled"; |
#define GENERIC_LONG_DISABLED_en GENERIC_LONG_DISABLED_de |
static const char GENERIC_SHORT_OUT_de[] PROGMEM = "Ou%1u"; // z.B. Servo 3 und Servo 4 |
#define GENERIC_SHORT_OUT_en GENERIC_SHORT_OUT_de |
static const char GENERIC_LONG_OUT_de[] PROGMEM = "Out %1u"; |
#define GENERIC_LONG_OUT_en GENERIC_LONG_OUT_de |
static const char GENERIC_SHORT_CH_OR_NC_de[] PROGMEM = "kÄn"; |
static const char GENERIC_SHORT_CH_OR_NC_en[] PROGMEM = "nCh"; |
static const char GENERIC_LONG_CH_OR_NC_de[] PROGMEM = "Keine Änderung"; |
static const char GENERIC_LONG_CH_OR_NC_en[] PROGMEM = "no change"; |
static const char GENERIC_SHORT_CH_OR_FH_de[] PROGMEM = "FzH"; |
static const char GENERIC_SHORT_CH_OR_FH_en[] PROGMEM = "FtH"; |
static const char GENERIC_LONG_CH_OR_FH_de[] PROGMEM = "Front zu Home"; |
static const char GENERIC_LONG_CH_OR_FH_en[] PROGMEM = "front to home"; |
static const char GENERIC_SHORT_CH_OR_RH_de[] PROGMEM = "HzH"; |
static const char GENERIC_SHORT_CH_OR_RH_en[] PROGMEM = "RtH"; |
static const char GENERIC_LONG_CH_OR_RH_de[] PROGMEM = "Heck zu Home"; |
static const char GENERIC_LONG_CH_OR_RH_en[] PROGMEM = "rear to home"; |
static const char GENERIC_SHORT_CH_OR_SO_de[] PROGMEM = "wSt"; |
static const char GENERIC_SHORT_CH_OR_SO_en[] PROGMEM = "sOr"; |
static const char GENERIC_LONG_CH_OR_SO_de[] PROGMEM = "wie beim Start"; |
static const char GENERIC_LONG_CH_OR_SO_en[] PROGMEM = "same as start"; |
//------------------------------------------------ |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char code; // '0', '1', 'v', 'P', 'C' ... |
unsigned char min; |
unsigned char max; |
const char *shortText_de; |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
*************************************************/ |
// Anmerkung: kann bei Bedarf evtl. zu PROGMEM umschreiben |
//editGenericCode_t const editGenericCode[] PROGMEM = |
editGenericCode_t editGenericCode[] = |
{ |
{ 'v', 0, 0, GENERIC_SHORT_VALUE_de, GENERIC_SHORT_VALUE_en, GENERIC_LONG_VALUE_de, GENERIC_LONG_VALUE_en }, // nur Wert anzeigen - min/max wird uebergeordnet definiert |
// { 'z', 0, 0, GENERIC_SHORT_VALUE_ACCZ_de, GENERIC_SHORT_VALUE_ACCZ_en, GENERIC_LONG_VALUE_ACCZ_de, GENERIC_LONG_VALUE_ACCZ_en }, // nur Wert anzeigen x 4- min/max wird uebergeordnet definiert |
{ '0', 0, 0, GENERIC_SHORT_NO_de, GENERIC_SHORT_NO_en, GENERIC_LONG_NO_de, GENERIC_LONG_NO_en }, |
{ '1', 1, 1, GENERIC_SHORT_YES_de, GENERIC_SHORT_YES_en, GENERIC_LONG_YES_de, GENERIC_LONG_YES_en }, |
{ 'P', 255,248, GENERIC_SHORT_POTI_de, GENERIC_SHORT_POTI_en, GENERIC_LONG_POTI_de, GENERIC_LONG_POTI_en }, |
{ 'C', 1, 16, GENERIC_SHORT_CHANNEL_de, GENERIC_SHORT_CHANNEL_en, GENERIC_LONG_CHANNEL_de, GENERIC_LONG_CHANNEL_en }, |
{ 'S', 17, 28, GENERIC_SHORT_SERCHANNEL_de, GENERIC_SHORT_SERCHANNEL_en, GENERIC_LONG_SERCHANNEL_de, GENERIC_LONG_SERCHANNEL_en }, |
{ 'A', 0, 0, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, |
{ 'I', 0, 0, GENERIC_SHORT_INACTIV_de, GENERIC_SHORT_INACTIV_en, GENERIC_LONG_INACTIV_de, GENERIC_LONG_INACTIV_en }, |
{ 'W', 29, 29, GENERIC_SHORT_WPEVENT_de, GENERIC_SHORT_WPEVENT_en, GENERIC_LONG_WPEVENT_de, GENERIC_LONG_WPEVENT_en }, |
{ 'U', 30, 30, GENERIC_SHORT_MINIMUM_de, GENERIC_SHORT_MINIMUM_en, GENERIC_LONG_MINIMUM_de, GENERIC_LONG_MINIMUM_en }, |
{ 'M', 31, 31, GENERIC_SHORT_MIDDLE_de, GENERIC_SHORT_MIDDLE_en, GENERIC_LONG_MIDDLE_de, GENERIC_LONG_MIDDLE_en }, |
{ 'X', 32, 32, GENERIC_SHORT_MAXIMUM_de, GENERIC_SHORT_MAXIMUM_en, GENERIC_LONG_MAXIMUM_de, GENERIC_LONG_MAXIMUM_en }, |
{ 'O', 30, 30, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, // ein weiteres "Aus"; z.b. "Motors-Sicherheitsschalter" |
{ 'N', 31, 31, GENERIC_SHORT_AN_de, GENERIC_SHORT_AN_en, GENERIC_LONG_AN_de, GENERIC_LONG_AN_en }, // z.b. "Carefree" |
{ 'F', 30, 30, GENERIC_SHORT_FREE_de, GENERIC_SHORT_FREE_en, GENERIC_LONG_FREE_de, GENERIC_LONG_FREE_en }, // Free - z.b. "Carefree" |
{ 'H', 31, 31, GENERIC_SHORT_CH_de, GENERIC_SHORT_CH_en, GENERIC_LONG_CH_de, GENERIC_LONG_CH_en }, // Coming Home - z.b. "Carefree" |
{ 'Q', 32, 32, GENERIC_SHORT_PH_de, GENERIC_SHORT_PH_en, GENERIC_LONG_PH_de, GENERIC_LONG_PH_en }, // Positiosn Hold - z.b. "Carefree" |
{ 'D', 0, 0, GENERIC_SHORT_DISABLED_de, GENERIC_SHORT_DISABLED_en, GENERIC_LONG_DISABLED_de, GENERIC_LONG_DISABLED_en }, // Inaktiv - z.b. "Auto Start / Land" |
{ 'T', 247,246, GENERIC_SHORT_OUT_de, GENERIC_SHORT_OUT_en, GENERIC_LONG_OUT_de, GENERIC_LONG_OUT_en }, |
{ 'K', 0, 0, GENERIC_SHORT_CH_OR_NC_de, GENERIC_SHORT_CH_OR_NC_en, GENERIC_LONG_CH_OR_NC_de, GENERIC_LONG_CH_OR_NC_en }, // CommingHome Orientation, |
{ 'V', 1, 1, GENERIC_SHORT_CH_OR_FH_de, GENERIC_SHORT_CH_OR_FH_en, GENERIC_LONG_CH_OR_FH_de, GENERIC_LONG_CH_OR_FH_en }, // CommingHome Orientation, |
{ 'R', 2, 2, GENERIC_SHORT_CH_OR_RH_de, GENERIC_SHORT_CH_OR_RH_en, GENERIC_LONG_CH_OR_RH_de, GENERIC_LONG_CH_OR_RH_en }, // CommingHome Orientation, |
{ 'G', 3, 3, GENERIC_SHORT_CH_OR_SO_de, GENERIC_SHORT_CH_OR_SO_en, GENERIC_LONG_CH_OR_SO_de, GENERIC_LONG_CH_OR_SO_en }, // CommingHome Orientation, |
{ EOF, 0, 0, 0,0,0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_NoYes[] PROGMEM = "01"; // Nein, Ja (z.B. (fast) alle Checkboxen) |
static const char GENERIC_Value[] PROGMEM = "v"; // value mit min,max aus paramEditItem (in paramEditItemTable) (z.B. Nick/Roll P (0..20) / Min. Gas (0..247) ) |
static const char GENERIC_ValueACCZ[] PROGMEM = "D"; // value mit min,max aus paramEditItem (in paramEditItemTable) x 4 (ACC Z Landing pulse ) |
static const char GENERIC_ValuePoti[] PROGMEM = "vP"; // value, Poti (z.B. Gyro Gier P) |
static const char GENERIC_ValueOutPoti[] PROGMEM = "vTP"; // value, Out, Poti (z.B. Servo 3 & 4) |
static const char GENERIC_Cannel[] PROGMEM = "C"; // Channel (z.B. Gas / Gier / Nick / Roll) |
static const char GENERIC_AusChSerWpeMMM[] PROGMEM = "ACSWUMX"; // Aus, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum (z.B. Poti 1..8) |
static const char GENERIC_DisChSerWpeOff[] PROGMEM = "DCSWO"; // Disabled, Channel, Ser. Channel, WP Event, Off (z.B. Motors-Sicherheitsschalter) |
static const char GENERIC_DisChSerWpeOffOn[] PROGMEM = "DCSWON"; // Disabled, Channel, Ser. Channel, WP Event, Off, On (z.B. Carefree) |
static const char GENERIC_DisChSerWpeFreeCHPH[] PROGMEM = "DCSWFHQ"; // Disabled, Channel, Ser. Channel, WP Event, Free, CH, PH (z.B. GPS Modus Steuerung) |
static const char GENERIC_DisChSer[] PROGMEM = "DCS"; // Disabled, Channel, Ser. Channel (z.B. Auto Start / Land) |
static const char GENERIC_CH_Orientation[] PROGMEM = "KVRG"; // Keine Änderung, vorne Home, Heck Home, gleiche Richtung (Comming Home Orientation |
static const char GENERIC_InaChSerWpeMMM[] PROGMEM = "ICSWUMX"; // Inactiv, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum(z.B. Menu Key Channel |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion - z.B. editGeneric(); cmd = MKPARAM_EDIT oder MKPARAM_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
*************************************************/ |
//---------------------- |
// HINWEIS! |
// Die unten stehende Aufgliederung in die verschiedenen Menue-Bereiche ist nur zur ORIENTIERUNG! |
// Jede paramID wird nur EINMAL deklariert - die Zuordnung in die Menues erfolgt in den |
// nachfolgenden ID_MENU_xyz_Items Strukturen! |
//---------------------- |
paramEditItem_t const paramEditItemTable[] PROGMEM = |
{ |
//----------------------------------- |
// Menue: ID_MENU_KANAELE (Kanäle) |
//----------------------------------- |
{ param_Kanalbelegung_Gas , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gas_de, param_Kanalbelegung_Gas_en }, |
{ param_Kanalbelegung_Gear , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gear_de, param_Kanalbelegung_Gear_en }, |
{ param_Kanalbelegung_Nick , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Nick_de, param_Kanalbelegung_Nick_en }, |
{ param_Kanalbelegung_Roll , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Roll_de, param_Kanalbelegung_Roll_en }, |
{ param_Kanalbelegung_Poti1 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti1_de, param_Kanalbelegung_Poti1_en }, |
{ param_Kanalbelegung_Poti2 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti2_de, param_Kanalbelegung_Poti2_en }, |
{ param_Kanalbelegung_Poti3 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti3_de, param_Kanalbelegung_Poti3_en }, |
{ param_Kanalbelegung_Poti4 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti4_de, param_Kanalbelegung_Poti4_en }, |
{ param_Kanalbelegung_Poti5 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti5_de, param_Kanalbelegung_Poti5_en }, |
{ param_Kanalbelegung_Poti6 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti6_de, param_Kanalbelegung_Poti6_en }, |
{ param_Kanalbelegung_Poti7 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti7_de, param_Kanalbelegung_Poti7_en }, |
{ param_Kanalbelegung_Poti8 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti8_de, param_Kanalbelegung_Poti8_en }, |
{ param_MotorSafetySwitch , &editGeneric, GENERIC_DisChSerWpeOff , 0, 0, param_MotorSafetySwitch_de, param_MotorSafetySwitch_en }, |
{ param_GlobalConfig3_MotorSwitchMode , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_MotorSwitchMode_de, param_GlobalConfig3_MotorSwitchMode_en }, |
{ param_ExtraConfig_SensitiveRc , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_SensitiveRc_de, param_ExtraConfig_SensitiveRc_en }, |
{ param_GlobalConfig3_SpeakAll , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_SpeakAll_de, param_GlobalConfig3_SpeakAll_en }, |
//----------------------------------- |
// Menue: ID_MENU_KONFIGURATION (Konfiguration) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en }, |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_GlobalConfig_KompassAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassAktiv_de, param_GlobalConfig_KompassAktiv_en }, |
{ param_GlobalConfig_KompassFix , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassFix_de, param_GlobalConfig_KompassFix_en }, |
// param_ExtraConfig_SensitiveRc |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GlobalConfig_HeadingHold , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HeadingHold_de, param_GlobalConfig_HeadingHold_en }, |
//----------------------------------- |
// Menue: ID_MENU_STICK (Stick) |
//----------------------------------- |
{ param_Stick_P , &editGeneric, GENERIC_Value , 0, 20, param_Stick_P_de, param_Stick_P_en }, |
{ param_Stick_D , &editGeneric, GENERIC_Value , 0, 20, param_Stick_D_de, param_Stick_D_en }, |
{ param_StickGier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_ExternalControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ExternalControl_de, param_ExternalControl_en }, |
//----------------------------------- |
// Menue: ID_MENU_LOOPING (Looping) |
//----------------------------------- |
{ param_BitConfig_LoopOben , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopOben_de, param_BitConfig_LoopOben_en }, |
{ param_BitConfig_LoopUnten , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopUnten_de, param_BitConfig_LoopUnten_en }, |
{ param_BitConfig_LoopLinks , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopLinks_de, param_BitConfig_LoopLinks_en }, |
{ param_BitConfig_LoopRechts , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopRechts_de, param_BitConfig_LoopRechts_en }, |
{ param_LoopGasLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_LoopGasLimit_de, param_LoopGasLimit_en }, |
{ param_LoopThreshold , &editGeneric, GENERIC_Value , 0, 247, param_LoopThreshold_de, param_LoopThreshold_en }, |
{ param_LoopHysterese , &editGeneric, GENERIC_Value , 0, 247, param_LoopHysterese_de, param_LoopHysterese_en }, |
{ param_WinkelUmschlagNick , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagNick_de, param_WinkelUmschlagNick_en }, |
{ param_WinkelUmschlagRoll , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagRoll_de, param_WinkelUmschlagRoll_en }, |
//----------------------------------- |
// Menue: ID_MENU_HOEHE (Höhe) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en}, |
{ param_ExtraConfig_HeightLimit , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightLimit_de, param_ExtraConfig_HeightLimit_en }, |
{ param_ExtraConfig_HeightVario , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightVario_de, param_ExtraConfig_HeightVario_en }, // negiertes param_ExtraConfig_HeightLimit |
{ param_GlobalConfig_HoehenSchalter , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenSchalter_de, param_GlobalConfig_HoehenSchalter_en}, |
{ param_ExtraConfig_VarioBeep , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_VarioBeep_de, param_ExtraConfig_VarioBeep_en }, |
{ param_HoeheChannel , &editGeneric, GENERIC_DisChSerWpeOffOn, 0, 0, param_HoeheChannel_de, param_HoeheChannel_en }, |
{ param_Hoehe_MinGas , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_MinGas_de, param_Hoehe_MinGas_en }, |
{ param_Hoehe_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_P_de, param_Hoehe_P_en }, |
{ param_Luftdruck_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Luftdruck_D_de, param_Luftdruck_D_en }, |
{ param_Hoehe_ACC_Wirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_ACC_Wirkung_de, param_Hoehe_ACC_Wirkung_en }, |
{ param_MaxAltitude , &editGeneric, GENERIC_ValuePoti , 0, 247, param_MaxAltitude_de, param_MaxAltitude_en }, |
{ param_Hoehe_Verstaerkung , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_Verstaerkung_de, param_Hoehe_Verstaerkung_en }, |
{ param_Hoehe_HoverBand , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_HoverBand_de, param_Hoehe_HoverBand_en }, |
{ param_Hoehe_GPS_Z , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_GPS_Z_de, param_Hoehe_GPS_Z_en }, |
{ param_Hoehe_TiltCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_Tilt_Comp_de, param_Hoehe_Tilt_Comp_en }, |
{ param_Hoehe_StickNeutralPoint , &editGeneric, GENERIC_Value , 0, 160, param_Hoehe_StickNeutralPoint_de, param_Hoehe_StickNeutralPoint_en }, |
{ param_StartLandChannel , &editGeneric, GENERIC_DisChSer , 0, 0, param_StartLandChannel_de, param_StartLandChannel_en }, |
{ param_LandingSpeed , &editGeneric, GENERIC_Value , 0, 247, param_LandingSpeed_de, param_LandingSpeed_en }, |
{ param_LandingPulse , &editACCZLandingPulse, GENERIC_ValueACCZ , 0, 0, param_LandingPulse_de, param_LandingPulse_en }, |
//----------------------------------- |
// Menue: ID_MENU_KAMERA (Kamera) |
//----------------------------------- |
{ param_ServoNickControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickControl_de, param_ServoNickControl_en }, |
{ param_ServoNickComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickComp_de, param_ServoNickComp_en }, |
{ param_ServoNickFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickFails_de, param_ServoNickFails_en }, |
{ param_GlobalConfig3_ServoNickCompOff , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_ServoNickCompOff_de, param_GlobalConfig3_ServoNickCompOff_en }, // TODO: pruefen: ab welcher Version ist das drin ??? |
{ param_ServoCompInvert_SERVO_NICK_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVNick_de, param_ServoCompInvert_SVNick_en }, |
{ param_ServoCompInvert_SERVO_RELATIVE , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRelMov_de, param_ServoCompInvert_SVRelMov_en }, |
{ param_ServoNickMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMin_de, param_ServoNickMin_en }, |
{ param_ServoNickMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMax_de, param_ServoNickMax_en }, |
{ param_ServoFilterNick , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterNick_de, param_ServoFilterNick_en }, |
{ param_ServoRollControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollControl_de, param_ServoRollControl_en }, |
{ param_ServoRollComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollComp_de, param_ServoRollComp_en }, |
{ param_ServoRollFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollFails_de, param_ServoRollFails_en }, |
{ param_ServoCompInvert_SERVO_ROLL_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRoll_de, param_ServoCompInvert_SVRoll_en }, |
{ param_ServoRollMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMin_de, param_ServoRollMin_en }, |
{ param_ServoRollMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMax_de, param_ServoRollMax_en }, |
{ param_ServoFilterRoll , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterRoll_de, param_ServoFilterRoll_en }, |
{ param_ServoNickRefresh , &editGeneric, GENERIC_Value , 2, 8, param_ServoNickRefresh_de, param_ServoNickRefresh_en }, |
{ param_ServoManualControlSpeed , &editGeneric, GENERIC_Value , 0, 247, param_ServoManualControlSpeed_de, param_ServoManualControlSpeed_en }, |
{ param_Servo3 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo3_de, param_Servo3_en }, |
{ param_Servo3Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo3Fails_de, param_Servo3Fails_en }, |
{ param_Servo4 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo4_de, param_Servo4_en }, |
{ param_Servo4Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo4Fails_de, param_Servo4Fails_en }, |
{ param_Servo5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Servo5_de, param_Servo5_en }, |
{ param_Servo5Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo5Fails_de, param_Servo5Fails_en }, |
{ param_Servo3OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OnValue_de, param_Servo3OnValue_en }, |
{ param_Servo3OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OffValue_de, param_Servo3OffValue_en }, |
{ param_Servo4OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OnValue_de, param_Servo4OnValue_en }, |
{ param_Servo4OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OffValue_de, param_Servo4OffValue_en }, |
//{ param_CamOrientation , &editNA , 0 , 0, 0, param_CamOrientation_de, param_CamOrientation_en }, // wird evtl. nicht mehr unterstuetzt |
//----------------------------------- |
// Menue: ID_MENU_NAVICTRL (NaviCtrl) |
//----------------------------------- |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_NaviGpsModeChannel , &editGeneric, GENERIC_DisChSerWpeFreeCHPH , 0, 0, param_NaviGpsModeChannel_de, param_NaviGpsModeChannel_en }, |
{ param_NaviGpsGain , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsGain_de, param_NaviGpsGain_en }, |
{ param_NaviStickThreshold , &editGeneric, GENERIC_Value , 0, 247, param_NaviStickThreshold_de, param_NaviStickThreshold_en }, |
{ param_NaviGpsMinSat , &editGeneric, GENERIC_Value , 0, 247, param_NaviGpsMinSat_de, param_NaviGpsMinSat_en }, |
{ param_NaviGpsP , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsP_de, param_NaviGpsP_en }, |
{ param_NaviGpsI , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsI_de, param_NaviGpsI_en }, |
{ param_NaviGpsD , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsD_de, param_NaviGpsD_en }, |
{ param_NaviGpsPLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsPLimit_de, param_NaviGpsPLimit_en }, |
{ param_NaviGpsILimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsILimit_de, param_NaviGpsILimit_en }, |
{ param_NaviGpsDLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsDLimit_de, param_NaviGpsDLimit_en }, |
{ param_NaviGpsA , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsA_de, param_NaviGpsA_en }, |
{ param_NaviWindCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviWindCorrection_de, param_NaviWindCorrection_en }, |
{ param_NaviAccCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAccCompensation_de, param_NaviAccCompensation_en }, |
{ param_NaviMaxFlyingRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviMaxFlyingRange_de, param_NaviMaxFlyingRange_en }, |
{ param_NaviOperatingRadius , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOperatingRadius_de, param_NaviOperatingRadius_en }, |
{ param_NaviAngleLimitation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAngleLimitation_de, param_NaviAngleLimitation_en }, |
{ param_NaviPH_LoginTime , &editGeneric, GENERIC_Value , 0, 247, param_NaviPH_LoginTime_de, param_NaviPH_LoginTime_en }, |
{ param_ExtraConfig_GpsAid , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_GpsAid_de, param_ExtraConfig_GpsAid_en }, |
{ param_GlobalConfig3_DphMaxRadius , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_DphMaxRadius_de, param_GlobalConfig3_DphMaxRadius_en }, |
{ param_ComingHomeAltitude , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeAltitude_de, param_ComingHomeAltitude_en }, |
{ param_ComingHomeOrientation , &editGeneric, GENERIC_CH_Orientation , 0, 3, param_ComingHomeOrientation_de, param_ComingHomeOrientation_en }, |
{ param_SingleWpControlChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_SingleWpControlChannel_de, param_SingleWpControlChannel_en }, //ab Rev.106 |
{ param_MenuKeyChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_MenuKeyChannel_de, param_MenuKeyChannel_en }, |
{ param_SingleWpSpeed , &editGeneric, GENERIC_ValuePoti , 0, 247, param_SingleWpSpeed_de, param_SingleWpSpeed_en }, // ab Rev. 100 |
{ param_NaviDescendRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviDescendRange_de, param_NaviDescendRange_en }, |
//----------------------------------- |
// Menue: ID_MENU_AUSGAENGE (Ausgänge) |
//----------------------------------- |
{ param_J16Bitmask , &editBitmask, 0 , 0, 0, param_J16Bitmask_de, param_J16Bitmask_en }, |
{ param_J16Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J16Timing_de, param_J16Timing_en }, |
{ param_BitConfig_MotorBlink1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink1_de, param_BitConfig_MotorBlink1_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed1_de, param_BitConfig_MotorOffLed1_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_GlobalConfig3_UseNcForOut1 , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_UseNcForOut1_de, param_GlobalConfig3_UseNcForOut1_en }, // "mit WP-Event verknüpfen" |
{ param_NaviOut1Parameter , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOut1Parameter_de, param_NaviOut1Parameter_en }, // "AutoTrigger alle...[meter]" (bis Rev.98) |
{ param_AutoPhotoDistance , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoDistance_de, param_AutoPhotoDistance_en }, // "AutoTrigger alle [meter] in Distance" (ab Rev.100) |
{ param_AutoPhotoAtitudes , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoAtitudes_de, param_AutoPhotoAtitudes_en }, // "AutoTrigger alle [meter] in Altitude" (ab Rev.100) |
{ param_J17Bitmask , &editBitmask, 0 , 0, 0, param_J17Bitmask_de, param_J17Bitmask_en }, |
{ param_J17Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J17Timing_de, param_J17Timing_en }, |
{ param_BitConfig_MotorBlink2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink2_de, param_BitConfig_MotorBlink2_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed2_de, param_BitConfig_MotorOffLed2_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_WARN_J16_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J16_Bitmask_de, param_WARN_J16_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
{ param_WARN_J17_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J17_Bitmask_de, param_WARN_J17_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
//----------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES (Verschiedenes) |
//----------------------------------- |
{ param_Gas_Min , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Min_de, param_Gas_Min_en }, |
{ param_Gas_Max , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Max_de, param_Gas_Max_en }, |
{ param_KompassWirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_KompassWirkung_de, param_KompassWirkung_en }, |
//{ param_CompassOffset , &editGeneric, GENERIC_Value , 0, 255, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset , &editCompassOffset, 0 , 0, 0, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset_DisableDeclCalc , &editGeneric, GENERIC_NoYes , 0, 0, param_CompassOffset_DisableDeclCalc_de, param_CompassOffset_DisableDeclCalc_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CareFreeChannel , &editGeneric, GENERIC_DisChSerWpeOffOn , 0, 0, param_CareFreeChannel_de, param_CareFreeChannel_en }, |
{ param_ExtraConfig_LearnableCarefree , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_LearnableCarefree_de, param_ExtraConfig_LearnableCarefree_en }, |
{ param_UnterspannungsWarnung , &editGeneric, GENERIC_Value , 0, 247, param_UnterspannungsWarnung_de, param_UnterspannungsWarnung_en }, |
{ param_ComingHomeVoltage , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeVoltage_de, param_ComingHomeVoltage_en }, |
{ param_AutoLandingVoltage , &editGeneric, GENERIC_Value , 0, 247, param_AutoLandingVoltage_de, param_AutoLandingVoltage_en }, |
//{ param_ExtraConfig_33vReference , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_33vReference_de, param_ExtraConfig_33vReference_en }, // nicht mehr unterstuetzt! |
{ param_NotGasZeit , &editGeneric, GENERIC_Value , 0, 247, param_NotGasZeit_de, param_NotGasZeit_en }, |
{ param_GlobalConfig3_VarioFailsafe , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_VarioFailsafe_de, param_GlobalConfig3_VarioFailsafe_en }, |
{ param_NotGas , &editGeneric, GENERIC_Value , 0, 247, param_NotGas_de, param_NotGas_en }, |
{ param_FailSafeTime , &editGeneric, GENERIC_Value , 0, 247, param_FailSafeTime_de, param_FailSafeTime_en }, |
{ param_FailsafeChannel , &editGeneric, GENERIC_Value , 0, 16, param_FailsafeChannel_de, param_FailsafeChannel_en }, |
{ param_ExtraConfig_NoRcOffBeeping , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_NoRcOffBeeping_de, param_ExtraConfig_NoRcOffBeeping_en }, |
{ param_ExtraConfig_IgnoreMagErrAtStartup , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_IgnoreMagErrAtStartup_de, param_ExtraConfig_IgnoreMagErrAtStartup_en }, |
{ param_GlobalConfig3_NoSdCardNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoSdCardNoStart_de, param_GlobalConfig3_NoSdCardNoStart_en }, |
{ param_GlobalConfig3_NoGpsFixNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoGpsFixNoStart_de, param_GlobalConfig3_NoGpsFixNoStart_en }, |
//----------------------------------- |
// Menue: ID_MENU_GYRO (Gyro) |
//----------------------------------- |
{ param_Gyro_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_P_de, param_Gyro_P_en }, |
{ param_Gyro_Gier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_Gyro_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_I_de, param_Gyro_I_en }, |
{ param_Gyro_Gier_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_I_de, param_Gyro_Gier_I_en }, |
{ param_Gyro_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_D_de, param_Gyro_D_en }, |
{ param_DynamicStability , &editGeneric, GENERIC_ValuePoti , 0, 247, param_DynamicStability_de, param_DynamicStability_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GyroAccFaktor , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccFaktor_de, param_GyroAccFaktor_en }, |
{ param_GyroAccAbgleich , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccAbgleich_de, param_GyroAccAbgleich_en }, |
{ param_I_Faktor , &editGeneric, GENERIC_ValuePoti , 0, 247, param_I_Faktor_de, param_I_Faktor_en }, |
{ param_Driftkomp , &editGeneric, GENERIC_Value , 0, 247, param_Driftkomp_de, param_Driftkomp_en }, |
{ param_Gyro_Stability , &editGeneric, GENERIC_Value , 0, 247, param_Gyro_Stability_de, param_Gyro_Stability_en }, |
{ param_MotorSmooth , &editGeneric, GENERIC_Value , 0, 247, param_MotorSmooth_de, param_MotorSmooth_en }, |
//----------------------------------- |
// Menue: ID_MENU_BENUTZER (Benutzer) |
//----------------------------------- |
{ param_UserParam1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam1_de, param_UserParam1_en }, |
{ param_UserParam2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam2_de, param_UserParam2_en }, |
{ param_UserParam3 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam3_de, param_UserParam3_en }, |
{ param_UserParam4 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam4_de, param_UserParam4_en }, |
{ param_UserParam5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam5_de, param_UserParam5_en }, |
{ param_UserParam6 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam6_de, param_UserParam6_en }, |
{ param_UserParam7 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam7_de, param_UserParam7_en }, |
{ param_UserParam8 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam8_de, param_UserParam8_en }, |
//----------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG (Achskopplung) |
//----------------------------------- |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_AchsKopplung1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung1_de, param_AchsKopplung1_en }, |
{ param_AchsKopplung2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung2_de, param_AchsKopplung2_en }, |
{ param_CouplingYawCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_CouplingYawCorrection_de, param_CouplingYawCorrection_en }, |
//----------------------------------- |
// ENDE |
//----------------------------------- |
{ param_EOF, NOFUNC, 0,0,0, 0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
/************************************************************ |
NICHT MEHR UNTERSTUEZT: |
param_Receiver (Empfänger Typ: Hott, Jeti, Spektrum,...) |
************************************************************/ |
//############################################################################################# |
//# Menue's und Menuezuordnung |
//############################################################################################# |
//----------------------------------- |
// Menue ID's: Parameters Hauptmenue |
//----------------------------------- |
#define ID_MENU_FAVORITEN 1 // fuer spaeter reserviert.... |
#define ID_MENU_KANAELE 2 |
#define ID_MENU_KONFIGURATION 3 |
#define ID_MENU_STICK 4 |
#define ID_MENU_LOOPING 5 |
#define ID_MENU_HOEHE 6 |
#define ID_MENU_KAMERA 7 |
#define ID_MENU_NAVICTRL 8 |
#define ID_MENU_AUSGAENGE 9 |
#define ID_MENU_VERSCHIEDENES 10 |
#define ID_MENU_GYRO 11 |
#define ID_MENU_BENUTZER 12 |
#define ID_MENU_ACHSKOPPLUNG 13 |
#define ID_MENU_MIXERSETUP 14 // nicht verwendet / unterstuetzt |
#define ID_MENU_EASYSETUP 15 |
#define ID_MENU_TEST 66 // TEST / DEBUG |
//----------------------------------- |
// Zuordnungen von paramID's zu den jeweiligen Menue's |
// |
// Eintrag "SEPARATOR" - damit wird eine Trennlinie |
// im Menue dargestellt |
//----------------------------------- |
//------------------------------- |
// Menue: ID_MENU_KANAELE |
//------------------------------- |
unsigned char const ID_MENU_KANAELE_Items[] = |
{ |
param_Kanalbelegung_Gas, |
param_Kanalbelegung_Gear, |
param_Kanalbelegung_Nick, |
param_Kanalbelegung_Roll, |
param_Kanalbelegung_Poti1, |
param_Kanalbelegung_Poti2, |
param_Kanalbelegung_Poti3, |
param_Kanalbelegung_Poti4, |
param_Kanalbelegung_Poti5, |
param_Kanalbelegung_Poti6, |
param_Kanalbelegung_Poti7, |
param_Kanalbelegung_Poti8, |
SEPARATOR, |
param_MotorSafetySwitch, |
param_GlobalConfig3_MotorSwitchMode, |
SEPARATOR, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig3_SpeakAll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KONFIGURATION |
//------------------------------- |
unsigned char const ID_MENU_KONFIGURATION_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_GlobalConfig_GpsAktiv, |
param_GlobalConfig_KompassAktiv, |
param_GlobalConfig_KompassFix, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig_AchsenkopplungAktiv, |
param_GlobalConfig_DrehratenBegrenzer, |
param_GlobalConfig_HeadingHold, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_STICK |
//------------------------------- |
unsigned char const ID_MENU_STICK_Items[] = |
{ |
param_Stick_P, |
param_Stick_D, |
param_StickGier_P, |
SEPARATOR, |
param_ExternalControl, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_LOOPING |
//------------------------------- |
unsigned char const ID_MENU_LOOPING_Items[] = |
{ |
param_BitConfig_LoopOben, |
param_BitConfig_LoopUnten, |
param_BitConfig_LoopLinks, |
param_BitConfig_LoopRechts, |
SEPARATOR, |
param_LoopGasLimit, |
param_LoopThreshold, |
param_LoopHysterese, |
param_WinkelUmschlagNick, |
param_WinkelUmschlagRoll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_HOEHE |
//------------------------------- |
unsigned char const ID_MENU_HOEHE_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_ExtraConfig_HeightLimit, |
param_ExtraConfig_HeightVario, |
param_GlobalConfig_HoehenSchalter, |
param_ExtraConfig_VarioBeep, |
SEPARATOR, |
param_HoeheChannel, |
param_Hoehe_Verstaerkung, |
param_Hoehe_MinGas, |
param_Hoehe_HoverBand, |
param_Hoehe_P, |
param_Hoehe_GPS_Z, |
param_Hoehe_TiltCompensation, |
param_Luftdruck_D, |
param_Hoehe_StickNeutralPoint, |
param_Hoehe_ACC_Wirkung, |
param_MaxAltitude, |
SEPARATOR, |
param_StartLandChannel, |
param_LandingSpeed, |
param_LandingPulse, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KAMERA |
//------------------------------- |
unsigned char const ID_MENU_KAMERA_Items[] = |
{ |
param_ServoNickControl, |
param_ServoNickComp, |
param_ServoNickFailsave, |
param_GlobalConfig3_ServoNickCompOff, // erst ab FC-Rev 96 |
param_ServoCompInvert_SERVO_NICK_INV, |
param_ServoCompInvert_SERVO_RELATIVE, |
param_ServoNickMin, |
param_ServoNickMax, |
param_ServoFilterNick, |
SEPARATOR, |
param_ServoRollControl, |
param_ServoRollComp, |
param_ServoRollFailsave, |
param_ServoCompInvert_SERVO_ROLL_INV, |
param_ServoRollMin, |
param_ServoRollMax, |
param_ServoFilterRoll, |
SEPARATOR, |
param_ServoNickRefresh, |
param_ServoManualControlSpeed, |
param_Servo3, |
param_Servo3Failsave, |
param_Servo4, |
param_Servo4Failsave, |
param_Servo5, |
param_Servo5Failsave, |
SEPARATOR, |
param_Servo3OnValue, |
param_Servo3OffValue, |
param_Servo4OnValue, |
param_Servo4OffValue, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_NAVICTRL |
//------------------------------- |
unsigned char const ID_MENU_NAVICTRL_Items[] = |
{ |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_NaviGpsGain, |
param_NaviStickThreshold, |
param_NaviGpsMinSat, |
param_NaviGpsP, |
param_NaviGpsPLimit, |
param_NaviGpsI, |
param_NaviGpsILimit, |
param_NaviGpsD, |
param_NaviGpsDLimit, |
param_NaviGpsA, |
SEPARATOR, |
param_NaviWindCorrection, |
param_NaviAccCompensation, |
param_NaviMaxFlyingRange, |
param_NaviOperatingRadius, |
param_NaviAngleLimitation, |
param_NaviPH_LoginTime, |
param_ExtraConfig_GpsAid, |
param_GlobalConfig3_DphMaxRadius, |
SEPARATOR, |
param_ComingHomeAltitude, |
param_ComingHomeOrientation, |
SEPARATOR, |
param_SingleWpControlChannel, |
param_MenuKeyChannel, |
param_SingleWpSpeed, |
SEPARATOR, |
param_NaviDescendRange, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_AUSGAENGE |
//------------------------------- |
unsigned char const ID_MENU_AUSGAENGE_Items[] = |
{ |
param_J16Bitmask, // LED 1 Bitmaske |
param_J16Timing, |
param_BitConfig_MotorBlink1, |
param_BitConfig_MotorOffLed1, |
param_GlobalConfig3_UseNcForOut1, |
param_NaviOut1Parameter, |
param_AutoPhotoDistance, |
param_AutoPhotoAtitudes, |
SEPARATOR, |
param_J17Bitmask, // LED 2 Bitmaske |
param_J17Timing, |
param_BitConfig_MotorBlink2, |
param_BitConfig_MotorOffLed2, |
SEPARATOR, |
param_WARN_J16_Bitmask, |
param_WARN_J17_Bitmask, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES |
//------------------------------- |
unsigned char const ID_MENU_VERSCHIEDENES_Items[] = |
{ |
param_Gas_Min, |
param_Gas_Max, |
param_KompassWirkung, |
param_CompassOffset, |
param_CompassOffset_DisableDeclCalc, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
SEPARATOR, |
param_UnterspannungsWarnung, |
param_ComingHomeVoltage, |
param_AutoLandingVoltage, |
SEPARATOR, |
param_NotGasZeit, |
param_GlobalConfig3_VarioFailsafe, // Nutze "Vario-Höhe" für "Not-Gas" -> beeinflusst 'param_NotGas' bzgl. "%" |
param_NotGas, |
param_FailSafeTime, |
param_FailsafeChannel, |
param_ExtraConfig_NoRcOffBeeping, |
SEPARATOR, |
param_ExtraConfig_IgnoreMagErrAtStartup, |
param_GlobalConfig3_NoSdCardNoStart, |
param_GlobalConfig3_NoGpsFixNoStart, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_GYRO |
//------------------------------- |
unsigned char const ID_MENU_GYRO_Items[] = |
{ |
param_Gyro_P, |
param_Gyro_I, |
param_Gyro_D, |
param_Gyro_Gier_P, |
param_Gyro_Gier_I, |
SEPARATOR, |
param_DynamicStability, |
param_GlobalConfig_DrehratenBegrenzer, |
SEPARATOR, |
param_GyroAccFaktor, |
param_GyroAccAbgleich, |
SEPARATOR, |
param_I_Faktor, |
param_Driftkomp, |
param_Gyro_Stability, |
param_MotorSmooth, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_BENUTZER |
//------------------------------- |
unsigned char const ID_MENU_BENUTZER_Items[] = |
{ |
param_UserParam1, |
param_UserParam2, |
param_UserParam3, |
param_UserParam4, |
param_UserParam5, |
param_UserParam6, |
param_UserParam7, |
param_UserParam8, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG |
//------------------------------- |
unsigned char const ID_MENU_ACHSKOPPLUNG_Items[] = |
{ |
param_GlobalConfig_AchsenkopplungAktiv, |
param_AchsKopplung1, |
param_AchsKopplung2, |
param_CouplingYawCorrection, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_EASYSETUP |
//------------------------------- |
unsigned char const ID_MENU_EASYSETUP_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_HoeheChannel, |
param_Hoehe_StickNeutralPoint, |
param_StartLandChannel, |
SEPARATOR, |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_ExtraConfig_GpsAid, |
param_ComingHomeAltitude, |
SEPARATOR, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
EOF // *** MUST BE THE LAST! *** |
}; |
//############################################################################################# |
//# Error-Handling |
//############################################################################################# |
//# mittels des auf dem PKT angezeigten Errocodes (z.B. "E32") kann man hier schauen was bzw. |
//# wo (in welcher Funktion) der Fehler aufgetreten ist. |
//# |
//# Im Menue wird der Fehlercode rechts bei den Values dargestellt (dabei ertönen immer wieder |
//# Fehler-Beep's). |
//# In der Vollbildanzeige wird zusaetzlich ein uebergebener Wert (value) angezeigt - im Code |
//# kann man ermitteln was der Wert darstellt. |
//############################################################################################# |
#define E01_PARAMID_MENUEDITCATEGORY 1 // Menu_EditCategory() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E11_PARAMID_EDITGENERIC 11 // editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E12_PARAMID_EDITBITMASK 12 // editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E30_CODECHAR_NOT_FOUND 30 // find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E31_PARAMEDIT_ITEM_NOT_FOUND 31 // find_genericCodeByValue() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E32_CODECHAR_NOT_FOUND 32 // find_genericCodeByValue() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E33_VALUE_NOT_IN_FORMAT 33 // find_genericCodeByValue() - FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
#define E41_VALUE_MIN_MAX_RANGE 41 // strValueGeneric() - FEHLER: fuer die Range von 'v' (Value) wurde max < min angegeben bei Generic -> nicht unterstuetzt |
//-------------------------------------------------------------- |
// _error( errorcode, value, showscreen) |
// |
// zeigt Fehlermeldungen an |
// |
// PARAMETER: |
// errorcode : siehe Error-defines |
// value : zusaetzlicher Anzeigeparameter (nur in Screen-Anzeige) |
// showscreen: true = Fehlermeldung wird vollstaendig angezeigt. |
// - er Screen wird dabei geloescht |
// - Ende mit Taste 'Ende' |
// - langer Fehler-Beep |
// |
// false = es wird nur der Buffer mkparam_strValueBuffer |
// mit dem Fehlercode befuellt |
// - Anzeige im Menue ganz recht als "Enn" |
// - kurzer Fehler-Beep der jedoch bei jeder |
// Aktion im Menue zu hoeren ist |
// => dann auf Anzeige "Enn" achten! |
//-------------------------------------------------------------- |
void _error( uint8_t errorcode, uint8_t value, uint8_t showscreen) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("E%02u"), errorcode); // in mkparam_strValueBuffer ausgeben fuer das Menue |
if( showscreen ) |
{ |
lcd_cls(); |
lcdx_printp_center( 1, PSTR(" ERROR "), MINVERS, 0,-3); |
lcdx_printp_center( 2, PSTR("mkparameters.c"), MNORMAL, 0,0); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("Code : E%02u"), errorcode ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Value: %3u"), value ); |
lcd_printp_at(11, 7, strGet(ENDE), MNORMAL ); |
set_beep( 500, 0x000f, BeepNormal); // Beep Error |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
//-------------------------------- |
// Anmerkung: OG 25.03.2014 |
// |
// Ohne diese Verzoegerung funkioniert die Update-Erkennung via PKT-UpdateTool |
// meist nicht - ist das PKT zu schnell? |
// Kann man evtl. das PKT-UpdateTool diesbezueglich mal ueberpruefen ob man |
// Timings/Timeout's anpassen kann? |
// |
// Hinweis dazu: PKT_CtrlHook() bzw. die dort enthaltene Update-Anforderungspruefung |
// ist in den letzten Wochen beschleunigt geworden da unnoetiger Code fuer nicht |
// benoetigte Bildschirmausgaben entfernt wurde - das ist grundsaetzlich fuer das PKT |
// besser. |
// |
// Um das nachzustellen: |
// Unten das Delay auskommentieren und einen Fehler in Menu_EditCategory() provozieren |
// indem "paramID = 0;" einkommentiert wird (ist dort beschrieben) |
// Dann den Fehlerscreen im PKT anzeigen lassen und ein Update via PKT-UpdateTool initiieren, |
//-------------------------------- |
_delay_ms(150); |
PKT_CtrlHook(); |
} |
} |
else |
{ |
// kurzer Fehler-Beep - der kann wiederholt im Menue auftreten bei jedem Refresh des Menues |
set_beep( 160, 0x000f, BeepNormal); // Beep Error |
} |
} |
//############################################################################################# |
//# Edit-Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = find_paramEditItem( paramID ) |
// |
// sucht in paramEditItemTable (PROGMEM) eine paramID und |
// kopiert den Eintrag in das RAM |
// |
// PARAMETER: |
// paramID |
// |
// RUECKGABE: |
// true = paramID gefunden |
// false = nicht gefunden (Fehler) |
// |
// -- globale Modulvariablen -- |
// Ergebnisse in: |
// |
// paramEditItem : der gefundene Eintrag vom Typ paramEditItem_t (im RAM) |
// paramEditFormat: der Format-String paramEditItem.format im RAM |
//-------------------------------------------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ) |
{ |
unsigned char id; |
unsigned char *p; |
p = (unsigned char *) paramEditItemTable; |
while( true ) |
{ |
id = pgm_read_byte(p); // die aktuelle paramID aus paramEditDef |
if( id == paramID || id == param_EOF ) |
break; |
p += sizeof( paramEditItem_t ); |
} |
//----- |
// wenn die gesuchte paramID nicht gefunden wurde steht |
// im RAM-Buffer paramEdit.paramID == param_EOF !! |
//----- |
memcpy_P ( ¶mEditItem , p, sizeof(paramEditItem_t) ); // die Struktur -> in den RAM-Buffer kopieren (Zugriff: paramEditItem) |
strncpy_P( paramEditFormat, paramEditItem.format, MKPARAM_STRBUFFER_LEN); // format String -> in den RAM-Buffer kopieren (Zugriff: paramEditFormat) |
return( id!=param_EOF ); |
} |
//-------------------------------------------------------------- |
// ptrP = paramEditItemTitle() |
// |
// gibt einen Zeiger in PROGMEM auf den Titel/Bezeichnung eines |
// paramID's in der richtigen Sprache zurueck |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
/* |
const char * paramEditItemTitle( void ) |
{ |
if( Config.DisplayLanguage == 0 ) |
return( paramEditItem.title_de ); |
else |
return( paramEditItem.title_en ); |
} |
*/ |
//-------------------------------------------------------------- |
// print_paramEditItemTitle() |
// |
// gibt Titel/Bezeichnung auf den Bildschirm bei x=0, y=2 aus |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
void print_paramEditItemTitle( void ) |
{ |
lcd_printp_at( 0, 2, (Config.DisplayLanguage == 0 ? paramEditItem.title_de : paramEditItem.title_en ), MNORMAL); // Bezeichnung des paramID's |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( code ) |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCode( unsigned char code ) |
{ |
unsigned char codeIdx; |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != code ) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//----------------------------- |
// find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//----------------------------- |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E30_CODECHAR_NOT_FOUND, code, false); |
return false; |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
return true; |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( paramID, &v, &min, &max, pFormat) |
// |
// PARAMETER: |
// paramID |
// *v |
// *min |
// *max |
// *pFormat |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// paramEditItem |
// paramEditFormat: der zugehoerige Format-String |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCodeByValue( unsigned char paramID, unsigned char *v, unsigned char *min, unsigned char *max ) |
{ |
unsigned char codeIdx; |
char *pFormat; |
//------------------------------------------------- |
// 1. finde paramID in der paramEditItem-Tabelle |
// |
// sollte der RAM-Buffer paramEditItem bereits das |
// gesuchte enthalten dann ok |
//------------------------------------------------- |
if( paramEditItem.paramID != paramID ) |
find_paramEditItem( paramID ); |
//--- |
// FEHLER: paramEditItem nicht in Tabelle paramEditItemTable gefunden |
//--- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E31_PARAMEDIT_ITEM_NOT_FOUND, paramID, false); |
return false; |
} |
pFormat = paramEditFormat; // Zeiger auf das erste Zeichen vom Format-String (z.B. "DCSWFHQ") |
*v = paramGet( paramID ); // der aktuelle Wert des paraID |
while( true ) |
{ |
//------ |
// suche editGenericCode mittels aktuelles Zeichens *pFormat |
//------ |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//------ |
// FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//------ |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E32_CODECHAR_NOT_FOUND, *pFormat, false); |
return false; |
} |
//------ |
// suche den Value-Bereich (min/max) fuer das Code-Zeichen |
//------ |
if( editGenericCode[codeIdx].code == 'v' ) |
{ |
// bei code 'v' kommt min/max aus paramEditItem |
*min = paramEditItem.min; |
*max = paramEditItem.max; |
} |
else |
{ |
// ansonsten kommt min/max aus dem Code selber |
*min = editGenericCode[codeIdx].min; |
*max = editGenericCode[codeIdx].max; |
} |
// value gefunden (innerhalb von min/max)? |
// beruecksichtigt 'gedrehte' min/max (z.B. Poti1..8) |
if( (*min <= *max && *v >= *min && *v <= *max) || (*min > *max && *v >= *max && *v <= *min) ) |
{ |
// gefunden - Schleife verlassen |
break; |
} |
//------ |
// naechstes Code-Zeichen aus der Format-Maske |
//------ |
pFormat++; |
//------ |
// FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
//------ |
if( *pFormat == 0 ) // Ende des Format-Strings erreicht - Fehler... |
{ |
_error( E33_VALUE_NOT_IN_FORMAT, *v, false); |
return false; |
} |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
genericCode.min = *min; // ggf. min/max ueberschreiben durch min/max von paramEditItem |
genericCode.max = *max; |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * get_genericCodeText( uint8_t cmd ) |
{ |
if( cmd == MKPARAM_LONGVALUE ) |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.longText_de; |
else return genericCode.longText_en; |
} |
else |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.shortText_de; |
else return genericCode.shortText_en; |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// ok = strValueGeneric( paramID, cmd ) |
// |
// erstellt die String Representation des Wertes von paramID |
// im Buffer mkparam_strValueBuffer. |
// |
// Mit 'cmd' wird gesteuert ob es der Lang-Text oder der |
// Kurztext (3 Zeichen) ist. |
// |
// PARAMETER: |
// paramID: |
// cmd: MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
uint8_t strValueGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
unsigned char min; |
unsigned char max; |
if( find_genericCodeByValue( paramID, &v, &min, &max) ) |
{ |
//------------------- |
// value gefunden! |
//------------------- |
if( genericCode.code == 'v' ) // Code 'v' (Value) - keine Umrechnung von min/max |
{ |
if( max < min ) |
{ |
// max < min bei 'v' (Value) wird aktuell nicht unterstuezt da |
// kein Beispiel vorhanden ist was damit ggf. gemeint ist. |
// Wenn das irgendwann benoetigt wird muss das hier gecoded werden. |
// Bis dahin wird das als Fehler angesehen (ggf. vertippt beim coden). |
_error( E41_VALUE_MIN_MAX_RANGE, v, false); |
return false; |
} |
} |
else // alles andere ausser 'v' - der Wert wird in die Range von min/max umgerechnet |
{ |
if( min <= max ) |
v = v-min; |
else |
v = min-v; // OG Notiz: v = 248 = 8 # 248 = 8, 255 = 1 # min 255 max = 248 |
v++; // fuer Anzeige +1: statt '0' eine '1' anzeigen - Beispiel: statt "Poti0" zeige "Poti1" |
} |
//------------------- |
// ertstelle String |
//------------------- |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, get_genericCodeText(cmd), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// ok = getGenericNextValue( paramID, vInc ) |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t getGenericNextValue( unsigned char paramID, int8_t vInc ) |
{ |
int v; // value von paramID - signed int: damit -1 und >255 ausgewertet werden kann |
unsigned char value; // zum holen von v via find_genericCodeByValue() |
unsigned char min; |
unsigned char max; |
unsigned char codeIdx; |
char *pFormat; |
if( find_genericCodeByValue( paramID, &value, &min, &max) ) |
{ |
v = (int)value; // value vom paramID wird gecastet auf int um -1 und >255 fuer edit zu erkennen |
//------------------------------------------------------ |
// Pointer auf das Zeichen im Format-String ermitteln |
//------------------------------------------------------ |
pFormat = paramEditFormat; |
while( *pFormat != genericCode.code ) pFormat++; |
if( min > max ) // 'umgedrehte' min/max Werte wie bei Poti1..8 |
vInc = vInc * -1; |
v = v + vInc; |
//------------------------------------------------------ |
// min/max Grenzen des Code-Zeichens ueberschritten? |
//------------------------------------------------------ |
if( (min <= max && v < min) || (min > max && v > min) ) |
{ |
if( pFormat == paramEditFormat ) // Anfang vom Code-String? -> dann gehe zum Ende (letztes Zeichen) |
{ |
while( *(pFormat+1) ) pFormat++; |
} |
else |
{ |
pFormat--; // vorheriges Format-Zeichen = vorheriger Code |
} |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.max; |
else |
v = editGenericCode[codeIdx].max; |
} |
else if( (min <= max && v > max) || (min > max && v < max) ) |
{ |
pFormat++; // naechstes Format-Zeichen = naechster Code |
if( *pFormat == 0 ) // Ende des Format-Strings? |
pFormat = paramEditFormat; |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.min; |
else |
v = editGenericCode[codeIdx].min; |
} |
//------------------------------------------------------ |
// neuen Wert setzen |
//------------------------------------------------------ |
paramSet( paramID, (unsigned char)v ); |
return true; |
} // end: if( find_genericCode( paramID, &v, &min, &max) ) |
return false; |
} |
//-------------------------------------------------------------- |
// editGeneric( paramID, cmd ) |
// |
// PARAMETER: |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v_org; |
uint8_t redraw; |
int8_t vInc; |
if( cmd != MKPARAM_EDIT ) |
{ |
// wenn cmd = MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
strValueGeneric( paramID, cmd); // Ergebnis in: mkparam_strValueBuffer |
return; |
} |
find_paramEditItem( paramID ); |
//------------------------ |
// editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//------------------------ |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E11_PARAMID_EDITGENERIC, paramID, true); |
return; |
} |
lcd_cls(); |
v_org = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
vInc = 0; // 0, +1, -1 (inc/dec) |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
strValueGeneric( paramID, MKPARAM_LONGVALUE); // Lang-Text des Values anzeigen |
// nach strValueGeneric() ist auch genericCode initialisiert! |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %18s"), mkparam_strValueBuffer ); |
// DEBUG... |
#ifdef DEBUG_PARAMEDIT |
//lcd_printf_at_P( 16, 1, MNORMAL, PSTR("%c %3u"), genericCode.code, paramGet(paramID) ); |
#endif |
//lcd_printf_at_P( 16, 4, MNORMAL, PSTR("= %03u"), v ); |
if( abs(genericCode.max-genericCode.min) <= 20 ) |
_delay_ms( 200 ); // Verzoegerung |
vInc = 0; |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_press(1 << KEY_ESC) ) // Key: Cancel |
{ |
paramSet( paramID, v_org ); // org. Wert wieder herstellen |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // Key: OK |
{ |
break; |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) // Key: rechts / +1 |
{ |
vInc = +1; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) // Key: links / -1 |
{ |
vInc = -1; |
} |
if( vInc != 0 ) |
{ |
getGenericNextValue( paramID, vInc ); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
// editBitmask( paramID, cmd ) |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editBitmask( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
uint8_t redraw; |
uint8_t i; |
int8_t pos; |
find_paramEditItem( paramID ); |
//----------------------- |
// editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
v = paramGet( paramID ); |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%03u"), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
lcd_cls(); |
v = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
pos = 0; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 1, 7, PSTR("0/1 \x19 OK"), MNORMAL); // Keyline: -> 0/1 OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
lcdx_printp_at( 7, 6, PSTR("\x18"), MNORMAL, -2,2); // Keyline langer Tastendruck: -> 0/1 OK |
if( (paramID == param_J16Bitmask) || (paramID == param_J17Bitmask) ) |
{ |
lcdx_printp_at( 10, 5, PSTR("(Bit1=Idle)"), MNORMAL, 0,2); // |
} |
print_paramEditItemTitle(); // Bezeichnung des paramID's (Stelle x=0, y=2) |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
for(i = 0; i < 8; i++) |
{ |
lcd_putc( 8-i, 4, ( (v & (1 << i)) ? '1' : '0'), MNORMAL); |
} |
lcd_frect( 0, (8*5), 125-(11*6), 7, 0); // clear: Eingabemarkierung |
lcd_printp_at (pos+1, 5,PSTR("\x12"), MNORMAL); // Eingabemarkierung (Pfeil nach oben) |
lcd_printf_at_P( 15, 4, MNORMAL, PSTR("= %03u"), v ); // Anzeige des aktuellen Wertes Dezimal |
// _delay_ms( 200 ); // Verzoegerung |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if (get_key_short(1 << KEY_ENTER)) // Key: OK |
{ |
paramSet( paramID, v ); // Wert speichern |
break; // und verlassen |
} |
if( get_key_short(1 << KEY_PLUS) ) |
{ |
if(pos == 7) pos = 0; |
else pos++; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if(pos == 0) pos = 7; |
else pos--; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short (1 << KEY_MINUS) ) |
{ |
v ^= (1<<(7-pos)); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editCompassOffset( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
int v; // signed |
uint8_t redraw; |
uint8_t bit7; |
uint8_t bit8; |
uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editCompassOffset() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// Wert ermitteln (-60 bis 60) |
//----------------------- |
vu = paramGet( paramID ); |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
bit8 = ((vu & 0x80) ? true : false); // Bit 8 in Kombination mit Bit 7 = "Disable dec. Calc" |
// Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// Bit 8 != Bit 7: "Disable dec. Calc" AN |
lDisDeclCalc = ((bit8 == bit7) ? 0 : 1); // merken um das spaeter wieder in den Wert einzubauen |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
if( bit7 ) // Wert negativ? |
{ |
vu = vu - 1; // Umrechnung: 7-Bit Zweierkomplement |
vu = (vu ^ 0xff); // invertieren |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
v = (int)vu; |
v = v * -1; |
} |
else // Wert ist positiv |
{ |
v = (int)vu; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%3d"), v ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %3d"), v ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
//------------------------------------- |
// signed int in unsigned byte wandeln |
//------------------------------------- |
if( v < 0 ) // Wert negativ? |
{ |
// Umwandeln in 7-Bit Zweierkomplement |
v = v * -1; // Wert positiv machen |
vu = (unsigned char)v; // in unsigned Byte speichern |
vu = (vu ^ 0xff); // invertieren |
vu = vu + 1; // +1 |
} |
else // Wert ist positiv |
{ |
vu = (unsigned char)v; // in unsigned Byte speichern |
} |
//------------------------------------- |
// "Disable declination calc" wieder einrechnen |
//------------------------------------- |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
if( v <= -60 ) v = 60; |
else v = v - 1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
if( v >= 60 ) v = -60; |
else v = v + 1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
// int v; // signed |
uint8_t redraw; |
// uint8_t bit7; |
// uint8_t bit8; |
// uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editACCZLandingPulse() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
vu = paramGet( paramID ); |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%4d"), vu*4 ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %4d"), vu *4 ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
// //------------------------------------- |
// // signed int in unsigned byte wandeln |
// //------------------------------------- |
// if( v < 0 ) // Wert negativ? |
// { |
// // Umwandeln in 7-Bit Zweierkomplement |
// v = v * -1; // Wert positiv machen |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// vu = (vu ^ 0xff); // invertieren |
// vu = vu + 1; // +1 |
// } |
// else // Wert ist positiv |
// { |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// } |
// |
// //------------------------------------- |
// // "Disable declination calc" wieder einrechnen |
// //------------------------------------- |
// bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
// |
// if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
// else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// |
// if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
// else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
// if( v <= -60 ) v = 60; |
// else v = v - 1; |
// |
if( vu < 191 ) vu = 190; |
else vu = vu -1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
// if( v >= 60 ) v = -60; |
// else v = v + 1; |
if( vu >= 255 ) vu = 255; |
// if( vu == 1 ) vu = 191; |
else vu = vu +1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// editNA( paramID, cmd ) |
// |
// Hilfsfunktion die nur "nicht verfügbar" anzeigt wenn die |
// paramID noch nicht implementiert ist |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editNA( unsigned char paramID, uint8_t cmd ) |
{ |
if( cmd != MKPARAM_EDIT ) |
{ |
strncpy_P( mkparam_strValueBuffer, PSTR("NA!"), MKPARAM_STRBUFFER_LEN); // "NA!" bzw, "not available" |
return; |
} |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 2000, true, true ); // "nicht verfügbar" |
} |
//############################################################################################# |
//# TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editGenericTEST( unsigned char paramID ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
find_paramEditItem( paramID ); // Ergebnis in paramEditItem (RAM) |
if( paramEditItem.paramID == paramID ) |
{ |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("%S"), paramEditItem.title_de ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("format: %s"), paramEditFormat ); |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("value:%u"), paramGet(paramID) ); |
strValueGeneric( paramID, MKPARAM_LONGVALUE); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
strValueGeneric( paramID, MKPARAM_SHORTVALUE); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
} |
if( paramEditItem.paramID == param_EOF ) |
{ |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("! ERROR !") ); |
} |
//----------------------------------------- |
// Tasten... |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void TEST(void) |
{ |
//editGeneric( param_Gyro_Gier_P, MKPARAM_EDIT ); |
editGeneric( param_Kanalbelegung_Gas, MKPARAM_EDIT ); |
} |
//############################################################################################# |
//# Menues & Favoriten |
//############################################################################################# |
//-------------------------------------------------------------- |
// fav_add() |
// |
// einen Favoriten hinzufuegen |
// |
// Aufruf durch: wird durch den Menu-Controller aufgerufen |
//-------------------------------------------------------------- |
void fav_add( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//------------------------------ |
// welche paramID wurde gewaehlt? |
//------------------------------ |
paramID = MenuCtrl_GetItemId(); // gewaehlter Menuepunkt bzw. paramID (0 = keine itemID) |
//------------------------------ |
// ungueltige paramID? |
//------------------------------ |
if( paramID==0 || paramID==EOF ) |
{ |
return; // keine gueltige paramID gewaehlt -> ZURUECK |
} |
//------------------------------ |
// Duplikatsuche bzgl. paramID |
// (gilt nicht fuer SEPARATOR) |
//------------------------------ |
if( paramID!=SEPARATOR ) |
{ |
//------------------------------ |
// suche Duplikate fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=paramID); i++); |
//------------------------------ |
// Favoriten bereits vorhanden? |
// -> exit |
//------------------------------ |
if( Config.MKParam_Favs[i] == paramID ) |
{ |
set_beep( 300, 0x000f, BeepNormal); // Error Beep kurz |
PKT_Popup_P( 400, strGet(STR_FAV_EXIST),0,0,0); // "* Fav vorhanden *" (ca. 4 sec max.) |
return; |
} |
} // end: if( paramID!=SEPARATOR ) |
//------------------------------ |
// suche freien Speicherplatz fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0); i++); |
//------------------------------ |
// Favoriten voll? |
// -> exit |
//------------------------------ |
if( i >= MAX_MKPARAM_FAVORITES ) |
{ |
set_beep( 500, 0x000f, BeepNormal); // Error Beep lang |
PKT_Popup_P( 400, strGet(STR_FAV_FULL),0,0,0); // "* Fav ist voll *" (ca. 4 sec max.) |
return; |
} |
//------------------------------ |
// neuen Fav merken |
//------------------------------ |
Config.MKParam_Favs[i] = paramID; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
PKT_Popup_P( 400, strGet(STR_FAV_ADD),0,0,0); // "Fav hinzugefügt!" (ca. 4 sec max.) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Favoriten( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
if( Config.MKParam_Favs[0] == 0 ) // Favoriten vorhanden |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_FAV_NOTEXIST), false, 500, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
MenuCtrl_SetDelete( true ); // Menueeintraege loeschen (langer Druck auf "OK") |
MenuCtrl_SetMove( true ); // Menueeintraege verschieben (langer Druck auf hoch/runter) |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menu-Items |
//--------------- |
i = 0; |
while( (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0) ) |
{ |
paramID = Config.MKParam_Favs[i]; |
if( paramID == SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
} |
else |
{ |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
MenuCtrl_PushParamEdit( paramID ); |
// existiert die paramID in der Firmware des Kopters? |
if( !paramExist(paramID) ) |
{ |
MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} |
// DEBUG! (Simulation eines deaktivierten Fav's) |
// if( i==2 ) MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} // end: else: if( paramID == SEPARATOR ) |
i++; |
} // end: while(..) |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // KEY_ESC = Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// ggf. neue Menuereihenfolge |
// in der Config speichern |
//--------------- |
for( i=0; (i<MAX_MKPARAM_FAVORITES); i++) |
{ |
Config.MKParam_Favs[i] = MenuCtrl_GetItemIdByIndex( i ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_EditCategory( const unsigned char *menuitems ) |
{ |
unsigned char paramID; |
unsigned char lastmenuitem = 0; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
MenuCtrl_SetKey( KEY_ENTER_LONG, 0, &fav_add ); |
//--------------- |
// Menuitems |
//--------------- |
while( *menuitems != EOF ) |
{ |
paramID = *menuitems; |
if( paramID == SEPARATOR ) |
{ |
// wenn der vorherige Menueeintrag bereits ein SEPARATOR war |
// dann nicht erneut einen Separator anzeigen! |
// |
// Das kann ggf. passieren wenn z.B. aufgrund der FC-Revision |
// alle Zwischeneintraege ausgeblendet wurden |
if( lastmenuitem != SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
lastmenuitem = paramID; |
} |
} |
else |
{ |
//------------------------------------------------------ |
// TEST / DEBUG fuer beschriebenes Problem in _error() |
// |
// Wenn man das dortige Problem nachstellen will kann |
// man hier einen Fehlerscreen provozieren! |
//------------------------------------------------------ |
//paramID = 0; |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
// existiert die paramID in der Firmware des Kopters? |
if( paramExist(paramEditItem.paramID) ) |
{ |
MenuCtrl_PushParamEdit( paramEditItem.paramID ); |
lastmenuitem = paramID; |
} |
} |
menuitems++; |
} |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Parameters_MenuMain( uint8_t setting, char *settingname ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle( settingname ); |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
//MenuCtrl_PushML2_P( ID_MENU_TEST , MENU_ITEM, &TEST, ID_MENU_TEST_de , ID_MENU_TEST_en ); |
MenuCtrl_PushML2_P( ID_MENU_FAVORITEN , MENU_ITEM, NOFUNC, ID_MENU_FAVORITEN_de , ID_MENU_FAVORITEN_en ); // Favoriten: noch nicht implementiert... |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_MENU_KANAELE , MENU_ITEM, NOFUNC, ID_MENU_KANAELE_de , ID_MENU_KANAELE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KONFIGURATION, MENU_ITEM, NOFUNC, ID_MENU_KONFIGURATION_de, ID_MENU_KONFIGURATION_en ); |
MenuCtrl_PushML2_P( ID_MENU_STICK , MENU_ITEM, NOFUNC, ID_MENU_STICK_de , ID_MENU_STICK_en ); |
MenuCtrl_PushML2_P( ID_MENU_LOOPING , MENU_ITEM, NOFUNC, ID_MENU_LOOPING_de , ID_MENU_LOOPING_en ); |
MenuCtrl_PushML2_P( ID_MENU_HOEHE , MENU_ITEM, NOFUNC, ID_MENU_HOEHE_de , ID_MENU_HOEHE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KAMERA , MENU_ITEM, NOFUNC, ID_MENU_KAMERA_de , ID_MENU_KAMERA_en ); |
MenuCtrl_PushML2_P( ID_MENU_NAVICTRL , MENU_ITEM, NOFUNC, ID_MENU_NAVICTRL_de , ID_MENU_NAVICTRL_en ); |
MenuCtrl_PushML2_P( ID_MENU_AUSGAENGE , MENU_ITEM, NOFUNC, ID_MENU_AUSGAENGE_de , ID_MENU_AUSGAENGE_en ); |
MenuCtrl_PushML2_P( ID_MENU_VERSCHIEDENES, MENU_ITEM, NOFUNC, ID_MENU_VERSCHIEDENES_de, ID_MENU_VERSCHIEDENES_en ); |
MenuCtrl_PushML2_P( ID_MENU_GYRO , MENU_ITEM, NOFUNC, ID_MENU_GYRO_de , ID_MENU_GYRO_en ); |
MenuCtrl_PushML2_P( ID_MENU_BENUTZER , MENU_ITEM, NOFUNC, ID_MENU_BENUTZER_de , ID_MENU_BENUTZER_en ); |
MenuCtrl_PushML2_P( ID_MENU_ACHSKOPPLUNG , MENU_ITEM, NOFUNC, ID_MENU_ACHSKOPPLUNG_de , ID_MENU_ACHSKOPPLUNG_en ); |
//MenuCtrl_PushML2_P( ID_MENU_MIXERSETUP , MENU_ITEM, NOFUNC, ID_MENU_MIXERSETUP_de , ID_MENU_MIXERSETUP_en ); // nicht mehr unterstuetzt! |
MenuCtrl_PushML2_P( ID_MENU_EASYSETUP , MENU_ITEM, NOFUNC, ID_MENU_EASYSETUP_de , ID_MENU_EASYSETUP_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
switch( itemid ) |
{ |
case ID_MENU_FAVORITEN: Menu_Favoriten(); break; |
case ID_MENU_KANAELE: Menu_EditCategory( ID_MENU_KANAELE_Items ); break; |
case ID_MENU_KONFIGURATION: Menu_EditCategory( ID_MENU_KONFIGURATION_Items ); break; |
case ID_MENU_STICK: Menu_EditCategory( ID_MENU_STICK_Items ); break; |
case ID_MENU_LOOPING: Menu_EditCategory( ID_MENU_LOOPING_Items ); break; |
case ID_MENU_HOEHE: Menu_EditCategory( ID_MENU_HOEHE_Items ); break; |
case ID_MENU_KAMERA: Menu_EditCategory( ID_MENU_KAMERA_Items ); break; |
case ID_MENU_NAVICTRL: Menu_EditCategory( ID_MENU_NAVICTRL_Items ); break; |
case ID_MENU_AUSGAENGE: Menu_EditCategory( ID_MENU_AUSGAENGE_Items ); break; |
case ID_MENU_VERSCHIEDENES: Menu_EditCategory( ID_MENU_VERSCHIEDENES_Items ); break; |
case ID_MENU_GYRO: Menu_EditCategory( ID_MENU_GYRO_Items ); break; |
case ID_MENU_BENUTZER: Menu_EditCategory( ID_MENU_BENUTZER_Items ); break; |
case ID_MENU_ACHSKOPPLUNG: Menu_EditCategory( ID_MENU_ACHSKOPPLUNG_Items ); break; |
case ID_MENU_EASYSETUP: Menu_EditCategory( ID_MENU_EASYSETUP_Items ); break; |
default: //PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 200, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
break; |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# MK_Parameters() - Main-Function |
//############################################################################################# |
//-------------------------------------------------------------- |
// changed = MK_Parameters( setting, settingname ) |
//-------------------------------------------------------------- |
uint8_t MK_Parameters( uint8_t setting, char *settingname ) |
{ |
int changed; |
unsigned char *org_parameters; |
uint8_t size = paramsetSize(); |
//----------------------------------------------------------------- |
// Erkennung ob Aenderungen durch den Benutzer vorgenommen wurde |
// -> das aktuelle Paramset wird gespeichert um es spaeter mit |
// der bearbeiteten Version via memcmp zu vergleichen |
//----------------------------------------------------------------- |
org_parameters = malloc( size+1 ); // +1 fuer das erste settings-byte |
if( !org_parameters ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return false; |
} |
memcpy( org_parameters, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
//----------------------------------------------------------------- |
// MK-Parameter bearbeiten |
//----------------------------------------------------------------- |
MK_Parameters_MenuMain( setting, settingname ); |
//----------------------------------------------------------------- |
// Vergleich: Orginal-Daten <-> ggf. geaenderte Daten |
//----------------------------------------------------------------- |
changed = memcmp( org_parameters, (unsigned char *)(pRxData), size+1 ); |
free( org_parameters ); |
return( changed!=0 ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/mkparameters.h |
---|
0,0 → 1,95 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.h |
//# |
//# 27.03.2014 OG |
//# etliche neue Funktionen / Strukturen |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
// fuer cmd-Parameter der Edit-Funktionen |
#define MKPARAM_EDIT 1 // Wert bearbeiten |
#define MKPARAM_SHORTVALUE 2 // erzeugt einen 3-Zeichen String der den Wert darstellt (z.B. "Dis"); Ergebnis in strValueBuffer |
#define MKPARAM_LONGVALUE 3 // erzeugt einen String der den Wert in langer Form darstellt (z.B. "Disabled"); Ergebnis in strValueBuffer |
#define MKPARAM_STRBUFFER_LEN 20 |
//-------------------------------------------- |
// paramEditItem_t |
// deklariert die Edit-Daten einer paramID |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
extern paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
extern char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
extern char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ); |
void editGeneric( unsigned char paramID, uint8_t cmd ); |
void editBitmask( unsigned char paramID, uint8_t cmd ); |
void editDisableDeclCalc( unsigned char paramID, uint8_t cmd ); |
void editCompassOffset( unsigned char paramID, uint8_t cmd ); |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ); |
void editNA( unsigned char paramID, uint8_t cmd ); |
uint8_t MK_Parameters( uint8_t setting, char *settingname ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/mkparameters_messages.h |
---|
0,0 → 1,618 |
/***************************************************************************** |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters_messages.h |
//# |
//# 16.07.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.11a inkl. der ID's |
//# const char param_singleWpControlChannel_de |
//# const char param_MenuKeyChannel_de |
//# |
//# 09.04.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.09j |
//# const char param_ServoNickFails_de[],const char param_ServoRollFails_de[], const char param_Servo3Fails_de[], |
//# const char param_Servo4Fails_de[], const char param_Servo5Fails_de[] |
//# |
//# 26.09.2014 Cebra |
//# - add: Text Höhe Tilt Compensation (FC207f) |
//# |
//# 04.06.2014 OG |
//# - chg: Text von ID_MENU_FAVORITEN etwas gekuerzt |
//# |
//# 07.05.2014 OG |
//# - chg: ID_MENU_FAVORITEN_en |
//# |
//# 18.04.2014 OG |
//# - chg: Textanpassung: param_NaviGpsMinSat_de, param_NaviStickThreshold_de |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Textanpassung: param_WARN_J16_Bitmask_de, param_WARN_J17_Bitmask_de |
//# - chg: Textanpassung: param_J16Bitmask, param_J16Timing |
//# - chg: Textanpassung: param_J17Bitmask, param_J17Timing |
//# - chg: Textanpassung: param_SingleWpSpeed_de |
//# - chg: Textanpassung: param_ServoNickRefresh_de, param_ServoManualControlSpeed_de |
//# - chg: Textanpassung: param_ServoRollControl_de |
//# |
//# 07.04.2014 OG |
//# - chg: kleine Aenderungen an englischen Texten |
//# |
//# 06.04.2014 CB |
//# - add: englische Menütexte ergänzt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# |
//# 28.03.2014 OG |
//# - add: paramID-Texte fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 16.03.2014 OG |
//# erste groesstenteils fertige Version |
//# |
//# 23.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_MESSAGES_H |
#define _MKPARAMETERS_MESSAGES_H |
//############################################################################################# |
//# Texte: Menues von mkparameters |
//############################################################################################# |
static const char ID_MENU_TEST_de[] PROGMEM = "TEST/DEBUG"; |
#define ID_MENU_TEST_en ID_MENU_TEST_de |
static const char ID_MENU_FAVORITEN_de[] PROGMEM = "Favoriten"; |
static const char ID_MENU_FAVORITEN_en[] PROGMEM = "Favorites"; |
static const char ID_MENU_KANAELE_de[] PROGMEM = "Kanäle"; |
static const char ID_MENU_KANAELE_en[] PROGMEM = "Channel"; |
static const char ID_MENU_KONFIGURATION_de[] PROGMEM = "Konfiguration"; |
static const char ID_MENU_KONFIGURATION_en[] PROGMEM = "Configuration"; |
static const char ID_MENU_STICK_de[] PROGMEM = "Stick"; |
#define ID_MENU_STICK_en ID_MENU_STICK_de |
static const char ID_MENU_LOOPING_de[] PROGMEM = "Looping"; |
#define ID_MENU_LOOPING_en ID_MENU_LOOPING_de |
static const char ID_MENU_HOEHE_de[] PROGMEM = "Höhe"; |
static const char ID_MENU_HOEHE_en[] PROGMEM = "Altitude"; |
static const char ID_MENU_KAMERA_de[] PROGMEM = "Kamera"; |
static const char ID_MENU_KAMERA_en[] PROGMEM = "Camera"; |
static const char ID_MENU_NAVICTRL_de[] PROGMEM = "Navi-Ctrl"; |
#define ID_MENU_NAVICTRL_en ID_MENU_NAVICTRL_de |
static const char ID_MENU_AUSGAENGE_de[] PROGMEM = "Ausgänge"; |
static const char ID_MENU_AUSGAENGE_en[] PROGMEM = "Outputs"; |
static const char ID_MENU_VERSCHIEDENES_de[] PROGMEM = "Verschiedenes"; |
static const char ID_MENU_VERSCHIEDENES_en[] PROGMEM = "Miscellaneous"; |
static const char ID_MENU_GYRO_de[] PROGMEM = "Gyro"; |
#define ID_MENU_GYRO_en ID_MENU_GYRO_de |
static const char ID_MENU_BENUTZER_de[] PROGMEM = "Benutzer"; |
static const char ID_MENU_BENUTZER_en[] PROGMEM = "User"; |
static const char ID_MENU_ACHSKOPPLUNG_de[] PROGMEM = "Achskopplung"; |
static const char ID_MENU_ACHSKOPPLUNG_en[] PROGMEM = "Coupl Axes"; |
/* |
static const char ID_MENU_MIXERSETUP_de[] PROGMEM = "Mixer-Setup"; |
static const char ID_MENU_MIXERSETUP_en[] PROGMEM = "Config Mix"; |
*/ |
static const char ID_MENU_EASYSETUP_de[] PROGMEM = "Easy Setup"; |
#define ID_MENU_EASYSETUP_en ID_MENU_EASYSETUP_de |
//############################################################################################# |
//# Bezeichnungen der paramID's |
//############################################################################################# |
// Hier ist erstmal alles deklariert was auch in paramset.h vorhanden ist. |
// Wenn der Compiler richtig eingestellt ist, dann wird alles nicht benoetigte wegoptimiert! |
static const char param_Hoehe_MinGas_de[] PROGMEM = "Min. Gas"; |
static const char param_Hoehe_MinGas_en[] PROGMEM = "min. throttle"; |
static const char param_Luftdruck_D_de[] PROGMEM = "Luftdruck D"; |
static const char param_Luftdruck_D_en[] PROGMEM = "barometric D"; |
static const char param_HoeheChannel_de[] PROGMEM = "Höhe Sollwert"; |
static const char param_HoeheChannel_en[] PROGMEM = "hight setpoint"; |
static const char param_Hoehe_P_de[] PROGMEM = "Höhe P"; |
static const char param_Hoehe_P_en[] PROGMEM = "altitude P"; |
static const char param_Hoehe_Verstaerkung_de[] PROGMEM = "Verstärk./Rate"; |
static const char param_Hoehe_Verstaerkung_en[] PROGMEM = "gain rate"; |
static const char param_Hoehe_ACC_Wirkung_de[] PROGMEM = "Z-ACC"; |
#define param_Hoehe_ACC_Wirkung_en param_Hoehe_ACC_Wirkung_de |
static const char param_Hoehe_HoverBand_de[] PROGMEM = "Schwebe-Gas"; |
static const char param_Hoehe_HoverBand_en[] PROGMEM = "hoover throttle"; |
static const char param_Hoehe_GPS_Z_de[] PROGMEM = "GPS Z"; |
#define param_Hoehe_GPS_Z_en param_Hoehe_GPS_Z_de |
static const char param_Hoehe_Tilt_Comp_de[] PROGMEM = "Tilt Compensation"; |
#define param_Hoehe_Tilt_Comp_en param_Hoehe_Tilt_Comp_de |
static const char param_Hoehe_StickNeutralPoint_de[] PROGMEM = "Stick Neutr.Punkt"; |
static const char param_Hoehe_StickNeutralPoint_en[] PROGMEM = "stick neutr.point"; |
static const char param_Stick_P_de[] PROGMEM = "Nick/Roll P"; |
static const char param_Stick_D_de[] PROGMEM = "Nick/Roll D"; |
static const char param_StickGier_P_de[] PROGMEM = "Gier P"; |
#define param_Stick_P_en param_Stick_P_de |
#define param_Stick_D_en param_Stick_D_de |
#define param_StickGier_P_en param_StickGier_P_de |
static const char param_Gas_Min_de[] PROGMEM = "Min. Gas"; |
static const char param_Gas_Min_en[] PROGMEM = "min. throttle"; |
static const char param_Gas_Max_de[] PROGMEM = "Max. Gas"; |
static const char param_Gas_Max_en[] PROGMEM = "max. throttle"; |
static const char param_GyroAccFaktor_de[] PROGMEM = "ACC/Gyro Faktor"; |
#define param_GyroAccFaktor_en param_GyroAccFaktor_de |
static const char param_KompassWirkung_de[] PROGMEM = "Kompasswirkung"; |
static const char param_KompassWirkung_en[] PROGMEM = "compass effect"; |
static const char param_Gyro_P_de[] PROGMEM = "Gyro P"; |
static const char param_Gyro_I_de[] PROGMEM = "Gyro I"; |
static const char param_Gyro_D_de[] PROGMEM = "Gyro D"; |
static const char param_Gyro_Gier_P_de[] PROGMEM = "Gier P"; |
static const char param_Gyro_Gier_I_de[] PROGMEM = "Gier I"; |
static const char param_Gyro_Stability_de[] PROGMEM = "Gyro stab."; |
#define param_Gyro_P_en param_Gyro_P_de |
#define param_Gyro_I_en param_Gyro_I_de |
#define param_Gyro_D_en param_Gyro_D_de |
#define param_Gyro_Gier_P_en param_Gyro_Gier_P_de |
#define param_Gyro_Gier_I_en param_Gyro_Gier_I_de |
#define param_Gyro_Stability_en param_Gyro_Stability_de |
static const char param_UnterspannungsWarnung_de[] PROGMEM = "Unterspannung [0.1V]"; |
static const char param_UnterspannungsWarnung_en[] PROGMEM = "undervoltage [0.1V]"; |
static const char param_NotGas_de[] PROGMEM = "NOT-Gas"; |
static const char param_NotGas_en[] PROGMEM = "Emerg.Throttle"; |
static const char param_NotGasZeit_de[] PROGMEM = "NOT-Gas Zeit [0.1s]"; |
static const char param_NotGasZeit_en[] PROGMEM = "Emerg.Thro.Time[0.1s]"; |
static const char param_Receiver_de[] PROGMEM = "Receiver"; |
#define param_Receiver_en param_Receiver_de |
static const char param_I_Faktor_de[] PROGMEM = "Hauptregler I"; |
static const char param_I_Faktor_en[] PROGMEM = "main I"; |
static const char param_UserParam1_de[] PROGMEM = "Parameter 1"; |
static const char param_UserParam2_de[] PROGMEM = "Parameter 2"; |
static const char param_UserParam3_de[] PROGMEM = "Parameter 3"; |
static const char param_UserParam4_de[] PROGMEM = "Parameter 4"; |
#define param_UserParam1_en param_UserParam1_de |
#define param_UserParam2_en param_UserParam2_de |
#define param_UserParam3_en param_UserParam3_de |
#define param_UserParam4_en param_UserParam4_de |
static const char param_ServoNickControl_de[] PROGMEM = "Nick Ansteuerung"; |
static const char param_ServoNickControl_en[] PROGMEM = "nick servo ctrl"; |
static const char param_ServoNickComp_de[] PROGMEM = "Nick Kompensation"; |
static const char param_ServoNickComp_en[] PROGMEM = "nick servo comp."; |
static const char param_ServoNickFails_de[] PROGMEM = "Nick Failsave Wert"; |
static const char param_ServoNickFails_en[] PROGMEM = "nick failsave value"; |
static const char param_ServoNickMin_de[] PROGMEM = "Nick Servo Minimum"; |
static const char param_ServoNickMax_de[] PROGMEM = "Nick Servo Maximum"; |
#define param_ServoNickMin_en param_ServoNickMin_de |
#define param_ServoNickMax_en param_ServoNickMax_de |
static const char param_ServoRollControl_de[] PROGMEM = "Roll Ansteuerung"; |
static const char param_ServoRollControl_en[] PROGMEM = "roll servo ctrl."; |
static const char param_ServoRollComp_de[] PROGMEM = "Roll Kompensation"; |
static const char param_ServoRollComp_en[] PROGMEM = "roll compensation"; |
static const char param_ServoRollFails_de[] PROGMEM = "Roll Failsave Wert"; |
static const char param_ServoRollFails_en[] PROGMEM = "roll failsave value"; |
static const char param_ServoRollMin_de[] PROGMEM = "Roll Minimum"; |
static const char param_ServoRollMax_de[] PROGMEM = "Roll Maximum"; |
#define param_ServoRollMin_en param_ServoRollMin_de |
#define param_ServoRollMax_en param_ServoRollMax_de |
//static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschw."; // ALT |
static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschwindigkeit"; |
static const char param_ServoNickRefresh_en[] PROGMEM = "servo refresh"; |
static const char param_ServoManualControlSpeed_de[] PROGMEM = "Manu. Geschwindigkeit."; |
static const char param_ServoManualControlSpeed_en[] PROGMEM = "manuell Speed"; |
static const char param_CamOrientation_de[] PROGMEM = "Cam Richtung"; |
static const char param_CamOrientation_en[] PROGMEM = "cam orientation"; |
static const char param_Servo3_de[] PROGMEM = "Servo 3"; |
#define param_Servo3_en param_Servo3_de |
static const char param_Servo3Fails_de[] PROGMEM = "Servo 3 Failsave Wert"; |
static const char param_Servo3Fails_en[] PROGMEM = "Servo 3 failsave value"; |
static const char param_Servo4_de[] PROGMEM = "Servo 4"; |
#define param_Servo4_en param_Servo4_de |
static const char param_Servo4Fails_de[] PROGMEM = "Servo 4 Failsave Wert"; |
static const char param_Servo4Fails_en[] PROGMEM = "Servo 4 failsave value"; |
static const char param_Servo5_de[] PROGMEM = "Servo 5"; |
#define param_Servo5_en param_Servo5_de |
static const char param_Servo5Fails_de[] PROGMEM = "Servo 5 Failsave Wert"; |
static const char param_Servo5Fails_en[] PROGMEM = "Servo 5 failsave value"; |
static const char param_Servo3OnValue_de[] PROGMEM = "Servo 3 On Out1/2"; |
static const char param_Servo3OffValue_de[] PROGMEM = "Servo 3 Off Out1/2"; |
static const char param_Servo4OnValue_de[] PROGMEM = "Servo 4 On Out1/2"; |
static const char param_Servo4OffValue_de[] PROGMEM = "Servo 4 Off Out1/2"; |
#define param_Servo3OnValue_en param_Servo3OnValue_de |
#define param_Servo3OffValue_en param_Servo3OffValue_de |
#define param_Servo4OnValue_en param_Servo4OnValue_de |
#define param_Servo4OffValue_en param_Servo4OffValue_de |
static const char param_LoopGasLimit_de[] PROGMEM = "Looping Gas Limit"; |
static const char param_LoopGasLimit_en[] PROGMEM = "loop throttle limit"; |
static const char param_LoopThreshold_de[] PROGMEM = "Ansprechschwelle"; |
static const char param_LoopThreshold_en[] PROGMEM = "response threshold"; |
static const char param_LoopHysterese_de[] PROGMEM = "Hysterese"; |
static const char param_LoopHysterese_en[] PROGMEM = "hysteresis"; |
static const char param_AchsKopplung1_de[] PROGMEM = "Gier pos. Kopplung"; |
static const char param_AchsKopplung1_en[] PROGMEM = "gier pos. coupling"; |
static const char param_AchsKopplung2_de[] PROGMEM = "Nick/Roll Kopplung"; |
static const char param_AchsKopplung2_en[] PROGMEM = "nick/roll coupling"; |
static const char param_CouplingYawCorrection_de[] PROGMEM = "Gier Korrektur"; |
static const char param_CouplingYawCorrection_en[] PROGMEM = "gier correction"; |
static const char param_WinkelUmschlagNick_de[] PROGMEM = "Nick Umkehrpunkt"; |
static const char param_WinkelUmschlagNick_en[] PROGMEM = "nick turnover"; |
static const char param_WinkelUmschlagRoll_de[] PROGMEM = "Roll Umkehrpunkt"; |
static const char param_WinkelUmschlagRoll_en[] PROGMEM = "roll turnover"; |
static const char param_GyroAccAbgleich_de[] PROGMEM = "ACC/Gyro Komp. [1/x]"; |
static const char param_GyroAccAbgleich_en[] PROGMEM = "ACC/Gyro Comp. [1/x]"; |
static const char param_Driftkomp_de[] PROGMEM = "Drift-Kompensation"; |
static const char param_Driftkomp_en[] PROGMEM = "drift compensation"; |
static const char param_DynamicStability_de[] PROGMEM = "Dynamische Stabilität"; |
static const char param_DynamicStability_en[] PROGMEM = "dynamic stabiliy"; |
static const char param_UserParam5_de[] PROGMEM = "Parameter 5"; |
static const char param_UserParam6_de[] PROGMEM = "Parameter 6"; |
static const char param_UserParam7_de[] PROGMEM = "Parameter 7"; |
static const char param_UserParam8_de[] PROGMEM = "Parameter 8"; |
#define param_UserParam5_en param_UserParam5_de |
#define param_UserParam6_en param_UserParam6_de |
#define param_UserParam7_en param_UserParam7_de |
#define param_UserParam8_en param_UserParam8_de |
static const char param_J16Bitmask_de[] PROGMEM = "Out1: Bitmaske"; |
static const char param_J16Bitmask_en[] PROGMEM = "Out1: bitmask"; |
static const char param_J16Timing_de[] PROGMEM = "Out1: Timing [10ms]"; |
#define param_J16Timing_en param_J16Timing_de |
static const char param_J17Bitmask_de[] PROGMEM = "Out2: Bitmaske"; |
static const char param_J17Bitmask_en[] PROGMEM = "Out2: bitmask"; |
static const char param_J17Timing_de[] PROGMEM = "Out2: Timing [10ms]"; |
#define param_J17Timing_en param_J17Timing_de |
static const char param_WARN_J16_Bitmask_de[] PROGMEM = "Out1 Warn UBat"; |
static const char param_WARN_J16_Bitmask_en[] PROGMEM = "Out1 undervolt warn"; |
static const char param_WARN_J17_Bitmask_de[] PROGMEM = "Out2 Warn UBat"; |
static const char param_WARN_J17_Bitmask_en[] PROGMEM = "Out2 undervolt warn"; |
static const char param_AutoPhotoDistance_de[] PROGMEM = "Auto Trigger Dist.[m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoDistance_en param_AutoPhotoDistance_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_AutoPhotoAtitudes_de[] PROGMEM = "Auto Trigger Alt. [m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoAtitudes_en param_AutoPhotoAtitudes_de // ab Rev. 100 (z.B. FC 2.05e) |
#define param_NaviOut1Parameter_de param_AutoPhotoDistance_de // bis Rev. 98 |
#define param_NaviOut1Parameter_en param_AutoPhotoDistance_de // bis Rev. 98 |
static const char param_SingleWpSpeed_de[] PROGMEM = "SingleWP Speed 0.1m/s"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_SingleWpSpeed_en param_SingleWpSpeed_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_LandingPulse_de[] PROGMEM = "ACC Z Landing Pulse"; // ab Rev. 104 (FC 2.09d) |
#define param_LandingPulse_en param_LandingPulse_de // ab Rev. 104 (FC 2.09d) |
static const char param_NaviGpsModeChannel_de[] PROGMEM = "GPS Modus Steuerung"; |
static const char param_NaviGpsModeChannel_en[] PROGMEM = "GPS mode control"; |
static const char param_NaviGpsGain_de[] PROGMEM = "GPS Verstärkung [%]"; |
static const char param_NaviGpsGain_en[] PROGMEM = "GPS gain [%]"; |
static const char param_NaviGpsP_de[] PROGMEM = "GPS-P"; |
static const char param_NaviGpsI_de[] PROGMEM = "GPS-I"; |
static const char param_NaviGpsD_de[] PROGMEM = "GPS-D"; |
static const char param_NaviGpsPLimit_de[] PROGMEM = "GPS-P Limit"; |
static const char param_NaviGpsILimit_de[] PROGMEM = "GPS-I Limit"; |
static const char param_NaviGpsDLimit_de[] PROGMEM = "GPS-D Limit"; |
static const char param_NaviGpsA_de[] PROGMEM = "GPS Acc"; |
#define param_NaviGpsP_en param_NaviGpsP_de |
#define param_NaviGpsI_en param_NaviGpsI_de |
#define param_NaviGpsD_en param_NaviGpsD_de |
#define param_NaviGpsPLimit_en param_NaviGpsPLimit_de |
#define param_NaviGpsILimit_en param_NaviGpsILimit_de |
#define param_NaviGpsDLimit_en param_NaviGpsDLimit_de |
#define param_NaviGpsA_en param_NaviGpsA_de |
static const char param_NaviGpsMinSat_de[] PROGMEM = "Min. Satelliten"; |
static const char param_NaviGpsMinSat_en[] PROGMEM = "minimum satelite"; |
static const char param_NaviStickThreshold_de[] PROGMEM = "GPS Stick-Schwelle"; |
static const char param_NaviStickThreshold_en[] PROGMEM = "GPS stick threshold"; |
static const char param_NaviWindCorrection_de[] PROGMEM = "GPS Windkorrektur [%]"; |
static const char param_NaviWindCorrection_en[] PROGMEM = "GPS wind correct. [%]"; |
static const char param_NaviAccCompensation_de[] PROGMEM = "ACC Kompensation"; |
static const char param_NaviAccCompensation_en[] PROGMEM = "ACC compensation"; |
static const char param_NaviOperatingRadius_de[] PROGMEM = "max. Radius"; |
static const char param_NaviOperatingRadius_en[] PROGMEM = "max. radius"; |
static const char param_NaviAngleLimitation_de[] PROGMEM = "Winkel Limit"; |
static const char param_NaviAngleLimitation_en[] PROGMEM = "angle limit"; |
static const char param_NaviPH_LoginTime_de[] PROGMEM = "PH Login Zeit [s]"; |
static const char param_NaviPH_LoginTime_en[] PROGMEM = "PH login time [s]"; |
static const char param_ExternalControl_de[] PROGMEM = "Ext. Kontrolle"; |
static const char param_ExternalControl_en[] PROGMEM = "ext. control"; |
static const char param_OrientationAngle_de[] PROGMEM = "OrientationAngle"; |
#define param_OrientationAngle_en param_OrientationAngle_de |
static const char param_CareFreeChannel_de[] PROGMEM = "CareFree Steuerung"; |
static const char param_CareFreeChannel_en[] PROGMEM = "careFree control"; |
static const char param_MotorSafetySwitch_de[] PROGMEM = "Motor Sicherh.Schalt."; |
static const char param_MotorSafetySwitch_en[] PROGMEM = "motor safety switch"; |
static const char param_MotorSmooth_de[] PROGMEM = "Motor Glättung"; |
static const char param_MotorSmooth_en[] PROGMEM = "motor smooth"; |
static const char param_ComingHomeAltitude_de[] PROGMEM = "Com.Home Höhe [m]"; |
static const char param_ComingHomeAltitude_en[] PROGMEM = "coming home alti. [m]"; |
static const char param_FailSafeTime_de[] PROGMEM = "Fails. CH Zeit [s]"; |
static const char param_FailSafeTime_en[] PROGMEM = "fails. CH time [s]"; |
static const char param_MaxAltitude_de[] PROGMEM = "Max. Höhe [m]"; |
static const char param_MaxAltitude_en[] PROGMEM = "max. altitude [m]"; |
static const char param_FailsafeChannel_de[] PROGMEM = "Fails. Channel 0=Aus"; |
static const char param_FailsafeChannel_en[] PROGMEM = "fails. channel 0=off"; |
static const char param_ServoFilterNick_de[] PROGMEM = "Nick Filter"; |
static const char param_ServoFilterNick_en[] PROGMEM = "nick filter"; |
static const char param_ServoFilterRoll_de[] PROGMEM = "Roll Filter"; |
static const char param_ServoFilterRoll_en[] PROGMEM = "roll filter"; |
static const char param_StartLandChannel_de[] PROGMEM = "Auto StartLand Kanal"; |
static const char param_StartLandChannel_en[] PROGMEM = "auto start/land chan."; |
static const char param_LandingSpeed_de[] PROGMEM = "Landing Speed 0.1m/s"; |
#define param_LandingSpeed_en param_LandingSpeed_de |
static const char param_CompassOffset_de[] PROGMEM = "Compass Offset [\x0B]"; |
#define param_CompassOffset_en param_CompassOffset_de |
static const char param_AutoLandingVoltage_de[] PROGMEM = "Autoland. Volt [0.1V]"; |
#define param_AutoLandingVoltage_en param_AutoLandingVoltage_de |
static const char param_ComingHomeVoltage_de[] PROGMEM = "Coming H. Volt [0.1V]"; |
#define param_ComingHomeVoltage_en param_ComingHomeVoltage_de |
static const char param_BitConfig_de[] PROGMEM = "BitConfig"; |
static const char param_ServoCompInvert_de[] PROGMEM = "ServoCompInvert"; |
static const char param_ExtraConfig_de[] PROGMEM = "ExtraConfig"; |
static const char param_GlobalConfig3_de[] PROGMEM = "GlobalConfig3"; |
static const char param_Name_de[] PROGMEM = "Setting Name"; |
#define param_BitConfig_en param_BitConfig_de |
#define param_ServoCompInvert_en param_ServoCompInvert_de |
#define param_ExtraConfig_en param_ExtraConfig_de |
#define param_GlobalConfig3_en param_GlobalConfig3_de |
#define param_Name_en param_Name_de |
static const char param_ComingHomeOrientation_de[] PROGMEM = "CH Ausrichtung"; |
static const char param_ComingHomeOrientation_en[] PROGMEM = "CH orientation"; |
static const char param_SingleWpControlChannel_de[] PROGMEM = "Single WP Ctrl Chan."; |
#define param_SingleWpControlChannel_en param_SingleWpControlChannel_de |
static const char param_MenuKeyChannel_de[] PROGMEM = "Next WP Channel"; |
#define param_MenuKeyChannel_en param_MenuKeyChannel_de |
// subitems (Bit / Byte-Felder) |
static const char param_ServoCompInvert_SVNick_de[] PROGMEM = "Nick Umkehren"; |
static const char param_ServoCompInvert_SVNick_en[] PROGMEM = "nick inv. direction"; |
static const char param_ServoCompInvert_SVRoll_de[] PROGMEM = "Roll Umkehren"; |
static const char param_ServoCompInvert_SVRoll_en[] PROGMEM = "Roll inv. direction"; |
static const char param_ServoCompInvert_SVRelMov_de[] PROGMEM = "Nick Relativ"; |
static const char param_ServoCompInvert_SVRelMov_en[] PROGMEM = "nick relativ"; |
static const char param_ExtraConfig_HeightLimit_de[] PROGMEM = "Höhenbegrenzung"; |
static const char param_ExtraConfig_HeightLimit_en[] PROGMEM = "heigth limitation"; |
static const char param_ExtraConfig_HeightVario_de[] PROGMEM = "Vario-Höhe"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_HeightVario_en[] PROGMEM = "vario heigth"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_VarioBeep_de[] PROGMEM = "akust. Vario"; |
#define param_ExtraConfig_VarioBeep_en param_ExtraConfig_VarioBeep_de |
static const char param_ExtraConfig_SensitiveRc_de[] PROGMEM = "Erw. Empf.Sig.Prüfung"; |
static const char param_ExtraConfig_SensitiveRc_en[] PROGMEM = "enh. rec. sign. check"; |
static const char param_ExtraConfig_33vReference_de[] PROGMEM = "33vReference"; |
#define param_ExtraConfig_33vReference_en param_ExtraConfig_33vReference_de |
static const char param_ExtraConfig_NoRcOffBeeping_de[] PROGMEM = "k.Summer o.Sender"; |
static const char param_ExtraConfig_NoRcOffBeeping_en[] PROGMEM = "no beep without TX"; |
static const char param_ExtraConfig_GpsAid_de[] PROGMEM = "Dynamic PH"; |
#define param_ExtraConfig_GpsAid_en param_ExtraConfig_GpsAid_de |
static const char param_ExtraConfig_LearnableCarefree_de[] PROGMEM = "Teachable CF"; |
#define param_ExtraConfig_LearnableCarefree_en param_ExtraConfig_LearnableCarefree_de |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_de[] PROGMEM = "Kompass Fehler ignor."; |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_en[] PROGMEM = "ignore compass error"; |
static const char param_BitConfig_LoopOben_de[] PROGMEM = "Looping Oben"; |
static const char param_BitConfig_LoopOben_en[] PROGMEM = "looping up"; |
static const char param_BitConfig_LoopUnten_de[] PROGMEM = "Looping Unten"; |
static const char param_BitConfig_LoopUnten_en[] PROGMEM = "looping down"; |
static const char param_BitConfig_LoopLinks_de[] PROGMEM = "Looping Links"; |
static const char param_BitConfig_LoopLinks_en[] PROGMEM = "looping left"; |
static const char param_BitConfig_LoopRechts_de[] PROGMEM = "Looping Rechts"; |
static const char param_BitConfig_LoopRechts_en[] PROGMEM = "looping right"; |
static const char param_BitConfig_MotorBlink1_de[] PROGMEM = "nur wenn Motor An"; |
static const char param_BitConfig_MotorBlink1_en[] PROGMEM = "only with motor on"; |
static const char param_BitConfig_MotorOffLed1_de[] PROGMEM = " \x19 sofort an"; |
static const char param_BitConfig_MotorOffLed1_en[] PROGMEM = " \x19 immediately on"; |
#define param_BitConfig_MotorOffLed2_de param_BitConfig_MotorOffLed1_de |
#define param_BitConfig_MotorBlink2_de param_BitConfig_MotorBlink1_de |
#define param_BitConfig_MotorOffLed2_en param_BitConfig_MotorOffLed1_en |
#define param_BitConfig_MotorBlink2_en param_BitConfig_MotorBlink1_en |
static const char param_GlobalConfig3_NoSdCardNoStart_de[] PROGMEM = "k.Start o.SD-Karte"; |
static const char param_GlobalConfig3_NoSdCardNoStart_en[] PROGMEM = "no start w/o SD-card"; |
static const char param_GlobalConfig3_DphMaxRadius_de[] PROGMEM = "Max.Radius dPH"; |
#define param_GlobalConfig3_DphMaxRadius_en param_GlobalConfig3_DphMaxRadius_de |
static const char param_GlobalConfig3_VarioFailsafe_de[] PROGMEM = "NOT-Gas Vario Höhe"; |
static const char param_GlobalConfig3_VarioFailsafe_en[] PROGMEM = "Emerg.thr.vario h."; |
static const char param_GlobalConfig3_MotorSwitchMode_de[] PROGMEM = "Motor Swi.Mode"; |
#define param_GlobalConfig3_MotorSwitchMode_en param_GlobalConfig3_MotorSwitchMode_de |
static const char param_GlobalConfig3_NoGpsFixNoStart_de[] PROGMEM = "k.Start o.GPS Fix"; |
static const char param_GlobalConfig3_NoGpsFixNoStart_en[] PROGMEM = "no start w/o GPS Fix"; |
static const char param_GlobalConfig3_UseNcForOut1_de[] PROGMEM = "mit WP-Event verkn."; |
static const char param_GlobalConfig3_UseNcForOut1_en[] PROGMEM = "combine with WP-Event"; |
static const char param_GlobalConfig3_SpeakAll_de[] PROGMEM = "Alles Ansagen"; |
static const char param_GlobalConfig3_SpeakAll_en[] PROGMEM = "speak all"; |
static const char param_GlobalConfig3_ServoNickCompOff_de[] PROGMEM = "Nick Komp. Aus"; |
static const char param_GlobalConfig3_ServoNickCompOff_en[] PROGMEM = "nick compensation off"; |
static const char param_GlobalConfig_HoehenRegelung_de[] PROGMEM = "Höhenregelung"; |
static const char param_GlobalConfig_HoehenRegelung_en[] PROGMEM = "height control"; |
static const char param_GlobalConfig_HoehenSchalter_de[] PROGMEM = "Schalter Höhe"; |
static const char param_GlobalConfig_HoehenSchalter_en[] PROGMEM = "height switch"; |
static const char param_GlobalConfig_HeadingHold_de[] PROGMEM = "Heading Hold"; |
#define param_GlobalConfig_HeadingHold_en param_GlobalConfig_HeadingHold_de |
static const char param_GlobalConfig_KompassAktiv_de[] PROGMEM = "Kompass Aktiv"; |
static const char param_GlobalConfig_KompassAktiv_en[] PROGMEM = "compass aktiv"; |
static const char param_GlobalConfig_KompassFix_de[] PROGMEM = "Kompass Fix"; |
static const char param_GlobalConfig_KompassFix_en[] PROGMEM = "compass fix"; |
static const char param_GlobalConfig_GpsAktiv_de[] PROGMEM = "GPS Aktiv"; |
#define param_GlobalConfig_GpsAktiv_en param_GlobalConfig_GpsAktiv_de |
static const char param_GlobalConfig_AchsenkopplungAktiv_de[] PROGMEM = "Achs(ent)kopplung"; |
static const char param_GlobalConfig_AchsenkopplungAktiv_en[] PROGMEM = "(De)Coupl Axes"; |
static const char param_GlobalConfig_DrehratenBegrenzer_de[] PROGMEM = "Drehratenbegrenzung"; |
static const char param_GlobalConfig_DrehratenBegrenzer_en[] PROGMEM = "rotary rate limit."; |
static const char param_Kanalbelegung_Nick_de[] PROGMEM = "Nick"; |
static const char param_Kanalbelegung_Roll_de[] PROGMEM = "Roll"; |
#define param_Kanalbelegung_Nick_en param_Kanalbelegung_Nick_de |
#define param_Kanalbelegung_Roll_en param_Kanalbelegung_Roll_de |
static const char param_Kanalbelegung_Gas_de[] PROGMEM = "Gas"; |
static const char param_Kanalbelegung_Gas_en[] PROGMEM = "throttle"; |
static const char param_Kanalbelegung_Gear_de[] PROGMEM = "Gear"; |
#define param_Kanalbelegung_Gear_en param_Kanalbelegung_Gear_de |
static const char param_Kanalbelegung_Poti1_de[] PROGMEM = "Poti 1"; |
static const char param_Kanalbelegung_Poti2_de[] PROGMEM = "Poti 2"; |
static const char param_Kanalbelegung_Poti3_de[] PROGMEM = "Poti 3"; |
static const char param_Kanalbelegung_Poti4_de[] PROGMEM = "Poti 4"; |
static const char param_Kanalbelegung_Poti5_de[] PROGMEM = "Poti 5"; |
static const char param_Kanalbelegung_Poti6_de[] PROGMEM = "Poti 6"; |
static const char param_Kanalbelegung_Poti7_de[] PROGMEM = "Poti 7"; |
static const char param_Kanalbelegung_Poti8_de[] PROGMEM = "Poti 8"; |
#define param_Kanalbelegung_Poti1_en param_Kanalbelegung_Poti1_de |
#define param_Kanalbelegung_Poti2_en param_Kanalbelegung_Poti2_de |
#define param_Kanalbelegung_Poti3_en param_Kanalbelegung_Poti3_de |
#define param_Kanalbelegung_Poti4_en param_Kanalbelegung_Poti4_de |
#define param_Kanalbelegung_Poti5_en param_Kanalbelegung_Poti5_de |
#define param_Kanalbelegung_Poti6_en param_Kanalbelegung_Poti6_de |
#define param_Kanalbelegung_Poti7_en param_Kanalbelegung_Poti7_de |
#define param_Kanalbelegung_Poti8_en param_Kanalbelegung_Poti8_de |
static const char param_CompassOffset_DisableDeclCalc_de[] PROGMEM = "Disable Decl. Calc"; |
#define param_CompassOffset_DisableDeclCalc_en param_CompassOffset_DisableDeclCalc_de |
static const char param_NaviMaxFlyingRange_de[] PROGMEM = "Max. Flugradius [10m]"; |
static const char param_NaviMaxFlyingRange_en[] PROGMEM = "Max. Flightrad. [10m]"; |
static const char param_NaviDescendRange_de[] PROGMEM = "Fail SinkRadius [10m]"; |
#define param_NaviDescendRange_en param_NaviDescendRange_de |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/mksettings.c |
---|
0,0 → 1,707 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: MKSettings_Menu() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 26.03.2014 OG |
//# - add: etliche Aenderungen in allen Bereichen fuer das erste Release |
//# der neuen MK-Settings |
//# |
//# 27.02.2014 OG |
//# - chg: MKSettings_AskAction() Unterstuetzung von param_DUMMY |
//# |
//# 26.02.2014 OG |
//# - chg: MKSettings_Copy() auf KEYLINE2 geaendert |
//# |
//# 23.02.2014 OG |
//# - chg: MKSettings_Menu() Aufruf von MK_Parameters() geaendert |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mksettings.h" |
//--------------------------------------------------------------------------------------------- |
#define F_EXIT 0 |
#define F_REFRESH 1 |
char mksettings_menu_names[6][18]; // befuellt von: MKSettings_read_Names() |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char paramsetRevision; // Revision FC-Parameterset fuer die das Temp-Setting gueltig ist |
unsigned char *paramset; // malloc: Pointer auf kopierte Parameter-Daten |
} MKSetting_TMP_t; |
MKSetting_TMP_t MKSetting_TMP; |
#define ID_SETTING_COPY 10 // fuer MKSettings_Menu() |
#define ID_EDIT 20 // fuer MKSettings_AskAction() |
#define ID_ACTIVATE 21 // fuer MKSettings_AskAction() |
#define ID_COPY 22 // fuer MKSettings_AskAction() |
#define ID_SAVE 30 // fuer MKSettings_AskSaveSetting() |
#define ID_DISCARD 31 // fuer MKSettings_AskSaveSetting() |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init0() |
// |
// nur fuer main.c |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init0( void ) |
{ |
memset( &MKSetting_TMP, 0, sizeof(MKSetting_TMP_t) ); |
strcpy( mksettings_menu_names[5], "PKT: --empty--"); |
} |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init() |
// |
// loeschen / initialisieren vom PKT Temp-Setting |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init( void ) |
{ |
if( MKSetting_TMP.paramset != NULL ) |
{ |
free( MKSetting_TMP.paramset ); |
} |
MKSettings_TMP_Init0(); |
} |
//-------------------------------------------------------------- |
// from_setting = 6 : von TMP zu einem MK-Setting |
// from_setting <= 5: von MK-Setting zu TMP |
//-------------------------------------------------------------- |
uint8_t MKSettings_TMP_copy( uint8_t to_setting, uint8_t timeout ) |
{ |
uint8_t written; |
uint8_t size = paramsetSize(); |
if( MKSetting_TMP.paramset == NULL ) |
{ |
MKSetting_TMP.paramset = malloc( size+1 ); // +1 fuer das erste settings-byte |
} |
if( !MKSetting_TMP.paramset ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return 0; // -> EXIT |
} |
// von MK-Setting 1..5 zu TMP |
if( to_setting == 6 ) |
{ |
MKSetting_TMP.paramsetRevision = MKVersion.paramsetRevision; |
memcpy( MKSetting_TMP.paramset, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
return 6; |
} |
// von TMP zu MK-Setting 1..5 |
memcpy( (unsigned char *)(pRxData), MKSetting_TMP.paramset , size+1 ); // memcpy( dst, src, size)) |
written = MK_Setting_write( to_setting, timeout); |
return written; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_read_Names( void ) |
{ |
uint8_t setting; |
// die 5 Settings vom Kopter laden um die |
// Setting-Namen zu speichern |
for( setting=1; setting <= 5; setting++ ) |
{ |
if( !MK_Setting_load( setting, 20 ) ) |
return(0); // Fehler - setting konnte nicht geladen werden (timeout = 20) |
xsnprintf( mksettings_menu_names[setting-1], 16, "%1d: %s", setting, paramGet_p(param_Name) ); |
} |
// TMP-Setting |
if( MKSetting_TMP.paramset != NULL ) |
{ |
if( MKSetting_TMP.paramsetRevision != MKVersion.paramsetRevision ) |
{ |
// wenn die FC-Revision vom TMP-Setting abweicht vom zuletzt geladenen |
// dann wurde ggf. der Kopter gewechselt -> TMP-Setting verwerfen |
MKSettings_TMP_Init(); |
} |
else |
{ |
// den Namen aus dem TMP-Setting holen |
paramsetInit( MKSetting_TMP.paramset ); |
xsnprintf( mksettings_menu_names[5], 16, "PKT: %s", paramGet_p(param_Name) ); |
} |
} |
setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
return setting; |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskAction( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_EDIT, ID_ACTIVATE, ID_COPY |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskAction( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
if( !paramExist(param_DUMMY) ) // param_DUMMY -> das Parameterset wird nicht zum bearbeiten unterstuetzt |
MenuCtrl_Push_P( ID_EDIT , MENU_ITEM, NOFUNC, strGet(EDIT_SETTING) ); // "Setting ändern" |
if( setting != 6) |
MenuCtrl_Push_P( ID_ACTIVATE, MENU_ITEM, NOFUNC, strGet(PARA_AKTIVI) ); // "aktivieren" |
MenuCtrl_Push_P( ID_COPY , MENU_ITEM, NOFUNC, strGet(STR_COPY) ); // "kopieren" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskSaveSetting( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_SAVE, ID_DISCARD |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskSaveSetting( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
MenuCtrl_Push_P( ID_SAVE , MENU_ITEM, NOFUNC, strGet(STR_SAVE) ); // "speichern" |
MenuCtrl_Push_P( ID_DISCARD , MENU_ITEM, NOFUNC, strGet(STR_DISCARD) ); // "verwerfen" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_Copy( uint8_t from_setting ) |
{ |
const char *mask = PSTR("%15s"); |
uint8_t redraw = true; |
uint8_t loaded = 0; |
uint8_t to_setting; |
uint8_t written; |
uint8_t key; |
int8_t yoffs; |
lcd_cls(); |
to_setting = from_setting; |
while( true ) |
{ |
//------------------------ |
// anzeigen |
//------------------------ |
if( redraw ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, strGet(PARA_COPY), MINVERS); |
yoffs = -1; |
lcdx_printp_at( 0, 2, strGet(STR_VON), MNORMAL, 0,yoffs); // "von" |
lcdx_printf_at_P( 5, 2, MNORMAL, 3,yoffs, mask, mksettings_menu_names[from_setting-1] ); |
yoffs = -7; |
lcd_frect( 5*6, 4*8+yoffs, 15*6, 7, 0); |
lcdx_printp_at( 0, 4, strGet(STR_NACH), MNORMAL, 0,yoffs); // "nach" |
lcdx_printf_at_P( 5, 4, MNORMAL, 3,yoffs, mask, mksettings_menu_names[to_setting-1] ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
redraw = false; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
return 99; // nur "ENDE" |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
if( to_setting == 6 ) to_setting = 1; |
else to_setting++; |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
if( to_setting == 1 ) to_setting = 6; |
else to_setting--; |
redraw = true; |
} |
//------------------------------- |
// Taste: OK = Setting kopieren? |
//------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
lcdx_printp_center( 5, strGet(PARA_COPYQ), MNORMAL, 0,1); // "Wirklich kopieren?" (zentriert) |
lcd_rect_round( 0, 5*8-3, 127, 7+7, 1, R2); // Rahmen um die Frage |
lcd_frect( 0, 7*8, 127, 7, 0); // Keyline loeschen |
lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // neue Keyline: "Nein Ja" |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
key = 0; |
while( !key ) // Abfrage: Ja / Nein |
{ |
key = get_key_press(1 << KEY_ENTER) ? KEY_ENTER : key; // => "Ja" (=Setting kopieren) |
key = get_key_press(1 << KEY_ESC) ? KEY_ESC : key; // => "Nein" |
PKT_CtrlHook(); |
} |
//--------------------------- |
// "Ja" -> Setting kopieren! |
//--------------------------- |
if( key == KEY_ENTER ) // => "Ja" -> Setting kopieren! |
{ |
if( from_setting != 6 ) // kein PKT TMP-Setting |
{ |
loaded = MK_Setting_load( from_setting, 20 ); // timeout = 20 |
if( loaded != from_setting ) // Fehler beim laden - Datenverlust? |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return 0; // 0 = Ende/Abbruch |
} |
} |
if( to_setting == 6 ) // von Setting 1..5 nach TMP speichern |
{ |
MKSettings_TMP_copy( 6, 10 ); |
return loaded; |
} |
// Anzeige "speichern..." |
lcd_frect( 0, (8*4), 128, (8*4), 0); // Anzeigebereich löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
if( from_setting == 6 ) // von TMP nach Setting 1..5 |
{ |
written = MKSettings_TMP_copy( to_setting, 30 ); |
return written; |
} |
if( to_setting <= 5 ) // 'echtes' MK Setting speichern |
{ |
written = MK_Setting_write( to_setting, 30 ); // Timeout = 40 |
return written; |
} |
} |
if( key == KEY_ESC ) // => "Nein" -> nicht kopieren |
{ |
lcd_cls(); |
redraw = true; |
} |
} //end: if( get_key_press(1 << KEY_ENTER) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = true; |
} |
} // end: while( true ) |
return 0; |
} |
//-------------------------------------------------------------- |
// ret = MKSettings_Menu() |
// |
// Rueckgabe: |
// 0 = Ende/Fehler/Abbruch |
// 1 = Refresh |
//-------------------------------------------------------------- |
uint8_t MKSettings_Menu( void ) |
{ |
uint8_t i; |
uint8_t active_setting; |
uint8_t setting; |
uint8_t wahl; |
uint8_t wahl2; |
uint8_t changed; |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_LOADSETTINGS), false, 0, true, true ); // "lade Settings..." |
active_setting = MKSettings_read_Names(); |
if( !active_setting ) // Fehler: settings konnten nicht geladen werden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "MK Settings" |
//MenuCtrl_SetTitle_P( PSTR("MK Settings") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menue-Punkte |
//--------------- |
for( i=0; i< ( MKSetting_TMP.paramset!=NULL ? 6 : 5); i++ ) |
{ |
MenuCtrl_Push( i+1, MENU_SUB, NOFUNC, mksettings_menu_names[i] ); // Setting 1..5 |
} |
MenuCtrl_ItemSelect( active_setting ); // Menucursor auf aktives Setting setzen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) // Ende? |
{ |
break; // Ende |
} |
setting = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( setting >=1 && setting <= 6 ) |
{ |
wahl = MKSettings_AskAction( setting ); |
//-------------- |
// bearbeiten |
//-------------- |
if( wahl == ID_EDIT ) |
{ |
// TODO: Fehler abfangen! |
MK_Setting_load( setting, 25 ); // timeout = 25 |
changed = MK_Parameters( setting, mksettings_menu_names[setting-1] ); |
if( changed && (setting!=6) ) |
{ |
wahl2 = MKSettings_AskSaveSetting( setting ); |
if( wahl2 == ID_SAVE ) |
{ |
lcd_frect( 0, (8*7), 128, 7, 0); // Keyline löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
setting = MK_Setting_write( setting, 50); |
if( !setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
} |
} |
} |
//-------------- |
// aktivieren |
//-------------- |
if( wahl == ID_ACTIVATE ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_ACTIVATESETTING), false, 0, true, true ); // "aktiviere Setting..." |
active_setting = MK_Setting_change( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
else |
{ |
// neues Setting markieren |
for( i=1; i<=5; i++ ) MenuCtrl_ItemMark( i, false); // Markierungen loeschen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
} |
} // end: if( wahl == ID_ACTIVATE ) |
//-------------- |
// kopieren |
//-------------- |
if( wahl == ID_COPY ) |
{ |
active_setting = MKSettings_Copy( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
if( active_setting != 99 ) // =99 bedeutet: User hat die Funktion abgebrochen... |
{ |
MenuCtrl_Destroy(); // ok, kein Abbruch durch den User -> Menue neu aufbauen |
return F_REFRESH; // da sich ggf. Settings-Namen geaendert haben |
} |
} // end: if( wahl == ID_COPY ) |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
return F_EXIT; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// das muss ueberarbeitet werden.... |
//-------------------------------------------------------------- |
static uint8_t check_motorOff(void) |
{ |
NaviData_t *naviData; |
if( hardware == NC ) // Prüfung funktioniert nur mit NC |
{ |
SwitchToNC(); |
SendOutData( 'o', ADDRESS_NC, 1, 10, 1); |
mode = 'O'; |
rxd_buffer_locked = FALSE; |
timer = 200; |
while( !rxd_buffer_locked && timer>0 ); |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
// timer = MK_TIMEOUT; |
Decode64(); |
naviData = (NaviData_t *) pRxData; |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
return false; |
else |
return true; |
} |
return false; |
} |
return true; // hmm, wenn man nur eine FC hat dann wird hier immer gemeldet "Motoren sind aus" ? |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Settings( void ) |
{ |
//MKSettings_TMP_Init(); |
//if( true ) |
if( !check_motorOff() ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_SWITCHMOTOROFF), false, 400, true, true ); // "Motoren ausschalten!" |
return; |
} |
while( MKSettings_Menu() == F_REFRESH ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/mksettings.h |
---|
0,0 → 1,55 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.h |
//# |
//# 19.02.2014 OG - NEU |
//# - add: MKSettings_TMP_Init(), MKSettings_TMP_Init0() |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
void MK_Settings( void ); |
void MKSettings_TMP_Init0( void ); |
void MKSettings_TMP_Init( void ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/paramset.c |
---|
0,0 → 1,2583 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.c |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: unsigned char SingleWpControlChannel; (FC2.11a) |
//# unsigned char MenuKeyChannel; (FC2.11a) |
//# - add: paramset_106 (FC2.11a) |
//# |
//# 09.04.2015 Cebra (PKT384a) |
//# - add: unsigned char ServoFS_Pos[5] (FC 2.09i) |
//# - add: paramset_105 (FC2.09j) |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - add: param_LandingPulse (FC 2.09d) |
//# |
//# 26.01.2015 Cebra |
//# - add: Function transform_ComingHomeOrientation |
//# Ändert die Bits 4+5 für ServoCompInvert |
//# in Konfigurationstabelle der paramSubID: param_ComingHomeOrientation neu |
//# |
//# 26.09.2014 Cebra |
//# - add: Parameterset Rev. 103 (FC 2.07f) |
//# - chg: param_Hoehe_GPS_Z zu param_Hoehe_Verstaerkung im Paramset 103 |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 10.05.2014 OG |
//# - add: transform_CompassOffset_DisableDeclCalc() - transformiert true/false |
//# in das Byte von param_CompassOffset |
//# |
//# 09.05.2014 OG |
//# - chg: paramSet() - angepasst auf Transform-Funktionen |
//# - chg: paramGet() - angepasst auf Transform-Funktionen |
//# - add: eine neue Transformations-Zwischenschicht implementiert - abgebildet |
//# ueber die neue Tabelle paramTransform. |
//# Hier koennen Transform-Funktionen hinterlegt werden die einen |
//# ubyte8 Wert veraendern. |
//# Die Transformation wird von paramSet()/paramGet() aufgerufen. |
//# |
//# 17.04.2014 OG |
//# - FIX: _paramset_getsubitemid(): Pruefung von_Revision/bis_Revision |
//# korrigiert - fuehrte vorher ggf. zu falschen Anzeigen von SubItems! |
//# - chg: param_BitConfig_MotorOffLed1 auf bis_Revision 101 begrenzt |
//# - chg: param_BitConfig_MotorOffLed2 auf bis_Revision 101 begrenzt |
//# |
//# 09.04.2014 OG |
//# - chg: Rev. 101 auf param_DUMMY umgestellt um Platz zu sparen |
//# |
//# 08.04.2014 OG |
//# - add: Parameterset Rev. 102 (FC 2.05g) |
//# - fix: _paramset_getsubitemid() - Vergleich bzgl. von/bis_Revision umgedreht |
//# und equal zugefuegt |
//# |
//# 06.04.2014 OG |
//# - add: Parameterset Rev. 101 (FC 2.05f) |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: Parameterset Rev. 100 (FC 2.05e) |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 24.03.2014 OG |
//# - chg: _paramset_getsubitemid() erweitert Unterstuetzung von/bis FC-Revisionen |
//# - add: paramSubItem_t mit Unterstuetzung fuer von/bis FC-Revisionen |
//# |
//# 27.02.2014 OG |
//# - chg: die Revisions-Tabellen 90 bis 94 mittels param_DUMMY gekuerzt |
//# -> keine Parameter-Bearbeitung bei denen moeglich |
//# - add: vollstaendige Unterstuetzung von paramSubID's via paramGet(), paramSet() |
//# |
//# 26.02.2014 OG |
//# - add: paramSubItem[] - Zugriffstabelle auf Bit- und Bytefelder innerhalb |
//# einer paramID |
//# - chg: bei den paramset_nnn[] Tabellen 'const' ergaenzt |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 19.02.2014 OG |
//# - fix: paramsetInit() Parameter pData wurde nicht konsequent verwendet |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# - add: Rev-Tabellen von FC v0.88e bis FC v2.03d |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
//############################################################################ |
//# INHALT |
//# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//# 3a. interne Zugriffsfunktionen |
//# 3b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# 3c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# x. TEST / DEBUG |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
//--------------------------- |
// fuer transform_... |
//--------------------------- |
#define GETVALUE 1 // Wert setzen |
#define SETVALUE 2 // Wert lesen |
paramRevItem_t *paramsetRevTable; // Zeiger auf aktive Revision-Table oder 0 wenn nicht gesetzt -> wird gesetzt druch paramsetInit() / paramset.c |
unsigned char *mkparamset; // nur temp. gueltig solange der PKT-RX-Buffer (pRxData) noch nicht wieder ueberschrieben -> wird gesetzt druch paramsetInit() / paramset.c |
//############################################################################################# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//############################################################################################# |
//--------------------------------------------------------------------------------------- |
// die nachfolgenden Tabellen entsprechen der Struktur von paramRevItem_t aus paramset.h |
//--------------------------------------------------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 90 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88e |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_091 |
// |
// STRUKTUR-DIFF zu 0: |
// |
// DEFINE-DIFF zu 0: |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 91 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88m, 0.88n |
// |
// STRUKTUR-DIFF zu 090: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 090: |
// - add: #define PID_SPEAK_HOTT_CFG |
// - add: #define CFG3_MOTOR_SWITCH_MODE |
// - add: #define CFG3_NO_GPSFIX_NO_START |
//----------------------------------------------- |
// |
// |
// + paramID (paramRevItem_t -> paramID) |
// | |
// | + size in Bytes (paramRevItem_t -> size) |
// | | |
// | | |
// | | |
paramRevItem_t const paramset_091[] PROGMEM = |
{ |
{ param_DUMMY , 111 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_091[] |
//############################################################################################################################################################ |
//----------------------------------------------- |
// FC-Parameter Revision: 92 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90d |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_093 |
// |
// STRUKTUR-DIFF zu 091: |
// -add: param_NaviOut1Parameter |
//# |
// DEFINE-DIFF zu 091: |
// -add: #define CFG3_USE_NC_FOR_OUT1 0x20 |
// -add: #define CFG3_SPEAK_ALL |
// -add: #define SERVO_NICK_INV 0x01 |
// -add: #define SERVO_ROLL_INV 0x02 |
// -add: #define SERVO_RELATIVE 0x04 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 93 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90e, 0.90g, 0.90j |
// |
// STRUKTUR-DIFF zu 092: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 092: |
// - keine Aenderung erkennbar! |
//----------------------------------------------- |
paramRevItem_t const paramset_093[] PROGMEM = |
{ |
{ param_DUMMY , 112 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_093[] |
//----------------------------------------------- |
// FC-Parameter Revision: 94 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.91a |
// |
// STRUKTUR-DIFF zu 093: |
// - add: param_StartLandChannel |
// - add: param_LandingSpeed |
// |
// DEFINE-DIFF zu 093: |
// etwas undurchsichtig... |
// - add: #define EE_DUMMY |
// - add: #define PID_HARDWARE_VERSION |
//----------------------------------------------- |
paramRevItem_t const paramset_094[] PROGMEM = |
{ |
{ param_DUMMY , 114 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_094[] |
//----------------------------------------------- |
// FC-Parameter Revision: 95 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90L, 2.00a, , 2.00e |
// |
// STRUKTUR-DIFF zu 094: |
// - rename: param_MaxHoehe -> param_HoeheChannel |
// - rename: param_NaviGpsModeControl -> param_NaviGpsModeChannel |
// - rename: param_NaviGpsACC -> param_NaviGpsA |
// - rename: param_CareFreeModeControl -> param_CareFreeChannel |
// |
// DEFINE-DIFF zu 094: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_095[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_095[] |
//----------------------------------------------- |
// FC-Parameter Revision: 96 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: ??? |
// |
// STRUKTUR-DIFF zu 095: |
// - KEINE AENDERUNG der INTERNEN DATEN-STRUKTUR! |
// |
// DEFINE-DIFF zu 095: |
// GlobalConfig3 |
// - add: #define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 97 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.02b |
// |
// STRUKTUR-DIFF zu 096: |
// - add: param_CompassOffset |
// - add: param_AutoLandingVoltage |
// |
// DEFINE-DIFF zu 096: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_097[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_097[] |
//----------------------------------------------- |
// FC-Parameter Revision: 98 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.03d |
// |
// STRUKTUR-DIFF zu 097: |
// - add: param_ComingHomeVoltage |
// |
// DEFINE-DIFF zu 097: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_098[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_098[] |
//----------------------------------------------- |
// FC-Parameter Revision: 99 |
// keine Inormationen zu Rev. 99 vorhanden. |
// |
// Kommentar dazu von Holger Buss: |
// "Die Revision 0.99 kannst Du ignorieren." |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 100 |
// |
// ab FC-Version: ab ??? bis 2.05e |
// gefunden in: 2.05e |
// |
// STRUKTUR-DIFF zu 098: |
// - del: NaviOut1Parameter |
// - add: AutoPhotoDistance (ersetzt NaviOut1Parameter) |
// - add: AutoPhotoAtitudes |
// - add: SingleWpSpeed |
// |
// DEFINE-DIFF zu 098: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine kurze Zeit in Betaversionen |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_100[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_100[] |
//----------------------------------------------- |
// FC-Parameter Revision: 101 |
// |
// ab FC-Version: ab 2.05f bis 2.05f |
// gefunden in: 2.05f |
// |
// STRUKTUR-DIFF zu 100: |
// - add: Servo3OnValue |
// - add: Servo3OffValue |
// - add: Servo4OnValue |
// - add: Servo4OffValue |
// |
// DEFINE-DIFF zu 100: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine einzige Betaversion |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_101[] PROGMEM = |
{ |
{ param_DUMMY , 123 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_101[] |
//----------------------------------------------- |
// FC-Parameter Revision: 102 |
// |
// ab FC-Version: ab 2.05g bis ??? (min. 2.06b) |
// gefunden in: 2.05g, 2.06a |
// |
// STRUKTUR-DIFF zu 101: |
// - add: NaviMaxFlyingRange (ersetzt NaviOperatingRadius) |
// - add: NaviDescendRange |
// - del: NaviOperatingRadius |
// |
// DEFINE-DIFF zu 101: |
// - del: CFG3_DPH_MAX_RADIUS |
//----------------------------------------------- |
paramRevItem_t const paramset_102[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_102[] |
//----------------------------------------------- |
// FC-Parameter Revision: 103 |
// |
// ab FC-Version: ab 2.07f bis ??? |
// gefunden in: 2.07f |
// |
// STRUKTUR-DIFF zu 102: |
// keine |
// |
// DEFINE-DIFF zu 102: |
// - chg: Hoehe_GPS_Z zu Hoehe_TiltCompensation |
//----------------------------------------------- |
paramRevItem_t const paramset_103[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_103[] |
//----------------------------------------------- |
// FC-Parameter Revision: 104 |
// |
// ab FC-Version: ab 2.09d bis ??? |
// gefunden in: 2.09d |
// |
// STRUKTUR-DIFF zu 103: |
// add: LandingPulse |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_104[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_104[] |
//----------------------------------------------- |
// FC-Parameter Revision: 105 |
// |
// ab FC-Version: ab 2.09j bis ??? |
// gefunden in: 2.09j |
// |
// STRUKTUR-DIFF zu 104: |
// add: unsigned char ServoFS_Pos[5]; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_105[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_105[] |
//----------------------------------------------- |
// FC-Parameter Revision: 106 |
// |
// ab FC-Version: ab 2.11a bis ??? |
// gefunden in: 2.11a |
// |
// STRUKTUR-DIFF zu 105: |
// add: unsigned char SingleWpControlChannel; |
// unsigned char MenuKeyChannel; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_106[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_SingleWpControlChannel , 1 }, // unsigned char SingleWpControlChannel; // ab Rev. 106 (FC2.11a) |
{ param_MenuKeyChannel , 1 }, // unsigned char MenuKeyChannel; // ab Rev. 106 (FC2.11a) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_106[] |
//############################################################################################# |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//############################################################################################# |
typedef struct |
{ |
unsigned char Revision; |
const paramRevItem_t *RevisionTable; |
} paramsetRevMap_t; |
#define MAPEOF 255 |
//---------------------------------------------------------------- |
// Mappingtabelle zwischen FC-Revision und Paramset-Tabelle |
// |
// Anmerkung bzgl. Groesse: |
// Angenommen man wuerde die Rev 97 bis 90 weglassen, dann |
// wuerde man ca. 1 KByte sparen |
//---------------------------------------------------------------- |
paramsetRevMap_t paramsetRevMap[] = |
{ |
{ 106 , paramset_106 }, // 2.11a |
{ 105 , paramset_105 }, // 2.09j |
{ 104 , paramset_104 }, // 2.09d |
{ 103 , paramset_103 }, // 2.07f |
{ 102 , paramset_102 }, // 2.05g (bis min. 2.06b) |
{ 101 , paramset_101 }, // 2.05f (eingeschraenkte Unterstuetzung - ) nur in einer einzigen Betaversion vorhanden!) |
{ 100 , paramset_100 }, // 2.05e (Anmerkung OG 06.04.2014: ggf. spaeter loeschen um Platz zu sparen da diese Version nur in einigen wenigen Betaversionen vorhanden ist) |
// Rev. 99: nicht vorhanden; ignorieren |
{ 98 , paramset_098 }, // 2.03d |
{ 97 , paramset_097 }, // 2.02b |
{ 96 , paramset_095 }, // ??? (keine Struktur-Aenderung zu 095) |
{ 95 , paramset_095 }, // 0.90L, 2.00a, 2.00e |
{ 94 , paramset_094 }, // eingeschraenkte Unterstuetzung; 0.91a |
{ 93 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90e, 0.90g, 0.90j |
{ 92 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90d (keine Struktur-Aenderung zu 093) |
{ 91 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88m, 0.88n |
{ 90 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88e (keine Struktur-Aenderung zu 091) |
{ MAPEOF, 0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramSubID; // groesser/gleich PARAMSUBITEMS |
unsigned char paramID; // -> mapping auf entsprechende paramID |
unsigned char subType; // SUBTYPE_BIT oder SUBTYPE_BYTE |
unsigned char subIndex; // bei SUBTYPE_BIT: Bitmask z.b. 0x02 |
// bei SUBTYPE_BYTE: index des Bytes (siehe param_Kanalbelegung) |
unsigned char von_Revision; // ab welcher FC-Revision ist das SubItem vorhanden (0=immer) |
unsigned char bis_Revision; // bis zu welcher FC-Revision ist das SubItem vorhanden (0=immer) |
} paramSubItem_t; |
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
// +- von_Revision |
// | |
// +- paramSubID +- gehoert zu? +- Bit/Byte? +- Bit/Byte der paramID | +- bis_Revision |
// | | | | | | |
//--|-----------------------------------------|----------------------|--------------|----------------------------------|--|---------------------------------------------- |
paramSubItem_t const paramSubItem[] PROGMEM = |
{ |
{ param_ServoCompInvert_SERVO_NICK_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_NICK_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
{ param_ServoCompInvert_SERVO_ROLL_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_ROLL_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
{ param_ServoCompInvert_SERVO_RELATIVE , param_ServoCompInvert, SUBTYPE_BIT, SERVO_RELATIVE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
{ param_ExtraConfig_HeightLimit , param_ExtraConfig, SUBTYPE_BIT, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
{ param_ExtraConfig_HeightVario , param_ExtraConfig, SUBTYPE_BITN, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 negiert zu param_ExtraConfig_HeightLimit |
{ param_ExtraConfig_VarioBeep , param_ExtraConfig, SUBTYPE_BIT, CFG2_VARIO_BEEP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
{ param_ExtraConfig_SensitiveRc , param_ExtraConfig, SUBTYPE_BIT, CFG_SENSITIVE_RC , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
{ param_ExtraConfig_33vReference , param_ExtraConfig, SUBTYPE_BIT, CFG_3_3V_REFERENCE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
{ param_ExtraConfig_NoRcOffBeeping , param_ExtraConfig, SUBTYPE_BIT, CFG_NO_RCOFF_BEEPING , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
{ param_ExtraConfig_GpsAid , param_ExtraConfig, SUBTYPE_BIT, CFG_GPS_AID , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
{ param_ExtraConfig_LearnableCarefree , param_ExtraConfig, SUBTYPE_BIT, CFG_LEARNABLE_CAREFREE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
{ param_ExtraConfig_IgnoreMagErrAtStartup , param_ExtraConfig, SUBTYPE_BIT, CFG_IGNORE_MAG_ERR_AT_STARTUP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
{ param_BitConfig_LoopOben , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_OBEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
{ param_BitConfig_LoopUnten , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_UNTEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
{ param_BitConfig_LoopLinks , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_LINKS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
{ param_BitConfig_LoopRechts , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_RECHTS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
{ param_BitConfig_MotorBlink1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
{ param_BitConfig_MotorOffLed1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED1 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
{ param_BitConfig_MotorOffLed2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED2 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
{ param_BitConfig_MotorBlink2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK2 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
{ param_GlobalConfig3_NoSdCardNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_SDCARD_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
{ param_GlobalConfig3_DphMaxRadius , param_GlobalConfig3, SUBTYPE_BIT, CFG3_DPH_MAX_RADIUS , 0,100}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 |
{ param_GlobalConfig3_VarioFailsafe , param_GlobalConfig3, SUBTYPE_BIT, CFG3_VARIO_FAILSAFE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
{ param_GlobalConfig3_MotorSwitchMode , param_GlobalConfig3, SUBTYPE_BIT, CFG3_MOTOR_SWITCH_MODE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
{ param_GlobalConfig3_NoGpsFixNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_GPSFIX_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
{ param_GlobalConfig3_UseNcForOut1 , param_GlobalConfig3, SUBTYPE_BIT, CFG3_USE_NC_FOR_OUT1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
{ param_GlobalConfig3_SpeakAll , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SPEAK_ALL , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
{ param_GlobalConfig3_ServoNickCompOff , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SERVO_NICK_COMP_OFF , 96, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
{ param_GlobalConfig_HoehenRegelung , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHENREGELUNG , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
{ param_GlobalConfig_HoehenSchalter , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHEN_SCHALTER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
{ param_GlobalConfig_HeadingHold , param_GlobalConfig, SUBTYPE_BIT, CFG_HEADING_HOLD , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
{ param_GlobalConfig_KompassAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
{ param_GlobalConfig_KompassFix , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_FIX , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
{ param_GlobalConfig_GpsAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_GPS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
{ param_GlobalConfig_AchsenkopplungAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_ACHSENKOPPLUNG_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
{ param_GlobalConfig_DrehratenBegrenzer , param_GlobalConfig, SUBTYPE_BIT, CFG_DREHRATEN_BEGRENZER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
{ param_Kanalbelegung_Nick , param_Kanalbelegung, SUBTYPE_BYTE, 0 , 0, 0}, // -> Kanalbelegung[12] |
{ param_Kanalbelegung_Roll , param_Kanalbelegung, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Kanalbelegung_Gas , param_Kanalbelegung, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Kanalbelegung_Gear , param_Kanalbelegung, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Kanalbelegung_Poti1 , param_Kanalbelegung, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_Kanalbelegung_Poti2 , param_Kanalbelegung, SUBTYPE_BYTE, 5 , 0, 0}, |
{ param_Kanalbelegung_Poti3 , param_Kanalbelegung, SUBTYPE_BYTE, 6 , 0, 0}, |
{ param_Kanalbelegung_Poti4 , param_Kanalbelegung, SUBTYPE_BYTE, 7 , 0, 0}, |
{ param_Kanalbelegung_Poti5 , param_Kanalbelegung, SUBTYPE_BYTE, 8 , 0, 0}, |
{ param_Kanalbelegung_Poti6 , param_Kanalbelegung, SUBTYPE_BYTE, 9 , 0, 0}, |
{ param_Kanalbelegung_Poti7 , param_Kanalbelegung, SUBTYPE_BYTE, 10 , 0, 0}, |
{ param_Kanalbelegung_Poti8 , param_Kanalbelegung, SUBTYPE_BYTE, 11 , 0, 0}, |
{ param_CompassOffset_DisableDeclCalc , param_CompassOffset, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 8 und 7 von CompassOffset kodiert |
{ param_ComingHomeOrientation , param_ServoCompInvert, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 5 und 4 von CervoCompInvert |
{ param_ServoNickFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 0 , 0, 0}, |
{ param_ServoRollFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Servo3Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Servo4Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Servo5Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_EOF , 0,0,0,0,0 } // END - MUST BE THE LAST!! |
}; // end: paramSubItem[] |
//############################################################################################# |
//# 3a. Transformations-Tabelle |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID oder paramSubID |
unsigned char (*transformfunc)( uint8_t cmd, unsigned char value, unsigned char newvalue ); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
} paramTransform_t; |
//--------------------------------------- |
// forward Deklarationen fuer transform |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, uint16_t newvalue ); |
//--------------------------------------- |
//--------------------------------------- |
paramTransform_t const paramTransform[] = |
{ |
{ param_CompassOffset_DisableDeclCalc , &transform_CompassOffset_DisableDeclCalc }, |
{ param_ComingHomeOrientation , &transform_ComingHomeOrientation }, |
// { param_LandingPulse, &transform_ValueACCZ}, |
{ param_EOF , NULL } // END - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 3b. Transformations-Funktionen |
//############################################################################################# |
//--------------------------------------- |
// transform_CompassOffset_DisableDeclCalc() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// Der true/false (Ja/Nein) Wert von param_CompassOffset_DisableDeclCalc |
// ist in dem Byte von param_CompassOffset in den Bits 7 & 8 einkodiert. |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
uint8_t bit7; |
uint8_t bit8; |
bit7 = ((value & 0x40) ? true : false); |
bit8 = ((value & 0x80) ? true : false); |
if( cmd == GETVALUE ) |
{ |
// Bit 8 == Bit 7: Disable dec. Calc AUS |
// Bit 8 != Bit 7: Disable dec. Calc AN |
value = ((bit8 == bit7) ? 0 : 1); |
} |
if( cmd == SETVALUE ) |
{ |
if( newvalue ) bit8 = !bit7; // Bit 8 != Bit 7: Disable dec. Calc AN |
else bit8 = bit7; // Bit 8 == Bit 7: Disable dec. Calc AUS |
if( bit8 ) value = (value | 0x80); // Bit 8 setzen |
else value = (value & (0x80 ^ 0xff)); // Bit 8 loeschen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
//--------------------------------------- |
// transform_ComingHomeOrientation() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// |
// ist in dem Byte von param_ServoCompInvert in den Bits 4 & 5 einkodiert, Bits 1-3 bleiben unverändert |
//--------------------------------------- |
uint8_t save =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
if( cmd == GETVALUE ) |
{ |
save = value; // altes "gemeinsames" Byte ServoCompInvert sichern |
value =((value & 0x18) >> 3); // CominghomeOrientation Bits nach rechts verschieben |
} |
if( cmd == SETVALUE ) |
{ |
value = ((newvalue << 3)|(save & 0x07)); // CominghomeOrientation Bits nach links verschieben und "alte" Bits 1-3 wieder dazufügen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
// |
// |
////--------------------------------------- |
//// transform_ValueACCZ() |
//// |
//// PARAMETER: |
//// cmd : GETVALUE || SETVALUE |
//// value : |
//// newvalue: nur wenn cmd == SETVALUE |
//// |
//// |
//// ACC Z Landing Pulse Wert für die Anzeige x 4 |
////--------------------------------------- |
// |
// |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, unint16_t newvalue ) |
// |
//uint16_t save16 =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
// |
//{ |
// if( cmd == GETVALUE ) |
// { |
// save16 = value; // altes Byte ACC Z Landing Pulse sichern |
// value = (value * 4); // Wert x 4 |
// } |
// |
// if( cmd == SETVALUE ) |
// { |
// value = (newvalue / 4); // Neuer Wert ACC Z Landingpulse /4 |
// } |
// |
// |
// return value; |
//} |
//############################################################################################# |
//# 4a. interne Zugriffsfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// paramSubID: 'echter' Parameter zum Suchen in paramSubItem[] zum |
// nachfolgeden Parameter geben das Suchergebnis zurueck |
// und sind alle 0 wenn nichts gefunden wurde |
// |
// Rueckgabe: |
// true/false |
//-------------------------------------------------------------- |
unsigned char _paramset_getsubitemid( unsigned char paramSubID, unsigned char *paramID, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char id; |
unsigned char *p; |
unsigned char von_Revision; |
unsigned char bis_Revision; |
*paramID = 0; |
*subType = 0; |
*subIndex = 0; |
p = (unsigned char *) paramSubItem; |
while( true ) |
{ |
id = pgm_read_byte(p+0); // paramSubItem[..].paramSubID - die aktuelle paramSubID |
if( (id == paramSubID) ) // gefunden? |
{ |
von_Revision = pgm_read_byte(p+4); // paramSubItem[..].von_Revision; |
bis_Revision = pgm_read_byte(p+5); // paramSubItem[..].bis_Revision; |
// in aktueller FC-Revision vorhanden? |
if( von_Revision && (MKVersion.paramsetRevision < von_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
if( bis_Revision && (MKVersion.paramsetRevision > bis_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
*paramID = pgm_read_byte(p+1); // paramSubItem[..].paramID; |
*subType = pgm_read_byte(p+2); // paramSubItem[..].subType; |
*subIndex = pgm_read_byte(p+3); // paramSubItem[..].subIndex; |
return true; |
} |
if( id == param_EOF ) break; |
p += sizeof( paramSubItem_t ); |
} |
return false; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize: false = offset zurueckgeben (normaler Modus) |
// true = size von paramID zurueckgeben (fuer paramSize()) |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffsetX( unsigned char paramID, unsigned char retItemSize, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char *p; |
unsigned char offset; |
unsigned char size; |
unsigned char this_paramID; |
unsigned char paramSubID; |
*subType = SUBTYPE_NO; // =0 |
*subIndex = 0; |
size = 0; |
offset = param_NOTFOUND; |
if( MKVersion.paramsetOK ) |
{ |
//----------------------- |
// eine paramSubID wurde uebergeben |
// -> ermittle zugehoerige paramID! |
//----------------------- |
if( (paramID >= param_SUBITEM) && (paramID != param_EOF) ) |
{ |
paramSubID = paramID; |
if( !_paramset_getsubitemid( paramSubID, ¶mID, subType, subIndex ) ) |
{ |
// keine paramID gefunden... |
if( retItemSize ) return size; // size == 0 |
else return offset; // offset == param_NOTFOUND |
} |
} |
//----------------------- |
// paramID suchen |
//----------------------- |
offset = 0; |
p = (unsigned char *) paramsetRevTable; |
while( true ) |
{ |
this_paramID = pgm_read_byte(p); // die aktuelle paramID |
size = pgm_read_byte(p+1); // size von paramID |
// gefunden oder Ende |
if( (this_paramID == paramID) || (this_paramID == param_EOF) ) |
{ |
break; |
} |
offset += size; |
p += sizeof( paramRevItem_t ); |
} |
// paramID nicht gefunden? |
if( (this_paramID == param_EOF) && (paramID != param_EOF) ) |
{ |
offset = param_NOTFOUND; |
size = 0; |
} |
} |
if( retItemSize ) return size; |
else return offset; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize == false: offset zurueckgeben (normaler Modus) |
// retItemSize == true : size von paramID zurueckgeben |
// --> fuer paramSize() |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffset( unsigned char paramID, unsigned char retItemSize ) |
{ |
unsigned char subType; |
unsigned char subIndex; |
return _paramset_getoffsetX( paramID, retItemSize, &subType, &subIndex ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// sucht/prueft ob eine Transform-Funktion vorhanden ist |
// |
// RUECKGABE: |
// Index: (Num 0..n) auf paramTransform[] |
// => param_EOF (=255) wenn keine transform-Funktion vorhanden |
//-------------------------------------------------------------- |
unsigned char _paramset_gettransformIndex( unsigned char paramID ) |
{ |
unsigned char i; |
i = 0; |
while( paramTransform[i].paramID != param_EOF ) |
{ |
if( paramTransform[i].paramID == paramID ) return i; |
if( paramTransform[i].paramID == param_EOF ) return param_EOF; |
i++; |
} |
return param_EOF; |
} |
//############################################################################################# |
//# 4b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# |
//# Hier sind die Getter/Setter um auf die einzelnen Werte der paramset-Struktur zuzugreifen. |
//# Dazu muss vorher das richtige paramset initialisert worden sein! |
//# |
//# ACHTUNG! |
//# Fuer die Daten wird direkt auf den RX-Buffer vom PKT zugegriffen! |
//# ==> der Buffer darf NICHT durch andere Datenpakete bereits wieder ueberschrieben worden sein! |
//# |
//# Anmerkung: |
//# Das obige 'Achtung' koennte man aendern durch eine Kopie des RX-Buffers. Das verbraucht |
//# zusaetzlichen RAM-Speicher -> probieren wir es erstmal so... |
//############################################################################################# |
//-------------------------------------------------------------- |
// size = paramSize( paramID ) |
//-------------------------------------------------------------- |
unsigned char paramSize( unsigned char paramID ) |
{ |
return( _paramset_getoffset(paramID,true) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// exist = paramExist( paramID ) |
// |
// Rueckgabe: |
// true : die paramID existiert im aktuellen paramset |
// false : die paramID existiert nicht |
//-------------------------------------------------------------- |
unsigned char paramExist( unsigned char paramID ) |
{ |
return( (_paramset_getoffset(paramID,true) != 0) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// pointer = paramGet_p( paramID ) |
// |
// ACHTUNG! Nicht faehig fuer Transform! |
// |
// RUECKGABE: |
// pointer direkt auf~ein Byte im Parameterset |
// (paramSubID nicht moeglich!) |
//-------------------------------------------------------------- |
unsigned char *paramGet_p( unsigned char paramID ) |
{ |
unsigned char offset; |
offset = _paramset_getoffset( paramID, false ); // false = offset zurueckgeben |
if( offset != param_NOTFOUND ) |
{ |
return( (unsigned char *) (mkparamset + offset) ); // Rueckgabe: Pointer auf die Daten |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// value = paramGet( paramID ) |
// |
// holt den Wert von paramID. paramSubID's werden dabei |
// unterstuetzt (Bit- und Bytefelder). |
// |
// Bei Bit-Feldern (z.B. GlobalConfig3) wird eine enstprechende |
// Bit-Maskierung automatisch durchgefuehrt |
// Ergebnis: true (=1) oder false (=0)false (=0) |
// |
// Bei Byte-Feldern (z.B. Kanalbelegung) wird der Wert des |
// entsprechenden Byte's zurueckgegeben |
// |
// Hinweis: anhand des Rueckgabewertes kann nicht ueberprueft |
// werden ob die paramID gefunden wurde bzw. existiert! |
// -> ggf. erst mit paramExist() pruefen ob der Wert existiert! |
//-------------------------------------------------------------- |
unsigned char paramGet( unsigned char paramID ) |
{ |
unsigned char offset; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char value; |
unsigned char transformIdx; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
value = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen (paramID) |
//------------------- |
// subIndex: Bit |
//------------------- |
if( subType == SUBTYPE_BIT ) |
{ |
value = ((value & subIndex) ? true : false); // Rueckgabe: true/false des gewaehlten Bit's |
} |
//------------------- |
// subIndex: Bit negiert |
//------------------- |
if( subType == SUBTYPE_BITN ) |
{ |
value = ((value & subIndex) ? false : true); // Rueckgabe: true/false des gewaehlten Bit's (negiert) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
value = (unsigned char) *(mkparamset + (offset+subIndex)); // subIndex Inhalt lesen |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
value = paramTransform[transformIdx].transformfunc( GETVALUE, value, value); |
} |
return value; // ubyte zurueckgeben |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ) |
{ |
unsigned char offset; |
unsigned char subOffset = 0; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char byte; |
unsigned char value; |
unsigned char transformIdx; |
value = newvalue; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
//------------------- |
// subIndex: Bit |
//------------------- |
if( (subType == SUBTYPE_BIT) || (subType == SUBTYPE_BITN) ) |
{ |
byte = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen |
if( subType == SUBTYPE_BITN ) // SUBTYPE_BITN = negiertes BIT |
value = (value ? false : true); // negieren |
if(value) byte = byte | subIndex; // Bit setzen |
else byte = byte & (subIndex ^ 0xff); // Bit loeschen |
value = byte; // neues Value fuer das gesamte Byte (mit eingerechnetem BIT) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
subOffset = subIndex; // das 'verschobene' Byte (hier nur den Offset verschieben) |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
byte = (unsigned char) *(mkparamset + offset + subOffset); // offset Inhalt lesen |
value = paramTransform[transformIdx].transformfunc( SETVALUE, byte, newvalue); |
} |
//------------------- |
// Byte speichern |
//------------------- |
*(mkparamset + offset + subOffset) = value; |
return true; // OK |
} |
//############################################################################################# |
//# 4c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# |
//# Init und Abfrage der Groesse in Bytes ddes gesamten Paramset's |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = paramsetInit( *pData ) |
// |
// Ermittelt zu einer empfangenen Revision die passende paramset-Tabelle |
// und setzt entsprechende Werte in MKVersion |
// |
// Wird von MK_load_setting() aufgerufen nach dem Einlesen der |
// MK-Parameter (Setting-Request 'q') |
// |
// Parameter: |
// *pData : Zeiger direkt auf den RX-Buffer des PKT |
// |
// Rueckgabe: |
// true : OK - Paramset ist initialisert; die aktuelle FC-Revision |
// steht in MKVersion.paramsetRevision |
// false : Fehler - keine passende paramset-Tabelle gefunden |
//-------------------------------------------------------------- |
unsigned char paramsetInit( unsigned char *pData ) |
{ |
unsigned char i; |
unsigned char *p; |
paramsetRevTable = 0; |
mkparamset = 0; |
MKVersion.paramsetOK = false; |
MKVersion.paramsetRevision = 0; |
MKVersion.mksetting = 0; |
MKVersion.mksettingName[0] = 0; |
//-------------------------------------------------- |
// pData == 0 -> nur 'reset' der Daten |
//-------------------------------------------------- |
if( pData == 0 ) |
{ |
return 0; |
} |
//-------------------------------------------------- |
// diese beiden Werte koennen in jedem Fall gesetzt werden |
// unabhaengig ob das gefundene Parameterset vom PKT |
// unterstuetzt wird oder nicht |
//-------------------------------------------------- |
MKVersion.mksetting = (unsigned char) *pData; |
MKVersion.paramsetRevision = (unsigned char) *(pData + 1); |
//-------------------------------------------------- |
// FC-Revision in paramsetRevMap suchen |
//-------------------------------------------------- |
i = 0; |
while( true ) |
{ |
if( (paramsetRevMap[i].Revision == MKVersion.paramsetRevision) || (paramsetRevMap[i].Revision == MAPEOF) ) |
break; |
i++; |
} |
//-------------------------------------- |
// Revision nicht gefunden -> return 0 |
//-------------------------------------- |
if( paramsetRevMap[i].Revision == MAPEOF ) |
{ |
return 0; |
} |
//-------------------------------------- |
// Revision gefunden |
//-------------------------------------- |
mkparamset = (unsigned char *) (pData + 1); |
paramsetRevTable = (paramRevItem_t *) paramsetRevMap[i].RevisionTable; |
MKVersion.paramsetOK = true; |
//-------------------------------------- |
// den aktuellen Setting-Namen kopieren |
// wird u.a. vom OSD verwendet |
//-------------------------------------- |
p = paramGet_p( param_Name ); |
if( p ) memcpy( MKVersion.mksettingName, p, 12 ); |
return MKVersion.paramsetOK; |
} |
//-------------------------------------------------------------- |
// size = paramsetSize() |
// |
// Gibt die Groesse des gesamten aktuellen Parameterset's |
// zurueck. Fuer Revision 098 z.B. 130 = 130 Bytes. |
// |
// Rueckgabe: |
// =0 : Fehler; nicht unterstuetzte FC-Revision oder nicht |
// initialisiert |
//-------------------------------------------------------------- |
unsigned char paramsetSize( void ) |
{ |
if( MKVersion.paramsetOK ) |
return _paramset_getoffset( param_EOF, false ); // false = offset zurueckgeben; der offset von param_EOF entspricht der Groesse des paramSet's! |
else |
return 0; |
} |
//############################################################################################# |
//# x. TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
// TEST / DEBUG |
//-------------------------------------------------------------- |
#ifdef DEBUG_PARAMSET |
void paramsetDEBUG( void ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
//memcpy( &MKVersion.NCVersion, (Version_t *) (pRxData), sizeof(MKVersion_t) ); |
/* |
typedef struct |
{ |
Version_t NCVersion; // |
Version_t FCVersion; // |
unsigned char paramsetRevision; // von der FC |
unsigned char paramsetOk; // true wenn Revision in paramset.c vorhanden |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
*/ |
//setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
//---------------------------------- |
// Anzeige: FC/NC Version |
//---------------------------------- |
lcdx_printp_at( 0, 0, PSTR("FC:"), MNORMAL, 0,0); |
MKVersion_print_at( 3, 0, FC, MNORMAL, 0,0); |
lcdx_printp_at( 13, 0, PSTR("NC:"), MNORMAL, 0,0); |
MKVersion_print_at( 16, 0, NC, MNORMAL, 0,0); |
//---------------------------------- |
// Anzeige: FC Revision |
//---------------------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("Rev: %03u"), MKVersion.paramsetRevision ); |
if( MKVersion.paramsetOK ) |
lcdx_printp_at( 8, 1, PSTR(" ok!"), MINVERS, 0,0); |
else |
lcdx_printp_at( 8, 1, PSTR(" err"), MINVERS, 0,0); |
// Size von paramset |
lcd_printf_at_P( 13, 1, MNORMAL, PSTR("Size=%3u"), paramsetSize() ); |
//---------------------------------- |
// Anzeige: aktuelles MK Setting |
//---------------------------------- |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("Set:%2u %s"), MKVersion.mksetting, MKVersion.mksettingName ); |
/* |
unsigned char *p; |
p = paramGet_p( param_Name ); |
if( p ) |
{ |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 1, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 2, 3, MNORMAL, PSTR("%c"), *p ); |
} |
*/ |
//---------------------------------- |
//---------------------------------- |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("Size Name:%3u"), paramSize(param_Name) ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
paramSet_UByte( param_Name, 'x' ); |
lcd_printf_at_P( 3, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
*/ |
/* |
uint8_t v; |
v = MKVersion_Cmp( MKVersion.FCVer, 2,0,'f' ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("v:%3u"), v ); |
*/ |
/* |
unsigned char paramID = 1; |
unsigned char paramSubType = 1; |
unsigned char paramSubIndex =1; |
_paramset_getsubitemid( param_GlobalConfig_DrehratenBegrenzer, ¶mID, ¶mSubType, ¶mSubIndex ); |
//lcd_printf_at_P( 0, 4, MNORMAL, PSTR("ID:%u T:%u I:%u"), paramID, paramSubType, paramSubIndex ); |
*/ |
//#define param_ExtraConfig_NoRcOffBeeping 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
//#define param_ExtraConfig_GpsAid 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
//#define param_ExtraConfig_LearnableCarefree 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
//#define param_ExtraConfig_IgnoreMagErrAtStartup 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
//#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
//#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
//paramGet( unsigned char paramID ); |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
//#define param_Kanalbelegung_Gas 242 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
//#define param_Kanalbelegung_Gear 243 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
//#define param_Kanalbelegung_Nick 240 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
//#define param_Kanalbelegung_Roll 241 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
// lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), paramGet(param_Kanalbelegung_Gas), paramGet(param_Kanalbelegung_Gear), paramGet(param_Kanalbelegung_Nick), paramGet(param_Kanalbelegung_Roll) ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u"), paramGet(param_Kanalbelegung) ); |
/* |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
unsigned char v; |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
*/ |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%u %u"), |
paramGet( param_Kanalbelegung_Gas ), |
paramGet( param_Kanalbelegung_Gear ) |
); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti1 ), |
paramGet( param_Kanalbelegung_Poti2 ), |
paramGet( param_Kanalbelegung_Poti3 ), |
paramGet( param_Kanalbelegung_Poti4 ) |
); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti5 ), |
paramGet( param_Kanalbelegung_Poti6 ), |
paramGet( param_Kanalbelegung_Poti7 ), |
paramGet( param_Kanalbelegung_Poti8 ) |
); |
*/ |
// lcd_printf_at_P( 0, 3, MNORMAL, PSTR("HR:%u H-Limit:%u"), |
// paramGet( param_GlobalConfig_HoehenRegelung ), |
// paramGet( param_ExtraConfig_HeightLimit ) |
// ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Auto-S-L:%u"), |
// paramGet( param_StartLandChannel ) |
// ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("CH Orientation:%x"), paramGet( param_ServoCompInvert)); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("ACC Land pulse:%x"), paramGet( param_LandingPulse)); |
; |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
#endif // #ifdef DEBUG_PARAMSET |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings/paramset.h |
---|
0,0 → 1,357 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.h |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: #define param_SingleWpControlChannel (FC2.11a) |
//# #define param_MenuKeyChannel (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: #define param_ServoNickFailsave, #define param_ServoRollFailsave, #define param_Servo3Failsave |
//# #define param_Servo4Failsave, #define param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: #define param_ComingHomeOrientation 241 ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: paramID fuer Rev.103 (FC 2.07f) |
//# -> param_Hoehe_TiltCompensation |
//# |
//# 08.04.2014 OG |
//# - add: paramID's fuer Rev.102 (FC 2.05g) |
//# -> param_NaviMaxFlyingRange, param_NaviDescendRange |
//# |
//# 06.04.2014 OG |
//# - add: paramID's fuer Rev.101 |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: paramID's fuer Rev.100 |
//# -> param_AutoPhotoDistance, param_AutoPhotoAtitudes |
//# -> param_SingleWpSpeed |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 27.02.2014 OG |
//# - del: paramID's fuer FC-Revisionen < 95 entfernt |
//# |
//# 27.02.2014 OG |
//# - chg: paramGet(), paramSet() - Unterstuetzen auch Bit- und Bytefelder |
//# |
//# 26.02.2014 OG |
//# - add: defines fuer die Unterstuetzung von paramSubID's ergaenzt |
//# paramSubItems bieten direkten Zugriff auf Bit- und Bytefelder |
//# von paramID's - die paramSubItems wurde in die defines der |
//# paramID's integriert indem sie am Ende gelistet werden (>=160) |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 25.02.2014 OG |
//# - add: defines zu param_Kanalbelegung_xyz (240 bis 251) |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
#ifndef _PARAMSET_H |
#define _PARAMSET_H |
//--------------------------- |
// Typ einer paramSubID |
//--------------------------- |
#define SUBTYPE_NO 0 // keine paramSubID |
#define SUBTYPE_BIT 1 // Bit-Feld (z.B. param_GlobalConfig) |
#define SUBTYPE_BITN 2 // Bit-Feld (z.B. param_GlobalConfig) negiert! |
#define SUBTYPE_BYTE 3 // Byte-Feld (z.B. Kanalbelegung) |
//######################################################################################################################################################## |
//---------------------------------------------------------------------------------------- |
// Die Reihenfolge ist egal - aber(!) darauf achten das keine ID doppelt vergeben wird! |
// Wenn neue Feleder hinzukommen an das Ende (vor EOF - keine ID 255!) hinzufuegen! |
// Bitte die Datentypen im Kommentar notieren! |
// |
// -> paramID |
//---------------------------------------------------------------------------------------- |
#define param_Revision 1 // unsigned char |
#define param_Kanalbelegung 2 // unsigned char [12] GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
#define param_GlobalConfig 3 // unsigned char 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
#define param_Hoehe_MinGas 4 // unsigned char Wert : 0-100 |
#define param_Luftdruck_D 5 // unsigned char Wert : 0-250 |
#define param_HoeheChannel 6 // unsigned char Wert : 0-32 |
#define param_Hoehe_P 7 // unsigned char Wert : 0-32 |
#define param_Hoehe_Verstaerkung 8 // unsigned char Wert : 0-50 |
#define param_Hoehe_ACC_Wirkung 9 // unsigned char Wert : 0-250 |
#define param_Hoehe_HoverBand 10 // unsigned char Wert : 0-250 |
#define param_Hoehe_GPS_Z 11 // unsigned char Wert : 0-250 |
#define param_Hoehe_StickNeutralPoint 12 // unsigned char Wert : 0-250 |
#define param_Stick_P 13 // unsigned char WERT : 0..20 |
#define param_Stick_D 14 // unsigned char WERT : 0..20 |
#define param_StickGier_P 15 // unsigned char Wert : |
#define param_Gas_Min 16 // unsigned char Wert : 0-32 |
#define param_Gas_Max 17 // unsigned char Wert : 33-250 |
#define param_GyroAccFaktor 18 // unsigned char Wert : 1-64 |
#define param_KompassWirkung 19 // unsigned char Wert : 0-32 |
#define param_Gyro_P 20 // unsigned char Wert : 10-250 |
#define param_Gyro_I 21 // unsigned char Wert : 0-250 |
#define param_Gyro_D 22 // unsigned char Wert : 0-250 |
#define param_Gyro_Gier_P 23 // unsigned char Wert : 10-250 |
#define param_Gyro_Gier_I 24 // unsigned char Wert : 0-250 |
#define param_Gyro_Stability 25 // unsigned char Wert : 0-16 |
#define param_UnterspannungsWarnung 26 // unsigned char Wert : 0-250 |
#define param_NotGas 27 // unsigned char Wert : 0-250 //Gaswert bei Empängsverlust |
#define param_NotGasZeit 28 // unsigned char Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
#define param_Receiver 29 // unsigned char 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
#define param_I_Faktor 30 // unsigned char Wert : 0-250 |
#define param_UserParam1 31 // unsigned char Wert : 0-250 |
#define param_UserParam2 32 // unsigned char Wert : 0-250 |
#define param_UserParam3 33 // unsigned char Wert : 0-250 |
#define param_UserParam4 34 // unsigned char Wert : 0-250 |
#define param_ServoNickControl 35 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoNickComp 36 // unsigned char Wert : 0-250 // Einfluss Gyro/Servo |
#define param_ServoNickMin 37 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoNickMax 38 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoRollControl 39 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoRollComp 40 // unsigned char Wert : 0-250 |
#define param_ServoRollMin 41 // unsigned char Wert : 0-250 |
#define param_ServoRollMax 42 // unsigned char Wert : 0-250 |
#define param_ServoNickRefresh 43 // unsigned char Speed of the Servo |
#define param_ServoManualControlSpeed 44 // unsigned char |
#define param_CamOrientation 45 // unsigned char |
#define param_Servo3 46 // unsigned char Value or mapping of the Servo Output |
#define param_Servo4 47 // unsigned char Value or mapping of the Servo Output |
#define param_Servo5 48 // unsigned char Value or mapping of the Servo Output |
#define param_LoopGasLimit 49 // unsigned char Wert: 0-250 max. Gas während Looping |
#define param_LoopThreshold 50 // unsigned char Wert: 0-250 Schwelle für Stickausschlag |
#define param_LoopHysterese 51 // unsigned char Wert: 0-250 Hysterese für Stickausschlag |
#define param_AchsKopplung1 52 // unsigned char Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
#define param_AchsKopplung2 53 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_CouplingYawCorrection 54 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_WinkelUmschlagNick 55 // unsigned char Wert: 0-250 180°-Punkt |
#define param_WinkelUmschlagRoll 56 // unsigned char Wert: 0-250 180°-Punkt |
#define param_GyroAccAbgleich 57 // unsigned char 1/k (Koppel_ACC_Wirkung) |
#define param_Driftkomp 58 // unsigned char |
#define param_DynamicStability 59 // unsigned char |
#define param_UserParam5 60 // unsigned char Wert : 0-250 |
#define param_UserParam6 61 // unsigned char Wert : 0-250 |
#define param_UserParam7 62 // unsigned char Wert : 0-250 |
#define param_UserParam8 63 // unsigned char Wert : 0-250 |
#define param_J16Bitmask 64 // unsigned char for the J16 Output |
#define param_J16Timing 65 // unsigned char for the J16 Output |
#define param_J17Bitmask 66 // unsigned char for the J17 Output |
#define param_J17Timing 67 // unsigned char for the J17 Output |
#define param_WARN_J16_Bitmask 68 // unsigned char for the J16 Output |
#define param_WARN_J17_Bitmask 69 // unsigned char for the J17 Output |
#define param_NaviOut1Parameter 70 // unsigned char for the J16 Output |
#define param_NaviGpsModeChannel 71 // unsigned char Parameters for the Naviboard |
#define param_NaviGpsGain 72 // unsigned char |
#define param_NaviGpsP 73 // unsigned char |
#define param_NaviGpsI 74 // unsigned char |
#define param_NaviGpsD 75 // unsigned char |
#define param_NaviGpsPLimit 76 // unsigned char |
#define param_NaviGpsILimit 77 // unsigned char |
#define param_NaviGpsDLimit 78 // unsigned char |
#define param_NaviGpsA 79 // unsigned char |
#define param_NaviGpsMinSat 80 // unsigned char |
#define param_NaviStickThreshold 81 // unsigned char |
#define param_NaviWindCorrection 82 // unsigned char |
#define param_NaviAccCompensation 83 // unsigned char New since 0.86 -> was: SpeedCompensation |
#define param_NaviOperatingRadius 84 // unsigned char bis Rev. 101 |
#define param_NaviAngleLimitation 85 // unsigned char |
#define param_NaviPH_LoginTime 86 // unsigned char |
#define param_ExternalControl 87 // unsigned char for serial Control |
#define param_OrientationAngle 88 // unsigned char Where is the front-direction? |
#define param_CareFreeChannel 89 // unsigned char switch for CareFree |
#define param_MotorSafetySwitch 90 // unsigned char |
#define param_MotorSmooth 91 // unsigned char |
#define param_ComingHomeAltitude 92 // unsigned char |
#define param_FailSafeTime 93 // unsigned char |
#define param_MaxAltitude 94 // unsigned char |
#define param_FailsafeChannel 95 // unsigned char if the value of this channel is > 100, the MK reports "RC-Lost" |
#define param_ServoFilterNick 96 // unsigned char |
#define param_ServoFilterRoll 97 // unsigned char |
#define param_StartLandChannel 98 // unsigned char |
#define param_LandingSpeed 99 // unsigned char |
#define param_CompassOffset 100 // unsigned char |
#define param_AutoLandingVoltage 101 // unsigned char in 0,1V 0 -> disabled |
#define param_ComingHomeVoltage 102 // unsigned char in 0,1V 0 -> disabled |
#define param_BitConfig 103 // unsigned char (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
#define param_ServoCompInvert 104 // unsigned char 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
#define param_ExtraConfig 105 // unsigned char bitcodiert |
#define param_GlobalConfig3 106 // unsigned char bitcodiert |
#define param_Name 107 // char [12] Name vom Parameterset |
#define param_crc 108 // unsigned char |
#define param_AutoPhotoDistance 109 // unsigned char ab Rev. 100 (ersetzt NaviOut1Parameter) // Auto Photo |
#define param_AutoPhotoAtitudes 110 // unsigned char ab Rev. 100 |
#define param_SingleWpSpeed 111 // unsigned char ab Rev. 100 |
#define param_Servo3OnValue 112 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo3OffValue 113 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OnValue 114 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OffValue 115 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_NaviMaxFlyingRange 116 // unsigned char ab Rev. 102 (FC 2.05g) (ersetzt NaviOperatingRadius) // in 10m |
#define param_NaviDescendRange 117 // unsigned char ab Rev. 102 (FC 2.05g) |
#define param_Hoehe_TiltCompensation 118 // unsigned char ab Rev. 103 (FC 2.07f) |
#define param_LandingPulse 119 // unsigned char ab Rev. 104 (FC 2.09d) |
#define param_ServoFS_Pos 120 // unsigned char [5] ServoNickFailsave[0],ServoRollFailsave[1],Servo3Failsave[2],Servo4Failsave[3],Servo5Failsave[4] ab Rev. 105 (FC 2.09i) |
#define param_SingleWpControlChannel 121 // unsigned char ab Rev. 106 (FC 2.11a) |
#define param_MenuKeyChannel 122 // unsigned char ab Rev. 106 (FC 2.11a) |
//------- ^^^ HIER NEUE paramID's einfuegen! ^^^ |
// ab hier sind Sub-Items zu einer paramID definiert |
// - einige paramID's werden nochmals untergliedert in Bit- oder Bytefelder |
// - die Zuordnung der Sub-Items zu paramID's erfolgt in paramset.c |
#define param_SUBITEM 160 // <- ab diesem Wert handelt es sich um ein paramSubID und nicht um eine paramID |
#define param_Kanalbelegung_Nick 170 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
#define param_Kanalbelegung_Roll 171 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
#define param_Kanalbelegung_Gas 172 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
#define param_Kanalbelegung_Gear 173 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
#define param_Kanalbelegung_Poti1 174 // SUBTYPE_BYTE - Kanalbelegung [4] (unsigned char) |
#define param_Kanalbelegung_Poti2 175 // SUBTYPE_BYTE - Kanalbelegung [5] (unsigned char) |
#define param_Kanalbelegung_Poti3 176 // SUBTYPE_BYTE - Kanalbelegung [6] (unsigned char) |
#define param_Kanalbelegung_Poti4 177 // SUBTYPE_BYTE - Kanalbelegung [7] (unsigned char) |
#define param_Kanalbelegung_Poti5 178 // SUBTYPE_BYTE - Kanalbelegung [8] (unsigned char) |
#define param_Kanalbelegung_Poti6 179 // SUBTYPE_BYTE - Kanalbelegung [9] (unsigned char) |
#define param_Kanalbelegung_Poti7 180 // SUBTYPE_BYTE - Kanalbelegung [10] (unsigned char) |
#define param_Kanalbelegung_Poti8 181 // SUBTYPE_BYTE - Kanalbelegung [11] (unsigned char) |
#define param_ServoCompInvert_SERVO_NICK_INV 190 // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
#define param_ServoCompInvert_SERVO_ROLL_INV 191 // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
#define param_ServoCompInvert_SERVO_RELATIVE 192 // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
#define param_ExtraConfig_HeightLimit 200 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
#define param_ExtraConfig_HeightVario 201 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 ist negiert zu param_ExtraConfig_HeightLimit |
#define param_ExtraConfig_VarioBeep 202 // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
#define param_ExtraConfig_SensitiveRc 203 // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
#define param_ExtraConfig_33vReference 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
#define param_ExtraConfig_NoRcOffBeeping 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
#define param_ExtraConfig_GpsAid 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
#define param_ExtraConfig_LearnableCarefree 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
#define param_ExtraConfig_IgnoreMagErrAtStartup 208 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
#define param_BitConfig_LoopOben 210 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
#define param_BitConfig_LoopUnten 211 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
#define param_BitConfig_LoopLinks 212 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
#define param_BitConfig_LoopRechts 213 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
#define param_BitConfig_MotorBlink1 214 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
#define param_BitConfig_MotorOffLed1 215 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
#define param_BitConfig_MotorOffLed2 216 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
#define param_BitConfig_MotorBlink2 217 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
#define param_GlobalConfig3_DphMaxRadius 221 // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 nur bis Rev 101 |
#define param_GlobalConfig3_VarioFailsafe 222 // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
#define param_GlobalConfig3_MotorSwitchMode 223 // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
#define param_GlobalConfig3_UseNcForOut1 225 // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
#define param_GlobalConfig3_SpeakAll 226 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
#define param_GlobalConfig3_ServoNickCompOff 227 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
#define param_GlobalConfig_HoehenRegelung 230 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
#define param_GlobalConfig_HoehenSchalter 231 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
#define param_GlobalConfig_HeadingHold 232 // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
#define param_GlobalConfig_KompassAktiv 233 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
#define param_GlobalConfig_KompassFix 234 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
#define param_GlobalConfig_GpsAktiv 235 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
#define param_GlobalConfig_AchsenkopplungAktiv 236 // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define param_GlobalConfig_DrehratenBegrenzer 237 // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
#define param_ServoNickFailsave 238 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[0] ab FC 2.09j |
#define param_ServoRollFailsave 239 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[1] ab FC 2.09j |
#define param_Servo3Failsave 240 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[2] ab FC 2.09j |
#define param_Servo4Failsave 241 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[3] ab FC 2.09j |
#define param_Servo5Failsave 242 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[4] ab FC 2.09j |
// hier: spezielle Subitems |
#define param_CompassOffset_DisableDeclCalc 243 // wird gemappt auf: param_CompassOffset - der Wert ist dort in Bit 8 und 7 kodiert |
#define param_ComingHomeOrientation 244 // unsigned char 0x08, 0x10, Bits im Feld ServoCompInvert |
//--------------------------------------------------------------------------------------- |
// IMMER am Ende!! |
#define param_DUMMY 254 // DO NOT CHANGE! DUMMY => spacer werte |
#define param_EOF 255 // DO NOT CHANGE! EOF => markiert das Ende von paramID-Listen |
#define param_NOTFOUND 255 // DO NOT CHANGE! NOTFOUND => kein paramID gefunden |
//######################################################################################################################################################## |
//--------------------------- |
// struct: einzelnes Parameter-Item |
//--------------------------- |
typedef struct |
{ |
unsigned char paramID; |
unsigned char size; |
} paramRevItem_t; |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char paramsetInit( unsigned char *pData ); |
unsigned char paramsetSize( void ); |
unsigned char paramExist( unsigned char paramID ); |
unsigned char paramSize( unsigned char paramID ); |
unsigned char paramGet( unsigned char paramID ); |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ); |
unsigned char *paramGet_p( unsigned char paramID ); |
// TEST / DEBUG |
void paramsetDEBUG( void ); |
#endif // _PARAMSET_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/mksettings |
---|
Property changes: |
Added: svn:ignore |
+_FC_eeprom_Versions_Archiv |
+ |
+_old_source |
+ |
+mkparameters.c_sic |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/motortest/motortest.c |
---|
0,0 → 1,396 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY motortest.c |
//# |
//# 13.05.2014 OG |
//# - chg: motor_test() - #ifdef USE_I2CMOTORTEST erweitert um unsigned int SerLoop; |
//# wegen Warning: variable set but not used |
//# |
//# 12.02.2014 OG |
//# - chg: motor_test() Verschiebung von var 'buffer' wegen "unused variable 'buffer'" |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "motortest.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../motortest/twimaster.h" |
//#include "menu.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../messages.h" |
uint8_t m; |
uint8_t mmode; // 0=Value 1=Motor |
uint8_t v; |
volatile uint8_t i2c_state; |
volatile uint8_t motor_addr = 0; |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// Senden der Motorwerte per I2C-Bus |
// |
void SendMotorData(uint8_t m,uint8_t v) |
{ |
if (m==0) |
for(m=0;m<MAX_MOTORS;m++) // alle Motoren |
{ |
// Motor[m].SetPoint = MotorTest[m]; |
Motor[m].SetPoint = v; |
Motor[m].SetPointLowerBits = 0; |
// Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution |
// Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
} |
else |
{ |
Motor[m-1].SetPoint = v; |
Motor[m-1].SetPointLowerBits = 0; |
} |
if(I2C_TransferActive) |
I2C_TransferActive = 0; // enable for the next time |
else |
{ |
motor_write = 0; |
I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
} |
} |
//-------------------------------------------------------------- |
// |
void Search_BL (void) |
{ |
uint8_t i = 0; |
unsigned int timer; |
lcd_cls (); |
MotorenEin =0; |
MotorTest[i] = 0; |
lcd_printp (PSTR("Suche BL-Ctrl"), 0); |
// Check connected BL-Ctrls |
BLFlags |= BLFLAG_READ_VERSION; |
motor_read = 0; // read the first I2C-Data |
SendMotorData(0,0); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
timer = SetDelay(1); |
for(i=0; i < MAX_MOTORS; i++) |
{ |
SendMotorData(i,0); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
{ |
lcd_printp_at (0, 1, PSTR("Found BL-Ctrl:"), 0); |
lcd_print_hex_at (18,1,i,0); |
lcd_printp_at (0, 2, PSTR("Version:"), 0); |
lcd_print_hex_at (8,2,Motor[i].Version,0); |
lcd_printp_at (11, 2, PSTR("SetPoi:"), 0); |
lcd_print_hex_at (18,2,Motor[i].SetPoint,0); |
lcd_printp_at (0, 3, PSTR("SetPoiL:"), 0); |
lcd_print_hex_at (8,3,Motor[i].SetPointLowerBits,0); |
lcd_printp_at (11, 3, PSTR("State :"), 0); |
lcd_print_hex_at (18,3,Motor[i].State,0); |
lcd_printp_at (0, 4, PSTR("ReadMod:"), 0); |
lcd_print_hex_at (8,4,Motor[i].ReadMode,0); |
lcd_printp_at (11, 4, PSTR("Currnt:"), 0); |
lcd_print_hex_at (18,4,Motor[i].Current,0); |
lcd_printp_at (0, 5, PSTR("MaxPWM :"), 0); |
lcd_print_hex_at (8,5,Motor[i].MaxPWM,0); |
lcd_printp_at (11, 5, PSTR("Temp :"), 0); |
write_ndigit_number_u (18,5,Motor[i].Temperature,3,1,0); |
} |
} //End For I |
} |
#endif |
//-------------------------------------------------------------- |
// |
void motor (uint8_t m,uint8_t v) |
{ |
memset (buffer, 0, 16); |
if(m == 0) |
{ |
memset (buffer, v, 16); |
} |
else |
{ |
buffer[m-1] = v; |
} |
SendOutData('t', ADDRESS_FC, 1, buffer, 16); |
} |
//-------------------------------------------------------------- |
// |
void motor_test (uint8_t MotorMode) |
{ |
lcd_cls (); |
mmode = 1; // 1=Motor |
m = 1; |
v = 0; |
#ifdef USE_I2CMOTORTEST |
unsigned int SerLoop; |
SerLoop = 10; |
char buffer[7]; |
if (MotorMode == I2C_Mode) |
{ |
Search_BL(); |
do |
{ |
lcd_printp_at (11, 7, PSTR("Ende Check"), 0); |
if (get_key_press (1 << KEY_ESC)) |
{ |
get_key_press(KEY_ALL); |
return; |
} |
} |
while (!get_key_press (1 << KEY_ENTER)); |
} |
#endif |
lcd_cls(); |
lcd_printp (PSTR(" BL-Ctrl Test "), 2); |
lcd_printp_at (2, 2, PSTR("Motor: 1"), 0); |
lcd_printp_at (2, 3, PSTR("Value: 0"), 0); |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x1a \x1b"), 0); |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
uart1_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r"); |
#endif |
if (MotorMode == FC_Mode) |
{ |
if (hardware == NC && current_hardware == NC) |
{ |
SwitchToFC(); |
} |
} |
do |
{ |
// mmode 0=Value 1=Motor |
if ((mmode == 0) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (v < 254)) |
{ |
v++; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 0) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (v > 0)) |
{ |
if (MotorMode == FC_Mode) |
lcd_frect (((v * (14*8) / 255) + 8), (8*5), ((14*8) / 255), 6, 0); |
v--; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (m < 16)) |
{ |
m++; |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (m > 0)) |
{ |
m--; |
if(m > 0) |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) |
lcd_printp_at (9, 2, PSTR("All"), 0); |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
if (v > 0) |
{ |
m = 0; |
v=0; |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
lcd_cls_line (0, 5, 21); |
if(m > 0) write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) lcd_printp_at (9, 2, PSTR("All"), 0); |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
SendMotorData(m,v); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
} |
#endif |
if(mmode == 0) // 0=Value |
{ |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
lcd_printp_at (0, 3, PSTR(" "), 0); |
mmode = 1; // 1=Motor |
} |
else |
{ |
lcd_printp_at (0, 2, PSTR(" "), 0); |
lcd_printp_at (0, 3, PSTR("\x1d"), 0); |
mmode = 0; // 0=Value |
} |
} |
//if (get_key_press (1 << KEY_ENTER))// |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
SendMotorData(m,v); |
timer = SetDelay(1); |
lcd_printp_at (0, 3, PSTR("SetPoint :"), 0); |
write_ndigit_number_u (13,3,Motor[m-1].SetPoint,3,0,0); |
lcd_printp_at (0, 4, PSTR("Current :"), 0); |
lcd_print_hex_at (13,4,Motor[m-1].Current,0); |
write_ndigit_number_u (13,4,Motor[m-1].Current,3,0,0); |
lcd_printp_at (0, 5, PSTR("Temperature:"), 0); |
write_ndigit_number_u (13,5,Motor[m-1].Temperature,3,0,0); |
lcd_printp_at (0, 6, PSTR("Version:"), 0); |
lcd_print_hex_at (8,6,Motor[m-1].Version,0); |
lcd_printp_at (11, 6, PSTR("State :"), 0); |
lcd_print_hex_at (18,6,Motor[m-1].State,0); |
if (Motor[m-1].SetPoint > 0) |
{ |
if (SerLoop == 0) |
{ |
itoa( m-1, buffer, 10); // convert interger into string (decimal format) |
uart1_puts(buffer); // and transmit string to UART |
uart1_puts(";"); |
itoa( Motor[m-1].Version, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPoint, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPointLowerBits, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].State, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].ReadMode, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Current, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].MaxPWM, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Temperature, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts("\r"); |
uart1_puts("\n"); |
SerLoop =200; |
} |
else |
SerLoop--; |
} |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
else |
#endif |
motor (m,v); //if (MotorMode == I2C_Mode)// |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
if (MotorMode == FC_Mode) |
{ |
motor(0,0); // switch all engines off at exit |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/motortest/motortest.h |
---|
0,0 → 1,46 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _MOTORTEST_H |
#define _MOTORTEST_H |
#define I2C_Mode 1 // Motortest Lokal |
#define FC_Mode 2 // Motortest ueber FC |
void motor_test (uint8_t MotorMode); |
void SendMotorData(uint8_t m,uint8_t v); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/motortest/twimaster.c |
---|
0,0 → 1,523 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) Holger Buss, Ingo Busker |
// + Nur f?r den privaten Gebrauch |
// + www.MikroKopter.com |
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt f?r das gesamte Projekt (Hardware, Software, Bin?rfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur f?r den privaten (nicht-kommerziellen) Gebrauch zul?ssig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best?ckung und Verkauf von Platinen oder Baus?tzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver?ffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m?ssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien ver?ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gew?hr auf Fehlerfreiheit, Vollst?ndigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir ?bernehmen keinerlei Haftung f?r direkte oder indirekte Personen- oder Sachsch?den |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zul?ssig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY twimaster.c |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/twi.h> |
#include <util/delay.h> |
#include "../eeprom/eeprom.h" |
#include "../motortest/twimaster.h" |
#include "../timer/timer.h" |
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint8_t I2C_TransferActive = 0; |
volatile uint16_t I2CTimeout = 100; |
uint8_t MissingMotor = 0; |
uint8_t RequiredMotors = 1; |
char MotorenEin = 0; |
volatile uint8_t BLFlags = 0; |
MotorData_t Motor[MAX_MOTORS]; |
// bit mask for witch BL the configuration should be sent |
volatile uint16_t BLConfig_WriteMask = 0; |
// bit mask for witch BL the configuration should be read |
volatile uint16_t BLConfig_ReadMask = 0; |
// buffer for BL Configuration |
BLConfig_t BLConfig; |
#define I2C_WriteByte(byte) {TWDR = byte; TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define I2C_ReceiveByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);} |
#define I2C_ReceiveLastByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define TWI_BASE_ADDRESS 0x52 |
uint8_t RAM_Checksum(uint8_t* pBuffer, uint16_t len) |
{ |
uint8_t crc = 0xAA; |
uint16_t i; |
for(i=0; i<len; i++) |
{ |
crc += pBuffer[i]; |
} |
return crc; |
} |
//-------------------------------------------------------------- |
// Initialize I2C (TWI) |
// |
void I2C_Init(char clear) |
{ |
#ifdef USE_I2CMOTORTEST |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
// SDA is INPUT |
DDRC &= ~(1<<DDC1); |
// SCL is output |
DDRC |= (1<<DDC0); |
// pull up SDA |
//PORTC |= (1<<PORTC0)|(1<<PORTC1); |
// TWI Status Register |
// prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
// set TWI Bit Rate Register |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; |
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
if(clear) for(i=0; i < MAX_MOTORS; i++) |
{ |
Motor[i].Version = 0; |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
Motor[i].State = 0; |
Motor[i].ReadMode = BL_READMODE_STATUS; |
Motor[i].Current = 0; |
Motor[i].MaxPWM = 0; |
Motor[i].Temperature = 0; |
} |
sei(); |
SREG = sreg; |
#endif |
} |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
void I2C_Reset(void) |
{ |
// stop i2c bus |
I2C_Stop(TWI_STATE_MOTOR_TX); |
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
I2C_TransferActive = 0; |
I2C_Init(0); |
I2C_WriteByte(0); |
BLFlags |= BLFLAG_READ_VERSION; |
} |
#endif |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// I2C ISR |
// |
ISR (TWI_vect) |
{ |
static uint8_t missing_motor = 0, motor_read_temperature = 0; |
static uint8_t *pBuff = 0; |
static uint8_t BuffLen = 0; |
switch (twi_state++) |
{ |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
I2C_TransferActive = 1; |
// skip motor if not used in mixer |
// while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
BLConfig_WriteMask = 0; // reset configuration bitmask |
motor_write = 0; // reset motor write counter for next cycle |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(TWI_BASE_ADDRESS + TW_READ + (motor_read<<1) ); // select slave address in rx mode |
} |
else I2C_WriteByte(TWI_BASE_ADDRESS + TW_WRITE + (motor_write<<1) ); // select slave address in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint |
// if old version has been detected |
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
{ |
twi_state = 4; //jump over sending more data |
} |
// the new version has been detected |
else if(!( (Motor[motor_write].SetPointLowerBits && (RequiredMotors < 7)) || BLConfig_WriteMask || BLConfig_ReadMask ) ) |
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
twi_state = 4; //jump over sending more data |
} |
break; |
case 2: // lower bits of setpoint (higher resolution) |
if ((0x0001<<motor_write) & BLConfig_ReadMask) |
{ |
Motor[motor_write].ReadMode = BL_READMODE_CONFIG; // configuration request |
} |
else |
{ |
Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request |
} |
// send read mode and the lower bits of setpoint |
I2C_WriteByte((Motor[motor_write].ReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
// configuration tranmission request? |
if((0x0001<<motor_write) & BLConfig_WriteMask) |
{ // redirect tx pointer to configuration data |
pBuff = (uint8_t*)&BLConfig; // select config for motor |
BuffLen = sizeof(BLConfig_t); |
} |
else |
{ // jump to end of transmission for that motor |
twi_state = 4; |
} |
break; |
case 3: // send configuration |
I2C_WriteByte(*pBuff); |
pBuff++; |
if(--BuffLen > 0) |
twi_state = 3; // if there are some bytes left |
break; |
case 4: // repeat case 0-4 for all motors |
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received |
{ |
if(!missing_motor) |
missing_motor = motor_write + 1; |
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) |
Motor[motor_write].State++; // increment error counter and handle overflow |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive Data |
case 5: // TWI_STATE_MOTOR_RX |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted but no ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
if(++motor_read >= MAX_MOTORS) |
{ // all motors read |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
} |
else |
{ // motor successfully addressed |
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
{ |
// new BL found |
switch(Motor[motor_read].ReadMode) |
{ |
case BL_READMODE_CONFIG: |
pBuff = (uint8_t*)&BLConfig; |
BuffLen = sizeof(BLConfig_t); |
break; |
case BL_READMODE_STATUS: |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
else BuffLen = 1;// read Current only |
break; |
} |
} |
else // old BL version |
{ |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if((BLFlags & BLFLAG_READ_VERSION) || (motor_read == motor_read_temperature)) BuffLen = 2; // Current & MaxPwm |
else BuffLen = 1; // read Current only |
} |
if(BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else |
{ |
I2C_ReceiveByte(); // read next byte |
} |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 6: // receive bytes |
*pBuff = TWDR; |
pBuff++; |
BuffLen--; |
if(BuffLen>1) |
{ |
I2C_ReceiveByte(); // read next byte |
} |
else if (BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else // nothing left |
{ |
if(BLFlags & BLFLAG_READ_VERSION) |
{ |
// if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN) && (Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
if((Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
else Motor[motor_read].Version = 0; |
} |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_TransferActive = 0; |
return; |
} |
twi_state = 6; // if there are some bytes left |
break; |
case 21: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
case 22: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
I2CTimeout = 10; |
// repeat case 18...22 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ |
dac_channel = 0; // reset dac channel counter |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
break; |
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
I2C_TransferActive = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
uint8_t I2C_WriteBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
// return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor) |
{ |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
} |
// check BL configuration to send |
if(BLConfig.Revision != BLCONFIG_REVISION) |
return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) |
return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
if(!motor) // 0 means all |
{ |
BLConfig_WriteMask = 0xFF; // all motors at once with the same configuration |
} |
else //only one specific motor |
{ |
BLConfig_WriteMask = 0x0001<<(motor-1); |
} |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_WriteMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_write = 0; |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
while(BLConfig_WriteMask && !CheckDelay(timer)); // repeat until the BL config has been sent |
if(BLConfig_WriteMask) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); |
return(BLCONFIG_SUCCESS); |
} |
//-------------------------------------------------------------- |
uint8_t I2C_ReadBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor == 0) |
return (BLCONFIG_ERR_READ_NOT_POSSIBLE); |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
BLConfig_ReadMask = 0x0001<<(motor-1); |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_ReadMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_read = 0; |
BLConfig.Revision = 0; // bad revision |
BLConfig.crc = 0; // bad checksum |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
}while(BLConfig_ReadMask && !CheckDelay(timer)); // repeat until the BL config has been received from all motors |
// validate result |
if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
return(BLCONFIG_SUCCESS); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/motortest/twimaster.h |
---|
0,0 → 1,80 |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
+ |
+#include <inttypes.h> |
+#include "../mk-data-structs.h" |
+ |
+#define TWI_STATE_MOTOR_TX 0 |
+#define TWI_STATE_MOTOR_RX 5 |
+#define TWI_STATE_GYRO_OFFSET_TX 18 |
+ |
+extern volatile uint8_t twi_state; |
+extern volatile uint8_t motor_write; |
+extern volatile uint8_t motor_read; |
+extern volatile uint8_t I2C_TransferActive; |
+ |
+extern uint8_t MissingMotor; |
+ |
+#define MAX_MOTORS 12 |
+#define MOTOR_STATE_PRESENT_MASK 0x80 |
+#define MOTOR_STATE_ERROR_MASK 0x7F |
+#define MOTOR_STATE_NEW_PROTOCOL_MASK 0x01 |
+#define BLFLAG_TX_COMPLETE 0x01 |
+#define BLFLAG_READ_VERSION 0x02 |
+ |
+extern volatile uint8_t BLFlags; |
+extern char MotorenEin; |
+unsigned char MotorTest[16]; |
+#define BL_READMODE_STATUS 0 |
+#define BL_READMODE_CONFIG 16 |
+ |
+ |
+ |
+extern MotorData_t Motor[MAX_MOTORS]; |
+ |
+#define BLCONFIG_REVISION 2 |
+ |
+#define MASK_SET_PWM_SCALING 0x01 |
+#define MASK_SET_CURRENT_LIMIT 0x02 |
+#define MASK_SET_TEMP_LIMIT 0x04 |
+#define MASK_SET_CURRENT_SCALING 0x08 |
+#define MASK_SET_BITCONFIG 0x10 |
+#define MASK_RESET_CAPCOUNTER 0x20 |
+#define MASK_SET_DEFAULT_PARAMS 0x40 |
+#define MASK_SET_SAVE_EEPROM 0x80 |
+ |
+#define BITCONF_REVERSE_ROTATION 0x01 |
+#define BITCONF_RES1 0x02 |
+#define BITCONF_RES2 0x04 |
+#define BITCONF_RES3 0x08 |
+#define BITCONF_RES4 0x10 |
+#define BITCONF_RES5 0x20 |
+#define BITCONF_RES6 0x40 |
+#define BITCONF_RES7 0x80 |
+ |
+ |
+ |
+extern BLConfig_t BLConfig; |
+ |
+extern volatile uint16_t I2CTimeout; |
+ |
+void I2C_Init(char); // Initialize I2C |
+#define I2C_Start(start_state) {twi_state = start_state; BLFlags &= ~BLFLAG_TX_COMPLETE; TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);} |
+#define I2C_Stop(start_state) {twi_state = start_state; TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);} |
+void I2C_Reset(void); // Reset I2C |
+ |
+#define BLCONFIG_SUCCESS 0 |
+#define BLCONFIG_ERR_MOTOR_RUNNING 1 |
+#define BLCONFIG_ERR_MOTOR_NOT_EXIST 2 |
+#define BLCONFIG_ERR_HW_NOT_COMPATIBLE 3 |
+#define BLCONFIG_ERR_SW_NOT_COMPATIBLE 4 |
+#define BLCONFIG_ERR_CHECKSUM 5 |
+#define BLCONFIG_ERR_READ_NOT_POSSIBLE 6 |
+ |
+uint8_t I2C_WriteBLConfig(uint8_t motor); |
+uint8_t I2C_ReadBLConfig(uint8_t motor); |
+ |
+#endif |
+ |
+ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osd.c |
---|
0,0 → 1,4919 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
* Oliver Gemesi "olli42" for additions und changes in OSDScreen * |
*****************************************************************************/ |
//-------------------- |
// Debug |
//-------------------- |
//#define DEBUG_OSD_TIME // mit dem NC-Simulator koennen keine brauchbaren Zeit/Datum Daten ermittelt werden |
// am Anfang von osd() wird mit dieser Einstellung eine gueltige Fake-Zeit/Date gesetzt |
//#define DEBUG_OSD_STAT_MOTORRUN // erzwingt Statistik Aufzeichnung auch wenn die Motoren nicht laufen |
//############################################################################ |
//# HISTORY osd.c |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 21.06.2014 OG |
//# - chg: osd() - bei auftreten von mk_timeout wieder OSD_MK_Connect(MK_CONNECT) |
//# aktiviert -> dadurch wird u.a. wieder auf die NC umgeschaltet falls |
//# etwas anderes (z.B. ein anderes PKT) auf die FC umgeschaltet hat |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 02.06.2014 OG |
//# - fix: Beep_Waypoint() Target-Reach Beep auch bei Single-WP's (WP-Listen |
//# die nur aus einem WP bestehen) |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# - add: include "../osd/osdold_screens.h" |
//# - add: include "../osd/osdold_messages.h" |
//# - chg: osd() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# aufrufende Menue erledigt wird |
//# - add: OSD_Screen_MKDisplay() um Beep_Waypoint() ergaenzt |
//# - chg: OSD_Screen_Waypoints() umgestellt auf Beep_Waypoint() |
//# - add: Beep_Waypoint() |
//# |
//# 31.05.2014 OG |
//# - chg: OSD_Screen_MKDisplay() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 26.05.2014 OG |
//# - add: etliche Aenderungen/Erweiterung am Screen "Waypoints" (auch neues Layout) |
//# |
//# 24.05.2014 OG |
//# - fix: OSD_Screen_Waypoints(), OSD_Screen_MKDisplay() - Anzeige des akt. WP-Index |
//# - chg: OSD_Screen_3DLage() - optisches Facelift |
//# - fix: OSD_Screen_3DLage() - es wurden ggf. Kreise gezeichnet die ungueltig |
//# und in undefinierten Speicher gingen - wurde korrigiert |
//# - chg: OSD0,1,2 angepasst an OSD_Element_CompassDirection() mit xoffs,yoffs |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 20.05.2014 OG |
//# - chg: OSD_Element_Flag_Label() - Anpassung zur Rahmenbestimmung eines Flags |
//# |
//# 19.05.2014 OG |
//# - fix: osd() - Tastensteuerung bei MK-Empfangsverlust |
//# - chg: osd() - wenn MK-Setting nicht ermittelt werden konnte dann exit |
//# - add: OSD_Screen_MKDisplay() - Anzeige von akt. Waypoint und Anzahl der Waypoints |
//# - chg: OSD_Popup_MKSetting() - gibt true/false fuer Ok zurueck und Beep bei Fehler |
//# - chg: OSD_Popup_MKSetting() - timeout von 15 auf 9 reduziert |
//# - chg: OSD_Popup_MKSetting(), OSD_Popup_MKError() - Layout, Multi-Sprachenunterstuetzung |
//# |
//# 02.05.2014 OG |
//# - del: Popup_Draw() (jetzt in lcd.c) |
//# |
//# 02.05.2014 OG |
//# - chg: kleine Aenderung an Codereihenfolge von MkError_Save() wegen Compiler-Warning |
//# |
//# 28.04.2014 OG |
//# Anmerkung OSD-MK-Display: wenn der MK-Display Modus eingeschaltet wird, dann werden die Display Daten |
//# angefordert und die BL-Daten etwas reduziert. Hin und wieder kann es passieren das ein Tastendruck im |
//# OSD-MK-Display-Modus nicht durch kommt (aber nicht allzu stoerend) - kann evtl. verbessert werden wenn |
//# auch die OSD-Daten reduziert werden (waere jedoch einiges mehr Aufwand). |
//# - Unterstuetzung von MK-Display (nur NC-Modus) im OSD |
//# (einschalten mit langem Druck dritte Taste von links, abschalten ebenso) |
//# - Info-Anzeige bzgl. langem Tastendruck fuer MK-Display erweitert und deutsche Uebersetzung von "long press" |
//# - Umstrukturierung von osd.c fuer MK-Display |
//# - Aenderungen/Erweiterungen an verschiedenen weiteren Funktionen wie OSD_MK_GetData() fuer MK-Display |
//# - add: OSD_MK_ShowTimeout() - neues Design/Layout |
//# - del: OSD_Timeout() (ersetzt durch OSD_MK_ShowTimeout()) |
//# - chg: verschiedene Aenderungen an Timings |
//# - add: neuer Timer fuer MK-Display: timer_get_displaydata |
//# - fix: es gab bei einem Datenpaket-Timer einen Konflikt mit 'timer' - ein neuer Timer wurde aktiviert (timer_mk_timeout) |
//# |
//# 22.04.2014 OG |
//# - add: OSD_Element_HomeCircleX() - soll das alte OSD_Element_HomeCircle() ersetzen, wenn |
//# die alten OSD-Screens endgueltig rausfliegen |
//# - chg: Aenderung Anzeige OSD_Screen_UserGPS() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_MKStatus() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_Electric() (abgerundete Ecken und Code Einsparung) |
//# |
//# 01.03.2014 OG |
//# - chg: OSD_Popup_MKError() auf lcdx_printp_center() umgestellt |
//# |
//# 16.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - chg: OSD_MK_GetData() prueft mit MKVersion_Cmp() auf NC-Version v0.30g |
//# ob GPS-Zeit-Datenpakete ('t') von der NC angefordert werden koennen |
//# |
//# 12.02.2014 OG |
//# - chg: define MKVERSIONnnn rund um OSD_MK_UTCTime() entfernt |
//# -> TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//# -> keine Unterstuetzung mehr durch zu alte NC-Versionen |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 10.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MKVersion_Setting_print() |
//# |
//# 08.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf neue MKVersion Struktur |
//# |
//# 04.02.2014 OG |
//# - fix: OSD_Element_BattLevel2() Aufruf von writex_ndigit_number_u_100th() |
//# |
//# 03.02.2014 OG |
//# - chg: OSD_Element_BattLevel2() unterstuetzt Config.OSD_ShowCellU zur Anzeige |
//# der MK-Spannung als Einzelzelle (kalk. auf Basis der ermittelten Zellenzahl) |
//# |
//# 29.01.2014 OG |
//# - add: neue MK-Errorcodes 32,33,34 |
//# - add: #include "mk/mkbase.h" |
//# |
//# 24.01.2014 OG |
//# - chg: OSD_Popup_MKSetting(): MK-Settings wird beim Start vom OSD wieder angezeigt |
//# auch wenn die falsche FC-Revision vorhanden ist mit folgender Regel: |
//# a) wenn falsche FC-Settings-Revision dann nur die Nummer |
//# b) wenn richtige FC-Revision dann mit Nummer und Namen |
//# |
//# 06.01.2014 Cebra |
//# - add: MK-Settingsname wird bei falscher FC Version nicht angezeigt |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screens koennen vom Benutzer an-/ausgeschaltet werden (osd(), ScreenCtrl_Push()) |
//# - chg: OSD-Screen Namen jetzt in messages.c |
//# - fix: MK-Error Check prueft ob die Fehlernummer gueltig ist |
//# - del: alten Screen Electric (der ohne Nachkomma bei den Stroemen) |
//# |
//# 03.07.2013 OG |
//# - chg: OSD_Popup_MKSetting() - zentrierte Anzeige des Setting-Namens |
//# - chg: OSD_Popup_MKSetting() - timing |
//# - chg: osd() - Screen-Umschaltung beschleunigt |
//# - chg: timinigs bei OSD_MK_Connect(), osd() (werden Verbindungsfehler reduziert?) |
//# |
//# 02.07.2013 OG |
//# - chg: CheckMKLipo() - MK-Unterspannungs-Beep nur wenn Motoren laufen |
//# - del: unbenutzte Variablen |
//# |
//# 30.06.2013 OG |
//# - add: grafische Akku-Anzeige auch fuer Screen Navigation und ElectricN |
//# |
//# 30.06.2013 OG |
//# - add: Screen General: das Batt-Symbol zeigt grafisch den Fuellstand |
//# des MK-Lipos |
//# - add: Mittelwert fuer Gesamtstrom |
//# - add: calc_avg() - Mittelwertberechnung |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - add: USE_OSD_PKTHOOK in osd() - aktuell noch nicht klar ob PKT_Hook() |
//# die Kommunikation stoert |
//# |
//# 27.06.2013 OG |
//# - chg: numerische Winkelanzeige im Screen "Navigation" von -180 bis 180 Grad |
//# 0 Grad = vorderer Ausleger zeigt zur Startposition |
//# - fix: Homesicht des MK's (Winkelanzeige, Grafik) |
//# ab nun: keine Benutzereinstellung mehr dafuer |
//# es gilt: 0 Grad bzw. Strich nach unten -> der vordere Ausleger des MK |
//# zeigt auf seine Startposition |
//# |
//# 19.06.2013 OG |
//# - fix: redundante PKT-Error 40 eliminiert durch merken via old_PKTErrorcode |
//# -> hilft wenn der MK ausgeschaltet wird waehrend man in der PKT-OSD-Anzeige ist |
//# - fix: last_FlyTime in osd() |
//# |
//# 18.06.2013 OG |
//# - chg: OSD_Timeout() erweitert um Errorlogging (Code 40 = "PKT RX lost") |
//# - add: Fehlerliste "PKT RX lost" (Code: 40) |
//# |
//# 15.06.2013 OG |
//# - chg: Anzeige OSD_Popup_MKSetting() kann durch Benutzer via Setup an/ausgeschaltet werden |
//# |
//# 15.06.2013 OG |
//# - chg: OSD_Popup_MKSetting() optimiert, Layout, fix |
//# - chg: PKT_CtrlHook erstmal wieder disabled in osd() (evtl. Probleme mit Timings?) |
//# |
//# 13.06.2013 cebra |
//# - add: Settings Popup beim Start des OSD-Screens |
//# |
//# 11.06.2013 OG |
//# - add: Mittelwertberechnung BL-Strom [MK_GetData()] |
//# - chg: Config.OSD_ScreenMode wird vor PKT_CtrlHook() gesetzt [osd()] |
//# |
//# 27.05.2013 OG |
//# - fix: OSD_MK_UTCTime() (neue "t" Version) Problem wegen cast fuer Sekunden |
//# |
//# 19.05.2013 OG |
//# - chg: osd() erweitert um PKT_CtrlHook() um u.a. PKT-Updates zu ermoeglichen |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: OSD_Screen_ElectricN() - zeigt BL-Stroeme mit Nachkomma an |
//# - del: define SHOW_OSD_BLCURRENT_10TH (ersetzt durch neue Screen-Variante) |
//# - chg: osd() umgestellt auf OSD Screen Controler; Anpassungen dafuer an allen Screens |
//# - add: Funktionen fuer Screen Controler (Screen_*()) |
//# |
//# 15.05.2013 OG |
//# - chg: OSD_Screen_Electric() zeigt BL-Strom mit Nachkomma an |
//# (einstellbar via define SHOW_OSD_BLCURRENT_10TH bzgl. neue/alte Anzeige) |
//# - add: define DEBUG_OSD_STAT_MOTORRUN erzwingt Statistik Aufzeichnung auch wenn |
//# die Motoren nicht laufen |
//# - fix: osd() Statistik-Ende Zeit wird nach Landung gesetzt |
//# - chg: diverse Aufraeumarbeiten |
//# - chg: OSD_Screen_Statistics() umgetsellt auf calc_BLmax(() |
//# - add: calc_BLmax() ermittelt max. BL Current & Temp |
//# - del: draw_symbol_degree(), draw_symbol_rc() - wurden ersetzt durch |
//# Zeichen im font 8x6 (SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY) |
//# |
//# 14.05.2013 OG |
//# - add: OSD_MK_Connect() vereinheitlicht MK-Verbindungsinitialisierung |
//# und MK-Abo-Renew |
//# - chg: OSD_MK_UTCTime() atomisiert via ATOMIC_BLOCK() |
//# - chg: OSD_Screen_Debug_RX() verschiedene Optimierungen und Beschreibungen |
//# |
//# 14.05.2013 OG |
//# - chg: OSD_MK_UTCTime() wird mittels define jetzt mit neuem oder mit altem |
//# Algorithmus einkompiliert. |
//# ALT bei: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
//# NEU bei: alles andere (also neuer): die neue Zeit/Datumsermittlung |
//# |
//# 13.05.2013 OG |
//# - fix: an allen Stellen mit naviData->CompassHeading ein Modulo 360 ergaenzt |
//# um Winkelangaben der FC geradezuziehen falls der Kopter beim Mixersetup |
//# 'verdreht' wurde (Problem von helitron im Forum - Winkelanzeige > 360 Grad) |
//# Ist ggf. ab Firmware NC 0.30h nicht mehr notwendig da Holger das |
//# korrigieren wollte (muesste noch geprueft werden) |
//# |
//# 11.05.2013 OG |
//# - add: OSD_MK_UTCTime_NEW() - ab NC 0.30g (Alpha, noch nicht im Einsatz) |
//# |
//# 05.05.2013 OG |
//# - chg: OSD_Popup_Info() & OSD_Popup_MKError() wurden vereinheitlicht |
//# mittels Popup_Draw() |
//# - chg: Anzeigezeiten der Popup's etwas verkuerzt |
//# |
//# 03.05.2013 OG |
//# - fix: OSD_MK_GetData() - griff beim lesen der BL's auf naviData fuer |
//# die Statistik - relevante naviData-Werte werden nun vor dem |
//# wechseln auf BL-Data gemerkt |
//# - chg: Erfassung der Statistik Start/Ende-Zeit verbessert |
//# - chg: bei Aufrufen von writex_datetime_time()/writex_datetime_time() den |
//# Parameter 'timeoffset' entfernt |
//# - add: define DEBUG_OSD_TIME um eine Fake-Zeit/Datum bei Gebrauch des |
//# NC-Simulators zu setzen |
//# |
//# 29.04.2013 OG |
//# - chg: Plausibilitaetscheck in OSD_MK_UTCTime() bzgl. des Datums |
//# |
//# 28.04.2013 OG |
//# - chg: die alten OSD-Screens angepasst auf define USE_FONT_BIG |
//# |
//# 24.04.2013 OG |
//# - chg: directions_p[] geaendert auf Kompatibilitaet mit Atmel Studio 6 |
//# |
//# 21.04.2013 OG |
//# - chg: define AltimeterAdjust nach osd.h verschoben |
//# |
//# 14.04.2013 OG |
//# - add: weitere Statistik-Aufzeichnungen |
//# - fix: MK_GetData() - Statistik-Aufzeichnung nur fuer wirklich vorhandene BL's |
//# |
//# 04.04.2013 OG |
//# - fix: 3 warnings |
//# |
//# 04.04.2013 OG |
//# - chg: MK-Errortext-Anzeige umgestellt auf naviData->Errorcode |
//# weniger Datenkomminkation mit dem MK und schnelleres Ansprechverhalten |
//# |
//# 03.04.2013 OG |
//# - chg: defines OSD_DEMO, OSD_SCREEN_DEBUG, OSD_SCREEN_OLD |
//# umbenannt nach: USE_OSD_DEMO, USE_OSD_SCREEN_DEBUG, USE_OSD_SCREEN_OLD |
//# und verschoben nach: main.h |
//# |
//# 28.03.2013 OG |
//# - fix: MKErr_Log_Init() - es fehlte Multiplikation mit MAX_MKERR_LOG |
//# - chg: Code Formatierungen |
//# |
//# 28.03.2013 CB |
//# - add: replace OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//# |
//# 28.03.2013 OG |
//# - fix: abo_timer wieder aktiviert fuer 'o' und 'k' |
//# |
//# 28.03.2013 OG |
//# - chg: osd_statistic_t - erweitert um diverse Werte und Timestamps (werden apeter implementiert) |
//# - add: mkerror_t MKErr_Log[MAX_MKERR_LOG] - Vorbereitung um MK-Errors zu loggen |
//# |
//# 27.03.2013 OG |
//# - add: Datum / Zeit vom MK lesen |
//# - chg: Kommunikation mit dem MK optimiert |
//# - chg: auf neue messages.c angepasst |
//# - struct von den Statistiken auf PKTdatetime PKTdatetime_t |
//# - verschiedene andere Aenderungen |
//# |
//# 22.03.2013 OG |
//# erhebliche Aenderungen / Ergaenzungen - hier nur das Wichtigste |
//# - neue Funktion "User GPS" (UGPS) - mit langem Druck auf Taste KEY_ENTER werden die aktuellen GPS-Daten abgespeichert |
//# * gespeichert wird nur wenn wenn der MK GPS-Ok meldet |
//# * wenn Ok erfolgt ein bestaetigungs Beep; wenn nicht Ok erfolgt ein Error-Beep |
//# - neuer Screen "User GPS" - Anzeige der letzten der 3 UGPS-Koordinaten (intern wird mehr gespeichert) |
//# - Anzeige von MK-Error Messages als Popup mit einstellbarer Anzeigezeit (kann mit Taste abgebrochen werden) |
//# * kann getestet werden z.B. durch Ausschalten der Funke (= "RC Signal lost") |
//# * siehe: http://www.mikrokopter.de/ucwiki/ErrorCodes |
//# - OSD-Info ist nun ein Popup - autom. ausblenden nach einstellbarer Zeit; im Hintergrund werden weiterhin MK-Daten empfangen und ausgewertet |
//# - neuer Debug-Screen "Debug RXPackages" - zeigt die Anzahl empfangener Datenpakete der verschiedenen Bereiche (um z.B. Timings einzustellen) |
//# - erweiterte Kommunikation mit dem MK fuer BL-Daten und Error-Messages |
//# - Timings der MK-Datenkommunikation weitreichend einstellbar durch defines |
//# - Datenstrukturen fuer OSD-Statistiken und BL-Statistiken |
//# - neue timer: timer_get_erdata, timer_get_bldata, timer_osd_refresh, timer_pkt_uptime |
//# - mit dem define OSD_SCREEN_OLD koennen ggf. die alten OSD-Screens ausgeblendet werden (spart ein paar KByte's) |
//# - verschiedene Optimierungen |
//# |
//# 12.03.2013 OG |
//# - add: Get_BL_Data() - BL-Ctrl via NC auslesen (BETA) (siehe dort auch Kommentare im func-Header) |
//# - add: neuer Screen "Electric" |
//# - chg: Layout von Screen "Navigation" - Pixelverschiebungen |
//# - chg: Layout von Screen "MK-Status" - Pixelverschiebungen und Anzeige 'Strom' ersetzt durch 'entn. Kapazitaet' |
//# - add: in osd() LiPO-Cell Erkennung hinzugefuegt (fuer Screen "Electric") |
//# |
//# 10.03.2013 OG |
//# - fix: doppelte Degree-Anzeige in OSD_Element_CompassDegree() |
//# - add: neuer Screen "MK-Status" |
//# - add: 7 neue OSD-Flags |
//# - chg: Screen-Refresh Zeit via timer2 (einstellbar durch define TIME_OSD_REFRESH) |
//# - chg: mit define OSD_DEBUG_SCREEN kann ein zusaetzlicher Screen verwendet werden zum testen/entwickeln |
//# - del: entfernt CFG2_HEIGHT_LIMIT in OSD_Element_AltitudeControl() (bis bessere Loesung gefunden ist) |
//# |
//# 08.03.2013 OG |
//# - del: OSD_Screen_Element() und cleanup in osd.h |
//# - add: OSD_Element_UpDown() (steigend/sinken via Pfeilen) |
//# - chg: OSD_Element_UpDown() in Screen "General" und "Navigation" hinzugefuegt (rechts neben der Hoehenanzeige) |
//# - chg: Screen "General" die Sat-Warnung wurde auf OSD_Element_Flag(OSD_FLAG_S0) geaendert |
//# - chg: Anzeige von Flag 'nicht genug GPS-Sateliten' (OSD_FLAG_S0) auf "S!" geändert |
//# |
//# 07.03.2013 OG |
//# - Hinweis bzgl. LowBatt-Anzeige in den Screens "General" und "Navigation": |
//# Es gibt zwei unabhängige LowBatt-Warnungen. |
//# 1. die PKT LowBatt-Warnung: sie arbeitet mit der im PKT hinterlegten |
//# LowBatt Spannung und stellt den Spannungswert inkl. der Einheit "V" |
//# invers dar wenn der Warnwert erreicht wurde (inkl. lautem PKT-Peepen) |
//# 2. die MK-LowBatt Warnung: hierbeit wird das Flag "BA" angezeigt wenn |
//# der MK eine LowBatt Warnung sendet |
//# Dadurch hat man nun zwei verschiedene LowBatt Warnungen die man auf Wunsch |
//# verschieden einstellen kann. Bei mir ist die PKT-LowBatt etwas niedriger |
//# eingestellt als die MK-Warnung und bedeutet "es ist aller hoechste Zeit zu landen". |
//# Die Spannung der MK-LowBat ist ein wenig hoeher eingestellt und |
//# zeigt mir "es ist bald Zeit zu landen". |
//# - del: Kommunikation zu FC - siehe Kommentare in osd() |
//# - chg: Kommunikation zu NC - siehe Kommentare in osd() |
//# - add: neuer Screen "Navigation" |
//# - chg: Layout Screen "Statistics" - Einheiten um zwei Pixel nach rechts verschoben |
//# - chg: Layout von Screen "General" modifiziert (u.a. xoffs,yoffs Pixelverschiebungen) |
//# - add: OSD_FLAG_BA in Screen "General" |
//# - add: die OSD_Element_xyz() Funktionen in osd.h aufgenommen |
//# - chg: an verschiedenen Stellen die Char-Drawmode defines MNORMAL, MINVERS, usw. eingebaut |
//# - del: Kompatibilitaetscode fuer "3D-Lage" ueber Hauptmenue entfernt |
//# - chg: die Funktionen OSD_Element_Switch() und OSD_Element_SwitchLabel() heissen |
//# nun OSD_Element_Flag() und OSD_Element_Flag_Label() |
//# - chg: die defines OSD_SWITCH_xy heissen jetzt OSD_FLAG_xy |
//# - fix: letzte GPS-Koordinaten werden jetzt permanent Config.LastLatitude, Config.LastLongitude gespeichert |
//# |
//# 03.03.2013 OG |
//# - add: delay in Mainloop von osd() um springende Werte abzudaempfen (TEST) |
//# - add: Startverzoegerung der Screens bis NaviData sich etwas stabilisiert hat (TEST) |
//# - add: OSD Startup Message "connecting MK..." |
//# - add: 'Emergency Landing' (EL) Anzeige in Screen "General" |
//# - del: OUT1/OUT2 Anzeige in Screen "General" |
//# - add: RC-Quality in Screen "General" |
//# - add: func: draw_symbol_rc() (alternative RC-Quality Symbol) |
//# - fix: Hoehenanzeige fuer Screens "OSD0" und "OSD1" |
//# - fix: OSD_Element_SwitchLabel() angepasst fuer x=0 und y=0 |
//# - add: OSD_Element_Switch/Label() erweitert um OSD_SWITCH_FS |
//# - fix: Screen-Redraw nach OSD_MK_TIMEOUT() und anderen Fehlermeldungen |
//# - chg: messages.c: STATS_ITEM_0 bis STATS_ITEM_6 angepasst (1 char kuerzer) |
//# - chg: Layout von OSD_Info() - mehr background-clear und etwas kleiner |
//# |
//# 02.03.2013 OG |
//# - chg: keine internal func in Screen's wegen warnings bei anderen |
//# - del: Screen "OSD3" |
//# - fix: Hoehenanzeige in Screen "General" (Minuszeichen) |
//# - add: MK LowBat Warning in Screen "General" |
//# - add: neues Degree Symbol (als func) in Screen General (kleiner als das Char 0x1E) |
//# - add: weitere Flags in OSD_Element_Flag() |
//# |
//# 01.03.2013 OG |
//# - Reskrukturierung Code (+ neuer OSD-Screens und einiges mehr) |
//############################################################################ |
//############################################################################ |
//# HINWEISE: |
//# |
//# 1. define: USE_OSD_DEMO (main.h) |
//# mit define OSD_DEMO wird ein Demo-Modus bei den neuen Screens einge- |
//# schaltet - damit werden u.a. alle Flag's angezeigt fuer Scree-Fotos |
//# |
//# 2. define: USE_OSD_SCREEN_DEBUG (main.h) |
//# mit diesem define wird ein zusaetzlicher Screen "Debug" einkompiliert |
//# fuer Test / Experimente / Debug von OSD-Elementen |
//# |
//# 3. define: USE_OSD_SCREEN_OLD (main.h) |
//# ein-/ ausschalten der alten OSD-Screens OSD0, OSD1, OSD2 (spart ca. 3 KByte) |
//# |
//# 4. Timings der MK-Datenkommunikation |
//# die Timings sind weitreichend enstellbar ueber die define im Abschnitt 'Timings' |
//# der Debug-Screen 'Debug RXPackages' informiert ueber die Anzahl eingegangener |
//# Datenpakete der verschiedenen Bereiche (OSD, BL-Daten, Error-Message) |
//# |
//# 5. Informationen zum Display |
//# DOG: 128 x 64 Pixel with 6x8 Font => 21 x 8 |
//# |
//# 6. MK-Kommunikationsprotokoll Referenz |
//# http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=%28%28----%28-*%29%28\r%29%3F\n%29%28.*%29CategoryCoding%29#en.2BAC8-SerialCommands.Flight-Ctrl |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
//-------------------- |
// Funktionen ein-/ausbinden |
//-------------------- |
// -> siehe main.h |
//-------------------- |
// Timings |
//-------------------- |
#define TIME_OSD_REFRESH 45 // Screen Refresh (Steuerung via 'timer_osd_refresh') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_POPUP_INFO 400 // 4 Sekunden Popup-Info zeigen (kann mit Taste abgebrochen werden) |
#define TIME_POPUP_MKERROR 700 // 7 Sekunden Popup-MK-Error zeigen (kann mit Taste abgebrochen werden) |
#define TXINTERVAL_OSDDATA 10 // Intervall mit der der MK OSD-Daten senden soll (n*10 = ms) |
#define TIME_GET_BLDATA 35 // Zeitintervall in der BL-Daten vom MK geholt werden (Steuerung via 'timer_get_bldata') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_READ_BLDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
#define TXINTERVAL_BLDATA 7 // Intervall mit der der MK BL-Daten senden soll (n*10 = ms) |
#define TIME_GET_TIDATA 12000 // alles 120 Sekunden Zeit/Datum vom MK lesen (die Zwischenzeit wird von einem PKT-Timer uebernommen) |
#define TIME_GET_DISPLAYDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
// weiteres |
//-------------------- |
#define OSD_POPUP_NONE 0 |
#define OSD_POPUP_INFO 1 |
#define OSD_POPUP_MKERROR 2 |
#define MK_CONNECT 1 |
#define MK_ABORENEW 2 |
// global definitions and global vars |
NaviData_t *naviData; |
uint16_t heading_home; |
uint8_t drawmode; |
uint8_t OSDScreenRefresh; |
// flags from last round to check for changes |
uint8_t old_AngleNick = 0; |
uint8_t old_AngleRoll = 0; |
uint16_t old_hh = 0; |
// aktuell nicht benoetigt - siehe Kommentar in osd.c |
//mk_param_struct_t *mk_param_struct; |
//uint8_t Flags_ExtraConfig; |
//uint8_t Flags_GlobalConfig; |
//uint8_t Flags_GlobalConfig3; |
// cache old vars for blinking attribute, checkup is faster than full |
// attribute write each time |
volatile uint8_t OSD_active = 0; |
uint8_t Vario_Beep_Up = 0; |
uint8_t Vario_Beep_Down = 0; |
uint8_t Vario_Beep_Up_Interval = 9; |
uint8_t Vario_Beep_Down_Interval = 6; |
uint8_t Vario_Threshold = 0; |
uint8_t Vario_Threshold_Value = 7; |
uint8_t OldWP = 0; |
uint8_t NextWP = 0; |
uint8_t WP_old = 0; // Screen: "Waypoints" |
uint8_t WP_last = false; // Screen: "Waypoints" |
#define MAX_CELL_VOLTAGE 43 // max cell voltage for LiPO |
#define MIN_CELL_VOLTAGE 32 // min cell voltage for LiPO |
// Flags |
volatile uint8_t error = 0; |
uint8_t cells = 0; |
uint8_t BattLowVoltageWarning = 0; |
uint8_t CellIsChecked = 0; |
uint8_t AkkuWarnThreshold = 0; |
uint16_t duration = 0; |
//----------------------------------------------------------- |
// Buffer |
//----------------------------------------------------------- |
BLData_t blData[OSD_MAX_MOTORS]; // speichert gelesene BL-Datenpakete |
pkt_gpspos_t GPS_Current; // aktuelle GPS-Position |
u8 old_PKTErrorcode; // speichert den letzen Errorcode vom PKT damit dieser nicht wiederholt gespeichert wird |
u8 old_MKErrorcode; // speichert den letzen Errorcode vom MK damit dieser nicht wiederholt angezeigt wird |
//----------------------------------------------------------- |
// OSD Daten |
//----------------------------------------------------------- |
NaviData_t osdData; // Buffer |
//----------------------------------------------------------- |
// MK-DISPLAY |
//----------------------------------------------------------- |
uint8_t mkdisplayMode = false; |
uint8_t mkdisplayCmd = 0xff; |
char mkdisplayData[81]; // Buffer (80 +1 fuer term. Char) |
//--------------------- |
// DEBUG |
//--------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
uint16_t readCounterOSD; // Anzahl gelesener Datenpakete von NC Modus 'o' (Request OSD-Data) |
uint16_t readCounterTIME; // Anzahl gelesener Datenpakete von NC (Time) |
uint16_t readCounterDISPLAY; |
uint16_t readCounterBL[OSD_MAX_MOTORS]; // Anzahl gelesener Datenpakete pro BL via NC Modus 'k' (BL Ctrl Status) |
#endif // USE_OSD_SCREEN_DEBUG |
//--------------------- |
// Strings & Co. |
//--------------------- |
static const char mkerror00[] PROGMEM = "No Error"; |
static const char mkerror01[] PROGMEM = "FC not compatible"; |
static const char mkerror02[] PROGMEM = "MK3Mag not compati."; |
static const char mkerror03[] PROGMEM = "no FC communication"; |
static const char mkerror04[] PROGMEM = "no MK3Mag communic."; |
static const char mkerror05[] PROGMEM = "no GPS communication"; |
static const char mkerror06[] PROGMEM = "bad compass value"; |
static const char mkerror07[] PROGMEM = "RC Signal lost"; |
static const char mkerror08[] PROGMEM = "FC spi rx error"; |
static const char mkerror09[] PROGMEM = "no NC communication"; |
static const char mkerror10[] PROGMEM = "FC Nick Gyro"; |
static const char mkerror11[] PROGMEM = "FC Roll Gyro"; |
static const char mkerror12[] PROGMEM = "FC Yaw Gyro"; |
static const char mkerror13[] PROGMEM = "FC Nick ACC"; |
static const char mkerror14[] PROGMEM = "FC Roll ACC"; |
static const char mkerror15[] PROGMEM = "FC Z-ACC"; |
static const char mkerror16[] PROGMEM = "Pressure sensor"; |
static const char mkerror17[] PROGMEM = "FC I2C"; |
static const char mkerror18[] PROGMEM = "Bl Missing"; |
static const char mkerror19[] PROGMEM = "Mixer Error"; |
static const char mkerror20[] PROGMEM = "Carefree Error"; |
static const char mkerror21[] PROGMEM = "GPS lost"; |
static const char mkerror22[] PROGMEM = "Magnet Error"; |
static const char mkerror23[] PROGMEM = "Motor restart"; |
static const char mkerror24[] PROGMEM = "BL Limitation"; |
static const char mkerror25[] PROGMEM = "Waypoint range"; |
static const char mkerror26[] PROGMEM = "No SD-Card"; |
static const char mkerror27[] PROGMEM = "SD Logging aborted"; |
static const char mkerror28[] PROGMEM = "Flying range!"; |
static const char mkerror29[] PROGMEM = "Max Altitude"; |
static const char mkerror30[] PROGMEM = "No GPS Fix"; |
static const char mkerror31[] PROGMEM = "compass not calibr."; |
static const char mkerror32[] PROGMEM = "BL selftest"; |
static const char mkerror33[] PROGMEM = "no ext. compass"; |
static const char mkerror34[] PROGMEM = "compass sensor"; |
#define MAX_MKERROR_NUM 34 // maximale Error-Nummer vom MK (verwendet in osd()) |
static const char mkerror35[] PROGMEM = ""; // free for MK |
static const char mkerror36[] PROGMEM = ""; // free for MK |
static const char mkerror37[] PROGMEM = ""; // free for MK |
static const char mkerror38[] PROGMEM = ""; // free for MK |
static const char mkerror39[] PROGMEM = ""; // free for MK |
static const char pkterror40[] PROGMEM = "PKT RX lost"; |
//--------------------------------------- |
// wenn die Liste erweitert wird, |
// MAX_MKERROR_NUM in osd.h anpassen! |
//--------------------------------------- |
const char * const mkerrortext[] PROGMEM= |
{ |
mkerror00, |
mkerror01, |
mkerror02, |
mkerror03, |
mkerror04, |
mkerror05, |
mkerror06, |
mkerror07, |
mkerror08, |
mkerror09, |
mkerror10, |
mkerror11, |
mkerror12, |
mkerror13, |
mkerror14, |
mkerror15, |
mkerror16, |
mkerror17, |
mkerror18, |
mkerror19, |
mkerror20, |
mkerror21, |
mkerror22, |
mkerror23, |
mkerror24, |
mkerror25, |
mkerror26, |
mkerror27, |
mkerror28, |
mkerror29, |
mkerror30, |
mkerror31, |
mkerror32, |
mkerror33, |
mkerror34, |
mkerror35, |
mkerror36, |
mkerror37, |
mkerror38, |
mkerror39, |
pkterror40 |
}; |
//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W"; |
const char rose[48] PROGMEM = { |
0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', |
0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', |
0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', |
0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', |
}; |
// the center is char 19 (north), we add the current heading in 8th |
// which would be 22.5 degrees, but float would bloat up the code |
// and *10 / 225 would take ages... so we take the uncorrect way |
static const char str_NE[] PROGMEM = "NE"; |
static const char str_E[] PROGMEM = "E "; |
static const char str_SE[] PROGMEM = "SE"; |
static const char str_S[] PROGMEM = "S "; |
static const char str_SW[] PROGMEM = "SW"; |
static const char str_W[] PROGMEM = "W "; |
static const char str_NW[] PROGMEM = "NW"; |
static const char str_N[] PROGMEM = "N "; |
const char * const directions_p[] PROGMEM= |
{ |
str_NE, |
str_E, |
str_SE, |
str_S, |
str_SW, |
str_W, |
str_NW, |
str_N |
}; |
//########################################################### |
//# OSD Screen Controler |
//########################################################### |
#define SCREENCTRL_MAX 15 // Anzahl max. zu verwaltender Screens |
//--------------------------- |
// typedef: einzelner Screen |
//--------------------------- |
typedef struct |
{ |
const char *screenname; |
void (*screen)(void); |
} screen_t; |
//--------------------------- |
// typedef: Screenliste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; |
uint8_t count; |
screen_t screen[SCREENCTRL_MAX]; |
} screenlist_t; |
screenlist_t osdscreens; |
//-------------------------------------------------------------- |
// ScreenCtrl_Init() |
// |
// initialisiert die Screenliste - muss vor dem ersten |
// ScreenCtrl_Push() aufgerufen werden |
//-------------------------------------------------------------- |
void ScreenCtrl_Init( void ) |
{ |
osdscreens.active = 0; |
osdscreens.count = 0; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Push( *screenname, *screenfunc) |
// |
// fuegt einen Screen der Screenliste hinzu (siehe osd()) |
//-------------------------------------------------------------- |
void ScreenCtrl_Push( uint8_t screenid, const char *screenname, void (*screenfunc)(void)) |
{ |
if( osdscreens.count < SCREENCTRL_MAX ) |
{ |
// wenn screenid = 0 dann immer anzeigen (Screen ist nicht vom Benutzer auswaehlbar) |
if( (screenid==0) || ((Config.OSD_UseScreen & (1 << screenid)) != 0) ) |
{ |
osdscreens.screen[osdscreens.count].screenname = screenname; |
osdscreens.screen[osdscreens.count].screen = screenfunc; |
osdscreens.count++; |
} |
} |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Set( screennum) |
// |
// aktiviert einen bestimmten Screen aus der Screenliste |
//-------------------------------------------------------------- |
void ScreenCtrl_Set( uint8_t screennum ) |
{ |
osdscreens.active = 0; |
if( screennum < osdscreens.count ) |
{ |
osdscreens.active = screennum; |
} |
} |
//-------------------------------------------------------------- |
// num = ScreenCtrl_GetNum() |
// |
// gibt die Nummer des aktuell aktivierten Screens zurueck |
//-------------------------------------------------------------- |
uint8_t ScreenCtrl_GetNum( void ) |
{ |
return osdscreens.active; |
} |
//-------------------------------------------------------------- |
// name = ScreenCtrl_GetName() |
// |
// gibt den Namen des aktuell aktivierten Screens zurueck |
// Rueckgabe ist ein Pointer auf einen String im PROGMEM |
//-------------------------------------------------------------- |
const char * ScreenCtrl_GetName( void ) |
{ |
return osdscreens.screen[osdscreens.active].screenname; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Next() |
// |
// zum naechsten Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Next( void ) |
{ |
osdscreens.active++; |
if( osdscreens.active >= osdscreens.count ) |
{ |
osdscreens.active = 0; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Previous() |
// |
// zum vorherigen Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Previous( void ) |
{ |
if( osdscreens.active == 0 ) |
{ |
osdscreens.active = osdscreens.count-1; |
} |
else |
{ |
osdscreens.active--; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Show() |
// |
// ruft den aktuell Screen auf |
//-------------------------------------------------------------- |
void ScreenCtrl_Show( void ) |
{ |
osdscreens.screen[osdscreens.active].screen(); |
} |
//########################################################### |
//########################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_BLmax( osd_BLmax_t *blmax ) |
{ |
uint8_t i; |
memset( blmax, 0, sizeof(osd_BLmax_t) ); |
for( i=0; i<Config.OSD_Statistic.BL_Count; i++) |
{ |
if( Config.OSD_Statistic.BL[i].max_Current > blmax->max_BL_Current ) |
{ |
blmax->max_BL_Current = Config.OSD_Statistic.BL[i].max_Current; |
blmax->max_BL_Current_Index = i; |
} |
if( Config.OSD_Statistic.BL[i].max_Temp > blmax->max_BL_Temp ) |
{ |
blmax->max_BL_Temp = Config.OSD_Statistic.BL[i].max_Temp; |
blmax->max_BL_Temp_Index = i; |
} |
} |
} |
//-------------------------------------------------------------- |
// STAT_Init() |
// |
// initialisiert die Statistic Werte neu |
//-------------------------------------------------------------- |
void STAT_Init(void) |
{ |
// init: statistic |
memset( &Config.OSD_Statistic, 0, sizeof(osd_statistic_t) ); |
Config.OSD_Statistic.min_UBat = 255; |
Config.OSD_Statistic.min_RCQuality = 255; |
Config.OSD_Statistic.min_AngleNick = 126; |
Config.OSD_Statistic.min_AngleRoll = 126; |
} |
//-------------------------------------------------------------- |
// MKErr_Log_Init() |
// |
// initialisiert die MK Errorcodes neu |
//-------------------------------------------------------------- |
void MKErr_Log_Init(void) |
{ |
// init: MK Errorlog |
memset( &Config.MKErr_Log, 0, sizeof(mkerror_t)*MAX_MKERR_LOG ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MkError_Save( uint8_t Errorcode ) |
{ |
uint8_t i; |
for( i=MAX_MKERR_LOG-1; i>0; i--) |
{ |
Config.MKErr_Log[i] = Config.MKErr_Log[i-1]; |
} |
Config.MKErr_Log[0].Errorcode = Errorcode; |
memcpy( &Config.MKErr_Log[0].set_Time, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
// OSD_MK_Connect() |
// |
// Verbindung zum MK herstellen oder ABO-Renew |
// |
// modus: MK_CONNECT oder MK_ABORENEW |
//-------------------------------------------------------------- |
void OSD_MK_Connect( uint8_t modus ) |
{ |
uint8_t tx_interval; |
if( modus == MK_CONNECT ) |
{ |
SwitchToNC(); |
// switch off: 3d data => kompatibel mit SmartOSD???? |
//tx_interval = 0; |
//SendOutData ('c', ADDRESS_ANY, 1, &tx_interval, 1); |
//_delay_ms(25); |
tx_interval = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tx_interval, 1); // switch off: debug |
timer = 3; while( timer > 0 ); // short delay |
} |
tx_interval = TXINTERVAL_OSDDATA; // switch on: sending osd-data |
SendOutData( 'o', ADDRESS_NC, 1, &tx_interval, 1); // request: OSD Data from NC every ..ms |
timer = 3; while( timer > 0 ); // short delay |
// switch on: sending bl-data |
tx_interval = TXINTERVAL_BLDATA; // 5 => 50 ms (send packet every n*10 ms) |
SendOutData( 'k', ADDRESS_ANY, 1, &tx_interval, 1); // request: BL Ctrl Status |
mode = 'O'; |
abo_timer = ABO_TIMEOUT; |
rxd_buffer_locked = FALSE; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_MK_ShowTimeout( void ) |
{ |
if( old_PKTErrorcode != 40 ) |
{ |
MkError_Save( 40 ); // Logge: "PKT RX lost" |
old_PKTErrorcode = 40; |
} |
lcd_cls (); |
lcd_frect_round( 0, 0, 127,10, 1, R1); // Rect: Invers (Titel) |
lcdx_printp_center( 0, strGet(OSD_ERROR), MINVERS, 1,2); // "FEHLER: Datenverlust!" |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,6); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,6); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+5, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,9); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,11); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,11); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+8, 127, (2*8)+4, 1); // Rahmen |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline: "Ende" |
set_beep ( 0, 0, BeepOff); // Falls Spannungswarnung an war Beeper aus (ist das notwendig?) |
} |
/*************************************************************************************************************************************** |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// Fuer: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
// -> erstmal keine Unterstuetzung mehr... |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// ALTE Funktion fuer FC < 0.90h (und dementsprechend NC < 0.30h) |
// |
// Setzt die PKT globale UTCTime mithilfe des MK. |
// |
// Foraussetzung: |
// - der NC-Modus muss aktiv sein (SwitchToNC) |
// - aktuell nur fuer osd.c |
// |
// Rueckgabe: |
// true = ok, UTCTime gespeichert |
// false = Zeit/Datum nicht gelesen |
// |
// Hack: |
// Gelesen wird die Seite 3 des NC-Display's. Dort wird Zeit |
// und Datum an den entsprechnenden Positionen via Zeichen an- |
// gezeigt. Die Zeichen werden ausgelesen und in die interne UTCTime |
// gespeichert. |
// |
// HINWEIS: |
// Ab NC > v0.30b (evtl. v0.30c) wird die NC ein neues Datenpaket |
// senden ("T") mit strukturierten Informationen zu Datum/Zeit. |
// Das wurde von Holger eingebaut. |
// Wird spaeter auch im PKT implementiert. |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
uint32_t sec; |
uint32_t min; |
uint32_t hour; |
uint16_t year; |
uint8_t month; |
uint8_t day; |
uint8_t page; |
uint8_t ok = false; |
mode = 'L'; // read: MK-Display Page |
page = 3; // anfordern von Seite 3 (der NC) |
SendOutData ('l', ADDRESS_ANY, 1, &page, 1); // request: MK-Display Page 3 |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
sec = (uint32_t)(pRxData[66+7+2] - '0') + 10 * (uint32_t)(pRxData[66+6+2] - '0'); // seconds: sec |
min = (uint32_t)(pRxData[66+4+2] - '0') + 10 * (uint32_t)(pRxData[66+3+2] - '0'); // seconds: min |
hour = (uint32_t)(pRxData[66+1+2] - '0') + 10 * (uint32_t)(pRxData[66+0+2] - '0'); // seconds: hour |
sec += (min * 60) + (hour * 3600); |
year = (uint16_t)(pRxData[46+9+2] - '0') + 10 * (uint16_t)(pRxData[46+8+2] - '0'); // year |
year += 100 * (uint16_t)(pRxData[46+7+2] - '0') + 1000 * (uint16_t)(pRxData[46+6+2] - '0'); // year |
day = (uint8_t)(pRxData[46+4+2] - '0') + 10 * (uint8_t)(pRxData[46+3+2] - '0'); // day |
month = (uint8_t)(pRxData[46+1+2] - '0') + 10 * (uint8_t)(pRxData[46+0+2] - '0'); // month |
// in der globalen UTCTime speichern (hoffentlich funkt kein timer dazwischen) |
if( year > 2000 && year < 2200 ) // plausibilitaets check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = sec; |
UTCTime.day = day; |
UTCTime.month = month; |
UTCTime.year = year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
***************************************************************************************************************************************/ |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// NEUE Ermittlung der Zeit vom MK |
// ab NC v0.30g (bzw. NC 0.30h fuer FC 0.90h) |
// |
// TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
DateTime_t *rx_DateTime; |
uint8_t tx_interval; |
uint8_t ok = false; |
mode = 'T'; // read: MK-Display Page |
tx_interval = 1; // |
SendOutData ('t', ADDRESS_NC, 1, &tx_interval, 1); // request: DateTime |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
rx_DateTime = (DateTime_t *) (pRxData); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
if( rx_DateTime->Year > 2000 && rx_DateTime->Year < 2200 ) // Plausibilitaets Check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = ((uint32_t)(rx_DateTime->Hour))*3600 + ((uint32_t)(rx_DateTime->Min))*60 + (uint32_t)(rx_DateTime->Sec); |
UTCTime.day = rx_DateTime->Day; |
UTCTime.month = rx_DateTime->Month; |
UTCTime.year = rx_DateTime->Year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int32_t calc_avg( int32_t avg, int32_t nvalue, int32_t count, int32_t factor) |
{ |
avg = avg + (( ( nvalue * factor) - avg) / count); |
return avg; |
} |
//-------------------------------------------------------------- |
// OSD_MK_GetData() |
// |
// Holt Daten der BL-Ctrl via NC. |
// |
// Die Werte der BL's 1-12 kommen in mehr oder weniger beliebiger |
// Reihenfolge an. Die Daten fuer vorhandene BL's werden oefter |
// gesendet als die von nicht vorhandenen BL's. |
// |
// Diese Funktioen liest fuer die Zeit BL_READ_TIME die BL-Daten |
// vom MK ein und ordnet diese der PKT-internen Speichstruktur zu. |
// |
// Hierbei ist nicht gewaehrleistet, dass die Daten von jedem BL |
// in einem festen Zeitrahmen ermittelt werden. Die Folge ist |
// ein 'zufaelliger' Aufbau der Anzeige der BL-Daten und auch |
// eine nicht exakt bestimmbare Refreshtime der Werte. |
// |
// Optimieren kann man ggf. die Refreshzeit aller BL-Werte durch |
// tx_intervall fuer Kommando "k" und durch BL_READ_TIME. |
// Brauchbare Startwerte sind (noch experimentell): |
// tx_interval = 5 (fuer "k") = 50 ms |
// BL_READ_TIME = 25 = 250 ms |
// |
// Wenn dieses Verhalten verbessert werden soll muss ggf. |
// die ISR() (usart.c) fuer Kommando "k" angepasst werden um |
// in einem Schwung mehrere gesendete BL-Pakete aufeinmal |
// aufzunehmen und als Paket zur Verfuegung zu stellen. |
//-------------------------------------------------------------- |
void OSD_MK_GetData( void ) |
{ |
BLData_t *rx_blData; |
uint8_t blIndex; |
uint8_t FCStatusFlags; |
uint8_t v; |
uint8_t skipBL = false; |
FCStatusFlags = naviData->FCStatusFlags; // save naviData->FCStatusFlags for use with BL-Data |
//###################################### |
//# ZEIT/DATUM |
//###################################### |
// lese UTC-Time vom MK |
//-------------------------------------- |
if( timer_get_tidata == 0 ) |
{ |
//-------------------------------------------------- |
// das 'T' Datenpaket der NC fuer OSD_MK_UTCTime() |
// gibt es erst ab NC v0.30g (!) |
// |
// --> Versionspruefung der NC-Firmware |
//-------------------------------------------------- |
v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version >= "0.30g" |
if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
{ |
if( !OSD_MK_UTCTime(20) ) |
timer_get_tidata = 50; // erfolglos: versuche es nach einer 1/2 Sekunde erneut |
else |
timer_get_tidata = TIME_GET_TIDATA; // alle 60 Sekunden refresh - den Rest uebernimmt ein Timer des PKT |
} |
} |
//###################################### |
//# MK-DISPLAY |
//###################################### |
// switch to: (h) |
//-------------------------------------- |
//if( timer_get_displaydata == 0 ) |
if( mkdisplayMode && timer_get_displaydata == 0 ) |
{ |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
/* |
if( mkdisplayCmd != 0xff ) |
{ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 1, &mkdisplayCmd, 1); |
} |
*/ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
mkdisplayCmd = 0xff; // 0xff = aktuelle Seite |
timer = 20; while( (timer>0) && !rxd_buffer_locked ); // |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
memcpy( mkdisplayData, (const void *)&rxd_buffer[3+ 0], 80 ); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterDISPLAY++; |
#endif // USE_OSD_SCREEN_DEBUG |
} |
timer_get_displaydata = TIME_GET_DISPLAYDATA; // n*10 ms |
skipBL = true; |
} // end: if( mkdisplayMode && timer_get_displaydata == 0 ) |
//###################################### |
//# BL-Daten |
//###################################### |
// switch to: 'BL Ctrl Status' (k) |
//-------------------------------------- |
if( !skipBL && timer_get_bldata == 0 ) |
{ |
mode = 'K'; // read: BL Ctrl Status |
rxd_buffer_locked = FALSE; |
timer = TIME_READ_BLDATA; // fuer x Zeit werden BL-Daten gelesen |
while( timer>0 ) // lese Daten-Pakete fuer die angegebene Zeit |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
rx_blData = (BLData_t *) (pRxData); |
// die BL-Daten kommen in beliebiger Reihenfolge an |
// Hier werden sie entsprechend ihres Index gesichert |
blIndex = rx_blData->Index; |
if( blIndex >= 0 && blIndex < OSD_MAX_MOTORS ) |
{ |
memcpy( &blData[blIndex], rx_blData, sizeof(BLData_t)); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterBL[blIndex]++; |
#endif // USE_OSD_SCREEN_DEBUG |
// Statistiken |
if( (blData[blIndex].Status & 0xf0) && (FCStatusFlags & FC_STATUS_MOTOR_RUN) ) // nur wenn BL/Motor vorhanden und Motoren laufen |
{ |
// BL Statistik: Anzahl empf. Datenpakete (fuer Mittelwert) |
Config.OSD_Statistic.BL[blIndex].count++; |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t) calc_avg( (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current, |
(int32_t)blData[blIndex].Current, |
(int32_t)Config.OSD_Statistic.BL[blIndex].count, |
100); |
// ALT |
// BL Statistik: Mittelwert: Strom (*100 um Rundungsfehler zu reduzieren) |
//avg = (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current; |
//avg = avg + (( ( (int32_t)blData[blIndex].Current * 100) - avg) / (int32_t)Config.OSD_Statistic.BL[blIndex].count); |
//Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t)avg; |
// BL Statistik: Max: Strom |
if( blData[blIndex].Current > Config.OSD_Statistic.BL[blIndex].max_Current) Config.OSD_Statistic.BL[blIndex].max_Current = blData[blIndex].Current; |
// BL Statistik: Max: Temperatur |
if( blData[blIndex].Temperature > Config.OSD_Statistic.BL[blIndex].max_Temp) Config.OSD_Statistic.BL[blIndex].max_Temp = blData[blIndex].Temperature; |
if( blIndex+1 > Config.OSD_Statistic.BL_Count ) Config.OSD_Statistic.BL_Count = blIndex+1; |
} |
} |
rxd_buffer_locked = FALSE; |
} |
} |
timer_get_bldata = TIME_GET_BLDATA; // n*10 ms |
} // end: if( timer_get_bldata == 0 ) |
//-------------------------------------- |
// back to: OSD-Data |
//-------------------------------------- |
mode = 'O'; // read: OSD-Data |
rxd_buffer_locked = FALSE; // ready to receive new data |
timer_mk_timeout = MK_TIMEOUT; // reset osd MK_TIMEOUT timer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void variobeep(int16_t vario) |
{ |
#ifdef USE_SOUND |
{ //start Beep |
if (vario >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
// set_beep ( 100, 0xffff, BeepNormal); |
duration = 52 -vario; |
// if (duration =0); duration = 1; |
// write_ndigit_number_u (0,6,duration,5,0); |
playTone(300+vario*2,duration,Config.Volume); |
// playTone(300,duration,volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
if (vario <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
duration = 50 -vario; |
// write_ndigit_number_u (0,7,duration,5,0); |
// if (duration < vario) ; duration = 0; |
// playTone(300,50,volume); |
playTone(300+vario*2,duration,Config.Volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (vario == 0) Vario_Threshold = 0; //Startverzögerung löschen |
} // end Beep |
#endif |
} |
//-------------------------------------------------------------- |
// Diese Funktion Beept unabhaengig von der Einstellung Config.OSD_VarioBeep |
// Aufruf ggf. mit: if( Config.OSD_VarioBeep ) Beep_Vario(); |
// |
// Ansonten: |
// -> hier noch aufräumen in Zusammenhang mit func variobeep() |
//-------------------------------------------------------------- |
void Beep_Vario(void) |
{ |
if ( (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) && (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL)) |
{ //start Beep |
if (naviData->Variometer <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 300, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (naviData->Variometer == 0) Vario_Threshold = 0; //Startverzögerung löschen |
if (naviData->Variometer >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 100, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
} // end Beep |
} |
//-------------------------------------------------------------- |
// Quelle Mikrokopter FC-Software Holger + Ingo |
//-------------------------------------------------------------- |
void CheckMKLipo( void ) |
{ |
if( Config.MK_LowBat < 50 ) // automatische Zellenerkennung |
{ |
if( CellIsChecked <= 2 ) // nur beim Start 1x prüfen |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE) |
break; |
} |
BattLowVoltageWarning = cells * Config.MK_LowBat; |
CellIsChecked++; |
} |
} |
else BattLowVoltageWarning = Config.MK_LowBat; |
if( naviData->UBat < BattLowVoltageWarning) |
{ |
if( AkkuWarnThreshold <= 4) |
{ |
AkkuWarnThreshold++; |
} |
else if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
{ |
// MK-Unterspannungs-Beep nur wenn Motoren laufen |
set_beep ( 1000, 0x0020, BeepSevere); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_heading_home(void) |
{ |
int16_t degree; |
//--------------------------------------- |
// warum modulo 360: |
// |
// die NC kann unter gewissen Umstaenden Winkel > 360 Grad |
// senden -> fragt H&I warum |
//--------------------------------------- |
degree = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
if( degree < 0 ) |
heading_home = (int16_t)(360 + degree); |
else |
heading_home = (int16_t)degree; |
} |
//-------------------------------------------------------------- |
// convert the <heading> gotton from NC into an index |
uint8_t heading_conv (uint16_t heading) |
{ |
if (heading > 23 && heading < 68) |
return 0; //direction = "NE"; |
else if (heading > 67 && heading < 113) |
return 1; //direction = "E "; |
else if (heading > 112 && heading < 158) |
return 2; //direction = "SE"; |
else if (heading > 157 && heading < 203) |
return 3; //direction = "S "; |
else if (heading > 202 && heading < 248) |
return 4; //direction = "SW"; |
else if (heading > 247 && heading < 293) |
return 5; //direction = "W "; |
else if (heading > 292 && heading < 338) |
return 6; //direction = "NW"; |
return 7; //direction = "N "; |
} |
//-------------------------------------------------------------- |
// draw a compass rose at <x>/<y> for <heading> |
void draw_compass (uint8_t x, uint8_t y, uint16_t heading, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t front = 19 + (heading / 22); |
for (uint8_t i = 0; i < 9; i++) |
lcdx_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// variometer |
// x, y in Pixel! |
//-------------------------------------------------------------- |
void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
lcd_rect (x, y - ((hight) / 2), width, hight, 1); |
lcd_frect (x + 1, y - ((hight) / 2) + 1, width - 2, hight - 2, 0); |
lcd_line (x, y, x + width, y, 1); |
if (variometer > 0) // steigend |
{ |
switch (variometer / 5) |
{ |
case 0: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
break; |
case 1: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
break; |
case 2: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
break; |
default: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
lcd_frect (x + 1, y - 6, 8, 1, 1); // 15 > |
break; |
} |
} |
else if (variometer < 0) // fallend |
{ |
switch ((variometer) / -5) |
{ |
case 0: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
break; |
case 1: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
break; |
case 2: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
break; |
default: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
lcd_frect (x + 1, y + 5, 8, 1, 1); // -15 > |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
// variometer 2 |
// |
// x, y in Pixel |
// x, y top, left |
//-------------------------------------------------------------- |
/* |
void draw_variometer2( uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
uint8_t max = 5; // max: 5 m/sec == 100% |
lcd_rect (x, y, width, hight, 1); |
lcd_frect(x + 1, y + 1, width - 2, hight - 2, 0); |
lcd_line (x, y + ((hight) / 2), x + width, y + ((hight) / 2), 1); |
} |
*/ |
//-------------------------------------------------------------- |
// Home symbol |
// draw Homesymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_homesymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_plot (x,y-4,1); |
lcd_line (x+1,y-1,x+1,y-5,1); |
lcd_plot (x+2,y-6,1); |
lcd_plot (x+3,y-7,1); |
lcd_plot (x+4,y-6,1); |
lcd_line (x+5,y-1,x+5,y-5,1); |
lcd_plot (x+6,y-4,1); |
lcd_plot (x+3,y-1,1); |
lcd_plot (x+3,y-2,1); |
lcd_line (x+1,y,x+5,y,1); |
} |
//-------------------------------------------------------------- |
// Target symbol |
// draw Targetsymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_targetsymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_circle (x+3, y-3, 4, 1); |
lcd_line (x,y-3,x+6,y-3,1); |
lcd_line (x+3,y,x+3,y-6,1); |
lcd_circle (x+3, y-3, 2, 1); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
if (Altimeter > (300 / AltimeterAdjust) || Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
writex_ndigit_number_s ( x, y, (Altimeter / (30 / AltimeterAdjust)), 4, 0, mode, xoffs,yoffs); |
else // up to 10m write meters.dm |
writex_ndigit_number_s_10th( x, y, (Altimeter / (3 / AltimeterAdjust)), 3, 0, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcd_o_circle (uint16_t x, uint16_t y, int16_t breite, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 2; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x+xoffs, y+yoffs, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcd_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcdx_o_circle( uint8_t x, uint8_t y, int8_t width, uint8_t mode) |
{ |
int8_t radius = width / 2; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x, y, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcdx_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_mk( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: rund |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_round( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_ellipse( x+5, y+5, 5+1, 5, 1); // Aussenkreis |
//lcd_line( x+2, y+5, x+8, y+5, 1); // Innenkreuz Horizontal (laenger) |
lcd_line( x+3, y+5, x+7, y+5, 1); // Innenkreuz Horizontal (kuerzer) |
lcd_line( x+5, y+3, x+5, y+7, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: eckig |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_diamond( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_home( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+5, 10, 8, 1); // Mitte |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach Rechts |
lcd_rect( x+4, y+10, 2, 3, 1); // Tuere |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_sat( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+2, 4, 2, 1); // linker Fluegel |
lcd_rect( x+8, y+2, 4, 2, 1); // rechter Fluegel |
lcd_rect( x+4, y+0, 4, 6, 1); // Mitte, oben |
lcd_line( x+6, y+7, x+2, y+11, 1); // Strahl Links |
lcd_line( x+6, y+7, x+10, y+11, 1); // Strahl Rechts |
lcd_line( x+1, y+12, x+11, y+12, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini2( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y-1, x+4, y-1, 1); // |
//lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+0, x+6, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+0, 0); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_battery( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+2, y+0, 2, 2, 1); // der kleine Knubbel oben |
lcd_rect( x+0, y+2, 6, 15, 1); // body |
} |
//-------------------------------------------------------------- |
// RC symbol |
// alternatives Symbol fuer RC-Quality |
//-------------------------------------------------------------- |
void draw_symbol_rc( uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 1; |
x += 1; |
lcd_plot( x+3, y+4, 1); |
lcd_line( x+2, y+2, x+4, y+2, 1); |
lcd_line( x+1, y+0, x+5, y+0, 1); |
} |
//############################################################## |
//# spezielle Beeps |
//############################################################## |
//-------------------------------------------------------------- |
// Beep_Waypoint() |
// |
// Beep bei Waypoint Wechsel und wenn die WP-Liste abgearbeitet |
// ist |
//-------------------------------------------------------------- |
void Beep_Waypoint( void ) |
{ |
//----------------- |
// BEEP: WP erreicht |
//----------------- |
if( naviData->WaypointIndex > WP_old ) |
{ |
set_beep( 30, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( (naviData->WaypointIndex == naviData->WaypointNumber) |
&& (naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
&& (!WP_last) |
) |
{ |
set_beep( 400, 0xffff, BeepNormal ); // langer Bestaetigungs-Beep |
WP_last = true; |
} |
if( naviData->WaypointIndex != WP_old ) |
{ |
WP_old = naviData->WaypointIndex; |
} |
if( (naviData->WaypointIndex != naviData->WaypointNumber) // wenn aktueller WP != WP-Anzahl -> WP_last abschalten |
|| !(naviData->NCFlags & NC_FLAG_TARGET_REACHED) // wenn kein TR mehr an ist -> WP_last abschalten |
) |
{ |
WP_last = false; |
} |
// alter Code zur Orientierung |
//if( OldWP != naviData->WaypointIndex ) |
//{ |
// OldWP = naviData->WaypointIndex; |
// NextWP = true; |
//} |
// |
//if( (NextWP==true) && (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ) |
//{ |
// set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
// NextWP = false; |
//} |
} |
//############################################################## |
//# OSD-ELEMENTS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs, yoffs) |
// |
// xC, yC : x,y in Characters |
// item : OSD_FLAG_AH, OSD_FLAG_PH, usw. |
// lOn : true / false |
// xoffs,yoffs : x,y Pixelverschiebung |
//-------------------------------------------------------------- |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs) |
{ |
int8_t x = (xC*6)-2; |
int8_t y = (yC*8)-1; |
uint8_t w = 14; |
uint8_t h = 8; |
const char *labels[OSD_FLAG_COUNT] = |
{ |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!"), // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
}; |
//lcd_plot( x-2, y-2, 1); // Referenz |
if( yC==0 && yoffs<=0 ) { y = 0; h = 7; } |
if( xC==0 && xoffs<=0 ) { x = 0; w = 12; } |
if( lOn ) |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 1); // Filler |
lcdx_printp_at( xC, yC, labels[item], MINVERS, xoffs,yoffs); // Label |
} |
else |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 0); // clear |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t lOn = 0; |
// FC_StatusFlags 0.88 |
switch( item ) |
{ |
// Altitue Hold |
case OSD_FLAG_AH : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL); |
break; |
// Position Hold |
case OSD_FLAG_PH : lOn = (naviData->NCFlags & NC_FLAG_PH); |
break; |
// Coming Home |
case OSD_FLAG_CH : lOn = (naviData->NCFlags & NC_FLAG_CH); |
break; |
// Care Free |
case OSD_FLAG_CF : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE); |
break; |
// Out1 |
case OSD_FLAG_O1 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE); |
break; |
// Out2 |
case OSD_FLAG_O2 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE); |
break; |
// LowBat warning (MK) |
case OSD_FLAG_BA : lOn = (naviData->FCStatusFlags & FC_STATUS_LOWBAT); |
break; |
// Calibrate |
case OSD_FLAG_CA : lOn = (naviData->FCStatusFlags & FC_STATUS_CALIBRATE); |
break; |
// Start |
case OSD_FLAG_ST : lOn = (naviData->FCStatusFlags & FC_STATUS_START); |
break; |
// Motor Run |
case OSD_FLAG_MR : lOn = (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN); |
break; |
// Fly |
case OSD_FLAG_FY : lOn = (naviData->FCStatusFlags & FC_STATUS_FLY); |
break; |
// Emergency Landing |
case OSD_FLAG_EL : lOn = (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING); |
break; |
// RC Failsave Active |
case OSD_FLAG_FS : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE); |
break; |
// GPS ok |
case OSD_FLAG_GP : lOn = (naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// GPS-Sat not ok (GPS NOT ok) |
case OSD_FLAG_S0 : lOn = !(naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// Vario Trim Up |
case OSD_FLAG_TU : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_UP); |
break; |
// Vario Trim Down |
case OSD_FLAG_TD : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_DOWN); |
break; |
// Free |
case OSD_FLAG_FR : lOn = (naviData->NCFlags & NC_FLAG_FREE); |
break; |
// Range Limit |
case OSD_FLAG_RL : lOn = (naviData->NCFlags & NC_FLAG_RANGE_LIMIT); |
break; |
// No Serial Link |
case OSD_FLAG_SL : lOn = (naviData->NCFlags & NC_FLAG_NOSERIALLINK); |
break; |
// Target Reached |
case OSD_FLAG_TR : lOn = (naviData->NCFlags & NC_FLAG_TARGET_REACHED); |
break; |
// Manual Control |
case OSD_FLAG_MC : lOn = (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL); |
break; |
} |
OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// Anzeige von Steigen / Sinken |
//-------------------------------------------------------------- |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs) |
{ |
x = (x*6) + xoffs; |
y = (y*8) + yoffs; |
lcd_frect( x, y, 6, 7, 0); // clear |
if( naviData->Variometer > 2 ) // steigen mehr als 0.2 m/sec (ein guter Wert muss noch in der Praxis ermittelt werden) |
{ |
lcd_line( x+2, y+0, x+0, y+2, 1); |
lcd_line( x+2, y+0, x+4, y+2, 1); |
} |
else if( naviData->Variometer < -2 ) // sinken mehr als 0.2 m/sec |
{ |
lcd_line( x+2, y+6, x+0, y+4, 1); |
lcd_line( x+2, y+6, x+4, y+4, 1); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Altitude( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
switch( nStyle ) |
{ |
case 0 : |
case 1 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
break; |
case 2 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x+4, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MBIG); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x+4, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MBIG); |
lcd_putc( x+8, y, 'm', MBIG); |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
break; |
} |
#else |
if( nStyle == 2 ) |
{ |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
x += 4; |
} |
else |
{ |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
} |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
#endif |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==1 |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ) |
{ |
uint16_t Balken = 0; |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); |
if( cells > 0 ) // LipobargraphAnzeige nur wenn Anzahl der Lipozellen bekannt sind |
{ |
write_ndigit_number_u (x+6, y, cells, 1, 0, drawmode); |
lcd_printp_at (x+7, y, PSTR("S"), drawmode); |
if( cells==3 ) |
{ |
lcd_rect(x*6, y*8, 28, 7, 1); // Rahmen |
Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/12; |
if((Balken > 0) && (Balken <28)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if(Balken <= 26) lcd_frect(Balken+(x*6)+1, (y*8)+1, 26-Balken, 5, 0); // loeschen |
} |
if( cells==4 ||cells==5 ) |
{ |
lcd_rect(x*6, y*8, 30, 7, 1); // Rahmen |
if (cells == 4) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/15; |
if (cells == 5) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/19; |
if ((Balken > 0) && (Balken <=29)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if (Balken <= 27) lcd_frect(Balken+(x*6)+1, (y*8)+1, 28-Balken, 5, 0); // loeschen |
} |
} // end if: cells > 0 (TODO: Anzeige fuer cells==0 implementieren) |
} |
//-------------------------------------------------------------- |
// fuer die neuen OSD-Screens |
//-------------------------------------------------------------- |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
drawmode = (naviData->UBat < BattLowVoltageWarning ? MINVERS : MNORMAL); |
if( Config.OSD_ShowCellU ) |
{ |
// kalk. Einzelzellen Spannungsanzeige |
writex_ndigit_number_u_100th( x, y, (uint16_t)((naviData->UBat*10)/cells), 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("v"), drawmode, xoffs+1,yoffs); // Einheit (Einzelzellenanzeige ca., berechnet) |
} |
else |
{ |
// Gesamtspannungsanzeige |
writex_ndigit_number_u_10th( x, y, naviData->UBat, 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("V"), drawmode, xoffs+1,yoffs); // Einheit (Gesamtspannung) |
} |
lcd_line( (x+4)*6, y*8, (x+4)*6, y*8+7, (drawmode==MINVERS ? 1 : 0) ); // filler zwischen Spannung und "V" |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==0 |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
if( nStyle <= 1) |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 4 : 3); // Fett-Schrift |
if( nStyle <= 1) |
{ |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
} |
else |
{ |
write_ndigit_number_u_10th (x-2, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR("V"), drawmode); |
} |
#else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
if( nStyle == 2) |
x += 3; |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
#endif |
} |
//-------------------------------------------------------------- |
// OSD_Element_Battery_Bar( x, y, length, width, orientation) |
// |
// neue Variante (OG 06/2013) |
// |
// Parameter: |
// length : Gesamtlaenge des Bar's |
// width : Breite in Pixel (fest) |
// orientation: ORIENTATION_V - Vertikal (x,y oberer,linker Punkt) |
// ORIENTATION_H |
//-------------------------------------------------------------- |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation) |
{ |
uint8_t bat_umax; |
uint8_t bat_umin; |
uint8_t bat_uact; |
int8_t bat_p; |
// die Min/Max Spannungswerte sind jetzt erstmal fest vorgegeben bzw. berechnet, eine individuelle |
// Anpassung waere moeglich aber wenn besser ist es, wenn der User das nicht machen muesste... |
bat_umax = cells * 42; |
//bat_umin = cells * 32; |
bat_umin = cells * 34; // 3.4 Volt pro Zelle Minimum |
bat_uact = naviData->UBat; |
bat_p = length * (bat_uact-bat_umin) / (bat_umax-bat_umin); |
if( bat_p < 0 ) bat_p = 0; |
if( bat_p > length ) bat_p = length; |
if( width == 1 ) width = 0; // eine kleine Eigenart von frect um es zu ueberreden eine Linie zu zeichnen |
if( orientation == ORIENTATION_V ) |
{ |
lcd_frect( x, y , width, length-bat_p, 0); // clear: vertical |
lcd_frect( x, y+length-bat_p, width, bat_p, 1); // draw: vertical |
} |
else |
{ |
lcd_frect( x+bat_p, y, length-bat_p, width, 0); // clear: |
lcd_frect( x , y, bat_p , width, 1); // draw: |
} |
} |
//-------------------------------------------------------------- |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( Config.OSD_LipoBar ) |
OSD_Element_BatteryLevel_Bar( x, y); |
else |
OSD_Element_BatteryLevel_Text( x, y, nStyle); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Capacity( uint8_t x, uint8_t y ) |
{ |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? 2 : 0); |
write_ndigit_number_u (x, y, naviData->UsedCapacity, 5, 0, drawmode); |
lcd_printp_at (x+5, y, PSTR("mAh"), drawmode); |
// BeepTime = 3000; |
// BeepMuster = 0x0020; |
} |
//-------------------------------------------------------------- |
// OSD_Element_CompassDegree( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
x += 3; |
break; |
case 2 : |
#ifdef USE_FONT_BIG |
write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MBIG); |
#else |
write_ndigit_number_u (x+5, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
#endif |
x += 8; |
break; |
} |
lcd_putc( x, y, 0x1E, MNORMAL); // degree symbol |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printp_at (x, y, (const char *) (pgm_read_word ( &(directions_p[heading_conv((naviData->CompassHeading)%360)]))), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ) |
{ |
draw_compass (x, y, (naviData->CompassHeading)%360, 0,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Current( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u_10th (x, y, naviData->Current, 3, 0,0); |
lcd_printp_at (x+4, y, PSTR("A"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ) |
{ |
write_time (x, y, naviData->FlyingTime); |
lcd_printp_at (x+5, y, PSTR("m"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,0); |
lcd_printp_at (x+3, y, PSTR("Kmh"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ) |
{ |
calc_heading_home(); |
lcd_o_circle( x, y, breite, 1, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (old_hh + 180), rOffset, 0, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (heading_home + 180), rOffset, 1, xoffs,yoffs); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
// NEU! 22.04.2014 OG |
// soll das alte OSD_Element_HomeCircle() komplett ersetzen wenn |
// die alten OSD-Screens endgueltig rausfliegen |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
{ |
calc_heading_home(); |
lcd_ellipse_line( px, py, rx, ry, (old_hh + 180), 0); |
lcd_ellipse_line( px, py, rx, ry, (heading_home + 180), 1); |
lcd_ellipse( px, py, rx, ry, 1); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, heading_home, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//-------------------------------------------------------------- |
// OSD_Element_HomeDistance( x, y, nStyle) |
//-------------------------------------------------------------- |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_homesymbol(x+4, y); |
break; |
case 2 : lcd_printp_at (x, y, PSTR("Home"), 0); |
write_ndigit_number_u (x+5, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_LEDOutput( x, y, bitmask) |
// |
// bitmask: LED1 = FC_STATUS2_OUT1_ACTIVE |
// LED2 = FC_STATUS2_OUT2_ACTIVE |
//-------------------------------------------------------------- |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ) |
{ |
uint8_t lOn; |
lOn = (naviData->FCStatusFlags2 & bitmask ? 1 : 0); // Bit gesetzt? |
lOn = (Config.OSD_InvertOut ? !lOn : lOn); // Invertieren? |
lOn = (lOn ? 1 : 0); // auf 0 oder 1 setzen |
lcd_fcircle (x * 6 + 5, y * 8 + 3, Config.OSD_LEDform, lOn); |
lcd_circle (x * 6 + 5, y * 8 + 3, 3, 1); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED1Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT1_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED2Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT2_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Manuell( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Manuell( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL) |
lcd_putc (x, y, 'M', 0); // rc transmitter |
else |
lcd_putc (x, y, 'X', 0); // clear |
} |
//-------------------------------------------------------------- |
// OSD_Element_RCIntensity( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->RC_Quality, 3, 0,0); |
//lcd_printp_at (x+3, y, PSTR("\x1F"), 0); // RC-transmitter |
if (naviData->NCFlags & NC_FLAG_NOSERIALLINK) |
{ |
lcd_printpns_at(x+3, y, PSTR(" "), 0); // Clear |
} |
else |
{ |
lcd_printpns_at(x+3, y, PSTR("PC"), 0); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_SatsInUse( x, y, nStyle) |
// |
// nStyle == 0: "00s" |
// nStyle == 1: wie 0 |
// nStyle == 2: "00 Sat" |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
drawmode = (naviData->NCFlags & NC_FLAG_GPS_OK ? 0 : 2); |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_putc (x+2, y, 0x08, drawmode); |
break; |
case 2 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR(" Sat"), drawmode); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Variometer( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Variometer( uint8_t x, uint8_t y ) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
draw_variometer (x, y, 10, 14, naviData->Variometer); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Target( x, y, nStyle) |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
// nStyle = 0,1: "000m" |
// nStyle = 2,3: "Ziel 000m -" |
//-------------------------------------------------------------- |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( nStyle <= 1 ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_targetsymbol(x+4,y); |
} |
else |
{ |
lcd_printp_at (x, y, PSTR("Ziel"), 0); |
write_ndigit_number_u (x+5, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
} |
} |
//-------------------------------------------------------------- |
// TODO: |
// - pruefen ob beep hier an richtiger Stelle ist |
//-------------------------------------------------------------- |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ) |
{ |
uint8_t FC_Fallspeed; |
FC_Fallspeed = (unsigned int)naviData->Variometer; |
FC_Fallspeed = 255-FC_Fallspeed; |
drawmode = ( (naviData->Variometer < 0) && (FC_Fallspeed > Config.OSD_Fallspeed) ? 2 : 0); |
if( Config.OSD_VarioBeep ) |
Beep_Vario(); // Beep ??? |
if( drawmode == 2 ) |
{ |
if( !Config.HWSound ) |
set_beep ( 1000, 0x0060, BeepNormal); // muss ein Beep hier hin???? |
else |
variobeep(naviData->Variometer); // muss ein Beep hier hin???? |
} |
write_ndigit_number_s_10th (x, y, naviData->Variometer, 3,0, drawmode); |
lcd_printpns_at(x+4, y, PSTR("ms"), drawmode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ) |
{ |
if (!OldWP == naviData->WaypointIndex) |
{ |
// BeepTime = 500; |
// BeepMuster = 0x0080; |
OldWP = naviData->WaypointIndex; |
NextWP = true; |
} |
if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
{ |
set_beep ( 500, 0x0080, BeepNormal); |
NextWP = false; |
} |
write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
lcd_printp_at (x, y, PSTR("WP"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Bearing/ 10, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//############################################################## |
//# OSD-SCREENS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD-Screen "General" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
void OSD_Screen_General( void ) |
{ |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// Display: 128 x 64 with 6x8 Font => 21 x 8 |
// Linien: Horizontal |
lcd_line (0, 28, 127, 28, 1); // mitte |
lcd_line (0, 51, 127, 51, 1); // unten |
// Linien: Vertikal |
lcd_line (65, 0, 65, 50, 1); // mitte |
//----------------------------------------- |
// Block: Oben - Links |
//----------------------------------------- |
draw_icon_battery(0,4); |
lcdx_printp_at( 7, 2, PSTR(" mA"), MNORMAL, 0,2); |
//----------------------------------------- |
// Block: Oben - Rechts |
//----------------------------------------- |
lcdx_printp_at( 12, 0, PSTR("Alt:") , MNORMAL, 0,0); |
lcdx_printp_at( 12, 1, PSTR("Dir:") , MNORMAL, 0,1); |
lcdx_putc( 20, 1, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
lcdx_printp_at( 12, 2, PSTR(" I:") , MNORMAL, 0,2); |
lcdx_putc( 20, 2, 'A', MNORMAL, 2,2); |
//----------------------------------------- |
// Block: Unten - Links |
//----------------------------------------- |
draw_icon_sat(0,33); |
lcdx_printp_at( 6, 5, PSTR(" kmh"), MNORMAL, 0,1); |
//----------------------------------------- |
// Block: Unten - Rechts |
//----------------------------------------- |
draw_icon_home( 70, 32); |
lcdx_putc( 20, 4, 'm', MNORMAL, 2,0); |
lcdx_putc( 20, 5, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
//----------------------------------------- |
// unterste Zeile |
//----------------------------------------- |
draw_symbol_rc( 20, 7); // RC-transmitter |
//lcdx_putc( 20, 7, SYMBOL_RCQUALITY, MNORMAL, 2,0); |
} |
//----------------- |
// Batt Level (Graph) |
//----------------- |
OSD_Element_Battery_Bar( 2, 8, 11, 2, ORIENTATION_V); |
//----------------- |
// Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 2, 0, 0,0 ); |
//----------------- |
// LowBat Warnung MK |
//----------------- |
OSD_Element_Flag( 8, 0, OSD_FLAG_BA, 0,0 ); |
//----------------- |
// Flugzeit |
//----------------- |
writex_time(2, 1, naviData->FlyingTime, MNORMAL, 0,1); |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 2, 2, naviData->UsedCapacity, 5, 0, drawmode, 0,2); |
//----------------- |
// Höhe |
//----------------- |
writex_altimeter( 16, 0, naviData->Altimeter, MNORMAL, 0,0 ); |
/* |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s ( 16, 0, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0, MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th( 16, 0, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0, MNORMAL); |
*/ |
//----------------- |
// steigen / sinken |
//----------------- |
OSD_Element_UpDown( 20, 0, 2,0); |
//----------------- |
// Compass Degree |
//----------------- |
writex_ndigit_number_u (17, 1, (naviData->CompassHeading)%360, 3, 0,MNORMAL, 0,1); |
//----------------- |
// Strom |
//----------------- |
//write_ndigit_number_u_10th( 16, 2, naviData->Current, 3, 0,0); // alternativ mit Nachkomma |
writex_ndigit_number_u( 17, 2, naviData->Current/10, 3, 0,MNORMAL, 0,2); |
//----------------- |
// Sat Anzahl |
//----------------- |
write_ndigit_number_u (4, 4, naviData->SatsInUse, 2, 0,MNORMAL); |
//----------------- |
// Sat Warnung "!" |
//----------------- |
/* |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) |
lcd_printp_at( 9, 4, PSTR(" "), MNORMAL); |
else |
lcd_printp_at( 9, 4, PSTR("!"), MNORMAL); |
*/ |
OSD_Element_Flag( 8, 4, OSD_FLAG_S0, -1,0 ); // Sat Warnung (GPS not ok) |
//----------------- |
// Geschwindigkeit |
//----------------- |
writex_ndigit_number_u( 3, 5, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL, 0,1); |
//----------------- |
// Home Distance |
//----------------- |
write_ndigit_number_u( 17, 4, naviData->HomePositionDeviation.Distance / 10, 3, 0,MNORMAL); |
//----------------- |
// Home Winkel |
//----------------- |
writex_ndigit_number_u( 16, 5, heading_home, 4, 0,MNORMAL, 0,1); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 1, 7, OSD_FLAG_CF, 0,0 ); // Care Free |
OSD_Element_Flag( 4, 7, OSD_FLAG_AH, 0,0 ); // Altitude Hold |
OSD_Element_Flag( 7, 7, OSD_FLAG_PH, 0,0 ); // Position Hold |
OSD_Element_Flag( 10, 7, OSD_FLAG_CH, 0,0 ); // Coming Home |
OSD_Element_Flag( 13, 7, OSD_FLAG_EL, 0,0 ); // Emergency Landing |
//----------------- |
// RC-Quality (MK) |
//----------------- |
write_ndigit_number_u( 17, 7, naviData->RC_Quality, 3, 0,MNORMAL); |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 8, 0, OSD_FLAG_BA, true, 0,0 ); // DEMO: Batterie Warnung |
OSD_Element_Flag_Label( 8, 4, OSD_FLAG_S0, true, -1,0 ); // DEMO: Sat Warnung (GPS not ok) |
OSD_Element_Flag_Label( 1, 7, OSD_FLAG_CF, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 4, 7, OSD_FLAG_AH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 7, 7, OSD_FLAG_PH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 10, 7, OSD_FLAG_CH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 13, 7, OSD_FLAG_EL, true, 0,0 ); // DEMO |
#endif |
} |
//-------------------------------------------------------------- |
// OSD-Screen "Navigation" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
void OSD_Screen_Navigation( void ) |
{ |
int8_t xoffs, yoffs; |
int16_t degree; |
uint8_t minus; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcdx_printp_at( 0, 2, PSTR("Alt:"), MNORMAL, 0,2); // Hoehe |
lcdx_printp_at( 0, 5, PSTR("Home:"), MNORMAL, 0,3); // Home Distance |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 0; |
yoffs = 3; |
writex_altimeter( 0, 3, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 4, 3, 1,yoffs); |
//----------------- |
// Home Distance |
//----------------- |
yoffs = 3; |
writex_ndigit_number_u( 0, 6, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, 0,yoffs+1); |
lcdx_printp_at( 4, 6, PSTR("m"), MNORMAL, 2,yoffs+1); // Home |
//----------------- |
// Home Circle |
//----------------- |
//void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( 64, 38, 25, 22 ); // entspricht dem ehemaligem Huellkreis |
OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
lcd_frect( (9*6)-3+xoffs, (4*8)-2+yoffs, (3*6)+4, (1*8)+2, 0); // inner clear |
lcd_rect ( (9*6)-4+xoffs, (4*8)-3+yoffs, (3*6)+6, (1*8)+4, 1); // inner rect |
lcd_frect( 61+xoffs, 57+yoffs, 2, 2, 1); // bottom mini rect |
degree = (int16_t)heading_home; |
minus = false; |
if( degree >= 180 ) degree = 360 - degree; |
else minus = true; |
writex_ndigit_number_u( 9, 4, degree, 3, 0,MNORMAL, xoffs+1,yoffs); // Degree (Winkel) |
if( minus && degree != 0 ) |
lcd_line( (9*6)-2+xoffs, (4*8)+3+yoffs, (9*6)-1+xoffs, (4*8)+3+yoffs, 1); |
//----------------- |
// Variometer |
//----------------- |
// OG: Variometer wird erstmal nicht angezeigt weil es den Screen zu voll macht |
// wenn doch muessen erst grafische Anpassungen an den Variometer-Code gemacht |
// werden |
//void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
//draw_variometer( 95, 38, 7, 30, naviData->Variometer); |
//draw_variometer( 94, 38, 7, 21, naviData->Variometer); |
//draw_variometer2( 94, 28, 7, 21, naviData->Variometer); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3, 0); // MK Batt Warning |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0, 0); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0,-3); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0,-6); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0,-1); // Coming Home |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3,-1); // GPS-Sat not ok (GPS NOT ok) |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3,0); // DEMO |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0,0); // DEMO |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0,-3); // DEMO |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0,-6); // DEMO |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0,-1); // DEMO |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3,-1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_NAVIGATION |
//-------------------------------------------------------------- |
// OSD-Screen "OSD_Screen_Waypoints_OLD" |
// |
// alte, alternative Variante - wenn sich die neue Variante |
// durchsetzt kann das hier geloescht werden |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
/* |
void OSD_Screen_Waypoints_OLD( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, 127, 12+25, 1); // Trennlinie in der Mitte |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
//draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 6; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+2,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -2,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( px, py, rx, ry ) |
//OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
OSD_Element_HomeCircleX( 112, 24, 10, 8 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-1, 33, 2, 1, 1); // bottom mini rect |
lcd_frect( 112-1, 33, 2, 0, 1); // bottom mini rect |
lcd_frect( 112-0, 33, 0, 1, 1); // bottom mini rect |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 6; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+2,yoffs); |
uint8_t wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//----------------- |
// Sat Anzahl |
//----------------- |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, 0,2); |
draw_icon_satmini( 117, 42); |
//----------------- |
// WP: Distance |
//----------------- |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// Oben: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
// Variante: TR neben Waypoint-Anzahl |
//OSD_Element_Flag( 13, 5, OSD_FLAG_TR, 6,2); // Target Reached |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
*/ |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "Waypoints" |
// |
// aktuelle Variante! |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
void OSD_Screen_Waypoints( void ) |
{ |
int8_t xoffs, yoffs; |
uint8_t v; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, (15*6+5), 12+25, 1); // Trennlinie in der Mitte |
lcd_line( (15*6+5), 12+25, (15*6+5), 63, 1); // Linie Vertikal rechts, unten |
lcd_plot ((15*6+5), 12+25, 0); |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben,Links: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben,Links: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben,Rechts: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Oben,Mitte: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 5; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+3,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -3,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
// orginal |
//OSD_Element_HomeCircleX( 110, 25, 11, 9 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-3, 35, 2, 1, 1); // bottom mini rect |
// 1 pixel weiter links |
OSD_Element_HomeCircleX( 109, 25, 11, 9 ); // leicht erweiterter Huellkreis |
lcd_frect( 112-4, 35, 2, 1, 1); // bottom mini rect |
// etwas groesser |
//OSD_Element_HomeCircleX( 109, 26, 12, 10 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-4, 37, 2, 1, 1); // bottom mini rect |
//----------------- |
// Home: Sat Anzahl |
//----------------- |
//yoffs = -2; // alternativ |
yoffs = 0; |
//naviData->SatsInUse = 10; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, -1,2+yoffs); |
draw_icon_satmini( 115, 42+yoffs); |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 5; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+3,yoffs); |
v = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) v = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), v, naviData->WaypointNumber ); |
//----------------- |
// WP: Countdown Target-Holdtime |
//----------------- |
xoffs = 6; |
yoffs = 2; |
v = naviData->TargetHoldTime; |
if( v > 0 ) |
{ |
lcdx_printf_at_P( 12, 5, MINVERS, xoffs+2,yoffs, PSTR("%2d"), v ); |
lcd_line( (12*6)+xoffs+2, (5*8)+yoffs-1, (12*6)+xoffs+2+11, (5*8)+yoffs-1, 1); |
lcd_line( (12*6)+xoffs+1, (5*8)+yoffs-1, (12*6)+xoffs+1, (5*8)+yoffs+7, 1); |
} |
else |
{ |
lcd_frect( (12*6)+xoffs+1, (5*8)+yoffs-1, (2*8)-3, 8, 0); |
} |
//----------------- |
// WP: Distance |
//----------------- |
xoffs = 5; |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "User GPS" |
// |
// OSDScreenRefresh: OSD_SCREEN_REFESH = update values |
// OSD_SCREEN_REDRAW = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_USERGPS |
void OSD_Screen_UserGPS( void ) |
{ |
uint8_t y, i; |
int8_t yoffs; |
uint8_t show_gps_altitude = 0; // 1=GPS-Höhe anzeigen; 0=barymetrische Höhe anzeigen |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 9, 127, 63-9, 1, R2); // Rahmen |
lcdx_printp_at( 6, 0, PSTR("UGPS"), MNORMAL, 2,0); |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Sat Ok |
//----------------- |
OSD_Element_Flag( 12, 0, OSD_FLAG_S0, 5,0); // GPS-Sat not ok (GPS NOT ok) |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// die letzen 3 User-GPS Daten anzeigen |
//----------------- |
yoffs = -4; |
for(i=0; i<3; i++) |
{ |
y = (i*2)+2; |
lcd_frect( 0, (y*8)+yoffs-1, 127, 7, 1); // inverser Hintergrund |
writex_ndigit_number_u( 0, y+0, (i+1), 1, 0 , MINVERS, 4,yoffs); // Index |
writex_datetime_time( 4, y+0, Config.GPS_User[i].timestamp, MINVERS, 0,yoffs); |
//writex_time( 3, y+0, GPS_User[i].time_pkt, MINVERS, 0,yoffs); |
//lcdx_printp_at( 12, y+0, PSTR("Alt:") , MINVERS, 0,yoffs); |
lcdx_printp_at( 20, y+0, PSTR("m") , MINVERS, 0,yoffs); |
if( show_gps_altitude ) // GPS-Hoehe oder barymetrische Hoehe? |
{ |
// GPS Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("G"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_ndigit_number_s( 16, y+0, Config.GPS_User[i].GPSData.Altitude/1000, 4, 0, MINVERS, 0,yoffs); // GPS Hoehe in Meter |
} |
else |
{ |
// barymetrische Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("B"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_altimeter( 16, y+0, Config.GPS_User[i].Altimeter, MINVERS, 0,yoffs ); |
} |
lcd_line( 1, (y*8)+yoffs+7, 125, (y*8)+yoffs+7, 0); // clear: Invers unterste Linie |
writex_gpspos( 1, y+1, Config.GPS_User[i].GPSData.Latitude , MNORMAL, 0,yoffs); // GPS Lat |
writex_gpspos( 12, y+1, Config.GPS_User[i].GPSData.Longitude, MNORMAL, 0,yoffs); // GPS Long |
//writex_gpspos( 1, y+1, GPS_User[i].GPSData.Latitude + 12867000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
//writex_gpspos( 12, y+1, GPS_User[i].GPSData.Longitude+ 8568000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
yoffs++; |
} |
} |
#endif // USE_OSD_SCREEN_USERGPS |
//-------------------------------------------------------------- |
// OSD-Screen "Status" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_MKSTATUS |
void OSD_Screen_MKStatus( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 10, 127, 63-10, 1, R2); // Rahmen |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // "mAh" (entnommene Kapazität) |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Strom |
//----------------- |
//writex_ndigit_number_u_10th( 7, 0, naviData->Current, 4, 0,MNORMAL, 1,0); // Strom mit Nachkomma |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
//writex_ndigit_number_u( 7, 0, naviData->UsedCapacity * 10, 5, 0, drawmode, -3,0); // DEBUG |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Flags |
//----------------- |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0+xoffs, 0+yoffs); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0+xoffs,-3+yoffs); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0+xoffs,-6+yoffs); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0+xoffs,-1+yoffs); // Coming Home |
xoffs -= 4; |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3+xoffs, 0+yoffs); // MK Batt Warning |
OSD_Element_Flag( 16, 4, OSD_FLAG_EL, -3+xoffs,-3+yoffs); // Emergency Landing |
OSD_Element_Flag( 16, 6, OSD_FLAG_RL, -3+xoffs,-6+yoffs); // Range Limit |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3+xoffs,-1+yoffs); // GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag( 12, 2, OSD_FLAG_CA, 0+xoffs, 0+yoffs); // Calibrate |
OSD_Element_Flag( 12, 4, OSD_FLAG_ST, 0+xoffs,-3+yoffs); // Start |
OSD_Element_Flag( 12, 6, OSD_FLAG_MR, 0+xoffs,-6+yoffs); // Motor Run |
OSD_Element_Flag( 12, 7, OSD_FLAG_FY, 0+xoffs,-1+yoffs); // Fly |
xoffs -= 4; |
OSD_Element_Flag( 9, 2, OSD_FLAG_O1, -2+xoffs, 0+yoffs); // Out1 |
OSD_Element_Flag( 9, 4, OSD_FLAG_O2, -2+xoffs,-3+yoffs); // Out2 |
OSD_Element_Flag( 9, 6, OSD_FLAG_TR, -2+xoffs,-6+yoffs); // Target Reached |
OSD_Element_Flag( 9, 7, OSD_FLAG_MC, -2+xoffs,-1+yoffs); // Manual Control |
xoffs -= 4; |
OSD_Element_Flag( 6, 2, OSD_FLAG_TU, -4+xoffs, 0+yoffs); // Vario Trim Up |
OSD_Element_Flag( 6, 4, OSD_FLAG_TD, -4+xoffs,-3+yoffs); // Vario Trim Down |
OSD_Element_Flag( 6, 6, OSD_FLAG_FR, -4+xoffs,-6+yoffs); // Free |
OSD_Element_Flag( 6, 7, OSD_FLAG_SL, -4+xoffs,-1+yoffs); // No Serial Link |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
/* |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!") // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
*/ |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0+xoffs, 0+yoffs); // DEMO: Carefree |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0+xoffs,-3+yoffs); // DEMO: Altitude Hold |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0+xoffs,-6+yoffs); // DEMO: Position Hold |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0+xoffs,-1+yoffs); // DEMO: Coming Home |
xoffs -= 4; |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3+xoffs, 0+yoffs); // DEMO: MK Batt Warning |
OSD_Element_Flag_Label( 16, 4, OSD_FLAG_EL, true, -3+xoffs,-3+yoffs); // DEMO: Emergency Landing |
OSD_Element_Flag_Label( 16, 6, OSD_FLAG_RL, true, -3+xoffs,-6+yoffs); // DEMO: Range Limit |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3+xoffs,-1+yoffs); // DEMO: GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag_Label( 12, 2, OSD_FLAG_CA, true, 0+xoffs, 0+yoffs); // DEMO: Calibrate |
OSD_Element_Flag_Label( 12, 4, OSD_FLAG_ST, true, 0+xoffs,-3+yoffs); // DEMO: Start |
OSD_Element_Flag_Label( 12, 6, OSD_FLAG_MR, true, 0+xoffs,-6+yoffs); // DEMO: Motor Run |
OSD_Element_Flag_Label( 12, 7, OSD_FLAG_FY, true, 0+xoffs,-1+yoffs); // DEMO: Fly |
xoffs -= 4; |
OSD_Element_Flag_Label( 9, 2, OSD_FLAG_O1, true, -2+xoffs, 0+yoffs); // DEMO: Out1 |
OSD_Element_Flag_Label( 9, 4, OSD_FLAG_O2, true, -2+xoffs,-3+yoffs); // DEMO: Out2 |
OSD_Element_Flag_Label( 9, 6, OSD_FLAG_TR, true, -2+xoffs,-6+yoffs); // DEMO: Target Reached |
OSD_Element_Flag_Label( 9, 7, OSD_FLAG_MC, true, -2+xoffs,-1+yoffs); // DEMO: Manual Control |
xoffs -= 4; |
OSD_Element_Flag_Label( 6, 2, OSD_FLAG_TU, true, -4+xoffs, 0+yoffs); // DEMO: Vario Trim Up |
OSD_Element_Flag_Label( 6, 4, OSD_FLAG_TD, true, -4+xoffs,-3+yoffs); // DEMO: Vario Trim Down |
OSD_Element_Flag_Label( 6, 6, OSD_FLAG_FR, true, -4+xoffs,-6+yoffs); // DEMO: Free |
OSD_Element_Flag_Label( 6, 7, OSD_FLAG_SL, true, -4+xoffs,-1+yoffs); // DEMO: No Serial Link |
#endif |
} |
#endif // USE_OSD_SCREEN_MKSTATUS |
//-------------------------------------------------------------- |
// OSD_Screen_Electric() |
// |
// Anzeige BL-Temperaturen und Stroeme |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_ELECTRIC |
void OSD_Screen_Electric( void ) |
{ |
uint8_t x, y, x0, y0; |
int8_t yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line (33, 0, 33, 19, 1); // Linie vertikal oben, links |
lcd_line (93, 0, 93, 19, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 19, 127, 63-19, 1, R2); // Rahmen |
lcd_line (0, 41, 127, 41, 1); // Linie horizontal mitte |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // entnommene Kapazität |
lcdx_printp_at( 12,1 , PSTR("A") , MNORMAL, -1,2); // aktueller Strom |
writex_ndigit_number_u( 19, 1, cells, 1, 0, MNORMAL, 2,2); // LiPO Cells Wert |
lcdx_printp_at( 20,1 , PSTR("s") , MNORMAL, 2,2); // LiPO Cells "s" |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
//writex_time(16, 0, naviData->FlyingTime+900, MNORMAL, 2,0); // DEBUG |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Strom |
//----------------- |
writex_ndigit_number_u_10th( 7, 1, naviData->Current, 4, 0,MNORMAL, -3,2); // Strom mit Nachkomma |
//----------------- |
// BL 1-8 Temp & Strom |
//----------------- |
x0 = 1; |
y0 = 4; |
for( y=0; y<2; y++) // 2 Zeilen (mit je 4 BL's/Motoren) |
{ |
if( y==0 ) yoffs = -9; |
else yoffs = -3; |
for( x=0; x<4; x++) // und 4 BL's/Motoren pro Zeile |
{ |
if( blData[y*4+x].Status & 0xf0 ) // BL/Motor vorhanden? |
{ |
if( blData[y*4+x].Temperature != 0 ) // Anzeige nur wenn Temp != 0 wegen BL-Ctrl v1 die keine Temperatur senden |
{ |
writex_ndigit_number_u( (x*5)+x0, (y*2)+y0+0, ( blData[y*4+x].Temperature ), 3, 0,MNORMAL, 0,yoffs); // Temperatur |
lcdx_putc ( (x*5)+3+x0, (y*2)+y0+0, SYMBOL_SMALLDEGREE ,MNORMAL, 1,yoffs); |
} |
// Variante: mit Nachkommastellen |
writex_ndigit_number_u_10th ( (x*5)+x0, (y*2)+y0+1, ( blData[y*4+x].Current), 3, 0, MNORMAL, 0,yoffs); // Strom |
} |
} |
} |
#ifdef USE_OSD_DEMO |
OSD_Element_Flag_Label( 19, 1, OSD_FLAG_BA, true, 0,1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_ELECTRIC_N |
//-------------------------------------------------------------- |
// OSD_Screen_Statistics() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_STATISTIC |
void OSD_Screen_Statistics( void ) |
{ |
osd_BLmax_t blmax; |
uint8_t line = 0; |
// max. der BL's ermitteln |
calc_BLmax( &blmax ); |
//--------------------------- |
// max Altitude |
lcd_printp_at (0, line, strGet(STATS_ITEM_0), MNORMAL); |
write_ndigit_number_s (14, line, Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// max Speed |
// max_GroundSpeed = 1; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_1), MNORMAL); |
write_ndigit_number_u (15, line, (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("kmh"), MNORMAL, 2,0); |
//--------------------------- |
// max Distance |
// max_Distance = 64512; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_2), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Distance / 10, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// min voltage |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_3), MNORMAL); |
if( Config.OSD_Statistic.min_UBat==255 ) |
lcd_printp_at(14, line, PSTR(" 0"), MNORMAL); |
else |
write_ndigit_number_u_10th (14, line, Config.OSD_Statistic.min_UBat, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'V', MNORMAL, 2,0); |
//--------------------------- |
// Used Capacity |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_6), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Capacity, 4, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("mAh"), MNORMAL, 2,0); |
//--------------------------- |
// max Current |
// max_Current = 1000; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_5), MNORMAL); |
write_ndigit_number_u_10th (13, line, Config.OSD_Statistic.max_Current, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Current |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Curr:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Current_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u_10th (14, line, blmax.max_BL_Current, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Temp |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Temp:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Temp_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u (14, line, blmax.max_BL_Temp, 4, 0,MNORMAL); |
lcdx_printp_at( 18, line, PSTR("\013C"), MNORMAL, 2,0); |
} |
#endif // USE_OSD_SCREEN_STATISTIC |
//-------------------------------------------------------------- |
// OSD_Screen_3DLage() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_3DLAGE |
void OSD_Screen_3DLage( void ) |
{ |
uint16_t head_home; |
uint8_t Nick; |
uint8_t Roll; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_rect_round( 0, 0, 127, 63-0, 1, R2); // Rahmen |
} |
head_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_line(26,32,100,32,1); // horizontal // |
lcd_line(63,0,63,63,1); // vertical // |
// 45' Angel |
lcd_line(61,11,65,11,1); // -- // |
lcd_line(40,30,40,34,1); // | // |
lcd_line(86,30,86,34,1); // | // |
lcd_line(61,53,65,53,1); // -- // |
lcdx_printp_at( 9, 0, strGet(OSD_3D_V), MNORMAL, 0,4); // V |
lcdx_printp_at( 3, 3, strGet(OSD_3D_L), MNORMAL, 0,0); // L |
lcdx_printp_at(17, 3, strGet(OSD_3D_R), MNORMAL, 0,0); // R |
lcdx_printp_at( 9, 7, strGet(OSD_3D_H), MNORMAL, 0,-3); // H |
// Oben, Links: Ni |
lcdx_printp_at(0, 0, strGet(OSD_3D_NICK), MNORMAL, 5,4); // Ni |
writex_ndigit_number_s (2, 0, naviData->AngleNick, 3, 0, MNORMAL, 7,4); |
lcdx_putc (5, 0, SYMBOL_SMALLDEGREE, MNORMAL, 7,4); |
// Unten, Links: Ro |
lcdx_printp_at(0, 7, strGet(OSD_3D_ROLL), MNORMAL, 5,-3); // Ro |
writex_ndigit_number_s (2, 7, naviData->AngleRoll, 3, 0, MNORMAL, 7,-3); |
lcdx_putc (5, 7, SYMBOL_SMALLDEGREE, MNORMAL, 7,-3); |
// Oben, Rechts: Ko |
//lcdx_printp_at(13, 0, strGet(OSD_3D_COMPASS), MNORMAL, -4,3); // Ko |
writex_ndigit_number_u (15, 0, (naviData->CompassHeading)%360, 3, 0, MNORMAL, -4,4); |
lcdx_putc (18, 0, SYMBOL_SMALLDEGREE, MNORMAL, -4,4); |
OSD_Element_CompassDirection( 19, 0, -2,4 ); |
Roll = ((-naviData->AngleRoll/2)+63); |
Nick = ((-naviData->AngleNick/2)+32); |
if( Roll < (9+1) ) Roll = (9+1); // nicht ausserhalb des Screens zeichnen! |
if( Roll > 127-(9+1) ) Roll = 127-(9+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick < (8+1) ) Nick = (8+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick > 63-(8+1) ) Nick = 63-(8+1); // nicht ausserhalb des Screens zeichnen! |
if( old_AngleRoll != 0 ) // nicht ausserhalb des Screens zeichnen! |
{ |
lcd_ellipse ( old_AngleRoll, old_AngleNick, 9, 8, 0); |
lcd_ellipse_line( old_AngleRoll, old_AngleNick, 8, 7, old_hh, 0); |
} |
lcd_ellipse ( Roll, Nick, 9, 8, 1); |
lcd_ellipse_line( Roll, Nick, 8, 7, head_home, 1); |
// remember last values (3DL) |
old_hh = head_home; |
old_AngleNick = Nick; |
old_AngleRoll = Roll; |
} |
#endif // USE_OSD_SCREEN_3DLAGE |
//-------------------------------------------------------------- |
// OSD_Screen_MKDisplay() |
// |
// das ist ein Spezialscreen der ausserhalb der regulaeren |
// OSD-Screens aufgerufen und bedient wird! |
//-------------------------------------------------------------- |
//#ifdef USE_OSD_SCREEN_MKDISPLAY |
void OSD_Screen_MKDisplay( void ) |
{ |
uint8_t wpindex; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal Oben: links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal Oben: rechts |
lcd_rect_round( 0, 2*7-2, 127, 5*7+3+3, 1, R2); // Rahmen unten fuer Inhalt Display |
//lcd_frect_round( 6*6+0, 0, 9*6+2, 9, 1, R1); // Umrahmung fuer "OSD-Displ" |
//lcdx_printp_at( 7, 0, PSTR("OSD-Disp"), MINVERS, -3,1); // "OSD-Displ" |
//lcdx_printp_at(15, 0, PSTR("l"), MINVERS, -4,1); // das "l" von "OSD-Displ" (1 Pixel nach links) |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 0, MNORMAL, -1,0, PSTR("WP:%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//lcdx_printf_at_P(13, 0, MNORMAL, -3,1, PSTR("R:%d"), (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("WP:%d"), naviData->WaypointIndex); |
//write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
//lcd_printp_at (x, y, PSTR("WP"), 0); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Navi-Kreis |
//----------------- |
//OSD_Element_HomeCircleX( 64, 5, 6, 5, true ); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
//------------------------------------------ |
mkdisplayData[80] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &mkdisplayData[60], MNORMAL, 5,3); |
mkdisplayData[60] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &mkdisplayData[40], MNORMAL, 5,2); |
mkdisplayData[40] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &mkdisplayData[20], MNORMAL, 5,1); |
mkdisplayData[20] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &mkdisplayData[0], MNORMAL, 5,0); |
Beep_Waypoint(); |
} |
//#endif // USE_OSD_SCREEN_MKDISPLAY |
//############################################################## |
#ifdef USE_OSD_SCREEN_DEBUG |
//############################################################## |
//************************************************************** |
//* OSD_DEBUG_SCREEN - Experimental-Code usw. |
//* - nicht fuer die Oeffentlichkeit bestimmt |
//* - gesteuert ueber define OSD_DEBUG_SCREEN |
//************************************************************** |
//-------------------------------------------------------------- |
// OSD_Screen_Debug() |
//-------------------------------------------------------------- |
void OSD_Screen_Debug( void ) |
{ |
//char buffer[80]; |
static uint16_t debug_count = 0; |
//char buffer[30]; |
//uint8_t y, i; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_printp_at( 0, 0, PSTR("Debug"), 0); |
//timer_pkt_uptime = 0; |
} |
debug_count++; |
write_ndigit_number_u( 17, 0, (debug_count), 4, 0,MNORMAL); // |
// act. Current |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("act Current%3.1u A"), naviData->Current); |
// max. Current |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("max Current%3.1u A"), Config.OSD_Statistic.max_Current); |
// avg. Current |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("avg Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Current/100)); |
// avg. Current DEBUG |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("avgD Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Altimeter/100)); |
// stat. Packages |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("stat Pkg's%7u"), Config.OSD_Statistic.count_osd); |
/* |
// DEBUG: Heading |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("NC-Errorcode:%3u"), naviData->Errorcode); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("CH:%6d%6d"), naviData->CompassHeading, naviData->CompassHeading%360); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("HD:%6d%6d"), naviData->HomePositionDeviation.Bearing, naviData->HomePositionDeviation.Bearing%360); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)) + 180; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
calc_heading_home(); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("xx:%6d"), heading_home); |
*/ |
/* |
lcd_putc( 0, 3, 0x08, MNORMAL); // ASCII - 8 08 SAT Symbol |
lcd_putc( 2, 3, 0x09, MNORMAL); // ASCII - 9 09 Altitude Symbol |
lcd_putc( 4, 3, 0x0C, MNORMAL); // ASCII - 12 0C Enter Symbol |
lcd_putc( 6, 3, 0x1F, MNORMAL); // ASCII - 31 1F Antenne |
lcd_putc( 8, 3, 10, MNORMAL); // 'o' |
lcd_putc(10, 3, 13, MNORMAL); // 'o' |
//lcd_putc(10, 3, 0x06, MNORMAL); |
//lcd_putc(12, 3, 0x07, MNORMAL); |
lcd_putc( 0, 5, 0x1E, MNORMAL); |
lcd_putc( 4, 5, 0x7e, MNORMAL); |
lcd_putc( 6, 5, 0x7f, MNORMAL); |
lcd_putc( 8, 5, 0x18, MNORMAL); |
lcd_putc(10, 5, 0x19, MNORMAL); |
*/ |
//lcd_printp_at( 0, 1, PSTR("Free RAM:"), 0); |
//writex_ndigit_number_u ( 9, 1, get_freeRam(), 9, 0,MNORMAL, 0,0); // |
} |
//-------------------------------------------------------------- |
// OSD_Screen_Debug_RX() |
// |
// Anzeige gelesener Datenpakete (fuer Feinabstimmung) und weitere |
// Werte wie Zeit/Datum (mit/ohne Abgleich zur NC) |
// |
// Anzeige oben 1. Zeile: |
// Screenname : "Debug-RX" |
// PKT-Uptime : Minuten, Sekunden die das PKT aktuell eingeschaltet ist |
// Screen-Count: Anzahl der Aufrufe des Screens (abhaengig von der Refreshtime) |
// |
// Anzeige Zeit: |
// "N" oder "O": neuer(N) oder alter(O) Zeit-Algo fuer die NC (OSD_MK_UTCTime()) |
// 00:00:00 : Stunde, Minute, Sekunde (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// dd.mm.yyyy : Tag, Monat, Jahr (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// Solange keine richtige Zeit von der NC gemeldet wird, wird die PKT-Uptime |
// seit einschalten des PKT angezeigt. |
// |
// Anzeige unten: |
// OSD: Anzahl gelesener OSD-Pakete der NC |
// Time: Anzahl gelesener Time-Pakete der NC (Aktualisierung ca. jede Minute) |
// |
// BL: es werden die gelesenen Datenpakete der BL's angezeigt |
// von BL1 (links oben) bis BL8 (rechts unten) |
//-------------------------------------------------------------- |
void OSD_Screen_Debug_RX( void ) |
{ |
static uint16_t debug_count = 0; |
uint8_t y; |
int8_t yoffs; |
//uint8_t status; // FC Kommunikation |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_frect( 0, 0, 1, 8, 1); // title spacer |
lcdx_printp_at( 0, 0, PSTR("Debug-RX"), MINVERS, 1,0); // title |
lcd_line (0, 9, 127, 9, 1); // Linie horizontal |
lcd_line (0, 22, 127, 22, 1); // Linie horizontal |
//lcd_line (0, 31, 127, 31, 1); // Linie horizontal |
} |
//----------------------- |
// Zeile 0 |
//----------------------- |
writex_time( 11, 0, timer_pkt_uptime/100, MNORMAL, 0,0); |
debug_count++; |
write_ndigit_number_u ( 17, 0, (debug_count), 4, 0,MNORMAL); // |
//----------------------- |
// Anzeige Zeit / Datum |
//----------------------- |
yoffs = 5; |
writex_datetime_time( 1, 1, UTCTime, MNORMAL, 5,yoffs); // Zeit |
writex_datetime_date( 11, 1, UTCTime, MNORMAL, 0,yoffs); // Datum |
//----------------------- |
// gelesene Pakete: OSD und Zeit |
//----------------------- |
y = 4; |
yoffs = -5; |
lcdx_printp_at( 0, y+0, PSTR("OSD:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+0, ( readCounterOSD), 5, 0,MNORMAL, 0,yoffs); // |
lcdx_printp_at( 13, y+0, PSTR("Time:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 18, y+0, ( readCounterTIME), 3, 0,MNORMAL, 0,yoffs); // |
//---- |
lcdx_printp_at( 0, y+1, PSTR("Dis:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+1, ( readCounterDISPLAY), 5, 0,MNORMAL, 0,yoffs); // |
//----------------------- |
// gelesene Pakete: BL |
//----------------------- |
y = 6; |
lcd_frect( 0, y*8-1, 8, 17, 1); // Box |
lcdx_printp_at( 0, y+0, PSTR("B"), MINVERS, 2,0); |
lcdx_printp_at( 0, y+1, PSTR("L"), MINVERS, 2,0); |
write_ndigit_number_u ( 2, y+0, ( readCounterBL[0]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+0, ( readCounterBL[1]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+0, ( readCounterBL[2]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+0, ( readCounterBL[3]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 2, y+1, ( readCounterBL[4]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+1, ( readCounterBL[5]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+1, ( readCounterBL[6]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+1, ( readCounterBL[7]), 4, 0,MNORMAL); // |
} |
//############################################################## |
#endif // USE_OSD_SCREEN_DEBUG |
//############################################################## |
//----------------------------------------------------------- |
// ok = OSD_Popup_MKSetting() |
// |
// zeigt das aktuelle FC-Setting beim Start vom OSD an |
// |
// RUECKGABE: |
// true = Setting konnte gelesen werden |
// false = Fehler |
//----------------------------------------------------------- |
uint8_t OSD_Popup_MKSetting( void ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printp_center( 2, PSTR("PKT OSD") , MNORMAL, 0, 0); |
lcdx_printp_center( 6, PSTR("MK Setting"), MINVERS, 0,-8); |
MK_Setting_load( 0xff, 9 ); // 0xff == aktuelles Parameterset holen; 9 == timeout |
MKVersion_Setting_print( 7, MINVERS, 0,-4); // aus: mkbase.c |
if( MKVersion.mksetting == 0 ) |
set_beep( 500, 0xffff, BeepNormal ); // kein Setting - langer Beep ERROR |
clear_key_all(); |
timer = 300; // ca. 3 Sekunden zeigen; Abbruch mit einer Taste moeglich |
while( timer > 0 && !get_key_press(0xff) ); |
clear_key_all(); |
lcd_cls(); |
return (MKVersion.mksetting != 0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_MKError( u8 mkerrorcode ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printf_center_P( 6, MINVERS, 0,-8, PSTR("** MK-%S %02d **"), strGet(STR_ERROR), mkerrorcode ); // "MK-FEHLER" und Fehlernummer |
lcdx_printp_center( 7, (const char*) pgm_read_word(&mkerrortext[mkerrorcode]), MINVERS, 0,-4); // MK-Fehlertext |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_Info( uint8_t ScreenNum, const char *ScreenName) |
{ |
Popup_Draw( 5 ); // 5 Zeilen von unten nach oben fuer's Popup |
lcd_line( 3, 53-21, 124, 53-21, 0); // Linie: oben |
lcd_line( 3, 53, 124, 53, 0); // Linie: unten |
//----------------------- |
// ScreenNummer: ScreenName |
//----------------------- |
lcdx_printf_at_P( 0, 3, MINVERS, 5,-2, PSTR("%02d: %S"), ScreenNum, ScreenName ); |
//----------------------- |
// longpress Key's |
//----------------------- |
lcdx_printp_at( 0, 4, strGet(STR_LONGPRESS), MINVERS, 6,3); // "langer Tastendruck:" |
lcdx_printp_at(12, 5, PSTR("Disp"), MINVERS, 0,4); |
lcdx_printp_at(17, 5, PSTR("UGps"), MINVERS, 0,4); |
//----------------------- |
// shortpress Key's |
//----------------------- |
lcd_printp_at( 0, 7, strGet(KEYLINE3), MINVERS); |
lcd_printp_at(17, 7, PSTR("Info") , MINVERS); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_Pos_Save( pkt_gpspos_t *pGPS ) |
{ |
pGPS->Altimeter = naviData->Altimeter; // barymetrische Hoehe |
pGPS->HomeDistance = naviData->HomePositionDeviation.Distance; // Entfernung Home |
memcpy( &pGPS->GPSData, &naviData->CurrentPosition, sizeof(GPS_Pos_t) ); // sichern... |
memcpy( &pGPS->timestamp, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// GPS_User_Init() |
// |
// initialisiert die GPS Positionen neu |
//-------------------------------------------------------------- |
void GPS_User_Init( void ) |
{ |
memset( Config.GPS_User, 0, sizeof(pkt_gpspos_t)*MAX_GPS_USER ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_User_Save( void ) |
{ |
uint8_t i; |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) // nur wenn MK-GPS ok ist |
{ |
for( i=MAX_GPS_USER-1; i>0; i--) |
{ |
Config.GPS_User[i] = Config.GPS_User[i-1]; |
} |
GPS_Pos_Save( &Config.GPS_User[0] ); |
set_beep( 160, 0xffff, BeepNormal ); // Beep Ok |
} |
else |
{ |
set_beep( 600, 0x000f, BeepNormal ); // Beep Error (keine gueeltigen GPS-Daten) |
} |
} |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Init( void ) |
{ |
CellIsChecked = 0; |
cells = 0; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Check( void ) |
{ |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
} |
//############################################################## |
//# OSD MAIN LOOP |
//############################################################## |
//-------------------------------------------------------------- |
// OSD MAIN LOOP |
//-------------------------------------------------------------- |
void osd( void ) |
{ |
uint8_t osdexit = false; |
uint8_t mktimeout = false; |
uint8_t flying = false; |
uint8_t popup_state = OSD_POPUP_NONE; |
lcd_cls(); |
#ifdef DEBUG_OSD_TIME |
// Fake-Zeit/Datum setzen wenn der NC-Simulator verwendet wird |
if( UTCTime.year < 2000 ) |
{ |
UTCTime.seconds = ((uint32_t)13*3600)+(15*60)+42; // 13:15:42 |
UTCTime.day = 01; |
UTCTime.month = 05; |
UTCTime.year = 2013; |
} |
#endif |
//---------------------------------------- |
// Anzeige: aktuelles MK-Setting |
//---------------------------------------- |
if( (Config.OSD_ShowMKSetting) ) |
{ |
if( !OSD_Popup_MKSetting() ) |
return; |
} |
/* |
//----------------------------------------------------------------------------------------------- |
// 07.03.2013 OG: del |
// Dieser Teil hat immer wieder Probleme bereitet bei der Verbindung des PKT-OSD zum MK da |
// MK_TIMEOUTs zustande kamen. Eine Recherche im Code ergab, dass die Nutzdaten die |
// hierueber bezogen werden sich lediglich auf Flags_ExtraConfig beschraenkten (CFG2_HEIGHT_LIMIT). |
// Siehe dazu untere Kommentare. |
// |
// Der negative Effekt moeglicher MK_TIMEOUTs und Verzoegerungen sind es aktuell nicht Wert |
// CFG2_HEIGHT_LIMIT zu unterstuetzen. Dieses Feature ist erstmal raus. |
// |
// Falls gewuenscht wird, dass CFG2_HEIGHT_LIMIT wieder in das PKT-OSD kommt muss |
// es zuverlaessig an anderer Stelle implementiert werden - und zwar nicht in osd.c |
// weil es eine statische FC-Information ist (ggf. beim Verbindungsaufbau PKT <-> MK). |
// |
// Hat auch aktuell Auswirkung auf den Code OSD_Element_AltitudeControl() |
//----------------------------------------------------------------------------------------------- |
//lcd_printp_at( 0, 3, PSTR("connecting MK..."), 0); |
// |
//SwitchToFC(); |
// |
//status = load_setting(0xff); |
// |
//if( status == 255 ) |
//{ |
// lcd_printp_at(0, 0, PSTR("Keine Settings !!"), 0); // Keine Settings |
// _delay_ms(2000); |
//} |
//Flags_ExtraConfig = mk_param_struct->ExtraConfig; // OG: wird in osd.c nur verwendet von: OSD_Element_AltitudeControl() |
//Flags_GlobalConfig = mk_param_struct->GlobalConfig; // OG: wird nicht in osd.c verwendet |
//Flags_GlobalConfig3 = mk_param_struct->GlobalConfig3; // OG: wird nicht in osd.c verwendet |
*/ |
//------------------------- |
// MK-Display initialisieren |
//------------------------- |
memset( mkdisplayData, 0, 81 ); |
mkdisplayMode = false; |
mkdisplayCmd = 0xff; |
//------------------------- |
// BL-DATA initialisieren |
//------------------------- |
memset( blData, 0, sizeof(BLData_t)*OSD_MAX_MOTORS ); |
//------------------------- |
// Debug initialisieren |
//------------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD = 0; |
readCounterTIME = 0; |
readCounterDISPLAY = 0; |
memset( readCounterBL, 0, sizeof(uint16_t)*OSD_MAX_MOTORS ); |
#endif // USE_OSD_SCREEN_DEBUG |
//------------------------- |
// NC Datenkommunikation starten |
//------------------------- |
OSD_MK_Connect( MK_CONNECT ); |
OSD_active = true; // benoetigt fuer Navidata Ausgabe an SV2 |
//------------------------- |
// Clear statistics |
//------------------------- |
//STAT_Init(); |
CellIsChecked = 0; |
cells = 0; |
AkkuWarnThreshold = 0; |
OldWP = 0; |
NextWP = false; |
old_PKTErrorcode = 0; |
old_MKErrorcode = 0; |
//------------------------- |
// Init: OSD-Screens |
//------------------------- |
ScreenCtrl_Init(); |
ScreenCtrl_Push( OSDSCREEN_GENERAL , strGet(STR_OSDSCREEN_GENERAL) , &OSD_Screen_General ); |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScreenCtrl_Push( OSDSCREEN_NAVIGATION , strGet(STR_OSDSCREEN_NAVIGATION), &OSD_Screen_Navigation ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints ); |
#endif |
// *ALTERNATIVE* |
//#ifdef USE_OSD_SCREEN_WAYPOINTS |
//ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints0 ); |
//#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScreenCtrl_Push( OSDSCREEN_ELECTRIC , strGet(STR_OSDSCREEN_ELECTRIC) , &OSD_Screen_Electric ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScreenCtrl_Push( OSDSCREEN_MKSTATUS , strGet(STR_OSDSCREEN_MKSTATUS) , &OSD_Screen_MKStatus ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScreenCtrl_Push( OSDSCREEN_USERGPS , strGet(STR_OSDSCREEN_USERGPS) , &OSD_Screen_UserGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScreenCtrl_Push( OSDSCREEN_3DLAGE , strGet(STR_OSDSCREEN_3DLAGE) , &OSD_Screen_3DLage ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScreenCtrl_Push( OSDSCREEN_STATISTICS , strGet(STR_OSDSCREEN_STATISTIC) , &OSD_Screen_Statistics ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScreenCtrl_Push( OSDSCREEN_OSD0 , strGet(STR_OSDSCREEN_OSD0) , &OSD_Screen_OSD0 ); |
ScreenCtrl_Push( OSDSCREEN_OSD1 , strGet(STR_OSDSCREEN_OSD1) , &OSD_Screen_OSD1 ); |
ScreenCtrl_Push( OSDSCREEN_OSD2 , strGet(STR_OSDSCREEN_OSD2) , &OSD_Screen_OSD2 ); |
#endif |
#ifdef USE_OSD_SCREEN_DEBUG |
ScreenCtrl_Push( 0 , PSTR("Debug") , &OSD_Screen_Debug ); |
ScreenCtrl_Push( 0 , PSTR("Debug-RX") , &OSD_Screen_Debug_RX ); |
#endif |
ScreenCtrl_Set( Config.OSD_ScreenMode ); |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
timer_mk_timeout = MK_TIMEOUT; |
timer_osd_refresh = TIME_OSD_REFRESH; |
timer_get_bldata = 0; |
timer_get_tidata = 0; |
timer_get_displaydata = 0; |
//-------------------------------- |
// OSD Main-Loop |
//-------------------------------- |
while( !osdexit ) |
{ |
//################################ |
//# Empfange/verarbeite: OSD-Daten |
//################################ |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
Decode64(); |
memcpy( &osdData, (const void *)pRxData, sizeof(NaviData_t) ); // sichern in: osdData |
naviData = &osdData; |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD++; // gelesene Datenpakete |
#endif |
mktimeout = false; |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
//---------------------------------- |
// Winkel zu Home |
//---------------------------------- |
calc_heading_home(); |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
GPS_Pos_Save( &GPS_Current ); |
Config.LastLatitude = GPS_Current.GPSData.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = GPS_Current.GPSData.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// PKT Fehler reset |
//---------------------------------- |
if( old_PKTErrorcode == 40 ) // 40 = PKT Empfangsausfall ("PKT RX lost") |
{ |
// PKT-Verbindungsfehler zuruecksetzen |
// da an dieser Stelle ja bereits wieder ein gueltiges Datenpaket |
// von der NaviCtrl empfangen wurde |
old_PKTErrorcode = 0; |
clear_key_all(); |
} |
//---------------------------------- |
// remember statistics (only when engines running) |
//---------------------------------- |
#ifdef DEBUG_OSD_STAT_MOTORRUN |
if( true ) |
#else |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) // AM FLIEGEN -> Statistik aufzeichnen |
#endif |
{ |
flying = true; |
// --- gueltige Zeit von der NC vorhanden und noch keine Start-Zeit gesetzt? |
if( UTCTime.year != 0 && Config.OSD_Statistic.begin_StatTime.year == 0 ) |
{ |
memcpy( &Config.OSD_Statistic.begin_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Start Zeit/Datum sichern... |
} |
Config.OSD_Statistic.last_FlyTime = naviData->FlyingTime; |
Config.OSD_Statistic.count_osd++; // Anzahl OSD-Statistik Pakete |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.avg_Current = (uint16_t)calc_avg( (int32_t)Config.OSD_Statistic.avg_Current, |
(int32_t)naviData->Current, |
(int32_t)Config.OSD_Statistic.count_osd, |
(int32_t)100 ); |
if( naviData->Altimeter > Config.OSD_Statistic.max_Altimeter ) Config.OSD_Statistic.max_Altimeter = naviData->Altimeter; |
if( naviData->GroundSpeed > Config.OSD_Statistic.max_GroundSpeed ) Config.OSD_Statistic.max_GroundSpeed = naviData->GroundSpeed; |
if( naviData->HomePositionDeviation.Distance > Config.OSD_Statistic.max_Distance ) Config.OSD_Statistic.max_Distance = naviData->HomePositionDeviation.Distance; |
if( naviData->Current > Config.OSD_Statistic.max_Current ) Config.OSD_Statistic.max_Current = naviData->Current; |
if( naviData->UsedCapacity > Config.OSD_Statistic.max_Capacity ) Config.OSD_Statistic.max_Capacity = naviData->UsedCapacity; |
if( naviData->UBat < Config.OSD_Statistic.min_UBat ) Config.OSD_Statistic.min_UBat = naviData->UBat; |
if( naviData->TopSpeed > Config.OSD_Statistic.max_TopSpeed ) Config.OSD_Statistic.max_TopSpeed = naviData->TopSpeed; |
if( naviData->RC_Quality > Config.OSD_Statistic.max_RCQuality ) Config.OSD_Statistic.max_RCQuality = naviData->RC_Quality; |
if( naviData->RC_Quality < Config.OSD_Statistic.min_RCQuality ) Config.OSD_Statistic.min_RCQuality = naviData->RC_Quality; |
if( naviData->AngleNick < Config.OSD_Statistic.min_AngleNick ) Config.OSD_Statistic.min_AngleNick = naviData->AngleNick; |
if( naviData->AngleNick > Config.OSD_Statistic.max_AngleNick ) Config.OSD_Statistic.max_AngleNick = naviData->AngleNick; |
if( naviData->AngleRoll < Config.OSD_Statistic.min_AngleRoll ) Config.OSD_Statistic.min_AngleRoll = naviData->AngleRoll; |
if( naviData->AngleRoll > Config.OSD_Statistic.max_AngleRoll ) Config.OSD_Statistic.max_AngleRoll = naviData->AngleRoll; |
} |
else if( flying && UTCTime.year != 0 ) // GELANDET -> Statistik beenden |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
flying = false; |
} |
//----------------------- |
// Check: Akku Warnung |
//----------------------- |
CheckMKLipo(); |
//---------------------------------- |
// Show: OSD-Screens |
//---------------------------------- |
if( popup_state == OSD_POPUP_NONE && (timer_osd_refresh == 0 || OSDScreenRefresh == OSD_SCREEN_REDRAW) ) |
{ |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) lcd_cls(); |
if( !mkdisplayMode ) |
ScreenCtrl_Show(); |
else |
OSD_Screen_MKDisplay(); |
timer_osd_refresh = TIME_OSD_REFRESH; |
} |
OSDScreenRefresh = OSD_SCREEN_REFRESH; |
//---------------------------------- |
// Check: MK-Error |
//---------------------------------- |
if( naviData->Errorcode != old_MKErrorcode && naviData->Errorcode <= MAX_MKERROR_NUM ) |
{ |
if( naviData->Errorcode > 0 ) // raise Error |
{ |
// Fehler aufzeichnen |
MkError_Save( naviData->Errorcode ); |
// Fehler Anzeigen |
OSD_Popup_MKError( naviData->Errorcode ); |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error (MK-Error vorhanden) |
timer2 = TIME_POPUP_MKERROR; |
popup_state = OSD_POPUP_MKERROR; |
} |
else // reset Error |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
old_MKErrorcode = naviData->Errorcode; |
} |
rxd_buffer_locked = FALSE; // ready to receive new data |
//------------------------------------------- |
// hole weitere Daten vom MK (BL, Time, ...) |
// |
// muss am Schluss stehen da naviData-Buffer |
// dabei ueberschrieben wird! |
//------------------------------------------- |
OSD_MK_GetData(); // holt BL-Daten und NC-Zeit |
timer_mk_timeout = MK_TIMEOUT; |
} //end: if( rxd_buffer_locked ) // OSD-Daten |
//################################ |
//# der Rest... |
//################################ |
//-------------------------------- |
// TASTEN: MK-Timeout |
//-------------------------------- |
if( mktimeout ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
} |
//-------------------------------- |
// Popup beenden |
// wenn irgendeine Taste gedrückt oder Popup-Timeout |
//-------------------------------- |
if( !mktimeout && (popup_state != OSD_POPUP_NONE) && (get_key_press(255) || !timer2) ) // get_key_press(255) == alles an Tasten abfangen was moeglich ist |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
clear_key_all(); |
} |
//-------------------------------- |
// TASTEN: KEIN mkdisplay (OSD Modus) |
//-------------------------------- |
if( !osdexit && !mktimeout && !mkdisplayMode ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
if( get_key_long(1 << KEY_ESC) ) // ÊINSCHALTEN: mkdisplayMode |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = true; |
mkdisplayCmd = 0; // lesen display-Daten erzwingen |
} |
if( get_key_long (1 << KEY_ENTER) ) // User GPS-Position sichern |
{ |
GPS_User_Save(); |
} |
if( get_key_short (1 << KEY_ENTER) ) // Popup: Info |
{ |
if( popup_state == OSD_POPUP_NONE ) |
{ |
popup_state = OSD_POPUP_INFO; |
OSD_Popup_Info( ScreenCtrl_GetNum(), ScreenCtrl_GetName() ); |
timer2 = TIME_POPUP_INFO; |
} |
} |
if( get_key_press (1 << KEY_MINUS)) // previous screen |
{ |
ScreenCtrl_Previous(); |
} |
if( get_key_press (1 << KEY_PLUS)) // next Screen |
{ |
ScreenCtrl_Next(); |
} |
} |
//-------------------------------- |
// TASTEN: mkdisplay AKTIV |
//-------------------------------- |
if( !osdexit && !mktimeout && mkdisplayMode ) |
{ |
/* |
if( get_key_long(1 << KEY_ENTER) // ABSCHALTEN mkdisplayMode: longpress ENTER, ESC, MINUS, PLUS schaltet mkdisplay aus |
|| get_key_long(1 << KEY_ESC) |
|| get_key_long(1 << KEY_MINUS) |
|| get_key_long(1 << KEY_PLUS) ) |
{ |
*/ |
if( get_key_long(1 << KEY_ESC) ) // ABSCHALTEN mkdisplayMode: longpress ESC (Taste 3 von links) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep beim Modus-Wechsel |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = false; |
clear_key_all(); |
} |
if( get_key_short (1 << KEY_MINUS) ) mkdisplayCmd = 0xfe; // MK-Key: rechts (next page) |
if( get_key_short (1 << KEY_PLUS) ) mkdisplayCmd = 0xfd; // MK-Key: links (previous page) |
if( get_key_short (1 << KEY_ESC) ) mkdisplayCmd = 0xfb; // MK-Key: runter |
if( get_key_short (1 << KEY_ENTER) ) mkdisplayCmd = 0xf7; // MK-Key: hoch |
if( mkdisplayCmd != 0xff ) // wenn eine MK-Display Taste gedrueckt worde sofort Daten |
{ // holen und darstellen um Anzeigereaktion fuer Benutzer zu verbessern |
timer_get_displaydata = 0; // lesen erzwingen |
OSD_MK_GetData(); // Daten holen |
OSD_Screen_MKDisplay(); // MK-Display Anzeigen |
} |
} |
//----------------------- |
// abo_timer |
//----------------------- |
if( abo_timer == 0 ) |
{ |
OSD_MK_Connect( MK_ABORENEW ); |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
OSD_MK_Connect( MK_CONNECT ); // 21.06.2014 OG: wieder aktviert wegen Umschaltung auf NC |
} |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
#ifdef USE_OSD_PKTHOOK |
if( PKT_CtrlHook() ) OSDScreenRefresh = OSD_SCREEN_REDRAW; // Update vom Updatetool angefordert? |
#endif |
} // END: while( !osdexit ) |
//------------------------------------------ |
// PKT-OSD beenden |
//------------------------------------------ |
Config.OSD_ScreenMode = ScreenCtrl_GetNum(); // merken letzter Screen |
//------------------------------------------ |
// ggf. Statistik Ende Zeit/Datum sichern |
//------------------------------------------ |
if( flying && UTCTime.year != 0 ) // wenn noch am fliegen |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
} |
OSD_active = false; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osd.h |
---|
0,0 → 1,309 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osd.h |
//# |
//# 21.06.2014 OG |
//# - add: writex_altimeter() |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: weitere Exporte von Funktionen draw_icon...() |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - add: OSD_Element_Battery_Bar() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# |
//# 26.05.2014 OG |
//# - add: #define OSDSCREEN_WAYPOINTS |
//# |
//# 24.05.2014 OG |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 28.04.2014 OG |
//# - del: OSD_Timeout() |
//# |
//# 07.07.2013 OG |
//# - add: defines fuer Screen-ID's (verwendet in setup.c, osd.c) |
//# |
//# 30.06.2013 OG |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# |
//# 31.05.2013 OG |
//# Achtung! Aenderung eeprom-Kompatibilitaet wegen typedef Erweiterung! |
//# - chg: typedef: osd_statistic_BL_t fuer Mittelwerte |
//# - chg: typedef: osd_statistic_t fuer Mittelwerte |
//# |
//# 15.05.2013 OG |
//# - add: calc_BLmax() |
//# - add: struct osd_BLmax_t |
//# |
//# 04.05.2013 OG |
//# - chg: aktualisiert Kommentare in 'osd_statistic_t' |
//# - del: OSD_Debug_XX() |
//# |
//# 02.05.2013 OG |
//# - fix: struct osd_statistic_t: max_Distance von int16_t nach uint16_t |
//# |
//# 28.04.2013 OG |
//# - chg: osd(OSDMode) -> osd(void) |
//# - del: OSDDATA_Statistics() |
//# |
//# vorheriges: siehe osd.c |
//############################################################################ |
#ifndef _OSD_H |
#define _OSD_H |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#define OSD_MAX_MOTORS 8 // max. Anzahl vom PKT unterstuetzer Motoren (absolut MAX ist 12 da der MK nicht mehr unterstuetzt) |
#define MAX_GPS_USER 10 // max. Anzahl der GPS-Positionen durch Benutzer gespeichert |
#define MAX_MKERR_LOG 20 // max. Anzahl Eintraege im MK-Error-Log |
#define OSD_SCREEN_REFRESH 0 // Screen: Werte anzeigen |
#define OSD_SCREEN_REDRAW 1 // Screen: Labels und statischer Elemente neu zeichen, Werte anzeigen |
#define ORIENTATION_V 1 // fuer OSD_Element_Battery_Bar() |
#define ORIENTATION_H 2 |
// OSD-Screen ID's |
// maximal: 31 (!) wegen Bitcodierung in Config.OSD_UseScreen |
#define OSDSCREEN_GENERAL 0 |
#define OSDSCREEN_NAVIGATION 1 |
#define OSDSCREEN_ELECTRIC 2 |
#define OSDSCREEN_MKSTATUS 3 |
#define OSDSCREEN_USERGPS 4 |
#define OSDSCREEN_3DLAGE 5 |
#define OSDSCREEN_STATISTICS 6 |
#define OSDSCREEN_OSD0 7 |
#define OSDSCREEN_OSD1 8 |
#define OSDSCREEN_OSD2 9 |
#define OSDSCREEN_WAYPOINTS 10 |
// Flags |
#define OSD_FLAG_AH 0 // Altitue Hold |
#define OSD_FLAG_PH 1 // Position Hold |
#define OSD_FLAG_CF 2 // Care Free |
#define OSD_FLAG_CH 3 // Coming Home |
#define OSD_FLAG_O1 4 // Out1 (LED 1) |
#define OSD_FLAG_O2 5 // Out2 (LED 2) |
#define OSD_FLAG_BA 6 // LowBat warning (MK) |
#define OSD_FLAG_CA 7 // Calibrate |
#define OSD_FLAG_ST 8 // Start |
#define OSD_FLAG_MR 9 // Motor Run |
#define OSD_FLAG_FY 10 // Fly |
#define OSD_FLAG_EL 11 // Emergency Landing |
#define OSD_FLAG_FS 12 // RS Failsave Active |
#define OSD_FLAG_GP 13 // GPS ok |
#define OSD_FLAG_S0 14 // GPS-Sat not ok (GPS NOT ok) |
#define OSD_FLAG_TU 15 // Vario Trim Up |
#define OSD_FLAG_TD 16 // Vario Trim Down |
#define OSD_FLAG_FR 17 // Free |
#define OSD_FLAG_RL 18 // Range Limit |
#define OSD_FLAG_SL 19 // No Serial Link |
#define OSD_FLAG_TR 20 // Target Reached |
#define OSD_FLAG_MC 21 // Manual Control |
#define OSD_FLAG_COUNT 22 |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
//----------------------------------------------------------- |
// typedef: Statistiken |
//----------------------------------------------------------- |
typedef struct |
{ |
uint16_t count; // Anzahl Werte BL-Daten (fuer Mittelwertberechnung) |
uint8_t max_Current; // in 0.1 A steps |
uint16_t avg_Current; // Mittelwert Current (*100 fuer Rechengenauigkeit) |
uint8_t max_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_statistic_BL_t; |
typedef struct |
{ |
PKTdatetime_t begin_StatTime; // Datum/Zeit |
PKTdatetime_t end_StatTime; // Datum/Zeit |
uint16_t total_FlyTime; // gesamt Flugzeit seit Stat-Init |
uint16_t last_FlyTime; // letzte Flugzeit |
uint16_t count_osd; // TODO: Anzahl Werte OSD-Daten (fuer Mittelwertberechnung) |
uint16_t count_Errorcode; // TODO: Anzahl gemeldeter MK-Errors |
int16_t max_Altimeter; // max. Hoehe |
int16_t avg_Altimeter; // TODO: Mittelwert Hoehe () |
s16 max_Variometer; // TODO: ... |
uint16_t max_GroundSpeed; // max. Geschwindigkeit |
uint16_t avg_GroundSpeed; // TODO: Mittelwert Geschwindigkeit () |
s16 max_TopSpeed; // max. velocity in vertical direction in cm/s |
uint16_t max_Distance; // max. Entfernung |
uint16_t avg_Distance; // TODO: Mittelwert Entfernung () |
uint16_t max_Current; // max. Strom |
uint16_t avg_Current; // Mittelwert Strom () |
u8 max_RCQuality; // max. Empfangsqualitaet |
u8 min_RCQuality; // min. Empfangsqualitaet |
uint16_t avg_RCQuality; // TODO: Mittelwert Empfangsqualitaet () |
uint16_t max_Capacity; // max. entnommene Kapazitaet |
s8 max_AngleNick; // max. Nick |
s8 min_AngleNick; // min. Nick |
s8 max_AngleRoll; // max. Roll |
s8 min_AngleRoll; // min. Roll |
uint8_t min_UBat; // min. Spannung (V) |
uint8_t LiPoCells; // Anzahl der LiPo Zellen |
uint8_t BL_Count; // Anzahl erkannter BL's (Motoren) |
osd_statistic_BL_t BL[OSD_MAX_MOTORS]; // Werte der einzelnen BL's |
} osd_statistic_t; |
typedef struct |
{ |
uint8_t max_BL_Current_Index; // BL-Nummer |
unsigned char max_BL_Current; // in 0.1 A steps |
uint8_t max_BL_Temp_Index; // BL-Nummer |
unsigned char max_BL_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_BLmax_t; |
//----------------------------------------------------------- |
// typedef: Aufzeichnung von MK-Errors |
//----------------------------------------------------------- |
typedef struct |
{ |
u8 Errorcode; // 0 --> okay |
PKTdatetime_t set_Time; // Datum/Zeit |
PKTdatetime_t clear_Time; // Datum/Zeit |
} mkerror_t; |
//----------------------------------------------------------- |
// typedef: PKT GPS-Positionen |
//----------------------------------------------------------- |
typedef struct |
{ |
PKTdatetime_t timestamp; // Zeitstempel: UTC |
s16 Altimeter; // barymetrische Hoehe (entspricht: naviData->Altimeter) |
u16 HomeDistance; // distance to home in cm (entspricht: naviData->HomePositionDeviation.Distance) |
GPS_Pos_t GPSData; // GPS-Position (mk-data-structs.h) |
} pkt_gpspos_t; |
//----------------------------------------------------------- |
// global var's |
//----------------------------------------------------------- |
extern volatile uint8_t OSD_active; |
extern volatile uint8_t error; |
//----------------------------------------------------------- |
// strings |
//----------------------------------------------------------- |
extern const char * const mkerrortext[]; |
//----------------------------------------------------------- |
// Funktionen |
//----------------------------------------------------------- |
void osd( void ); |
void vario_beep_output (void); |
void CheckMKLipo(void); |
void STAT_Init(void); |
void GPS_User_Init(void); |
void MKErr_Log_Init(void); |
void calc_BLmax( osd_BLmax_t *blmax ); |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ); |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Capacity( uint8_t x, uint8_t y ); |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ); |
void OSD_Element_Current( uint8_t x, uint8_t y ); |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ); |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ); |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ); |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ); |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ); |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ); |
void OSD_Element_Manuell( uint8_t x, uint8_t y ); |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ); |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Variometer( uint8_t x, uint8_t y ); |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ); |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ); |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ); |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation); |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void MKLiPoCells_Init( void ); |
void MKLiPoCells_Check( void ); |
void draw_icon_satmini( uint8_t x, uint8_t y); |
void draw_icon_satmini2( uint8_t x, uint8_t y); |
void draw_icon_home( uint8_t x, uint8_t y); |
void draw_icon_target_diamond( uint8_t x, uint8_t y); |
void draw_icon_target_round( uint8_t x, uint8_t y); |
void draw_icon_mk( uint8_t x, uint8_t y); |
void OSD_MK_ShowTimeout( void ); |
//----------------------------------------------------------- |
// EXPORTS NUR FUER osdold_screens.c |
//----------------------------------------------------------- |
extern NaviData_t *naviData; |
extern uint8_t OSDScreenRefresh; |
#endif // _OSD_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osddata.c |
---|
0,0 → 1,838 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.c |
//# |
//# 14.06.2014 OG |
//# - chg: Menu_OSDData() - Bezeichnung von "OSD Daten" und "BL Daten" geaendert |
//# zu "OSD Statistik" und "BL Statistik" |
//# |
//# 11.05.2014 OG |
//# - chg: Text von SHOWMKERROR_de, SHOWMKERROR_en etwas gekuerzt |
//# - chg: Titel von Scrollbox-Listen angepasst |
//# - chg: Menu_OSDData() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: Menu_OSDData() del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - chg: ask_delete_data() - Layout und Optimierung |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# - del: OSDDATA_ShowLastGPS() |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 24.01.2014 OG |
//# - fix: OSDDATA_ClearAllData(): Beep wenn Daten geloescht werden |
//# (wie bei den anderen Löschungen) |
//# |
//# 30.06.2013 OG |
//# - add: Gesamtstrom-Mittelwert in OSDDATA_Statistics() und OSDDATA_BLStatistics() |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - chg: Funktionsnamen geaendert |
//# |
//# 19.06.2013 OG |
//# - add: last Flytime in OSDDATA_Statistics() |
//# - chg: Reihenfolge in OSDDATA_Statistics() und zusaetzliche Trennline |
//# - add: max Current (Gesamt) in OSDDATA_Statistics() |
//# |
//# 16.06.2013 OG |
//# - fix: "min. Voltage" Anzeige in OSDDATA_Statistics() (war veschoben) |
//# |
//# 13.06.2013 OG |
//# - chg: Uebersetzungen von CB in ask_delete_data() aufgenommen |
//# - chg: GPS-Menuetitel geaendert |
//# - fix: include pkt.h |
//# |
//# 11.06.2013 OG |
//# - chg: ask_delete_data() erweitert um Info was geloescht wird & Layout (TODO: Uebersetzungen) |
//# - add: Mittelwertanzeige fuer BL-Strom |
//# - add: Anzahl BL-Statistik-Datenpakete (nur wenn USE_OSD_SCREEN_DEBUG) |
//# - add: OSDDATA_BLStatistic() - die BL-Daten wurden in eigene Anzeigefunktion verschoben |
//# - add: OSDDATA_ClearAllData() - alle Daten loeschen |
//# - add: OSDDATA_ShowLastGPS() - letzte GPS-Position anzeigen |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - chg: PKT_CtrlHook() noch an einigen Stellen eingefuegt |
//# - fix: ctrl_osddata() clear screen vor ScrollBox_Refresh |
//# |
//# 15.05.2013 OG |
//# - chg: OSDDATA_Statistics() umgestellt auf calc_BLmax() (aus osd.c) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - add: weitere Anzeigen in OSD_Statistics() |
//# - add: Content in OSDDATA_UserGPS() und OSDDATA_MkError() |
//# |
//# 28.04.2013 OG |
//# - chg: auf die neuen Features von xprintf angepasst (siehe utils/xstring.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: Texte "Datenlöschen" in messages.c aufgenommen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../osd/osd.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../mk/mkbase.h" |
#define INIT_STATISTIC 1 |
#define INIT_MKERROR 2 |
#define INIT_USERGPS 3 |
#define INIT_LASTPOS 4 |
#define INIT_ALLDATA 5 |
//----------------------- |
// Menu_OSDDATA |
//----------------------- |
#define ID_SHOWSTATISTIK 1 |
#define ID_SHOWBLSTATISTIK 2 |
#define ID_SHOWMKERROR 3 |
#define ID_SHOWUSERGPS 4 |
#define ID_SHOWLASTGPS 5 |
#define ID_CLEARALLDATA 6 |
//static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Daten"; |
//static const char SHOWSTATISTIK_en[] PROGMEM = "OSD data"; |
//static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Daten"; |
//static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL data"; |
static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Statistik"; |
static const char SHOWSTATISTIK_en[] PROGMEM = "OSD statistics"; |
static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Statistik"; |
static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL statistics"; |
static const char SHOWMKERROR_de[] PROGMEM = "MK Fehler"; |
static const char SHOWMKERROR_en[] PROGMEM = "MK errors"; |
static const char SHOWUSERGPS_de[] PROGMEM = "GPS Userdaten"; |
static const char SHOWUSERGPS_en[] PROGMEM = "GPS userdata"; |
static const char SHOWLASTGPS_de[] PROGMEM = "GPS letzte Pos."; |
static const char SHOWLASTGPS_en[] PROGMEM = "GPS last pos."; |
static const char CLEARALLDATA_de[] PROGMEM = "LÖSCHE alle Daten"; |
static const char CLEARALLDATA_en[] PROGMEM = "DELETE all data"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t ask_delete_data( uint8_t initcode ) |
{ |
uint8_t refresh = true; |
clear_key_all(); |
set_beep ( 60, 0xffff, BeepNormal); |
while( true ) |
{ |
//-------------------------------------- |
// screen refresh |
//-------------------------------------- |
if( refresh ) |
{ |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill Black |
lcdx_printp_at( 1, 0, strGet(DELETE), MINVERS, 0,0); // Titel |
show_Lipo(); // LiPo anzeigen |
lcd_rect_round( 0, 20, 127, 27, 1, R2); // Rahmen fuer Benutzerfrage |
switch( initcode ) |
{ |
case INIT_STATISTIC: lcdx_printp_center( 2, strGet(STAT_OSDBL), MNORMAL, 0,9); break; |
case INIT_MKERROR : lcdx_printp_center( 2, strGet(STAT_ERROR), MNORMAL, 0,9); break; |
case INIT_USERGPS : lcdx_printp_center( 2, strGet(STAT_GPS) , MNORMAL, 0,9); break; |
case INIT_LASTPOS : lcdx_printp_center( 2, strGet(STAT_POS) , MNORMAL, 0,9); break; |
case INIT_ALLDATA : lcdx_printp_center( 2, strGet(STAT_ALL) , MNORMAL, 0,9); break; |
} |
lcdx_printp_center( 4, strGet(DELETE), MNORMAL, 0,4); // "löschen?" |
lcd_printp_at( 0, 7, strGet(START_LASTPOS2) , MNORMAL); // Keyline: "löschen Nein" |
refresh = false; |
} |
//-------------------------------------- |
// Update vom Updatetool angefordert? |
//-------------------------------------- |
if( PKT_CtrlHook() ) |
{ |
refresh = true; |
} |
//-------------------------------------- |
// loeschen |
//-------------------------------------- |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
clear_key_all(); |
return true; // Rueckgabe: true = loeschen |
} |
//-------------------------------------- |
// Ende |
//-------------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return false; // Rueckgabe: false = nicht loeschen |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ctrl_osddata( uint8_t isdata, uint8_t initcode ) |
{ |
uint8_t key = 0; |
if( !isdata ) ScrollBox_Push_P( MNORMAL, PSTR(" no data...") ); |
ScrollBox_SetKey( KEY_ENTER, " Del" ); |
do |
{ |
key = ScrollBox_Show(); |
if( key == KEY_ENTER ) |
{ |
if( ask_delete_data(initcode) ) |
{ |
switch( initcode ) |
{ |
case INIT_STATISTIC: STAT_Init(); break; |
case INIT_MKERROR: MKErr_Log_Init(); break; |
case INIT_USERGPS: GPS_User_Init(); break; |
} |
set_beep ( 400, 0xffff, BeepNormal); |
key = 0; // exit |
} |
else |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
} |
} while( key ); |
} |
//-------------------------------------------------------------- |
// add_statistic_head() |
// |
// fuegt Basisdaten den OSD- und BL-Listen hinzu |
//-------------------------------------------------------------- |
void add_statistic_head( void ) |
{ |
PKTdatetime_t dtlocal; |
//--------------------------- |
// begin: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.begin_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("Begin: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
//lcd_printp_at( 3, 3, strGet(DELETEDATA), MNORMAL); // "Daten loeschen?" |
dtlocal.day, dtlocal.month |
); |
//--------------------------- |
// end: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.end_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("End: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
dtlocal.day, dtlocal.month |
); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete OSD-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_Statistics( void ) |
{ |
int16_t v; |
osd_BLmax_t blmax; |
lcd_cls(); |
if( !ScrollBox_Create(25) ) |
return; // kein RAM mehr |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" OSD Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// last Flytime |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Last Fly %02u:%02u min"), (Config.OSD_Statistic.last_FlyTime/60), (Config.OSD_Statistic.last_FlyTime%60) ); |
//--------------------------- |
// max. Altitude |
//--------------------------- |
v = Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust); |
ScrollBox_Push_P( MNORMAL, PSTR("%cAltitude%7d m"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Distance |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cDistance%7u m"), SYMBOL_MAX, Config.OSD_Statistic.max_Distance/10 ); |
//--------------------------- |
// max. Ground-Speed |
//--------------------------- |
v = (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 90) / (uint32_t) 250); |
ScrollBox_Push_P( MNORMAL, PSTR("%cGrnd.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Vert.-Speed |
//--------------------------- |
v = Config.OSD_Statistic.max_TopSpeed; |
v = (v*36/100); // cm/s -> km/h*10 |
ScrollBox_Push_P( MNORMAL, PSTR("%cVert.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// Used Capacity |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Capacity%7d mAh"), Config.OSD_Statistic.max_Capacity ); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR(" BL Detected%4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Curr.%4.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC"), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//--------------------------- |
// min. Voltage |
//--------------------------- |
v = (Config.OSD_Statistic.min_UBat == 255) ? 0 : Config.OSD_Statistic.min_UBat; |
ScrollBox_Push_P( MNORMAL, PSTR("%cVoltage%6.1d V"), SYMBOL_MIN, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// max. RC-Quality |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MAX, Config.OSD_Statistic.max_RCQuality ); |
//--------------------------- |
// min. RC-Quality |
//--------------------------- |
v = (Config.OSD_Statistic.min_RCQuality==255) ? 0 : Config.OSD_Statistic.min_RCQuality; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MIN, v ); |
//--------------------------- |
// max. Nick |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleNick, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Nick |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleNick==126) ? 0 : Config.OSD_Statistic.min_AngleNick; |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// max. Roll |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleRoll, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Roll |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleRoll==126) ? 0 : Config.OSD_Statistic.min_AngleRoll; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete BL-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_StatisticsBL( void ) |
{ |
uint8_t i; |
osd_BLmax_t blmax; |
lcd_cls(); |
#ifdef USE_OSD_SCREEN_DEBUG |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*6)) ) |
return; // kein RAM mehr |
#else |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*4)) ) |
return; // kein RAM mehr |
#endif |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR("BL Detected %4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Current%2.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC" ), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// LIST: BL-Data |
//--------------------------- |
if( Config.OSD_Statistic.BL_Count > 0 ) |
{ |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
for( i=0; i < Config.OSD_Statistic.BL_Count; i++) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("%1d:%cCurrent%4.1u A"), (i+1), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Current) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cCurrent%4.1u A" ), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.BL[i].avg_Current/100) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cTemp.%8u %cC" ), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Temp), SYMBOL_BIGDEGREE ); |
#ifdef USE_OSD_SCREEN_DEBUG |
ScrollBox_Push_P( MNORMAL, PSTR(" RX-Pkg.%7u"), Config.OSD_Statistic.BL[i].count ); |
//ScrollBox_Push_P( MNORMAL, PSTR(" MC%12u A" ), Config.OSD_Statistic.BL[i].avg_Current ); |
#endif |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
} |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete Benutzer-GPS-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_UserGPS( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_GPS_USER*4) + 5 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" GPS Userdata") ); |
for( i=0; i<MAX_GPS_USER; i++) |
{ |
if( Config.GPS_User[i].GPSData.Latitude != 0) // nur gueltige GPS-Koordinaten anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Nummer / Datum / Zeit |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.GPS_User[i].timestamp ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
dtlocal.day, dtlocal.month, dtlocal.year, |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60) |
); |
/* |
sec = (uint16_t)(Config.GPS_User[i].timestamp.seconds); // Sekunden |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
Config.MKErr_Log[i].set_Time.day, Config.MKErr_Log[i].set_Time.month, Config.MKErr_Log[i].set_Time.year, |
(uint16_t)(sec/3600), (uint16_t)(sec/60%60), (uint16_t)(sec%60) |
); |
// Alternative Anzeige von Datum/Zeit: |
min = (uint16_t)(Config.GPS_User[i].timestamp.seconds/60); // Minuten |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%02u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year%100 |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%04u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u.%02u.%04u %02u:%02u"), i+1, |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year, |
(uint16_t)(min/60), (uint16_t)(min%60) |
); |
*/ |
//----------------------------------- |
// Zeile 2: Hoehe (GPS/Barymetrisch) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%7d mG%5.1d mB"), |
(Config.GPS_User[i].GPSData.Altitude/1000), // GPS-Hoehe in m |
(Config.GPS_User[i].Altimeter / (3 / AltimeterAdjust)) ); // Barymetrische-Hoehe in dm |
//----------------------------------- |
// Zeile 3: Koordinaten (Lat/Long) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%3.6ld%3.6ld"), |
Config.GPS_User[i].GPSData.Latitude/10, |
Config.GPS_User[i].GPSData.Longitude/10 ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_USERGPS ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_MkError() |
// |
// zeigt aufgezeichnete MK-Fehler an |
//-------------------------------------------------------------- |
void OSDDATA_MkError( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_MKERR_LOG*4) + 3 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" MK Errors") ); |
for( i=0; i<MAX_MKERR_LOG; i++) |
{ |
if( Config.MKErr_Log[i].Errorcode != 0) // nur vorhandene Errorcodes anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Zeit / Datum |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.MKErr_Log[i].set_Time ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u:%02u:%02u %02u.%02u.%04u"), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60), |
dtlocal.day, dtlocal.month, dtlocal.year |
); |
//----------------------------------- |
// Zeile 2: Error Code |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Code %02u"), Config.MKErr_Log[i].Errorcode ); |
//----------------------------------- |
// Zeile 3: Error Text |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%20S"), pgm_read_word(&mkerrortext[Config.MKErr_Log[i].Errorcode]) ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_MKERROR ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_ShowLastGPSPosition() |
// |
// zeigt die letzte aufgezeichnete GPS-Position an. |
// Möglichkeit zum loeschen der GPS-Pos ist vorhanden. |
//-------------------------------------------------------------- |
void OSDDATA_ShowLastGPSPosition( void ) |
{ |
uint8_t redraw = true; |
clear_key_all(); |
while( true ) |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearcsreen) |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,4); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,4); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+4, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,10); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,10); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+7, 127, (2*8)+4, 1); // Rahmen |
//lcd_printp_at(0, 7, strGet(START_LASTPOS2), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(KEYLINE4) , MNORMAL); // Keyline |
lcd_printp_at(18, 7, PSTR("Del") , MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press (1 << KEY_ESC) ) // Ende |
{ |
break; |
} |
if( (get_key_press (1 << KEY_ENTER)) ) // Del |
{ |
if( ask_delete_data(INIT_LASTPOS) ) |
{ |
set_beep ( 400, 0xffff, BeepNormal); |
WriteLastPosition( 0x00000000, 0x00000000 ); // letzte GPS Position loeschen |
break; // und beenden |
} |
redraw = true; |
} |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// OSDDATA_ClearAllData() |
// |
// löscht alle erhobenen Daten |
//-------------------------------------------------------------- |
void OSDDATA_ClearAllData( void ) |
{ |
if( ask_delete_data( INIT_ALLDATA ) ) |
{ |
STAT_Init(); |
MKErr_Log_Init(); |
GPS_User_Init(); // loeschen: User GPS |
WriteLastPosition( 0x00000000, 0x00000000 ); // loeschen: letzte GPS Position |
set_beep ( 400, 0xffff, BeepNormal); |
} |
} |
//-------------------------------------------------------------- |
// Menue fuer osddata |
//-------------------------------------------------------------- |
#ifdef USE_OSDDATA |
void Menu_OSDData( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Daten" |
//MenuCtrl_SetTitle_P( PSTR("OSD Daten") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( ID_SHOWSTATISTIK , MENU_ITEM, &OSDDATA_Statistics, SHOWSTATISTIK_de , SHOWSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWBLSTATISTIK, MENU_ITEM, &OSDDATA_StatisticsBL, SHOWBLSTATISTIK_de, SHOWBLSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWMKERROR , MENU_ITEM, &OSDDATA_MkError, SHOWMKERROR_de , SHOWMKERROR_en ); |
MenuCtrl_PushML2_P( ID_SHOWUSERGPS , MENU_ITEM, &OSDDATA_UserGPS, SHOWUSERGPS_de , SHOWUSERGPS_en ); |
MenuCtrl_PushML2_P( ID_SHOWLASTGPS , MENU_ITEM, &OSDDATA_ShowLastGPSPosition, SHOWLASTGPS_de , SHOWLASTGPS_en ); |
MenuCtrl_PushML2_P( ID_CLEARALLDATA , MENU_ITEM, &OSDDATA_ClearAllData, CLEARALLDATA_de , CLEARALLDATA_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osddata.h |
---|
0,0 → 1,38 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.h |
//# |
//# 29.03.2014 OG |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# |
//# 11.06.2013 OG |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - del: verschiedene andere exportierte Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _OSDDATA_H |
#define _OSDDATA_H |
void OSDDATA_ShowLastGPSPosition( void ); |
void Menu_OSDData( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osdold_messages.c |
---|
0,0 → 1,117 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
#include "../eeprom/eeprom.h" |
#include "osdold_messages.h" |
//--------------------------------------------------------------------------------- |
static const char OSD_ALTI_0_de[] PROGMEM = "Höhe aus "; |
static const char OSD_ALTI_0_en[] PROGMEM = "Alti. off "; |
static const char OSD_ALTI_1_de[] PROGMEM = "Höhe begr."; |
static const char OSD_ALTI_1_en[] PROGMEM = "Alti.Limit"; |
static const char OSD_VARIO_0_de[] PROGMEM = "Vario aus "; |
static const char OSD_VARIO_0_en[] PROGMEM = "Vario off "; |
static const char OSD_VARIO_1_de[] PROGMEM = "Vario Höhe"; |
static const char OSD_VARIO_1_en[] PROGMEM = "Vario Alt."; |
static const char OSD_CARE_FREE_0_de[] PROGMEM = " "; |
#define OSD_CARE_FREE_0_en OSD_CARE_FREE_0_de |
static const char OSD_CARE_FREE_1_de[] PROGMEM = "Care Free"; |
#define OSD_CARE_FREE_1_en OSD_CARE_FREE_1_de |
static const char OSD_NAVI_MODE_0_de[] PROGMEM = "Navi aus "; |
static const char OSD_NAVI_MODE_0_en[] PROGMEM = "Navi off "; |
static const char OSD_NAVI_MODE_1_de[] PROGMEM = "Pos. halten"; |
static const char OSD_NAVI_MODE_1_en[] PROGMEM = "Pos. Hold "; |
static const char OSD_NAVI_MODE_2_de[] PROGMEM = "Coming Home"; |
#define OSD_NAVI_MODE_2_en OSD_NAVI_MODE_2_de |
static const char OSD_FLAGS_0_de[] PROGMEM = " "; |
#define OSD_FLAGS_0_en OSD_FLAGS_0_de |
static const char OSD_FLAGS_1_de[] PROGMEM = "Justieren"; |
static const char OSD_FLAGS_1_en[] PROGMEM = "Calibrate"; |
static const char OSD_FLAGS_2_de[] PROGMEM = "Start "; |
#define OSD_FLAGS_2_en OSD_FLAGS_2_de |
static const char OSD_FLAGS_3_de[] PROGMEM = "Betrieb "; |
static const char OSD_FLAGS_3_en[] PROGMEM = "Run "; |
static const char OSD_FLAGS_4_de[] PROGMEM = "Fliegen "; |
static const char OSD_FLAGS_4_en[] PROGMEM = "Fly "; |
static const char OSD_FLAGS_5_de[] PROGMEM = "Landung "; |
static const char OSD_FLAGS_5_en[] PROGMEM = "Landing "; |
static const char OSD_FLAGS_6_de[] PROGMEM = "Akku leer"; |
static const char OSD_FLAGS_6_en[] PROGMEM = "Low Bat. "; |
static const char OSD_LED_Form_de[] PROGMEM = "Out1/2 Format"; |
static const char OSD_LED_Form_en[] PROGMEM = "Out1/2 format"; |
//------------------------------------------------------------------------------ |
const char * const strings_osdold[] PROGMEM= |
{ |
OSD_ALTI_0_de, OSD_ALTI_0_en, |
OSD_ALTI_1_de, OSD_ALTI_1_en, |
OSD_VARIO_0_de, OSD_VARIO_0_en, |
OSD_VARIO_1_de, OSD_VARIO_1_en, |
OSD_CARE_FREE_0_de, OSD_CARE_FREE_0_en, |
OSD_CARE_FREE_1_de, OSD_CARE_FREE_1_en, |
OSD_NAVI_MODE_0_de, OSD_NAVI_MODE_0_en, |
OSD_NAVI_MODE_1_de, OSD_NAVI_MODE_1_en, |
OSD_NAVI_MODE_2_de, OSD_NAVI_MODE_2_en, |
OSD_FLAGS_0_de, OSD_FLAGS_0_en, |
OSD_FLAGS_1_de, OSD_FLAGS_1_en, |
OSD_FLAGS_2_de, OSD_FLAGS_2_en, |
OSD_FLAGS_3_de, OSD_FLAGS_3_en, |
OSD_FLAGS_4_de, OSD_FLAGS_4_en, |
OSD_FLAGS_5_de, OSD_FLAGS_5_en, |
OSD_FLAGS_6_de, OSD_FLAGS_6_en, |
OSD_LED_Form_de, OSD_LED_Form_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
//LAST_STR_de, LAST_STR_en, |
}; |
char const * strGetOSDOLD( int str_no ) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 0 ) return (const char*) pgm_read_word( &strings_osdold[str_no*2] ); |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings_osdold[(str_no*2)+1] ); |
return (const char*) pgm_read_word( &strings_osdold[0] ); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osdold_messages.h |
---|
0,0 → 1,40 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#ifndef OSDOLD_MESSAGES_H |
#define OSDOLD_MESSAGES_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings |
//LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
OSD_ALTI_0, OSD_ALTI_1, OSD_VARIO_0, OSD_VARIO_1, OSD_CARE_FREE_0, OSD_CARE_FREE_1, |
OSD_NAVI_MODE_0, OSD_NAVI_MODE_1, OSD_NAVI_MODE_2, |
OSD_FLAGS_0, OSD_FLAGS_1, OSD_FLAGS_2, OSD_FLAGS_3, OSD_FLAGS_4, OSD_FLAGS_5, OSD_FLAGS_6, |
OSD_LED_Form |
} OSDOLDSTR; |
char const * strGetOSDOLD( int str_no ); |
#endif // USE_OSD_SCREEN_OLD |
#endif /* OSDOLD_MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osdold_screens.c |
---|
0,0 → 1,271 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
#ifdef USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// OSD_Element_AltitudeControl( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ) |
{ |
//--------------------------------------------------------- |
// 10.03.2013 OG: |
// CFG2_HEIGHT_LIMIT im Augenblick nicht unterstuetzt |
// Siehe Anmerkungen in osd() |
//--------------------------------------------------------- |
/* |
if (Flags_ExtraConfig & CFG2_HEIGHT_LIMIT) |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGet(OSD_ALTI_1), 0); // Höhe begr. |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_ALTI_0), 0); // Höhe aus |
} |
else |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
*/ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
//-------------------------------------------------------------- |
// OSD_Element_CareFree( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_CareFree( uint8_t x, uint8_t y ) |
{ |
lcd_printp_at (x, y, strGetOSDOLD( naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE ? OSD_CARE_FREE_1 : OSD_CARE_FREE_0 ), 0); |
} |
//-------------------------------------------------------------- |
// OSD_Element_NaviMode( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_FREE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_0), 0); // Navi aus |
else if (naviData->NCFlags & NC_FLAG_PH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_1), 0); // Pos. Hold |
else if (naviData->NCFlags & NC_FLAG_CH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_2), 0); // Coming Home |
} |
//-------------------------------------------------------------- |
// OSD_Element_StatusFlags( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ) |
{ |
// FC_StatusFlags 0.88 |
// #define FC_STATUS_MOTOR_RUN 0x01 |
// #define FC_STATUS_FLY 0x02 |
// #define FC_STATUS_CALIBRATE 0x04 |
// #define FC_STATUS_START 0x08 |
// #define FC_STATUS_EMERGENCY_LANDING 0x10 |
// #define FC_STATUS_LOWBAT 0x20 |
// #define FC_STATUS_VARIO_TRIM_UP 0x40 |
// #define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
if (naviData->FCStatusFlags & FC_STATUS_CALIBRATE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_1), 0); // Calibrate |
else if (naviData->FCStatusFlags & FC_STATUS_START) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_2), 0); // Start |
else if (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_3), 0); // Run |
else if (naviData->FCStatusFlags & FC_STATUS_FLY) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_4), 0); // Fly |
else if (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_5), 0); // Landing |
else if (naviData->FCStatusFlags & FC_STATUS_LOWBAT) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_6), 0); // LowBat |
else |
// lcd_printp_at (x, y, PSTR(" "), 0); // Clear |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_0), 0); // Clear |
} |
//############################################################## |
//# OLD OSD-Screens |
//############################################################## |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 0 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD0( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 3); |
OSD_Element_Altitude ( 11, 3, 0); |
OSD_Element_BatteryLevel ( 0, 7, 0); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 5); |
OSD_Element_CompassDegree ( 13, 0, 0); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 0, 1); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 16, 4, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 12, 4); |
OSD_Element_HomeDistance ( 10, 5, 0); |
OSD_Element_Target ( 10, 6, 0); |
//OSD_Element_TargetDegree ( x, y); |
OSD_Element_WayPoint ( 5, 6); |
OSD_Element_LED1Output ( 0, 6); |
OSD_Element_LED2Output ( 3, 6); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 4); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 12, 2); |
OSD_Element_SatsInUse ( 18, 2, 0); |
OSD_Element_StatusFlags ( 0, 2); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 1 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD1( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
//OSD_Element_AltitudeControl( x, y); |
OSD_Element_Altitude ( 1, 1, 1); |
OSD_Element_BatteryLevel ( 0, 7, 1); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
//OSD_Element_CareFree ( x, y); |
OSD_Element_CompassDegree ( 13, 0, 1); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 7, 6); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 1, 3, 7, 0, 0,0); |
OSD_Element_HomeDegree ( 8, 3); |
OSD_Element_HomeDistance ( 7, 2, 1); |
//OSD_Element_Target ( x, y, 1); |
//OSD_Element_TargetDegree ( x, y); |
//OSD_Element_WayPoint ( x, y); |
//OSD_Element_LED1Output ( x, y); |
//OSD_Element_LED2Output ( x, y); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 8, 5); |
OSD_Element_RCIntensity ( 15, 6); |
OSD_Element_VarioWert ( 15, 2); |
OSD_Element_SatsInUse ( 8, 4, 1); |
//OSD_Element_StatusFlags ( x, y); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 2 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD2( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 1); |
OSD_Element_Altitude ( 0, 4, 2); |
OSD_Element_BatteryLevel ( 13, 7, 2); |
OSD_Element_Capacity ( 0, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 3); |
OSD_Element_CompassDegree ( 12, 3, 2); |
//OSD_Element_CompassDirection( x, y, 0,0); |
//OSD_Element_CompassRose ( x, y); |
OSD_Element_FlyingTime ( 15, 5); |
//OSD_Element_GroundSpeed ( x, y); |
OSD_Element_HomeCircle ( 16, 0, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 11, 5); |
OSD_Element_HomeDistance ( 0, 5, 2); |
OSD_Element_Target ( 0, 6, 2); |
OSD_Element_TargetDegree ( 11, 6); |
//OSD_Element_WayPoint ( x, y); |
OSD_Element_LED1Output ( 12, 2); |
OSD_Element_LED2Output ( 14, 2); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 2); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 15, 4); |
OSD_Element_SatsInUse ( 10, 0, 2); |
OSD_Element_StatusFlags ( 0, 0); |
//OSD_Element_Variometer ( x, y); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/osd/osdold_screens.h |
---|
0,0 → 1,35 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#ifndef _OSDOLD_SCREENS_H |
#define _OSDOLD_SCREENS_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ); |
void OSD_Element_CareFree( uint8_t x, uint8_t y ); |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ); |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ); |
void OSD_Screen_OSD0( void ); |
void OSD_Screen_OSD1( void ); |
void OSD_Screen_OSD2( void ); |
#endif // USE_OSD_SCREEN_OLD |
#endif // _OSDOLD_SCREENS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/pkt/pkt.c |
---|
0,0 → 1,1227 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.c |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_Ask(), PKT_Ask_P() auf PKT_AskX() angepasst |
//# - add: PKT_AskX() - ehemals _pkt_ask(), jetzt erweitert um zusaetzliche progmen Parameter und exportiert |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() (ehemals in setup.c) |
//# - chg: PKT_Update() auf PKT_Ask_P() umgestellt |
//# - add: PKT_Ask(), PKT_Ask_P(), _pkt_ask() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P(), _pkt_title() |
//# - chg: PKT_TitleFromMenuItem() umgestellt auf _pkt_title() |
//# - chg: PKT_TitlePKTVersion() umbenannt zu PKT_TitlePKT() |
//# - fix: PKT_TitleFromMenuItem() funktioniert jetzt auch wenn kein Menue vorhanden ist |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - fix: PKT_CtrlHook() bzgl. PKT_CheckUpdate() - seit Februar 2014 Probleme |
//# mit Update-Anforderung vom PKT-Updatetool -> PKT war zu schnell |
//# Es wurde jetzt ein Timer in PKT_CtrlHook() eingebaut der den Aufruf |
//# von PKT_CheckUpdate() etwas einbremst ohne das PKT auszubremsen. |
//# Ich hoffe, das es ist Loesung ist (bin guter Dinge nach Test's) |
//# - chg: PKT_CheckUpdate() - Code optimiert |
//# - add: PKT_Info() - Anzeige ob SV2-Patch vorhanden ist oder nicht (ganz oben) |
//# - add: PKT_Info() - Anzeige fuer USE_OSD_SCREEN_WAYPOINTS |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# - add: #include "../utils/menuctrl.h" |
//# |
//# 06.05.2014 OG |
//# - chg: PKT_CheckUpdate() - seit Februar funktioniert die Update-Anforderung |
//# vom PKT-Updatetool an das PKT nicht immer zuverlaessig - fgf. weil sich |
//# seitens des PKT's Timings etwas geaendert haben (durch weglassen von Code). |
//# Hier ein Versuch etwas daran wieder zu aendern... its zwar weiterhin |
//# nicht immer zuverlaessig aber vieleicht etwas besser. |
//# Wahrscheinlich kann man das nur richtig korrigieren in Verbindung mit |
//# Anpassungen am PKT-Updatetool - aber das ist ein anderes Thema... |
//# - chg: PKT_Update() - ein kurzer Beep wenn das PKT-Update aufgerufen wird |
//# |
//# 05.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P(), _pkt_popup() |
//# |
//# 11.04.2014 OG |
//# - chg: _pkt_message() ergaenzt um clear_key_all() |
//# |
//# 09.04.2014 OG |
//# - chg: PKT_Update() - umgestellt auf ShowTitle_P() |
//# |
//# 08.04.2014 OG |
//# - chg: PKT_Update() - Text bzgl. "Druecke Start..." zentriert |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# - add: _pkt_message() zeigt einen Titel mit PKT Version und Lipo an |
//# |
//# 03.04.2014 OG |
//# - chg: _pkt_message() abgesichert bzgl. zu langen Texten |
//# |
//# 27.03.2014 OG |
//# - chg: PKT_SwitchOff() Anzeige optimiert |
//# |
//# 21.02.2014 OG |
//# - fix: PKT_CheckUpdate() Zeile 0 flimmerte wegen einer nicht mehr |
//# benoetigten Textausgabe |
//# - chg: PKT_SwitchOff() auf PKT_TitlePKTVersion() angepasst |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P(), _pkt_message() |
//# - chg: MK_Info() auf USE_MKSETTINGS angepasst (ehemals MKPARAMETER) |
//# |
//# 13.02.2014 OG |
//# - chg: Screenlayout von PKT_Update() |
//# - chg: Screenlayout von PKT_SwitchOff() |
//# |
//# 11.02.2014 OG |
//# - chg: PKT_Info() Anzeige der FC-Version entfernt |
//# |
//# 04.02.2014 OG |
//# - fix: #include "../lipo/lipo.h" hinzugefuegt |
//# |
//# 03.02.2014 OG |
//# - chg: Layout von PKT_SwitchOff() und bessere Unterstuetzung von PKT_CtrlHook() |
//# - add: PKT_ShowTitleVersion() |
//# |
//# 30.01.2014 OG |
//# - chg: geaenderte Anzeige der Reihenfolgen unterstuetzter Module in PKT_Info() |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 24.01.2014 OG |
//# - chg: PKT_SwitchOff(): Layout; Code eliminiert; Beep; |
//# add PKT_Hook() (für PKT-Update) |
//# - chg: PKT_Update(): Layout |
//# |
//# 21.10.2013 CB |
//# - add: PKT_Info WyFly hinzugefügt |
//# |
//# 07.07.2013 OG |
//# - del: PKT_Info(): USE_OSD_SCREEN_ELECTRIC_N |
//# 26.06.2013 OG |
//# - del: PKT_Info(): USE_PKT_STARTINFO |
//# |
//# 13.06.2013 OG |
//# - add: PKT-Hardwareversion in der Titelanzeige von PKT_Info() |
//# - chg: Layout Titelanzeige von PKT_Info() |
//# - del: USE_PCFASTCONNECT in PKT_Info() (nicht mehr benoetigt) |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 20.05.2013 OG |
//# - chg: PKT_CtrlHook() bei PKT-Update um WriteParameter() ergaenzt |
//# |
//# 19.05.2013 OG |
//# - add: PKT_CtrlHook() ruft ggf. PKT_Update() auf und kann spaeter weitere |
//# PKT interne Sachen verarbeiten wie z.B. senden von Daten an den PC |
//# - chg: PKT_CheckUpdate() umgestellt auf timer_pktctrl um Konflikte mit |
//# mit z.B. osd zu vermeiden |
//# - chg: PKT_Info() bzgl. Codeverbrauch verringert |
//# - fix: PKT_Info() Anzeige von USE_OSD_SCREEN_NAVIGATION |
//# - add: PC_Fast_Connect() aus menu.c |
//# - add: PKT_* Funktionen aus main.c bzw. menu.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "pkt.h" |
//############################################################################################# |
//############################################################################################# |
uint8_t pkt_progress_act = 0; |
uint8_t pkt_progress_max = 0; |
int8_t pkt_progress_yoffs = 0; |
uint8_t pkt_progress_width = 0; |
uint8_t pkt_progress_height = 0; |
#define PROGRESS_YOFFS -8 |
#define PROGRESS_WIDTH 96 |
//############################################################################################# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height) |
{ |
pkt_progress_act = 0; |
pkt_progress_max = max; |
pkt_progress_yoffs = yoffs; |
pkt_progress_width = 0; // Parameter width - aktuell nicht unterstuetzt |
pkt_progress_height = 0; // Parameter height - aktuell nicht unterstuetzt |
if( pkt_progress_width == 0 ) |
pkt_progress_width = PROGRESS_WIDTH; |
lcd_rect_round( 13, 32+pkt_progress_yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_Progress_Next( void ) |
{ |
uint8_t width; |
pkt_progress_act++; // Progress: hochzaehlen |
width = (pkt_progress_act*pkt_progress_width) / pkt_progress_max; // Progress: Breite berechnen |
lcd_frect( 16, 34+pkt_progress_yoffs, width, 2, 1); // Progress: anzeigen |
return true; |
} |
//############################################################################################# |
//# Anzeigefunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// PKT_KeylineUpDown( xUp, xDown, xoffs,yoffs) |
// |
// gibt in der Keyline (y=7) ein Up- und Down-Zeichen aus. |
// Das Up-Zeichen wird dabei um einen Pixel versetzt damit |
// es besser aussieht |
// |
// Parameter: |
// xUp : x-Position des Up-Zeichens in Zeile 7 |
// xDown: x-Position des Down-Zeichens in Zeile 7 |
// xoffs, yoffs: normalerweise 0,0 |
//-------------------------------------------------------------- |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs) |
{ |
lcdx_printp_at( xUp , 7, PSTR("\x1a") , MNORMAL, xoffs,yoffs+1); // Up |
lcdx_printp_at( xDown, 7, PSTR("\x1b") , MNORMAL, xoffs,yoffs); // Down |
} |
//############################################################################################# |
//# Titel Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _pkt_title( uint8_t progmem, const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
const char *pMask; |
if( clearscreen ) lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( progmem ) pMask = PSTR("%19S"); |
else pMask = PSTR("%19s"); |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, pMask, text ); |
if( lShowLipo ) show_Lipo(); |
} |
//-------------------------------------------------------------- |
// PKT_Title( text, lShowLipo, clearscreen ) |
// |
// text im RAM |
//-------------------------------------------------------------- |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( false, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Title_P( text, lShowLipo, clearscreen ) |
// |
// text im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( true, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// optional: rechts die PKT Lipo-Spannung anzeigen |
//-------------------------------------------------------------- |
void PKT_TitlePKTlipo( uint8_t lShowLipo ) |
{ |
//PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), true, false ); // showlipo, kein clearscreen |
PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), lShowLipo, true ); // showlipo und clearscreen |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// und rechts die PKT Lipo-Spannung |
//-------------------------------------------------------------- |
void PKT_TitlePKT( void ) |
{ |
PKT_TitlePKTlipo( true ); |
} |
//-------------------------------------------------------------- |
// PKT_TitleFromMenuItem( lShowLipo ) |
// |
// zeigt eine PKT-Titelzeile mit Text vom aufrufenden Menuepunkt |
// optional kann die PKT LiPo-Spannung angezeigt werden |
// |
// wenn kein Menue vorhandenist wird die PKT-Version angezeigt |
//-------------------------------------------------------------- |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ) |
{ |
const char *pStr; |
uint8_t isPGM; |
if( MenuCtrl_GetMenuIndex() < 0 ) |
{ |
PKT_TitlePKT(); // kein Menue vorhanden -> PKT-Version anzeigen |
return; |
} |
pStr = MenuCtrl_GetItemText(); |
isPGM = MenuCtrl_IsItemTextPGM(); |
_pkt_title( isPGM, pStr, lShowLipo, true ); // true = clearscreen |
} |
//############################################################################################# |
//# PKT Message |
//############################################################################################# |
//-------------------------------------------------------------- |
// _pkt_message( progmem, text, error, timeout, beep, clearscreen) |
// |
// INTERN |
// fuer: PKT_Message(), PKT_Message_P() |
// Parameter? -> siehe dort... |
//-------------------------------------------------------------- |
void _pkt_message( uint8_t progmem, const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
char *pStr; |
if( clearscreen ) |
lcd_cls(); |
PKT_TitlePKT(); |
if( error ) |
{ |
//lcd_frect_round( 0+36, (2*8)-2, 127-73, 8+2, 1, R1); // Fill: Schwarz |
lcd_frect_round( 0+36, (2*8)-1, 127-74, 8+2, 1, R1); // Fill: Schwarz |
lcdx_printp_center( 2, strGet(STR_ERROR), MINVERS, 0,1); // "FEHLER" |
} |
else |
lcdx_printp_center( 2, strGet(STR_PKT) , MNORMAL, 0,1); // "PKT" |
//----------------------- |
// Ausgabe der Nachricht |
//----------------------- |
pStr = buffered_sprintf_P( ( progmem ? PSTR("%20S") : PSTR("%20s")), text); // max. 20 Zeichen |
strrtrim( pStr ); // alle Leerzeichen rechts loeschen |
lcdx_print_center( 4, (uint8_t *)pStr, MNORMAL, 0,5); // Text zentriert; String im RAM |
lcd_rect_round( 0, 32, 127, 16, 1, R2); // Rahmen |
if( beep ) |
{ |
if( error ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( timeout ) |
{ |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline... tja, welche Variante nehmen wir denn nun Final??? OK oder ENDE? |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
//if( get_key_short(1 << KEY_ENTER) ) break; |
if( get_key_short(1 << KEY_ESC) ) break; |
PKT_CtrlHook(); |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Message( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im RAM) |
// |
// Parameter: |
// text : String im RAM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clearscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( false, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im PROGMEM) |
// |
// Parameter: |
// text : String im PROGMEM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clrscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( true, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_Datenverlust( timeout, beep) |
//-------------------------------------------------------------- |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ) |
{ |
//_pkt_message( true, text, error, timeout, beep, clearscreen ); |
_pkt_message( true, strGet(ERROR_NODATA), true, timeout, beep, true ); |
} |
//############################################################################################# |
//# PKT Popup |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// PARAMETER: |
// progmem : true = Texte in PROGMEM |
// false = Texte im RAM (RAM geht noch nicht!) |
// timeout : Zeit bis zum automatischen beenden (z.B. 400 = 4 Sekunden) |
// wenn timeout == 0 dann direkt beenden ohne auf Tastendruck zu warten |
// text1..4: Text (die Hoehe des Popups berechnet sich aus der Anzahl der Texte) |
// -> ein nicht benutzer Text wird mit NULL oder Dezimal 0 uebergeben |
//-------------------------------------------------------------- |
void _pkt_popup( uint8_t progmem, uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4) |
{ |
uint8_t n = 0; |
uint8_t yoffs = 0; |
if( text1 ) { n = 1; yoffs = 1; } |
if( text2 ) { n = 2; yoffs = 2; } |
if( text3 ) { n = 3; yoffs = 2; } |
if( text4 ) { n = 4; yoffs = 3; } |
Popup_Draw(n+1); |
if( text1 ) lcdx_printp_center( 8-n, text1, MINVERS, 0,-5-yoffs); // Text zentriert; String im PROGMEM |
if( text2 ) lcdx_printp_center( 9-n, text2, MINVERS, 0,-4-yoffs); // Text zentriert; String im PROGMEM |
if( text3 ) lcdx_printp_center( 10-n, text3, MINVERS, 0,-3-yoffs); // Text zentriert; String im PROGMEM |
if( text4 ) lcdx_printp_center( 11-n, text4, MINVERS, 0,-2-yoffs); // Text zentriert; String im PROGMEM |
if( timeout ) |
{ |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
if( get_key_short(1<<KEY_ESC) |
|| get_key_short(1<<KEY_ENTER) |
|| get_key_short(1<<KEY_PLUS) |
|| get_key_short(1<<KEY_MINUS) ) |
{ |
break; |
} |
} |
lcd_frect( 0, 58-((n+1)*8), 127, 5+((n+1)*8), 0); // Box clear - der Bereich des Popup's wird wieder geloescht (nur wenn timeout > 0!) |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Popup( timeout, text1, text2, text3, text4 ) |
// |
// Texte im RAM |
//-------------------------------------------------------------- |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( false, timeout, text1, text2, text3, text4 ); |
} |
//-------------------------------------------------------------- |
// PKT_Popup_P( timeout, text1, text2, text3, text4 ) |
// |
// Texte im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( true, timeout, text1, text2, text3, text4 ); |
} |
//############################################################################################# |
//# PKT Ask |
//############################################################################################# |
//-------------------------------------------------------------- |
// wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
// |
// PARAMETER: |
// asktype : ASK_YES_NO, ASK_NO_YES, ASK_CANCEL_OK, ASK_END_OK, ASK_END_START |
// text1 : 1. Zeile Text |
// text2 : optionale 2. Zeile Text |
// text_progmem : text1 & text2 in PROGMEM oder RAM (PROGMEM = true) |
// headline : Ueberschrift |
// headline_progmem: -> PROGMEM oder RAM |
// title : ganz oben der Titel (Zeile 0, Invers) |
// title_progmem : -> PROGMEM oder RAM |
// |
// RUECKGABE: |
// true : Ja |
// false : Nein |
//-------------------------------------------------------------- |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ) |
{ |
uint8_t redraw = true; |
uint8_t keyenter = false; |
uint8_t retcode = false; |
const char *pMaskP; |
const char *pMaskR; |
const char *pMask; |
pMaskP = PSTR("%S"); |
pMaskR = PSTR("%s"); |
set_beep( 35, 0xffff, BeepNormal ); // kurzer Beep |
while( true ) |
{ |
//------------------------ |
// Screen zeichnen |
//------------------------ |
if( redraw ) |
{ |
if( title == NULL ) PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearscreen) |
else if( title_progmem ) PKT_Title_P( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (PROGMEM) |
else PKT_Title( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (RAM) |
if( text2 == NULL ) |
{ |
// 1 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,2, pMask, headline); // headline |
lcd_rect_round( 0, 3*7+2+10, 127, 16+0, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,6, pMask, text1); // text1 |
} |
else |
{ |
// 2 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,-1, pMask, headline); // headline |
lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,-1, pMask, text1); // text1 |
lcdx_printf_center_P( 5, MNORMAL, 0, 0, pMask, text2); // text2 |
} |
//------------------------ |
// Keyline |
//------------------------ |
switch( asktype ) |
{ |
case ASK_NO_YES: lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_YES_NO: lcd_printp_at(12, 7, strGet(YESNO), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_CANCEL_OK: lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr. OK" |
lcd_printp_at(19, 7, PSTR("OK") , MNORMAL); |
break; |
case ASK_END_OK: lcd_printp_at(12, 7, strGet(KEYLINE4), MNORMAL); // Keyline: "Ende OK" |
break; |
case ASK_END_START: lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline: "Ende Start" |
break; |
} |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // 4. Taste |
{ |
keyenter = true; |
break; |
} |
} // end: while( true ) |
//------------------------- |
retcode = keyenter; |
if( asktype == ASK_YES_NO ) retcode = !retcode; |
clear_key_all(); |
return retcode; |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im RAM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, false, headline, true, title, true); |
//return _pkt_ask( false, asktype, text1, text2, headline, title); |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask_P( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im PROGMEM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, true, headline, true, title, true); |
//return _pkt_ask( true, asktype, text1, text2, headline, title); |
} |
//############################################################################################# |
//# PKT Gauge |
//############################################################################################# |
uint8_t Gauge_px; |
uint8_t Gauge_py; |
uint8_t Gauge_rx; |
uint8_t Gauge_ry; |
uint8_t Gauge_step; |
volatile uint8_t Gauge_active; // wird in timer.c abgefragt |
#define GAUGE_DEFAULT_PX 64 // Default X (Pixel) |
#define GAUGE_DEFAULT_PY 40 // Default Y (Pixel) |
#define GAUGE_DEFAULT_RX 10 // Default Radius X Gauge-Kreis |
#define GAUGE_DEFAULT_RY 8 // Default Radius Y Gauge-Kreis |
#define GAUGE_TIMER 40 // alle n aktualisieren (Timer Intervall) |
#define GAUGE_DEGREE 45 // Winkel von Schritt zu Schritt |
#define GAUGE_LROFFS -2 // die Laenge der Gauge-Linie verringern/vergroessern |
//-------------------------------------------------------------- |
// PKT_Gauge_Begin( py ) |
//-------------------------------------------------------------- |
void PKT_Gauge_Begin( uint8_t py ) |
{ |
Gauge_px = GAUGE_DEFAULT_PX; |
Gauge_py = GAUGE_DEFAULT_PY; |
Gauge_rx = GAUGE_DEFAULT_RX; |
Gauge_ry = GAUGE_DEFAULT_RY; |
if( py>0 ) Gauge_py = py; |
Gauge_step = 0; |
timer_gauge = GAUGE_TIMER; |
Gauge_active = true; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (0), 1); |
lcd_ellipse( Gauge_px, Gauge_py, Gauge_rx, Gauge_ry, 1); |
} |
//-------------------------------------------------------------- |
// PKT_Gauge_End() |
//-------------------------------------------------------------- |
void PKT_Gauge_End( void ) |
{ |
Gauge_active = false; |
} |
// uint8_t old_step; |
//-------------------------------------------------------------- |
// PKT_Gauge_Next() |
// |
// INTERN fuer Timer ISR! |
//-------------------------------------------------------------- |
void PKT_Gauge_Next( void ) |
{ |
uint8_t old_step; |
old_step = Gauge_step; |
Gauge_step++; |
if( Gauge_step >= (360/GAUGE_DEGREE)) Gauge_step = 0; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (old_step*GAUGE_DEGREE) , 0); |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (Gauge_step*GAUGE_DEGREE), 1); |
timer_gauge = GAUGE_TIMER; |
} |
//############################################################################################# |
//# PKT Kontrollfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Reset_EEprom( void ) |
{ |
// Text1: "PKT-EEProm" (EEPROM1) |
// Text2: "wirklich löschen?" (EEPROM2) |
// Head : "* ACHTUNG '" (STR_ATTENTION) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_YES_NO, strGet(EEPROM1), strGet(EEPROM2), strGet(STR_ATTENTION), NULL) ) |
{ |
Delete_EEPROM(); |
clr_V_On(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_SwitchOff( void ) |
{ |
// Text1: "PKT ausschalten?" (SHUTDOWN) |
// Text2: NULL |
// Head : "PKT" (STR_PKT) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_NO_YES, strGet(SHUTDOWN), NULL, strGet(STR_PKT), NULL) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Ask_Restart( const char *title ) |
{ |
// Text1: "jetzt ausschalten?" (STR_PKT_SWITCHOFF_NOW) |
// Text2: NULL |
// Head : "PKT neu starten!" (STR_PKT_RESTART_NOW) |
// Titel: via Parameter |
if( PKT_Ask_P( ASK_NO_YES, strGet(STR_PKT_SWITCHOFF_NOW), NULL, strGet(STR_PKT_RESTART_NOW), title) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Update( void ) |
{ |
// Text1: "Drücke 'Start' für" (UPDATE2) |
// Text2: "min. 2 Sekunden" (UPDATE3) |
// Head : "Verbinde PKT mit PC!" (UPDATE1) |
// Titel: "PKT Update" |
if( PKT_Ask_P( ASK_END_START, strGet(UPDATE2), strGet(UPDATE3), strGet(UPDATE1), PSTR("PKT Update")) ) |
{ |
uart1_putc('S'); // Startzeichen zum Updatetool schicken |
wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
while(true); |
} |
} |
//void PKT_Update( void ) |
//{ |
// ShowTitle_P( PSTR("PKT Update"), true ); |
// lcdx_printp_center( 2, strGet(UPDATE1), MNORMAL, 0,-1); // "Verbinde PKT mit PC!" |
// lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
// lcdx_printp_center( 4, strGet(UPDATE2), MNORMAL, 0,-1); // "Drücke 'Start' für" |
// lcdx_printp_center( 5, strGet(UPDATE3), MNORMAL, 0, 0); // "min. 2 Sekunden" |
// lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline |
// //PKTupdate = false; // Flag löschen |
// |
// set_beep( 80, 0x000f, BeepNormal); // kurzer Beep |
// |
// do |
// { |
// if( get_key_press (1 << KEY_ESC) ) |
// { |
// //get_key_press(KEY_ALL); |
// clear_key_all(); |
// return; |
// } |
// } |
// while( !(get_key_press (1 << KEY_ENTER)) ); |
// |
// uart1_putc('S'); // Startzeichen zum Updatetool schicken |
// wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
// while(true); |
//} |
//-------------------------------------------------------------- |
// prueft im Menue ob das Updatetool ein Update machen will |
//-------------------------------------------------------------- |
uint8_t PKT_CheckUpdate(void) |
{ |
unsigned int uart_data; |
uint8_t state = 0; |
uint8_t update = false; |
uint8_t error = false; |
uint8_t SendVersion = false; |
char s[7]; |
//---------------------------- |
// empfangen |
//---------------------------- |
if( uart1_available() > 0 ) // Zeichen von der USB-Schnittstelle vorhanden? |
{ |
timer_pktupdatecheck = 100; // Timeouttimer um die Funktion abzubrechen |
while( !timer_pktupdatecheck==0 && !update && !error && !SendVersion ) |
{ |
uart_data = uart1_getc(); |
if( !((uart_data & 0xFF00) == UART_NO_DATA) ) |
{ |
switch (state) |
{ |
case 0: if( uart_data == PKTESC ) |
{ |
state = 1; |
} |
else error = true; |
break; |
case 1: if( uart_data == PKTESC ) |
{ |
state = 2; |
} |
else error = true; |
break; |
case 2: if( uart_data == PKTUPDATE ) update = true; |
else if (uart_data == PKTVERSION) SendVersion = true; |
else error = true; |
break; |
} // end: switch |
} // end: if |
} // end: while |
//---------------------------- |
// Rueckmeldungen |
//---------------------------- |
if( error ) |
{ |
uart1_putc('E'); // Rueckmeldungen zum Updatetool |
} |
else if( update ) |
{ |
uart1_putc('U'); |
} |
else if( SendVersion ) |
{ |
uart1_putc('V'); |
itoa( EEpromVersion, s, 10 ); |
uart1_puts(s); |
} |
else if( timer_pktupdatecheck==0 ) |
{ |
uart1_putc('T'); |
} |
//uart1_flush(); |
} |
return update; |
} |
//-------------------------------------------------------------- |
// retcode = PKT_CtrlHook() |
// |
// ruft ggf. PKT_Update() auf und kann spaeter weitere PKT interne |
// Sachen verarbeiten wenn der Hook (=Nagel) in entsprechende |
// Routienen eingebaut ist (z.B. senden von Daten an den PC) |
// |
// Aktuell verwendet von: menuctrl, scrollbox, osd |
// |
// Rueckgabe: |
// retcode: 0 = nichts wurde gemacht (false) |
// 1 = es wurde etwas durchgefuehrt und die Aufruffunktion |
// muss gff. einen Screenrefresh durchfuehren |
// (z.B. wenn der PKT-Update Screen gezeigt wurde) |
//-------------------------------------------------------------- |
uint8_t PKT_CtrlHook( void ) |
{ |
uint8_t retcode = 0; |
// 31.05.2014 OG: PKT_CheckUpdate() darf nicht in 'Hoechstgeschwindigkeit' |
// aufgerufen werden da es sonst seitens des PKT-Updatetools ggf. zu einer |
// Fehlermeldung kommt - deswegen hier timer_pktupdatecheck der PKT_CheckUpdate |
// etwas einbremst sich aber nicht negativ auf die PKT-Performance auswirkt. |
if( !timer_pktupdatecheck ) |
{ |
if( PKT_CheckUpdate() ) // Update vom Updatetool angefordert? |
{ |
WriteParameter(); |
PKT_Update(); |
retcode = 1; |
} |
timer_pktupdatecheck = 10; |
} |
return retcode; |
} |
//-------------------------------------------------------------- |
// zeigt PKT-Version und Credits / Entwickler |
//-------------------------------------------------------------- |
void PKT_Info( void ) |
{ |
char *HWV_39m = "3.9m"; |
char *HWV_39x = "3.9x"; |
char *HWV = "????"; |
if( PCB_Version == PKT39m ) HWV = HWV_39m; |
if( PCB_Version == PKT39x ) HWV = HWV_39x; |
lcd_cls(); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" PKT v%8S %s"), PSTR(PKTSWVersion), HWV ); // "3.6.8a..." und die PKT-Hardwareversion |
ScrollBox_Push_P( MINVERS, PSTR(" %S SV2-Patch"), (PCB_SV2RxTxPatch ? strGet(STR_WITH) : strGet(STR_WITHOUT)) ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
ScrollBox_Push_P( MNORMAL, PSTR("(C) GNU GPL License") ); |
ScrollBox_Push_P( MNORMAL, PSTR(" NO WARRANTY") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#ifdef USE_MODULEINFO |
const char *fmt = PSTR("%15S %4S"); |
//--- MODULE SUPPORT INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Modules installed") ); |
#ifdef USE_BLUETOOTH |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(NOO) ); |
#endif |
#ifdef USE_WLAN |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(NOO) ); |
#endif |
#ifdef USE_MKSETTINGS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDEBUGDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDISPLAY |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(NOO) ); |
#endif |
#ifdef USE_OSDDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(NOO) ); |
#endif |
#ifdef USE_TRACKING |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(NOO) ); |
#endif |
#ifdef USE_FOLLOWME |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(NOO) ); |
#endif |
#ifdef USE_JOYSTICK |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(NOO) ); |
#endif |
#ifdef USE_I2CMOTORTEST |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(NOO) ); |
#endif |
#ifdef USE_SOUND |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(NOO) ); |
#endif |
#ifdef USE_FONT_BIG |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(NOO) ); |
#endif |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#endif // USE_MODULEINFO |
//--- CREDITS INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Credits") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2013 Oliver Gemesi") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Chr. Brandtner" ) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" Harald Bongartz") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Martin Runkel") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 gebad") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2010 Sebastian Boehm") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2009-2010 Peter Mack") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2008 Thomas Kaiser") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/pkt/pkt.h |
---|
0,0 → 1,148 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.h |
//# |
//# 25.06.2014 OG |
//# - add: PKT_AskX() |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() |
//# - add: PKT_Ask(), PKT_Ask_P() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P() |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# |
//# 06.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P() |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# |
//# 21.02.2014 OG |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P() |
//# |
//# 13.02.2014 OG |
//# - add: PKT_print_PKT_center() |
//# |
//# 03.02.2014 OG |
//# - add: PKT_ShowTitlePKTVersion() |
//# |
//# 13.06.2013 OG |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 19.05.2013 OG |
//# - Ausgliederungen aus verschiedenen anderen Sourcen |
//############################################################################ |
#ifndef _PKT_H |
#define _PKT_H |
#define PKTESC 27 // Parameter für PKT-Updatetool |
#define PKTUPDATE 'u' // starte Update |
#define PKTVERSION 'v' // sende PKT EEpromversion |
#define PKTSENDCONF 'e' // sende PKT Konfigdaten |
#define ASK_YES_NO 1 |
#define ASK_NO_YES 2 |
#define ASK_CANCEL_OK 3 |
#define ASK_END_OK 4 |
#define ASK_END_START 5 |
extern volatile uint8_t Gauge_active; |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_TitlePKT( void ); |
void PKT_TitlePKTlipo( uint8_t lShowLipo ); |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ); |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs); |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height); |
uint8_t PKT_Progress_Next( void ); |
void PKT_Gauge_Begin( uint8_t py); |
void PKT_Gauge_End( void ); |
void PKT_Gauge_Next( void ); |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ); |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ); |
uint8_t PKT_CtrlHook(void); |
uint8_t PKT_CheckUpdate(void); // prüft im Hauptmenü ob das Updatetool ein Update machen will |
void PKT_Reset_EEprom( void ); |
void PKT_Update(void); |
void PKT_SwitchOff(void); |
void PKT_Info(void); |
void PKT_Ask_Restart( const char *title ); |
#endif // end: #ifndef _PKT_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/setup/setup.c |
---|
0,0 → 1,2643 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.c |
//# |
//# 14.10.2015 Starter |
//# - FollowMe neu mit Offset |
//# - enum Show_int3 fehlte für das Anzeigen von Setupwerten. Nun ergänzt |
//# - Umbenennen der Pareameter Distance und Azimuth in Offset_lat und Offset_long |
//# |
//# 03.08.2015 CB |
//# - add: FollowMe Setup um Distance und Azimuth erweitert |
//# |
//# 27.06.2014 OG |
//# - chg: Setup_MAIN() - Reihenfolge von GPS-Maus/FollowMe/Tracking geaendert |
//# |
//# 25.06.2014 OG |
//# - chg: Text von GPS_SHOWDEV |
//# - chg: Setup_FollowMe() - deaktiviert/auskommentiert: FME_REFRESH |
//# - chg: Setup_GPSMouse() - deaktiviert/auskommentiert: GPS_TYPE, GPS_ACTIVE |
//# |
//# 24.06.2014 OG |
//# - chg: Setup_GPSMouse() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - del: verschiedene weitere Modul-Variablen entfernt weil nicht benoetigt |
//# - del: Variable CheckGPS |
//# - del: BT_ShowGPSData() - ist jetzt als GPSMouse_ShowData() in gps/gpsmouse.c |
//# - chg: BT_ShowGPSData() - Parameter und Rueckgabe |
//# - del: BT_ShowGPSData_OLD |
//# |
//# 21.06.2014 CB |
//# - chg: BT_SearchDevices - Timeoutanzeige bei Devicesuche |
//# |
//# 16.06.2014 OG |
//# - chg: BT_ShowGPSData() - neues Layout und Anzeige fuer "XCnt" (=RX-Count) |
//# hinzugefuegt; XCnt zeigt die Anzahl empfangener GPS-Datenpakete an |
//# und ist ein Indikator wie schnell die BT-GPS Maus Daten sendet |
//# |
//# 13.06.2014 OG |
//# - chg: Setup_PKTGeneric() - neue Einstellung "PKT aus nach" |
//# - del: ChangeWi_SV2() |
//# |
//# 12.06.2014 OG |
//# - del: Reset_EEprom() - ist jetzt als PKT_Reset_EEprom() in pkt.c |
//# |
//# 11.06.2014 OG |
//# - fix: Edit_generic() funktioniert jetzt auch, wenn kein vorgelagertes |
//# Menue vorhanden ist (ggf. muss das Label angepasst werden) |
//# |
//# 10.06.2014 OG |
//# - chg: verschiedene Funktionen umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - add: Setup_MKConnection() - hier ist auch das setzen der Baudrate Wi232/BT |
//# hin verschoben worden (war vorher im Wi.232 Bereicch) |
//# |
//# 06.06.2014 OG |
//# - chg: Setup_WI232_MenuCreate()... Eintraege geloescht... |
//# |
//# 04.06.2014 OG |
//# - chg: Setup_PKTGeneric() eine Menue-Trennlinie hinter Sommerzeit eingefuegt |
//# |
//# 01.06.2014 OG |
//# - chg: Edit_LED_Form() Parameter 'Text' entfernt und auf strGetOSDOLD() |
//# umgestellt |
//# - add: include "../osd/osdold_messages.h" |
//# |
//# 31.05.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt; angepasst auf neue |
//# Titelzeile; PKT_CtrlHook integriert; PKT Lipo-Anzeige; |
//# auf 'redraw' umgestellt |
//# |
//# 30.05.2014 OG |
//# - chg: Edit_LipoOffset() - umgeschrieben auf neues Layout, Code optimiert, |
//# Increment jetzt +/-50 (statt vorher 20), Offset bis 15000 (statt 10000) |
//# - chg: Edit_LipoOffset() - keine Parameter mehr, keine Rueckgabe |
//# |
//# 29.05.2014 OG |
//# - chg: Setup_OSD() umgestellt auf Menuetexte statt strGet() mit Textaenderung |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# (Text enfernt da vom Menue geerbt; optionale Hilfstexte moeglich) |
//# |
//# 27.05.2014 OG |
//# - del: Edit_generic_int(), Edit_printout_int() - nicht mehr benoetigt da |
//# anhaengende Werte umgemappt auf Edit_generic() |
//# |
//# 26.05.2014 OG |
//# - chg: Menuetext TIME_ZONE ergaenzt um "UTC" |
//# - chg: Setup_MAIN() - neue Anordnung der Menueeintraege mit Trennlinien |
//# - chg: Setup_OSDScreens() - OSDSCREEN_WAYPOINTS hinzugefuegt |
//# - chg: Text "LCD aus nach..." nach "LCD aus nach" |
//# - chg: Text "PKT allgemein" geaendert auf "PKT Allgemein" |
//# - del: PKT_LCDINVERS inkl. Sprachen |
//# - chg: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) entfernt, |
//# nicht mehr unterstuetzt |
//# - chg: Edit_printout() - Hilfstexte CHANGENORMREV1 und 2 fuer NormRev entfernt |
//# |
//# 23.05.2014 OG |
//# - add: Edit_generic() - Anzeige Titel mit PKT_TitleFromMenuItem() |
//# - fix: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) |
//# Edit-Range umgestellt von 0,4 auf 0,1 |
//# |
//# 11.05.2014 OG |
//# - chg: die Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# - chg: ein paar Menuetexte fuer Setup_MAIN() |
//# |
//# 18.04.2014 OG |
//# - chg: Setup_PKTGeneric() - PKT_BRIGHTNESS rausgenommen da es aktuell auch |
//# nicht mehr mit PKT 3.9m Hardware geht (spart ca. 650 Bytes) |
//# - chg: Text von "Tracking Servos" gekuerzt auf "Tracking" |
//# |
//# 09.04.2014 OG |
//# - chg: Edit_printout() Text von WlanMode 'Nein' auf 'Aus' geaendert |
//# |
//# 04.04.2014 OG |
//# - chg: Edit_String() auf ShowTitle_P() umgestellt |
//# - del: Edit_ShowTitle_P() |
//# - chg: in versch. Funktionen Titel-Strings fuer Edit_String() gekuerzt |
//# - chg: Edit_String() auf Edit_ShowTitle_P() umgestellt |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: Edit_printout(): WlanMode (Aus, AP-Mode, AdHoc) |
//# - chg: Edit_printout(): Code-Optimierung |
//# |
//# 01.04.2014 OG |
//# - add: Unterstuetzung fuer BLE (Bluetooth 4 Low Energy Zusatzmodul an SV2) |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 27.03.2014 OG |
//# - chg: Setup_PKTGeneric(), Edit_printout() Sprache "Niederländisch" nicht mehr waehlbar |
//# |
//# 04.02.2014 OG |
//# - chg: kleine Layoutaenderung von 'PKT_Akku' in Edit_printout() |
//# - add: PKT_CtrlHook() in Edit_generic(), Edit_generic_int(), Edit_LipoOffset() eingebaut |
//# - add: kleiner Hilfetext in Edit_printout() bei Aenderung von Einstellung |
//# "LCD Normal/Invers" bzgl. PKT neu starten nach Aenderung |
//# - chg: Anzeige von Edit_LipoOffset() geaendert und fix vom springen der |
//# Eingabe wenn Wert geaendert wird |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU in Setup_OSD() ("zeige Zellensp.") |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.07.2013 Cebra |
//# - chg: Fehler in Setup_WI232_MenuCreate, MenuCtrl_Push_P( WI_INSTALLED.. war auskommentiert, dadurch Fehler im Menu bei nicht |
//# aktivem Wi232 |
//# |
//# 18.07.2013 Cebra |
//# - chg: Edit_PKT_Accu entfernt, Edit_generic erweitert |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: Setup_OSDScreens(), Mark_OSDScreen(), Edit_OSDScreen() |
//# |
//# 04.07.2013 Cebra |
//# - chg: Setupmenü um Wlan erweitert |
//# |
//# 02.07.2013 Cebra |
//# - chg: Edit_String, geändert für die Nutzung von SSID und Wlan Passwort |
//# |
//# 30.06.2013 OG |
//# - del: Setup_OSD(): OSDS_HOMEVIEW |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO |
//# |
//# 24.06.2013 OG |
//# - chg: vor dem editieren des Bluetooth Namens bt_fixname() eingefuegt |
//# |
//# 24.06.2013 OG |
//# - fix: Edit_String(): Fehlermeldung bei EDIT_BT_NAME wenn erstes Zeichen = Space |
//# - chg: Edit_String(): Code-Layout und diverses |
//# - chg: Setup_BTM222(): Bearbeitung von BT_NAME geaendert |
//# - chg: Setup_BTM222(): Menuetexte geaendert |
//# - chg: Setup_MAIN(): Text im Setup-Hauptmenue von "BTM-222" auf "Bluetooth" geaendert |
//# |
//# 21.06.2013 OG |
//# - fix: Menuetext GPSMouse "Zeige Geräteliste" (Anzeige "ä" korrigiert) |
//# |
//# 15.06.2013 OG |
//# - add: "zeige MK-Setting" in Setup_OSD() hinzugefuegt |
//# |
//# 15.06.2013 OG |
//# - chg: Umstrukturierungen am OSD-Setup |
//# - chg: OSD Empfangsausfall von Setup_PKTGeneric() nach Setup_OSD() verschoben |
//# |
//# 13.06.2013 OG |
//# - del: Setup_Time() - jetzt in Setup_PKTGeneric() |
//# - del: Setup_PKTAccu() - jetzt in Setup_PKTGeneric() |
//# - del: SETUP_PKTDEBUG in Setup_MAIN() entfernt (auskommentiert, wenn notwendig ggf. mit einem Debug define regeln) |
//# - chg: diverse Aenderungen in Menutexten |
//# - fix: keine LCD-Helligkeitseinstellung bei HW 3.9x mehr durch Abfrage von PCB_Version |
//# |
//# 31.05.2013 cebra |
//# - chg: Umlaute, soweit gefunden, korrigiert |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: etliche Aenderungen rund um Bluetooth/BT-GPS Maus fuer bessere |
//# Usebility, Code-Einsparung und Code-Lesbarkeit |
//# - add: Wait4KeyOK() |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: BT_SelectDevice() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Setup_Time(), Setup_FollowMe(), Setup_OSD() |
//# - chg: weitere Speicheroptimierungen fuer USE_* |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# |
//# 20.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Menu_Setup(), Setup_PKTGeneric(), Setup_WI232() |
//# Setup_BTM222(), Setup_PKTAccu(), Setup_GPSMouse() |
//# - add: GPSMouse_ShowData() |
//# |
//# 19.05.2013 OG |
//# - chg: Aufruf von Update_PKT() umgestellt auf PKT_Update() |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.05.2013 Cebra |
//# - chg: Fehler bei On/Off Settings |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup |
//# |
//# 19.04.2013 Cebra |
//# - chg: Fehlerbehandlung im GPS Menü für den Fall das kein BTM22 eingebaut oder GPS-Device konfiguriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 12.04.2013 Cebra |
//# - chg: edit_generic um Sprungfaktor erweitert, erm�glicht bei hohen Werten einen schnelleren Wertewechsel |
//# |
//# 03.04.2013 Cebra |
//# - chg: find/replace Fehler bei lcd_puts_at beseitigt, GPS und BT-Maus Bereich |
//# |
//# 02.04.2013 Cebra |
//# - chg: Textfehler bei der Einstellung "Verbindung zum MK" beseitigt |
//# |
//# 04.03.2013 Cebra |
//# - del: OSD-Sreenmode ,is no longer necessary, last OSD-Screen is saved at shutdown |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../setup/setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../wi232/Wi232.h" |
#include "../bluetooth/bluetooth.h" |
#include "../connect.h" |
#include "../lipo/lipo.h" |
#include "../messages.h" |
#include "../eeprom/eeprom.h" |
#include "../tracking/tracking.h" |
#include "../uart/uart1.h" |
#include "../sound/pwmsine8bit.h" |
#include "../tracking/servo_setup.h" |
#include "../bluetooth/error.h" |
#include "../stick/stick_setup.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../wifly/wifly_setup.h" |
#include "../gps/gpsmouse.h" |
#include "../utils/scrollbox.h" |
#include "../osd/osdold_messages.h" |
uint8_t edit = 0; |
uint16_t Pre; |
char EditString[21]; |
uint8_t length_tmp; |
uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
//############################################################################ |
//############################################################################ |
//----------------------------- |
// Setup_Main() |
//----------------------------- |
#define SETUP_MKCONNECTION 1 |
#define SETUP_PKTCONFIG 2 |
#define SETUP_WI232 3 |
#define SETUP_BTM222 4 |
#define SETUP_SERVOCONFIG 5 |
#define SETUP_OSDVIEW 6 |
#define SETUP_TIME 7 |
#define SETUP_GPSMAUS 8 |
#define SETUP_FOLLOWME 9 |
#define SETUP_JOYSTICK 10 |
#define SETUP_PKTAKKU 11 |
#define SETUP_PKTUPDATE 12 |
#define SETUP_PKTDEBUG 13 |
#define SETUP_EEPROMRESET 14 |
#define SETUP_WIFLY 15 |
#define SETUP_OSDSCREENS 16 |
#define SETUP_BLE 17 |
static const char SETUP_MKCONNECTION_de[] PROGMEM = "Verbindung MK"; |
static const char SETUP_MKCONNECTION_en[] PROGMEM = "connection MK"; |
static const char SETUP_PKTCONFIG_de[] PROGMEM = "PKT Allgemein"; |
static const char SETUP_PKTCONFIG_en[] PROGMEM = "PKT general"; |
static const char SETUP_WI232_de[] PROGMEM = "Wi.232"; |
#define SETUP_WI232_en SETUP_WI232_de |
static const char SETUP_BTM222_de[] PROGMEM = "Bluetooth"; |
#define SETUP_BTM222_en SETUP_BTM222_de |
static const char SETUP_BLE_de[] PROGMEM = "Bluetooth BLE"; |
#define SETUP_BLE_en SETUP_BLE_de |
static const char SETUP_WIFLY_de[] PROGMEM = "WLAN WiFly"; |
#define SETUP_WIFLY_en SETUP_WIFLY_de |
static const char SETUP_SERVOCONFIG_de[] PROGMEM = "Tracking"; |
#define SETUP_SERVOCONFIG_en SETUP_SERVOCONFIG_de |
static const char SETUP_OSDVIEW_de[] PROGMEM = "OSD Anzeige"; |
static const char SETUP_OSDVIEW_en[] PROGMEM = "OSD display"; |
static const char SETUP_OSDSCREENS_de[] PROGMEM = "OSD Screens"; |
static const char SETUP_OSDSCREENS_en[] PROGMEM = "OSD screens"; |
static const char SETUP_GPSMAUS_de[] PROGMEM = "GPS Maus"; |
static const char SETUP_GPSMAUS_en[] PROGMEM = "GPS mouse"; |
static const char SETUP_FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define SETUP_FOLLOWME_en SETUP_FOLLOWME_de |
static const char SETUP_JOYSTICK_de[] PROGMEM = "Joystick"; |
#define SETUP_JOYSTICK_en SETUP_JOYSTICK_de |
static const char SETUP_PKTUPDATE_de[] PROGMEM = "PKT Update"; |
#define SETUP_PKTUPDATE_en SETUP_PKTUPDATE_de |
static const char SETUP_PKTDEBUG_de[] PROGMEM = "Debug PKT"; |
#define SETUP_PKTDEBUG_en SETUP_PKTDEBUG_de |
static const char SETUP_EEPROMRESET_de[] PROGMEM = "PKT Reset"; |
#define SETUP_EEPROMRESET_en SETUP_EEPROMRESET_de |
//############################################################################ |
//----------------------------- |
// Setup_PKTGeneric() ("PKT allgemein") |
//----------------------------- |
#define PKT_LANGUAGE 3 |
#define PKT_LIGHTOFF 4 |
#define PKT_BRIGHTNESS 5 |
#define PKT_CONTRAST 6 |
#define PKT_SOUNDMODUL 9 |
#define PKT_BEEPER 10 |
#define PKT_VOLUME 11 |
#define PKT_ACCUTYPE 12 |
#define PKT_ACCUMEASURE 13 |
#define TIME_ZONE 14 |
#define TIME_SUMMER 15 |
#define PKT_PKTOFF 16 |
static const char LANGUAGE_de[] PROGMEM = "Sprache"; |
static const char LANGUAGE_en[] PROGMEM = "language"; |
static const char TIME_ZONE_de[] PROGMEM = "Zeitzone UTC"; |
static const char TIME_ZONE_en[] PROGMEM = "time zone UTC"; |
static const char TIME_SUMMER_de[] PROGMEM = "Sommerzeit"; |
static const char TIME_SUMMER_en[] PROGMEM = "summer time"; |
static const char PKT_PKTOFF_de[] PROGMEM = "PKT aus nach"; |
static const char PKT_PKTOFF_en[] PROGMEM = "PKT off after"; |
static const char LIGHTOFF_de[] PROGMEM = "LCD aus nach"; |
static const char LIGHTOFF_en[] PROGMEM = "LCD off after"; |
//static const char BRIGHTNESS_de[] PROGMEM = "LCD Helligkeit"; |
//static const char BRIGHTNESS_en[] PROGMEM = "LCD brightness"; |
static const char CONTRAST_de[] PROGMEM = "LCD Kontrast"; |
static const char CONTRAST_en[] PROGMEM = "LCD contrast"; |
static const char BEEPER_de[] PROGMEM = "Hardware Beeper"; |
#define BEEPER_en BEEPER_de |
#ifdef USE_SOUND |
static const char SOUNDMODUL_de[] PROGMEM = "Soundmodul"; |
static const char SOUNDMODUL_en[] PROGMEM = "Soundmodule"; |
static const char VOLUME_de[] PROGMEM = "Lautstärke Sound"; |
static const char VOLUME_en[] PROGMEM = "Volume Sound"; |
#endif |
static const char ACCUTYPE_de[] PROGMEM = "Akku Typ"; |
static const char ACCUTYPE_en[] PROGMEM = "Accu type"; |
static const char ACCUMEASURE_de[] PROGMEM = "Akku Messung"; |
static const char ACCUMEASURE_en[] PROGMEM = "Accu measure"; |
//############################################################################ |
//----------------------------- |
// Setup_MKConnection() |
//----------------------------- |
#define MKCONN_CONNECTION 1 |
#define MKCONN_BAUDRATE 2 |
#define MKCONN_CONNECTION_de SETUP_MKCONNECTION_de |
#define MKCONN_CONNECTION_en SETUP_MKCONNECTION_en |
static const char MKCONN_BAUDRATE_de[] PROGMEM = "Baudrate Wi232/BT"; |
#define MKCONN_BAUDRATE_en MKCONN_BAUDRATE_de |
//############################################################################ |
//----------------------------- |
// Setup_WI232() |
//----------------------------- |
#define WI_INSTALLED 1 |
#define WI_TXRX 2 |
#define WI_GROUP 3 |
#define WI_MODE 4 |
#define WI_TIMEOUT 5 |
#define WI_MTU 6 |
#define WI_INIT 7 |
#define WI_PCCONFIG 8 |
static const char WI_INSTALLED_de[] PROGMEM = "Modul eingebaut"; |
static const char WI_INSTALLED_en[] PROGMEM = "module built in"; |
static const char WI_TXRX_de[] PROGMEM = "TX/RX Kanal"; |
static const char WI_TXRX_en[] PROGMEM = "TX/RX Channel"; |
static const char WI_GROUP_de[] PROGMEM = "NetW. Gruppe"; |
static const char WI_GROUP_en[] PROGMEM = "NetW. Group"; |
static const char WI_MODE_de[] PROGMEM = "NetW. Mode"; |
#define WI_MODE_en WI_MODE_de |
static const char WI_TIMEOUT_de[] PROGMEM = "TX Timeout"; |
#define WI_TIMEOUT_en WI_TIMEOUT_de |
static const char WI_MTU_de[] PROGMEM = "TX MTU"; |
#define WI_MTU_en WI_MTU_de |
static const char WI_INIT_de[] PROGMEM = "Initialisieren"; |
static const char WI_INIT_en[] PROGMEM = "initialize"; |
static const char WI_PCCONFIG_de[] PROGMEM = "Konfig. mit PC"; |
static const char WI_PCCONFIG_en[] PROGMEM = "config. with PC"; |
//############################################################################ |
//----------------------------- |
// Setup_BTM222() (Bluetooth) |
//----------------------------- |
#define BT_INSTALLED 1 |
#define BT_NAME 2 |
#define BT_PIN 3 |
#define BT_INIT 4 |
#define BT_MAC 5 |
#define BT_REID 6 |
#define BT_PCCONFIG 7 |
#define BT_SHOWCONFIG 8 |
#define BT_INSTALLED_de WI_INSTALLED_de |
#define BT_INSTALLED_en WI_INSTALLED_en |
static const char BT_NAME_de[] PROGMEM = "BT Name"; |
static const char BT_NAME_en[] PROGMEM = "BT name"; |
static const char BT_PIN_de[] PROGMEM = "BT Pin"; |
static const char BT_PIN_en[] PROGMEM = "BT pin"; |
#define BT_INIT_de WI_INIT_de |
#define BT_INIT_en WI_INIT_en |
static const char BT_MAC_de[] PROGMEM = "zeige BT MAC"; |
static const char BT_MAC_en[] PROGMEM = "show BT MAC"; |
static const char BT_REID_de[] PROGMEM = "RE-ID Notiz"; |
#define BT_REID_en BT_REID_de |
#define BT_PCCONFIG_de WI_PCCONFIG_de |
#define BT_PCCONFIG_en WI_PCCONFIG_en |
static const char BT_SHOWCONFIG_de[] PROGMEM = "zeige BT Konfig"; |
static const char BT_SHOWCONFIG_en[] PROGMEM = "show BT config"; |
//############################################################################ |
//----------------------------- |
// Setup_BLE() (Bluetooth 4 Low Energy) |
//----------------------------- |
#define BLE_INSTALLED 1 |
// Text ist in messages.c: MODULE_EXIST |
//############################################################################ |
//----------------------------- |
// Setup_GPSMouse() |
//----------------------------- |
#define GPS_DEVICES 1 |
#define GPS_SEARCH 2 |
#define GPS_TYPE 3 |
#define GPS_ACTIVE 4 |
#define GPS_SHOWDEV 5 |
#define GPS_DATA 6 |
static const char GPS_SEARCH_de[] PROGMEM = "suche GPS-Maus"; |
static const char GPS_SEARCH_en[] PROGMEM = "search GPS-mouse"; |
static const char GPS_DEVICES_de[] PROGMEM = "Geräteliste"; |
static const char GPS_DEVICES_en[] PROGMEM = "devicelist"; |
static const char GPS_SHOWDEV_de[] PROGMEM = "aktuelle Maus"; |
static const char GPS_SHOWDEV_en[] PROGMEM = "act. GPS-mouse"; |
static const char GPS_DATA_de[] PROGMEM = "GPS-Maus Daten"; |
static const char GPS_DATA_en[] PROGMEM = "GPS-mouse data"; |
static const char GPS_TYPE_de[] PROGMEM = "GPS-Maus Typ"; // aktuell nicht verwendet |
static const char GPS_TYPE_en[] PROGMEM = "GPS-mouse typ"; |
static const char GPS_ACTIVE_de[] PROGMEM = "GPS-Maus aktiv"; // aktuell nicht verwendet |
static const char GPS_ACTIVE_en[] PROGMEM = "GPS-mouse activ"; |
//############################################################################ |
//----------------------------- |
// Setup_FollowMe() |
//----------------------------- |
#define FME_SPEED 1 |
#define FME_RADIUS 2 |
#define FME_OFFSET_LAT 3 |
#define FME_OFFSET_LONG 4 |
// FOLLOW_ME Step 2 |
static const char FME_OFFSET_LATITUDE_de[] PROGMEM = "Offset Lat"; |
static const char FME_OFFSET_LATITUDE_en[] PROGMEM = "offset lat"; |
static const char FME_OFFSET_LONGITUDE_de[] PROGMEM = "Offset Long"; |
static const char FME_OFFSET_LONGITUDE_en[] PROGMEM = "offset long"; |
static const char FME_SPEED_de[] PROGMEM = "Speed"; |
#define FME_SPEED_en FME_SPEED_de |
static const char FME_RADIUS_de[] PROGMEM = "Toleranz Radius"; |
static const char FME_RADIUS_en[] PROGMEM = "tolerance radius"; |
//############################################################################ |
//----------------------------- |
// Setup_OSD() |
//----------------------------- |
#define OSDS_SHOWCELLU 1 |
#define OSDS_LOWBAT 2 |
#define OSDS_RCERROR 3 |
#define OSDS_MKSETTING 4 |
#define OSDS_MAH 5 |
#define OSDS_DATASV2 6 |
#define OSDS_OUTFORMAT 7 |
#define OSDS_OUTPOLARITY 8 |
#define OSDS_FALLSPEED 9 |
#define OSDS_VARIOBEEP 10 |
#define OSDS_VOLTBAR 11 |
static const char OSDS_LOWBAT_de[] PROGMEM = "Warn Unterspannung"; |
static const char OSDS_LOWBAT_en[] PROGMEM = "Warn LowBat"; |
static const char OSDS_MAH_de[] PROGMEM = "Warn mAh"; |
static const char OSDS_MAH_en[] PROGMEM = "Warn mAh"; |
static const char OSDS_RCERROR_de[] PROGMEM = "Warn RC-Fehler"; |
static const char OSDS_RCERROR_en[] PROGMEM = "Warn RC-Error"; |
static const char OSDS_SHOWCELLU_de[] PROGMEM = "zeige Zellspannung"; |
static const char OSDS_SHOWCELLU_en[] PROGMEM = "show cell voltage"; |
static const char OSDS_MKSETTING_de[] PROGMEM = "zeige MK Setting"; |
static const char OSDS_MKSETTING_en[] PROGMEM = "show mk setting"; |
static const char OSDS_DATASV2_de[] PROGMEM = "Navi Daten an SV2"; |
static const char OSDS_DATASV2_en[] PROGMEM = "Navi data to SV2"; |
#ifdef USE_OSD_SCREEN_OLD |
static const char OSDS_OUTFORMAT_de[] PROGMEM = "OUT1/2 Format"; |
static const char OSDS_OUTFORMAT_en[] PROGMEM = "OUT1/2 format"; |
static const char OSDS_FALLSPEED_de[] PROGMEM = "Max. Sinkrate m/s"; |
static const char OSDS_FALLSPEED_en[] PROGMEM = "max fallspeed m/s"; |
static const char OSDS_VARIOBEEP_de[] PROGMEM = "Variometer Beep"; |
static const char OSDS_VARIOBEEP_en[] PROGMEM = "Variometer beep"; |
static const char OSDS_OUTPOLARITY_de[] PROGMEM = "OUT1/2 Polarität"; |
static const char OSDS_OUTPOLARITY_en[] PROGMEM = "OUT1/2 polarity"; |
static const char OSDS_VOLTBAR_de[] PROGMEM = "Volt Balken"; |
static const char OSDS_VOLTBAR_en[] PROGMEM = "volt bargraph"; |
#endif |
//############################################################################ |
//############################################################################ |
//# Allgemeines |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyOK( void ) |
{ |
set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(19, 7, strGet(OK) , MNORMAL); |
while( !get_key_press(1 << KEY_ENTER) ) PKT_CtrlHook(); // PKT-Update usw... |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyENDE( uint8_t beep ) |
{ |
if( beep ) set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
PKT_CtrlHook(); // PKT-Update usw... |
} |
clear_key_all(); |
} |
//############################################################################ |
//# BT-Funktionen (GPS-Maus) |
//############################################################################ |
#ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// BT_CheckBTM222() |
// |
// prueft ob das Bluetooth Modul im PKT installiert ist |
//-------------------------------------------------------------- |
uint8_t BT_CheckBTM222( void ) |
{ |
if( !Config.UseBT ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return false; // -> kein BTM222 eingebaut |
} |
return true; |
} |
//-------------------------------------------------------------- |
// BT_SelectDevice() |
// |
// Auswahl eines BT-Device aus einer Liste von BT-Devices |
//-------------------------------------------------------------- |
void BT_SelectDevice( void ) |
{ |
uint8_t itemid; |
uint8_t event; |
uint8_t i; |
//------------------------------- |
// BTM222 Modul vorhanden? |
//------------------------------- |
if( !BT_CheckBTM222() ) return; |
//------------------------------- |
// BT Device-Suche durchgefuert? |
//------------------------------- |
if( bt_devicecount == 0 ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Suche durchführen!"), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( strGet(STR_BT_DEVICES) ); // "BT Geräte" |
//--------------- |
// Menuitems |
//--------------- |
for( i = 0; i < bt_devicecount; i++ ) |
{ |
MenuCtrl_Push( i, MENU_ITEM, NOFUNC, device_list[i].DevName ); |
if( strncmp(device_list[i].mac,Config.gps_UsedMac,14) == 0 ) |
MenuCtrl_ItemMark( i, true ); // aktuelles BT-Device markieren |
} |
//--------------- |
// Control |
//--------------- |
event = MenuCtrl_Control( MENUCTRL_EVENT ); |
if( event == MENUEVENT_ITEM ) |
{ |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
strncpy( Config.gps_UsedMac , device_list[itemid].mac , 14); |
strncpy( Config.gps_UsedDevName, device_list[itemid].DevName, 20); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// BT_SearchDevices() |
// |
// sucht BT-Devices |
//-------------------------------------------------------------- |
void BT_SearchDevices( void ) |
{ |
uint8_t wahl; |
char *title; |
//------------------- |
// 1. Frage: suchen? |
//------------------- |
if( Config.gps_UsedDevName[0] != 0 ) // ist eine BT GPS-Maus bereits vorhanden? |
title = Config.gps_UsedDevName; // dann zeige im Titel den Namen der GPS-Maus an |
else |
title = "BT Devices"; |
set_beep( 50, 0xffff, BeepNormal); |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
wahl = PKT_AskX( ASK_END_START, strGet(STR_SEARCH_BT_ASK), strGet(STR_BT_SEARCHTIME), true, MenuCtrl_GetItemText(), true, title, false); // "BT Geräte suchen?" |
//------------------- |
// 2. BT suchen |
//------------------- |
if( wahl ) |
{ |
if( BT_CheckBTM222() ) |
{ |
//PKT_Title_P( text, lShowLipo, clearscreen ) |
PKT_Title_P( PSTR("BT Suche"), true, true ); |
lcdx_printp_center( 2, strGet(STR_SEARCH_BT), MNORMAL, 0,1); // "suche BT Geräte" |
//PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
bt_downlink_init(); |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
bt_searchDevice(); |
set_BTOff(); |
//PKT_Gauge_End(); // Gauge: Ende |
if( bt_devicecount==0 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NO_BT_FOUND), true, 2000, true, true); |
} |
else |
{ |
set_beep( 50, 0xffff, BeepNormal); |
BT_SelectDevice(); |
} |
} |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//############################################################################ |
//# Edit-Funktionen |
//############################################################################ |
//-------------------------------------------------------------- |
// Ausgabe von Werten fuer Edit_generic() |
//-------------------------------------------------------------- |
void Edit_generic_printout( int16_t Value, int16_t min, int16_t max, uint8_t what, int8_t yoffs) |
{ |
const char *pStr = 0; // PGM |
const char *pstr = 0; // RAM |
//uint8_t x = 0; |
//uint8_t y = 3; |
switch( what ) |
{ |
case Show_uint3: pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Show_uint5: pstr = buffered_sprintf_P( PSTR("%5d"), Value ); |
break; |
case Show_int3: pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Show_uint10th: pstr = buffered_sprintf_P( PSTR("%2.1d"), Value ); |
break; |
case MK_Connection: switch( Value ) |
{ |
case 0: pStr = PSTR("Wi.232"); break; |
case 1: pStr = strGet(KABEL); break; |
} |
if( (Value == 0) && (Config.UseWi == false) ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strGet(STR_WI232_ACTIVATE), MNORMAL, 0,-7); // "aktiviere Wi.232!" |
} |
else |
{ |
lcd_frect( 0, 37, 127, 21-6, 0); // Hilfebereich loeschen |
} |
break; |
case GPSMOUSE: switch( Value ) |
{ |
case GPS_Bluetoothmouse1: pStr = PSTR("BT-Mouse"); break; |
case GPS_Mikrokopter: pStr = PSTR("Mikrokopter"); break; |
default: pStr = PSTR("unknown"); |
} |
break; |
case Wi_Netmode: switch( Value ) |
{ |
case false: pStr = strGet(SLAVE); break; |
case true : pStr = strGet(NORMAL); break; |
} |
break; |
case OnOff: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = strGet(ON); break; |
} |
break; |
case YesNo: switch( Value ) |
{ |
case 0: pStr = strGet(NOO); break; |
case 1: pStr = strGet(YES); break; |
} |
break; |
case WlanMode: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = PSTR("AP-Mode"); break; |
case 2: pStr = PSTR("AdHoc"); break; |
} |
break; |
case NormRev: switch( Value ) // wird noch von stick/stick_setup.c verwendet |
{ |
case 0 : pStr = strGet(NORMAL); break; |
case 1 : pStr = strGet(REVERSE); break; |
} |
break; |
case Kontrast: if( Value >= max ) |
{ |
Value = max; |
set_beep( 200, 0x0080, BeepNormal); |
} |
if( Value == min ) |
{ |
Value = min; |
set_beep( 200, 0x0080, BeepNormal); |
} |
lcd_set_contrast( Value ); |
pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Baudrate: pstr = buffered_sprintf_P( PSTR("%ld"), Baud_to_uint32(Value) ); |
break; |
case Language: switch( Value ) |
{ |
case 0: pStr = strGet(DEUTSCH); break; |
case 1: pStr = strGet(ENGLISCH); break; |
} |
break; |
case Sticktype: switch( Value ) |
{ |
case false: pStr = strGet(POTI); break; |
case true : pStr = strGet(TASTER); break; |
} |
break; |
/* |
case WlanSecurity: // Anmerkung 02.04.2014 OG: wird aktuell nicht benoetigt weil anders geloest |
switch (Value) |
{ |
case 0x0 : lcd_printp_at(15, 2, PSTR("Adhoc"), 0); |
break; |
case 0x1 : lcd_printp_at(15, 2, PSTR("WPA "), 0); |
break; |
} |
break; |
*/ |
case PKT_Akku: switch( Value ) |
{ |
case true : pStr = PSTR("Lipo"); break; |
case false: pStr = PSTR("LiIon"); break; |
} |
break; |
} // end: switch( what ) |
//lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
if( pStr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16S"), pStr ); // PGM |
if( pstr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16s"), pstr ); // RAM |
} |
//-------------------------------------------------------------- |
// int16_t Edit_generic( Value, min, max, what, Addfactor, strHelp1, strHelp2) |
// |
// generische Funktion zum aendern von Setup-Werten |
// |
// PARAMETER: |
// Value: Ausgangswert der zu aendern ist |
// min : minimaler Wert |
// max : maximaler Wert |
// what : Typ des Wertes, abhaengig davon wird der Wert dargestellt, ENUM dafuer in setup.h |
// Addfactor : Sprungwert der beim erhöhen und erniedrigen addiert wird |
// strHelp1 : Hilefetext 1 (PGM) |
// strHelp2 : Hilefetext 2 (PGM) |
//S |
// RUECKGABE: |
// int16_t Return : Ergebnis der Veraenderung des Wertes |
//-------------------------------------------------------------- |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ) |
{ |
uint8_t redraw = true; |
uint8_t noCancel = false; |
int8_t yoffs = 0; // yoffs fuer das Edit-Label |
int8_t yoffsV = 4; // yoffs fuer den Eingabebereich |
const char *pStr; |
Pre = Value; |
do |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // Titel mit PKT-LiPo Anzeige und clearcsreen |
//------------------------ |
// Platz reservieren fuer |
// spezielle Typen |
//------------------------ |
if( what == MK_Connection ) |
{ |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 1 zeiliger Hilfstext |
//------------------------ |
if( strHelp1 != NULL && strHelp2 == NULL ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strHelp1, MNORMAL, 0,-7); // Hilfe-Text 1 |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 2 zeiliger Hilfstext |
//------------------------ |
if( strHelp2 != NULL ) |
{ |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_center( 5, strHelp1, MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_center( 6, strHelp2, MNORMAL, 0,-5); // Hilfe-Text 2 |
yoffs = -4; // yoffs fuer das Edit-Label |
yoffsV = -2; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// Label |
//------------------------ |
pStr = PSTR("???"); |
if( MenuCtrl_GetMenuIndex() >= 0 ) pStr = MenuCtrl_GetItemText(); // das Label wird vom Menueeintrag uebernommen (Menuetext muss im PGM sein!) |
else |
{ |
if( what == Language ) // Ausnahme: wird ggf. beim Start vom PKT von HAL_HW3_9.c aufgerufen |
{ |
pStr = strGetLanguage( LANGUAGE_de, LANGUAGE_en); |
noCancel = true; |
} |
} |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,yoffs, PSTR("%S:"), pStr ); // Ausgabe des Labels |
lcd_printp_at( 0, 7, strGet(KEYLINE5) , MNORMAL); // Keyline: <- -> Abbr. OK |
if( noCancel ) lcd_printp_at(11, 7, PSTR(" "), MNORMAL); // Keyline: "Abbr." loeschen wenn noCancel |
Edit_generic_printout( Value, min, max, what, yoffsV); // aktuellen Eingabewert anzeigen |
redraw = false; |
} |
//------------------------ |
// PKT CtrlHook |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Key: PLUS |
//------------------------ |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS), 2)) ) |
{ |
if( Value <= (max-Addfactor) ) Value = Value + Addfactor; |
else Value = min; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: MINUS |
//------------------------ |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS), 2)) ) |
{ |
if( Value >= (min + Addfactor) ) Value = Value - Addfactor; |
else Value = max; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: ENTER |
//------------------------ |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
return Value; |
} |
//------------------------ |
// Key: ESC |
//------------------------ |
if( !noCancel && get_key_press(1 << KEY_ESC) ) // Ende ohne speichern |
{ |
break; |
} |
} |
while( true ); |
get_key_press(KEY_ALL); |
return Pre; |
} |
//-------------------------------------------------------------- |
// bei HW 3.9x geht das nicht mehr wegen dem Platinenlayout |
// bei HW 3.9m geht es theoretisch noch - aber durch einen Bug scheinbar |
// nicht mehr. Kann nur von jemanden mit HW 3.9m repariert werden. |
// ALSO -> raus und Speicher sparen |
//-------------------------------------------------------------- |
/* |
uint8_t Edit_DisplayHelligkeit(uint8_t Value, uint8_t min, uint8_t max) |
{ |
float ValCorr = 2.55; // (Value * ValCorr) maximal 255 |
Pre = Value; |
OCR2A = Value * ValCorr; |
lcd_cls(); |
// lcd_printp_at (0, 2, Text, 0); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
write_ndigit_number_u (16, 2, Value, 3, 0,0); |
lcd_printp_at (17, 2, PSTR("%"), 0); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_2), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
write_ndigit_number_u (16, 2,Value, 3, 0,0); |
lcd_frect ((8*0), (8*4), (Value * (16*8) / 100), 6, 1); |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (Value < max)) |
{ |
Value++; |
if (Value >= max) |
{ |
Value = max; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (Value > min)) |
{ |
lcd_frect (((Value - 1) * (16*8) / 100), (8*4), (16*8), 6, 0); |
Value--; |
if (Value == min) |
{ |
Value = min; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
OCR2A = Value * ValCorr; |
Config.LCD_Helligkeit = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
OCR2A = Pre * ValCorr; |
Config.LCD_Helligkeit = Pre; |
// WriteParameter(); |
return Pre; |
} |
} |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type) |
{ |
uint8_t redraw = true; |
uint8_t x = 1; |
uint8_t I = 0; |
uint8_t lOk = false; |
uint8_t i; |
for( i = 0; i < length; i++) |
{ |
EditString[i] = data[i]; |
} |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
for( i = 0; i < length; i++) |
{ |
lcd_putc ( (i*2)+1, 3, EditString[i], MNORMAL); |
lcd_printp_at( (i*2)+2, 3, PSTR(" ") , MNORMAL); |
} |
lcd_rect( (x*6)-3, (8*3)-2, 10, 10, 1); |
lcd_printp_at(13, 6, PSTR("C"), MNORMAL); |
lcd_printp_at( 0, 7, PSTR(" \x17 \x16 \x19 OK"), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
if( (type == EDIT_BT_NAME) || (type == EDIT_SSID) || (type == EDIT_WL_PASSWORD)) // Name |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 2)) && EditString[I] < 'z') |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
if (EditString[I] > '9' && EditString[I] < 'A') |
EditString[I] = 'A'; |
if (EditString[I] > 'Z' && EditString[I] < 'a') |
EditString[I] = 'a'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 2)) && EditString[I] > ' ') |
{ |
EditString[I]--; |
if (EditString[I] < 'a' && EditString[I] > 'Z') |
EditString[I] = 'Z'; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
if (EditString[I] < '0' && EditString[I] > ' ') |
EditString[I] = ' '; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
else if( type == EDIT_BT_PIN ) // PIN |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (EditString[I] < '9')) |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (EditString[I] > '0')) |
{ |
EditString[I]--; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
if( get_key_long (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME|| type == EDIT_SSID || type == EDIT_WL_PASSWORD) EditString[I] = ' '; // Zeichen loeschen |
if( type == EDIT_BT_PIN) EditString[I] = '0'; // Zeichen setzen |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if( get_key_short (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME ) length_tmp = bt_name_length; |
if( type == EDIT_BT_PIN ) length_tmp = bt_pin_length; |
if (type == EDIT_SSID ) length_tmp = wlan_ssid_length; |
if (type == EDIT_WL_PASSWORD ) length_tmp = wlan_password_length; |
if ((x / 2) + 2 > length_tmp) |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
} |
else |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x++; |
x++; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I++; //Zeiger auf Zeichen |
} |
} |
if( get_key_press (1 << KEY_ENTER) ) |
{ |
lOk = true; |
if( type == EDIT_BT_NAME && EditString[0] == ' ' ) // BT-Name: 1. Zeichen darf nicht Space sein |
{ |
lcdx_printp_at( 0, 5, PSTR(" FEHLER! 1. Zeichen! "), MNORMAL, 0,-4); |
set_beep ( 300, 0xf00f, BeepNormal); |
_delay_ms(2000); |
lcd_frect( 0, 5*8-4, 127, 7, 0); // loesche Fehlertext |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); // setze Cursor auf Position 1 |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
get_key_press(KEY_ALL); |
lOk = false; |
} |
} |
} while( !lOk ); |
return 1; |
} |
//-------------------------------------------------------------- |
// min,max sind in Setup_PKTGeneric() festgelegt |
//-------------------------------------------------------------- |
void Edit_LipoOffset( void ) |
{ |
uint8_t redraw = true; |
uint8_t inc = 50; // in diesen Schritten hoch/runter zaehlen |
uint16_t min = 0; // min. Offset Wert |
uint16_t max = 15000; // max. Offset Wert |
uint16_t oldValue; |
if( Config.Lipo_UOffset % inc ) // ggf. wurde frueher ein anderer Wert fuer das Increment 'inc' verwendet |
Config.Lipo_UOffset = (uint16_t)(Config.Lipo_UOffset/inc)*inc; // hier wird der Wert auf das aktuelle 'inc' angepasst |
oldValue = Config.Lipo_UOffset; |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,-4, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_at( 1, 5, strGet(STR_HELP_LIPOOFFSET1), MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_at( 1, 6, strGet(STR_HELP_LIPOOFFSET2), MNORMAL, 0,-5); // Hilfe-Text 2 |
lcd_printp_at( 0, 7, strGet(KEYLINE5), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// Wert ausgeben |
//------------------------- |
lcdx_printf_at_P( 0, 3, MNORMAL, 0,-2, PSTR("> %5d => %1.2dv"), Config.Lipo_UOffset, volt_avg ); |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------- |
// Tasten |
//------------------------- |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2)) && (Config.Lipo_UOffset < max) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset + inc; |
} |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2)) && (Config.Lipo_UOffset > min) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset - inc; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return; |
} |
} while( !get_key_press(1 << KEY_ESC) ); |
clear_key_all(); |
Config.Lipo_UOffset = oldValue; // Abbruch, orginalen Wert wieder herstellen |
return; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_OLD |
uint8_t Edit_LED_Form (uint8_t Value, uint8_t min, uint8_t max ) |
{ |
Pre = Value; |
lcd_cls(); |
lcd_printp_at(0, 2, strGetOSDOLD(OSD_LED_Form), MNORMAL); |
switch (Value) |
{ |
case 0x1: |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
break; |
case 0x3 : |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
break; |
break; |
} |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
if ((get_key_press (1 << KEY_PLUS)) && (Value == 1)) |
{ |
Value = 3; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
} |
if ((get_key_press (1 << KEY_MINUS)) && (Value == 3)) |
{ |
Value = 1; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
edit = 1; |
Config.OSD_LEDform = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
edit = 0; |
Config.OSD_LEDform = Pre; |
return Pre; |
} |
} |
#endif // USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// Show_MAC( progmem, headline, mac) |
//-------------------------------------------------------------- |
void Show_MAC( uint8_t progmem, const char *headline, const char *mac ) |
{ |
uint8_t i; |
uint8_t z; |
lcd_cls(); |
PKT_TitleFromMenuItem( true ); |
if( progmem ) |
lcdx_printp_center( 2, headline, MNORMAL, 0,2); |
else |
lcdx_print_center( 2, (uint8_t *)headline, MNORMAL, 0,2); |
z = 0; |
for( i = 0; i < 13; i++) |
{ |
lcdx_putc( z+2, 5, mac[i], MNORMAL, 0,-1); |
if( (i%2==1) && (i<11) ) |
{ |
z++; |
lcdx_printp_at( z+2, 5, PSTR(":"), MNORMAL, 0,-1); |
} |
z++; |
} |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
Wait4KeyENDE( false ); |
} |
//-------------------------------------------------------------- |
// Setup_OSD() |
//-------------------------------------------------------------- |
void Setup_OSD( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Anzeige" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( OSDS_LOWBAT , MENU_ITEM, NOFUNC , OSDS_LOWBAT_de , OSDS_LOWBAT_en ); |
//MenuCtrl_PushML2_P( OSDS_MAH , MENU_ITEM, NOFUNC , OSDS_MAH_de , OSDS_MAH_en ); |
MenuCtrl_PushML2_P( OSDS_RCERROR , MENU_ITEM, NOFUNC , OSDS_RCERROR_de , OSDS_RCERROR_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_SHOWCELLU , MENU_ITEM, NOFUNC , OSDS_SHOWCELLU_de , OSDS_SHOWCELLU_en ); |
MenuCtrl_PushML2_P( OSDS_MKSETTING , MENU_ITEM, NOFUNC , OSDS_MKSETTING_de , OSDS_MKSETTING_en ); |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_FALLSPEED , MENU_ITEM, NOFUNC , OSDS_FALLSPEED_de , OSDS_FALLSPEED_en ); |
MenuCtrl_PushML2_P( OSDS_VARIOBEEP , MENU_ITEM, NOFUNC , OSDS_VARIOBEEP_de , OSDS_VARIOBEEP_en ); |
MenuCtrl_PushML2_P( OSDS_VOLTBAR , MENU_ITEM, NOFUNC , OSDS_VOLTBAR_de , OSDS_VOLTBAR_en ); |
MenuCtrl_PushML2_P( OSDS_OUTFORMAT , MENU_ITEM, NOFUNC , OSDS_OUTFORMAT_de , OSDS_OUTFORMAT_en ); |
MenuCtrl_PushML2_P( OSDS_OUTPOLARITY , MENU_ITEM, NOFUNC , OSDS_OUTPOLARITY_de , OSDS_OUTPOLARITY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_DATASV2 , MENU_ITEM, NOFUNC , OSDS_DATASV2_de , OSDS_DATASV2_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == OSDS_LOWBAT ) { Config.MK_LowBat = Edit_generic( Config.MK_LowBat ,32,255, Show_uint10th,1 ,strGet(STR_HELP_LOWBAT1),strGet(STR_HELP_LOWBAT2)); } |
if( itemid == OSDS_SHOWCELLU ) { Config.OSD_ShowCellU = Edit_generic( Config.OSD_ShowCellU , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_MAH ) { Config.OSD_mAh_Warning = Edit_generic( Config.OSD_mAh_Warning , 0,30000, Show_uint5,100 ,NULL,NULL); } |
if( itemid == OSDS_RCERROR ) { Config.OSD_RCErrorbeep = Edit_generic( Config.OSD_RCErrorbeep , 0, 1, OnOff,1 ,NULL,NULL); } |
if( itemid == OSDS_MKSETTING ) { Config.OSD_ShowMKSetting = Edit_generic( Config.OSD_ShowMKSetting, 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_DATASV2 ) { Config.OSD_SendOSD = Edit_generic( Config.OSD_SendOSD , 0, 1, YesNo,1 ,NULL,NULL); } |
#ifdef USE_OSD_SCREEN_OLD |
if( itemid == OSDS_FALLSPEED ) { Config.OSD_Fallspeed = Edit_generic( Config.OSD_Fallspeed , 0,247, Show_uint10th,1 ,NULL,NULL); } |
if( itemid == OSDS_VARIOBEEP ) { Config.OSD_VarioBeep = Edit_generic( Config.OSD_VarioBeep , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_OUTFORMAT ) { Config.OSD_LEDform = Edit_LED_Form(Config.OSD_LEDform , 1, 3 ); } |
if( itemid == OSDS_OUTPOLARITY ) { Config.OSD_InvertOut = Edit_generic( Config.OSD_InvertOut , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_VOLTBAR ) { Config.OSD_LipoBar = Edit_generic( Config.OSD_LipoBar , 0, 1, YesNo,1 ,NULL,NULL); } |
#endif |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Mark_OSDScreen( uint8_t osdscreenid ) |
{ |
MenuCtrl_ItemMarkR( osdscreenid, (Config.OSD_UseScreen & (1 << osdscreenid)) != 0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Edit_OSDScreen( uint8_t osdscreenid ) |
{ |
if( (Config.OSD_UseScreen & (1 << osdscreenid)) == 0 ) |
Config.OSD_UseScreen = (Config.OSD_UseScreen | (1 << osdscreenid)); |
else |
Config.OSD_UseScreen = (Config.OSD_UseScreen ^ (1 << osdscreenid)); |
Mark_OSDScreen( osdscreenid ); |
} |
//-------------------------------------------------------------- |
// Setup_OSDScreens() |
//-------------------------------------------------------------- |
void Setup_OSDScreens( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Screens" |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
MenuCtrl_Push_P( OSDSCREEN_NAVIGATION, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_NAVIGATION) ); Mark_OSDScreen( OSDSCREEN_NAVIGATION ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
MenuCtrl_Push_P( OSDSCREEN_WAYPOINTS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_WAYPOINTS) ); Mark_OSDScreen( OSDSCREEN_WAYPOINTS ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC_N |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
MenuCtrl_Push_P( OSDSCREEN_MKSTATUS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_MKSTATUS) ); Mark_OSDScreen( OSDSCREEN_MKSTATUS ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
MenuCtrl_Push_P( OSDSCREEN_USERGPS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_USERGPS) ); Mark_OSDScreen( OSDSCREEN_USERGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
MenuCtrl_Push_P( OSDSCREEN_3DLAGE , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_3DLAGE) ); Mark_OSDScreen( OSDSCREEN_3DLAGE ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
MenuCtrl_Push_P( OSDSCREEN_STATISTICS, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_STATISTIC) ); Mark_OSDScreen( OSDSCREEN_STATISTICS ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_Push_P( OSDSCREEN_OSD0, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD0) ); Mark_OSDScreen( OSDSCREEN_OSD0 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD1, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD1) ); Mark_OSDScreen( OSDSCREEN_OSD1 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD2, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD2) ); Mark_OSDScreen( OSDSCREEN_OSD2 ); |
#endif |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
Edit_OSDScreen( itemid ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_FollowMe() |
//-------------------------------------------------------------- |
#ifdef USE_FOLLOWME |
void Setup_FollowMe( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( PSTR("FollowMe Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nicht verwendet. FollowMe wird aktuell ueber die Daten-Updaterate |
// der BT GPS-Maus getriggert! |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( FME_REFRESH , MENU_ITEM, NOFUNC , FME_REFRESH_de , FME_REFRESH_en ); |
MenuCtrl_PushML2_P( FME_OFFSET_LAT , MENU_ITEM, NOFUNC, FME_OFFSET_LATITUDE_de , FME_OFFSET_LATITUDE_en ); |
MenuCtrl_PushML2_P( FME_OFFSET_LONG, MENU_ITEM, NOFUNC, FME_OFFSET_LONGITUDE_de, FME_OFFSET_LONGITUDE_en ); |
MenuCtrl_PushML2_P( FME_SPEED , MENU_ITEM, NOFUNC, FME_SPEED_de , FME_SPEED_en ); |
MenuCtrl_PushML2_P( FME_RADIUS , MENU_ITEM, NOFUNC, FME_RADIUS_de , FME_RADIUS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC, GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//if( itemid == FME_REFRESH ) { Config.FM_Refresh = Edit_generic( Config.FM_Refresh ,250,60000, Show_uint3,1 ,NULL,NULL); } |
// FollowMeStep2: Eingabe in m, intern als mm behandeln |
if( itemid == FME_OFFSET_LAT ) { Config.FM_Offest_Latitude = Edit_generic( Config.FM_Offest_Latitude / 1000, -30, 30, Show_int3, 1, strGet(STR_METERS), NULL) * 1000; } |
if( itemid == FME_OFFSET_LONG) { Config.FM_Offset_Longitude = Edit_generic( Config.FM_Offset_Longitude / 1000, -30, 30, Show_int3, 1, strGet(STR_METERS), NULL) * 1000; } |
// FollowMe: |
if( itemid == FME_SPEED ) { Config.FM_Speed = Edit_generic( Config.FM_Speed , 0, 100, Show_uint3, 1, PSTR("0.1 m/s") , NULL); } |
if( itemid == FME_RADIUS ) { Config.FM_Radius = Edit_generic( Config.FM_Radius, 1, 20, Show_uint3, 1, strGet(STR_METERS), NULL); } |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif // USE_FOLLOWME |
//-------------------------------------------------------------- |
// Setup_GPSMouse() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_GPSMouse( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "GPS Maus" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( GPS_SEARCH , MENU_ITEM, &BT_SearchDevices , GPS_SEARCH_de , GPS_SEARCH_en ); |
MenuCtrl_PushML2_P( GPS_DEVICES , MENU_ITEM, &BT_SelectDevice , GPS_DEVICES_de , GPS_DEVICES_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil erstmal nicht weiter benoetigt. Wurde zwar verwendet in tracking.c |
// aber auch dort wurde betroffener Code deaktiviert - siehe Anmerkung tracking.c / PKT_tracking() |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_TYPE , MENU_ITEM, NOFUNC , GPS_TYPE_de , GPS_TYPE_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nirgendwo verwendet |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_ACTIVE , MENU_ITEM, NOFUNC , GPS_ACTIVE_de , GPS_ACTIVE_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_SHOWDEV , MENU_ITEM, NOFUNC , GPS_SHOWDEV_de , GPS_SHOWDEV_en ); |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC , GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// GPS_TYPE |
//-------------------- |
/* |
if( itemid == GPS_TYPE ) |
{ |
Config.gps_UsedGPSMouse = Edit_generic( Config.gps_UsedGPSMouse, 0,1,GPSMOUSE,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_ACTIVE |
//-------------------- |
/* |
if( itemid == GPS_ACTIVE ) |
{ |
Config.gps_UseGPS = Edit_generic( Config.gps_UseGPS, 0,1,YesNo,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_SHOWDEV |
//-------------------- |
if( itemid == GPS_SHOWDEV ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( false, Config.gps_UsedDevName, Config.gps_UsedMac); |
} |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// zeigt die Konfig des BTM-222 |
//-------------------------------------------------------------- |
void BTM222_Info( void ) |
{ |
lcd_cls(); |
set_Modul_On( Bluetooth ); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MNORMAL, PSTR("BTM-222 Config") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
bt_showsettings(); |
ScrollBox_Push_P( MNORMAL, PSTR("End of Config") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Setup_BTM222_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob BTM222 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_BTM222_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( BT_INSTALLED , MENU_ITEM, NOFUNC , BT_INSTALLED_de , BT_INSTALLED_en ); |
if( Config.UseBT ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_NAME , MENU_ITEM, NOFUNC , BT_NAME_de , BT_NAME_en ); |
MenuCtrl_PushML2_P( BT_PIN , MENU_ITEM, NOFUNC , BT_PIN_de , BT_PIN_en ); |
MenuCtrl_PushML2_P( BT_REID , MENU_ITEM, NOFUNC , BT_REID_de , BT_REID_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_INIT , MENU_ITEM, NOFUNC , BT_INIT_de , BT_INIT_en ); |
MenuCtrl_PushML2_P( BT_PCCONFIG , MENU_ITEM, &Port_FC2CFG_BT , BT_PCCONFIG_de , BT_PCCONFIG_en ); |
MenuCtrl_PushML2_P( BT_SHOWCONFIG, MENU_ITEM, &BTM222_Info , BT_SHOWCONFIG_de, BT_SHOWCONFIG_en); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_MAC , MENU_ITEM, NOFUNC , BT_MAC_de , BT_MAC_en ); |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BTM222() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_BTM222( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT; |
uint8_t i; |
char string[11]; |
Setup_BTM222_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
UseBT = Config.UseBT; |
Config.UseBT = Edit_generic( Config.UseBT ,0,1, YesNo,1 ,NULL,NULL); |
if( UseBT != Config.UseBT ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseBT ) // BTM222 wurde aktiviert - initialisieren |
{ |
BTM222_Initalize(); |
//if( bt_init() ) |
// Config.BTIsSet = true; |
//else |
// Config.BTIsSet = false; |
} |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_BTM222_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// BT_NAME |
//-------------------- |
if( itemid == BT_NAME ) |
{ |
bt_fixname(); |
strncpyfill( string, Config.bt_name, bt_name_length+1 ); // bt_name_length=10 + 1 Terminierende 0 |
Edit_String( string, bt_name_length, EDIT_BT_NAME ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
//lcd_print_at( 0, 5, EditString, 0); // DEBUG |
//lcdx_printf_at( 17, 5, MNORMAL, 0,0, "%3d", strlen(EditString) ); |
//_delay_ms(8000); |
strncpy( Config.bt_name, EditString, bt_name_length+1 ); |
} |
} |
//-------------------- |
// BT_PIN |
//-------------------- |
if( itemid == BT_PIN ) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
string[i] = Config.bt_pin[i]; |
} |
string[bt_pin_length] = 0; |
Edit_String( string, bt_pin_length, EDIT_BT_PIN ); |
if (edit == 1) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
Config.bt_pin[i] = EditString[i]; |
} |
} |
} |
//-------------------- |
// BT_INIT |
//-------------------- |
if( itemid == BT_INIT ) |
{ |
//lcd_cls (); |
//Old_Baudrate = Config.PKT_Baudrate; |
BTM222_Initalize(); |
/* |
if( bt_init() == true ) |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init ok"), MNORMAL); |
WriteBTInitFlag(); |
} |
else |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init Error"), MNORMAL); |
Config.BTIsSet = false; |
set_beep( 1000, 0x0040, BeepNormal); |
} |
_delay_ms(2500); // 2.5 Sekunden anzeigen fuer Benutzer |
*/ |
} |
//-------------------- |
// BT_MAC |
//-------------------- |
if( itemid == BT_MAC ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( true, PSTR("BTM-222 MAC"), Config.bt_Mac); |
} |
//-------------------- |
// BT_REID |
//-------------------- |
if( itemid == BT_REID ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
string[i] = Config.RE_ID[i]; |
} |
string[RE_ID_length] = 0; |
Edit_String( string, RE_ID_length, EDIT_BT_PIN ); |
if( edit == 1 ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
Config.RE_ID[i] = EditString[i]; |
} |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BLE() |
//-------------------------------------------------------------- |
void Setup_BLE( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( BLE_INSTALLED, MENU_ITEM, NOFUNC, strGet(MODULE_EXIST) ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
Config.UseBLE = Edit_generic( Config.UseBLE, 0, 1, YesNo, 1 ,strGet(STR_EXTSV2MODULE),NULL); |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_MKConnection() |
//-------------------------------------------------------------- |
void Setup_MKConnection( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MKCONN_CONNECTION , MENU_ITEM, NOFUNC , MKCONN_CONNECTION_de , MKCONN_CONNECTION_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( MKCONN_BAUDRATE , MENU_ITEM, NOFUNC , MKCONN_BAUDRATE_de , MKCONN_BAUDRATE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// MKCONN_CONNECTION |
//-------------------- |
if( itemid == MKCONN_CONNECTION ) |
{ |
old = Config.U02SV2; |
Config.U02SV2 = Edit_generic( Config.U02SV2, 0, 1, MK_Connection,1, NULL,NULL); |
if( Config.U02SV2 != old ) // Verbindung geaendert? |
{ |
if( Config.U02SV2 == 1 ) Change_Output( Uart02FC ); // 1 = Kabel |
else Change_Output( Uart02Wi ); // 0 = Wi.232 |
} |
if( (Config.U02SV2 == 0) && (Config.UseWi == false) ) // FEHLER: Wi.232 nicht aktiviert (Config.U02SV2 = 0 -> entspricht Wi.232) |
{ |
//PKT_Message_P( text , error, timeout, beep, clear) |
PKT_Message_P( strGet(STR_WI232_ACTIVATE), true , 3000 , true, true); |
} |
} |
//-------------------- |
// MKCONN_BAUDRATE |
//-------------------- |
if( itemid == MKCONN_BAUDRATE ) |
{ |
old = Config.PKT_Baudrate; |
Config.PKT_Baudrate = Edit_generic( Config.PKT_Baudrate ,1,6,Baudrate,1 ,PSTR("Standard: ! 57600 !"),NULL); |
if( Config.PKT_Baudrate != old ) |
{ |
// Wi.232 umkonfigurieren |
if( Config.UseWi ) |
{ |
Wi232_Initalize(); |
} |
// BTM222 umkonfigurieren |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
{ |
//set_BTOn(); |
BTM222_Initalize(); |
//set_BTOff(); |
} |
#endif // end: #ifdef USE_BLUETOOTH |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_WI232_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob Wi.232 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WI232_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Wi.232" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( WI_INSTALLED , MENU_ITEM, NOFUNC , strGet(CONNECT13) ); |
if( Config.UseWi ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_TXRX , MENU_ITEM, NOFUNC , WI_TXRX_de , WI_TXRX_en ); |
MenuCtrl_PushML2_P( WI_GROUP , MENU_ITEM, NOFUNC , WI_GROUP_de , WI_GROUP_en ); |
// 06.06.2014 OG |
//MenuCtrl_PushML2_P( WI_MODE , MENU_ITEM, NOFUNC , WI_MODE_de , WI_MODE_en ); |
//MenuCtrl_PushML2_P( WI_TIMEOUT , MENU_ITEM, NOFUNC , WI_TIMEOUT_de , WI_TIMEOUT_en ); |
//MenuCtrl_PushML2_P( WI_MTU , MENU_ITEM, NOFUNC , WI_MTU_de , WI_MTU_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_INIT , MENU_ITEM, NOFUNC , WI_INIT_de , WI_INIT_en ); |
//MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Port_USB2CFG_Wi , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Wi232_ConfigPC , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
} |
} |
//-------------------------------------------------------------- |
// Setup_WI232() |
//-------------------------------------------------------------- |
void Setup_WI232( void ) |
{ |
uint8_t itemid; |
uint8_t UseWi; |
Setup_WI232_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
if( itemid == WI_TXRX ) { Config.WiTXRXChannel = Edit_generic( Config.WiTXRXChannel , 0, 5, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_GROUP ) { Config.WiNetworkGroup = Edit_generic( Config.WiNetworkGroup , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MODE ) { Config.WiNetworkMode = Edit_generic( Config.WiNetworkMode , 0, 1, Wi_Netmode,1 ,NULL,NULL); } |
//if( itemid == WI_TIMEOUT ) { Config.WiTXTO = Edit_generic( Config.WiTXTO , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MTU ) { Config.WiUartMTU = Edit_generic( Config.WiUartMTU , 0, 127, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_INIT ) { Wi232_Initalize(); } |
//-------------------- |
// WI_INSTALLED |
//-------------------- |
if( itemid == WI_INSTALLED ) |
{ |
UseWi = Config.UseWi; |
Config.UseWi = Edit_generic( Config.UseWi, 0,1, YesNo,1, NULL,NULL); |
if( UseWi != Config.UseWi ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseWi ) |
Wi232_Initalize(); // Wi232 wurde aktiviert: init |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_WI232_MenuCreate(); |
continue; |
} |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_PKTGeneric() |
// |
// entspricht "PKT allgemein" |
//-------------------------------------------------------------- |
void Setup_PKTGeneric( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT allgemein" |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( PKT_LANGUAGE , MENU_ITEM, NOFUNC , LANGUAGE_de , LANGUAGE_en ); |
MenuCtrl_PushML2_P( TIME_ZONE , MENU_ITEM, NOFUNC , TIME_ZONE_de , TIME_ZONE_en ); |
MenuCtrl_PushML2_P( TIME_SUMMER , MENU_ITEM, NOFUNC , TIME_SUMMER_de , TIME_SUMMER_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( PKT_PKTOFF , MENU_ITEM, NOFUNC , PKT_PKTOFF_de , PKT_PKTOFF_en ); |
MenuCtrl_PushML2_P( PKT_LIGHTOFF , MENU_ITEM, NOFUNC , LIGHTOFF_de , LIGHTOFF_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
// 18.04.2014 OG: rausgenommen da es aktuell auch nicht mehr mit PKT 3.9m Hardware geht |
//if( PCB_Version == PKT39m ) // Helligkeit nur bei PKT39m - bei der PKT39x geht das nicht mehr |
// MenuCtrl_PushML2_P( PKT_BRIGHTNESS , MENU_ITEM, NOFUNC , BRIGHTNESS_de , BRIGHTNESS_en ); |
MenuCtrl_PushML2_P( PKT_CONTRAST , MENU_ITEM, NOFUNC , CONTRAST_de , CONTRAST_en ); |
MenuCtrl_PushML2_P( PKT_BEEPER , MENU_ITEM, NOFUNC , BEEPER_de , BEEPER_en ); |
#ifdef USE_SOUND |
MenuCtrl_PushML2_P( PKT_SOUNDMODUL , MENU_ITEM, NOFUNC , SOUNDMODUL_de , SOUNDMODUL_en ); |
MenuCtrl_PushML2_P( PKT_VOLUME , MENU_ITEM, NOFUNC , VOLUME_de , VOLUME_en ); |
#endif |
MenuCtrl_PushML2_P( PKT_ACCUTYPE , MENU_ITEM, NOFUNC , ACCUTYPE_de , ACCUTYPE_en ); |
MenuCtrl_PushML2_P( PKT_ACCUMEASURE , MENU_ITEM, NOFUNC , ACCUMEASURE_de , ACCUMEASURE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == PKT_LANGUAGE ) |
{ |
old = Config.DisplayLanguage; |
Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage , 0, 1, Language,1, NULL,NULL); |
if( old != Config.DisplayLanguage ) |
{ |
PKT_Ask_Restart( strGetLanguage(LANGUAGE_de,LANGUAGE_en) ); |
} |
} |
if( itemid == TIME_ZONE ) Config.timezone = Edit_generic( Config.timezone ,-12, 12, Show_uint3,1, PSTR("+1 = Berlin "),PSTR("-5 = New York")); |
if( itemid == TIME_SUMMER ) Config.summertime = Edit_generic( Config.summertime , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_LIGHTOFF ) Config.DisplayTimeout = Edit_generic( Config.DisplayTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_PKTOFF ) Config.PKTOffTimeout = Edit_generic( Config.PKTOffTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_BEEPER ) Config.HWBeeper = Edit_generic( Config.HWBeeper , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_SOUNDMODUL ) Config.HWSound = Edit_generic( Config.HWSound , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_VOLUME ) Config.Volume = Edit_generic( Config.Volume , 0, 50, Show_uint3,1, NULL,NULL); |
if( itemid == PKT_CONTRAST ) { Config.LCD_Kontrast = Edit_generic( Config.LCD_Kontrast , 10, 40, Kontrast,1, NULL,NULL); |
lcd_set_contrast( Config.LCD_Kontrast ); |
} |
//if( itemid == PKT_BRIGHTNESS ) Config.LCD_Helligkeit = Edit_DisplayHelligkeit(Config.LCD_Helligkeit,0,100); |
if( itemid == PKT_ACCUTYPE ) Config.PKT_Accutyp = Edit_generic( Config.PKT_Accutyp , 0, 1, PKT_Akku,1, NULL,NULL); |
if( itemid == PKT_ACCUMEASURE ) Edit_LipoOffset(); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Main() |
// |
// Das Hauptmenue fuer das gesamte Setup |
//-------------------------------------------------------------- |
void Setup_MAIN( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Setup" |
//MenuCtrl_SetTitle_P( PSTR("PKT Setup") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( SETUP_PKTCONFIG , MENU_SUB , &Setup_PKTGeneric , SETUP_PKTCONFIG_de , SETUP_PKTCONFIG_en ); |
MenuCtrl_PushML2_P( SETUP_OSDVIEW , MENU_SUB , &Setup_OSD , SETUP_OSDVIEW_de , SETUP_OSDVIEW_en ); |
MenuCtrl_PushML2_P( SETUP_OSDSCREENS , MENU_SUB , &Setup_OSDScreens , SETUP_OSDSCREENS_de , SETUP_OSDSCREENS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_MKCONNECTION , MENU_SUB , &Setup_MKConnection , SETUP_MKCONNECTION_de , SETUP_MKCONNECTION_en ); |
MenuCtrl_PushML2_P( SETUP_WI232 , MENU_SUB , &Setup_WI232 , SETUP_WI232_de , SETUP_WI232_en ); |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_BTM222 , MENU_SUB , &Setup_BTM222 , SETUP_BTM222_de , SETUP_BTM222_en ); |
#endif |
#if (defined(USE_SV2MODULE_BLE) || defined(USE_WLAN)) |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_BLE , MENU_SUB , &Setup_BLE , SETUP_BLE_de , SETUP_BLE_en ); |
#endif |
#ifdef USE_WLAN |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_WIFLY , MENU_SUB , &Setup_WiFly , SETUP_WIFLY_de , SETUP_WIFLY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_GPSMAUS , MENU_SUB , &Setup_GPSMouse , SETUP_GPSMAUS_de , SETUP_GPSMAUS_en ); |
#endif |
#ifdef USE_FOLLOWME |
MenuCtrl_PushML2_P( SETUP_FOLLOWME , MENU_SUB , &Setup_FollowMe , SETUP_FOLLOWME_de , SETUP_FOLLOWME_en ); |
#endif |
#ifdef USE_TRACKING |
MenuCtrl_PushML2_P( SETUP_SERVOCONFIG , MENU_SUB , &Setup_ServoTracking, SETUP_SERVOCONFIG_de , SETUP_SERVOCONFIG_en ); // tracking/servo_setup.c |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( SETUP_JOYSTICK , MENU_SUB , &Setup_Joysticks , SETUP_JOYSTICK_de , SETUP_JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_PKTUPDATE , MENU_ITEM, &PKT_Update , SETUP_PKTUPDATE_de , SETUP_PKTUPDATE_en ); |
//MenuCtrl_PushML2_P( SETUP_PKTDEBUG , MENU_ITEM, NOFUNC , SETUP_PKTDEBUG_de , SETUP_PKTDEBUG_en ); |
MenuCtrl_PushML2_P( SETUP_EEPROMRESET , MENU_ITEM, &PKT_Reset_EEprom , SETUP_EEPROMRESET_de , SETUP_EEPROMRESET_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// SETUP_PKTDEBUG |
//-------------------- |
if( itemid == SETUP_PKTDEBUG ) |
{ |
Config.Debug = Edit_generic( Config.Debug, 0,1,Show_uint3,1 ,NULL,NULL); |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/setup/setup.h |
---|
0,0 → 1,111 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.h |
//# |
//# 26.06.2014 OG |
//# - add: Setup_FollowMe() |
//# |
//# 22.06.2014 OG |
//# - del: Variable CheckGPS |
//# |
//# 13.06.2014 OG |
//# - del: ChangeWi_SV2() |
//# |
//# 11.06.2014 OG |
//# - del: Edit_Language() |
//# |
//# 31.04.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt |
//# |
//# 28.05.2014 OG |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# - Parameter 'Text' enfernt da vom Menue geerbt |
//# - Parameter fuer Hilfetexte |
//# |
//# 04.04.2014 OG |
//# - del: Edit_ShowTitle_P() |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: WlanMode |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um WlanSecurity erweitert |
//# |
//# 22.05.2013 OG |
//# - del: verschiedene Cleanups |
//############################################################################ |
#ifndef _setup_H |
#define _setup_H |
#define EDIT_BT_NAME 1 |
#define EDIT_BT_PIN 2 |
#define EDIT_SSID 3 |
#define EDIT_WL_PASSWORD 4 |
typedef enum // Varianten fuer die Anzeige der Werte im Setupmenüe |
{ |
Show_uint3, Show_int3, Show_uint5, Show_uint10th, GPSMOUSE, Wi_Netmode, |
OnOff, YesNo, Orientation, NormRev, Kontrast, Baudrate, Language, Sticktype, WlanMode, WlanSecurity, PKT_Akku, MK_Connection |
} EditMode; |
void Setup_MAIN(void); |
void Setup_FollowMe( void ); |
void Display_Setup (void); |
void Wi_Use (void); |
void BT_Use (void); |
void BT_SelectDevice (void); |
uint8_t BT_CheckBTM222( void ); |
void Wait4KeyOK( void ); |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ); |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type); |
uint8_t BT_ShowGPSData( uint8_t modus ); |
void BTM222_Info( void ); |
extern uint8_t bt_name_len; |
extern uint8_t length_tmp; |
extern uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
extern uint8_t edit; |
extern char EditString[21]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/setup |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/sound/pwmsine8bit.c |
---|
0,0 → 1,280 |
//file pwmsine8bit.c |
//generate 8 bit sinewave using wavetable lookup |
//12khz sample rate, 62500 hz pwm on pd5 oc1a |
//for ere co embmega32 16Mhz 38400 |
//May 3 07 Bob G initial edit 8 bit |
//May 5 07 Bob G add traffic tones |
//Jan 18 08 Bob G compile with 7.15 |
//#include <stdio.h> |
//#include <stdlib.h> |
#include <ctype.h> |
#include <math.h> //sin |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdint.h> |
#include "../timer/timer.h" |
#include "../main.h" |
#ifdef USE_SOUND |
#define INTR_OFF() asm("CLI") |
#define INTR_ON() asm("SEI") |
//------vars in bss------- |
volatile unsigned char sindat[256]; //8 bit sine table |
//unsigned int addat[8]; //internal 10 bit |
unsigned int idesfreq; |
unsigned char iattenfac; //0-255 |
unsigned char dispon; |
unsigned char freqincipart,freqincfpart; |
unsigned char waveptipart,waveptfpart; |
unsigned char tics; |
unsigned char generate; |
float basefreq; |
float freqratio; |
float sampfreq; //12khz |
float samppd; //83.33usec |
float fdesfreq; |
float attendb; //0-50 |
float attenfac; //.999-0 |
const float pi=3.141592654; |
#define MSB(w) ((char*) &w)[1] |
#define LSB(w) ((char*) &w)[0] |
#define TONE_BUFFER_SIZE 64 |
#define TONE_BUFFER_MASK ( TONE_BUFFER_SIZE - 1) |
static volatile unsigned char ToneBuf[TONE_BUFFER_SIZE]; |
static volatile unsigned char ToneBufHead; |
static volatile unsigned char ToneBufTail; |
//------------------------------- |
void cvtfreq2frac(void){ |
//calc freq ratio, separate into integer and fractional parts |
fdesfreq=idesfreq; |
freqratio=fdesfreq/basefreq; //calc freq ratio from basefreq |
freqincipart=(int)freqratio; //calc integer part |
freqincfpart=(freqratio-freqincipart)*256; //calc fractional part |
// cprintf("ipart %02x fpart %02x\n",freqincipart,freqincfpart); |
} |
//------------------------------- |
void cvtdb2fac(void){ |
//cvt db to attenuation factor |
attenfac=pow(10.0,-attendb/20.0); //10^0=1 |
iattenfac=(unsigned char)(attenfac*255.0); |
// cprintf("attenfac %#7.5f\n",attenfac); |
// cprintf("iattenfac %02x\n",iattenfac); |
} |
//------------------------- |
void initsindat(void){ |
//init 8 bit signed sin table 0-ff in ram 36dB s/n 46Hz basefreq |
unsigned int n; |
for(n=0; n < 256; n++){ |
sindat[n]=(signed char)127.0*sin(2.0*pi*n/256.0); //256 sin points |
} |
} |
//------------------------- |
void initvars(void){ |
//init vars |
// sampfreq=12000.0; //hz |
sampfreq=19530.0; //hz |
// printf("samp freq %#7.1f hz\n\r",sampfreq); |
// printf("samp freq %#7.1f hz\n\r",1234567.1); |
samppd=1.0/sampfreq; //83.33usec |
// printf("samppd %#7.1f usec \n\r",samppd*1e6); |
basefreq=sampfreq/256.0; //46.875hz |
// printf("basefreq %#7.5f hz\n\r",basefreq); |
idesfreq=400; |
cvtfreq2frac(); |
cvtdb2fac(); //0db =.9999= 0xff |
} |
//--------------------- |
void tone1(int hz) |
//tone at hz for ms |
{ |
uint8_t volume = 0; |
// TCNT2=0; |
// sound_timer = ms/5; |
// soundpause_timer = soundpause/5; |
idesfreq=hz; |
cvtfreq2frac(); |
attendb=volume; |
cvtdb2fac(); |
if (hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
} |
//------------------------------- |
void tone_handler(void) // wird aus Timerinterrupt aufgerufen |
{ |
uint16_t f_Hz=0; |
uint16_t dur_ms=0; |
char volume=0; |
char tmp_high = 0; |
char tmp_low = 0; |
unsigned char tmptail; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* f_Hz low */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_high = ToneBuf[tmptail]; /* f_Hz high */ |
}else{ |
return; |
} |
f_Hz = (((uint16_t) tmp_high) << 8) | tmp_low; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
volume = ToneBuf[tmptail]; /* volume */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* low Duratuion */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
tmp_high = ToneBuf[tmptail]; /* high Duration */ |
}else{ |
return; |
} |
dur_ms = (((uint16_t) tmp_high) << 8) | tmp_low; |
sound_timer = dur_ms/10; |
// TCNT2=0; |
idesfreq= f_Hz; |
cvtfreq2frac(); |
attendb= volume; |
cvtdb2fac(); |
if (f_Hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
// printf("tone_handler f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
} |
void tone_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (ToneBufHead + 1) & TONE_BUFFER_MASK; |
while ( tmphead == ToneBufTail ){ |
;/* wait for free space in buffer */ |
} |
ToneBuf[tmphead] = data; |
ToneBufHead = tmphead; |
}/* tone_putc */ |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume) |
{ |
// printf("Playtone f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
tone_putc(LSB(f_Hz)); |
tone_putc( MSB(f_Hz)); |
tone_putc( volume); |
tone_putc(LSB(dur_ms)); |
tone_putc(MSB(dur_ms)); |
} |
//----------------- |
void InitSound(void){//main program |
ToneBufHead = 0; |
ToneBufTail = 0; |
initvars(); |
initsindat(); |
Timer2_Init(); |
Timer3_Init(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/sound/pwmsine8bit.h |
---|
0,0 → 1,22 |
/* |
* pwmsine8bit.h |
* |
* Created on: 01.09.2012 |
* Author: cebra |
*/ |
#ifndef PWMSINE8BIT_H_ |
#define PWMSINE8BIT_H_ |
extern unsigned char freqincipart,freqincfpart; |
extern unsigned char waveptipart,waveptfpart; |
extern unsigned char iattenfac; //0-255 |
extern unsigned char generate; |
extern volatile unsigned char sindat[256]; //8 bit sine table |
void InitSound(void); |
void tone(int hz,uint8_t volume, uint16_t ms, uint16_t soundpause); |
void tone1(int hz); |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume); |
void tone_handler(void); |
#endif /* PWMSINE8BIT_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/stick/stick.c |
---|
0,0 → 1,507 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* Copyright (C) 2012 Martin Runkel * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
//#include "../tracking/servo.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "stick.h" |
#include "stick_setup.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#define SERVO_CORRECT 3.125 |
#include <util/delay.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
int16_t Pos_Stick[12]; // 1,5mS |
int16_t Pos_alt[5]; // |
uint8_t BalkenPos = 0; |
uint8_t Stick_Display = 0; |
//uint8_t serialChannelRichtung = 0; |
//uint8_t serialChannelNeutral = 0; |
//uint8_t serialChannelConfig = 2; |
//-------------------------------------------------------------- |
// |
void joystick (void) |
{ |
// uint8_t chg = 0; |
// uint8_t Pos_Stick = 150; // 1,5mS |
// uint8_t Pos_alt = 150; // |
//int16_t Pos_Stick[12]; // 1,5mS |
uint8_t chg = 0; |
//uint8_t BalkenPos = 0; |
uint8_t Stick_Nr = 0; |
//uint8_t Stick_Display = 0; |
uint8_t i = 0; |
memset (Pos_Stick, 150, 3); // füllt 3+1 Byte vom Pos_Stick[12] mit 150 |
//int16_t Pos_alt[5]; // |
int16_t Poti_Summe[5]; // |
memset (Poti_Summe, 0, 5); // füllt 3+1 Byte mit 0 |
int16_t Poti_Neutral[5]; // |
// ADC- init |
//if (Config.Lipomessung == true) |
//{ |
//} |
//for (uint8_t i = 0; i < 4; i++) |
if (Config.stick_typ[0] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA0); |
DDRA &= ~(1<<DDA0); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA0); |
DDRA |= (1<<DDA0); |
} |
if (Config.Lipomessung == false) |
{ |
if (Config.stick_typ[1] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA1); |
DDRA &= ~(1<<DDA1); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA1); |
DDRA |= (1<<DDA1); |
} |
} |
if (Config.stick_typ[2] == 0) |
{ |
PORTA &= ~(1<<PORTA2); |
DDRA &= ~(1<<DDA2); |
} |
else |
{ |
PORTA |= (1<<PORTA2); |
DDRA |= (1<<DDA2); |
} |
if (Config.stick_typ[3] == 0) |
{ |
PORTA &= ~(1<<PORTA3); |
DDRA &= ~(1<<DDA3); |
} |
else |
{ |
PORTA |= (1<<PORTA3); |
DDRA |= (1<<DDA3); |
} |
//if (Config.stick_dir[i] == 0) serialChannelRichtung &= ~(1<<i); else serialChannelRichtung |= (1<<i); |
//if (Config.stick_neutral[i] == 0) serialChannelNeutral &= ~(1<<i); else serialChannelNeutral |= (1<<i); |
//if (Config.Lipomessung == true) |
//serialChannelConfig |
// Ermittlung der Neutralposition |
for (uint8_t i = 0; i < 4; i++) |
{ |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
} |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
ADCSRA |= (1<<ADSC); // ADC Start |
/* |
Stick_Nr = 0; |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[Stick_Nr] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
// Stick-Neutralposition bestimmen |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 2 ; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
*/ |
//OCR1A = 150 * SERVO_CORRECT; // Servomitte |
lcd_cls (); |
// Kopfzeile und Rahmen zeichnen |
lcd_printp (PSTR(" serielle Potis 1-5 "), 2); |
//lcd_printp_at (7, 5, PSTR("%"), 0); |
//lcd_printp_at (16, 5, PSTR("mS"), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x19O\x18"), 0); |
for (i=0 ; i< 5 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_rect(3,BalkenPos, 120, 6, 1); // +-150% Rahmen |
lcd_line(23,BalkenPos,23,(BalkenPos+6),1); // -100% |
lcd_line(43,BalkenPos,43,(BalkenPos+6),1); // -50% |
lcd_frect(62,BalkenPos, 2, 6, 1); // 0% |
lcd_line(83,BalkenPos,83,(BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
} |
// Reset auf Mittelstellung |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
chg = 255; |
do |
{ |
if (!(ADCSRA & (1<<ADSC))) // wenn ADC fertig |
{ |
//Pos_Stick[Stick_Nr] = 150 + 128 - ((ADCW>>2)&0xff); |
//if (serialChannelRichtung & (1<<Stick_Nr)) |
if (Config.stick_dir[Stick_Nr] > 0) |
Pos_Stick[Stick_Nr] = Poti_Neutral[Stick_Nr] - ((ADCW>>2)&0xff); |
else |
Pos_Stick[Stick_Nr] = ((ADCW>>2)&0xff) - Poti_Neutral[Stick_Nr]; |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],-120,120); |
//if ((Stick_Nr==0) || (Stick_Nr==2)) // nur die Potis 1,2 sind nicht neutralisierend |
if (Config.stick_neutral[Stick_Nr]==0) // nicht neutralisierend |
{ |
Poti_Summe[Stick_Nr] += (Pos_Stick[Stick_Nr]/8) * abs(Pos_Stick[Stick_Nr]/8); |
LIMIT_MIN_MAX (Poti_Summe[Stick_Nr],-(120*128),(120*128)); |
Pos_Stick[Stick_Nr]= Poti_Summe[Stick_Nr] / 128; // nicht neutralisierend |
} |
Pos_Stick[Stick_Nr] += 150; |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],Config.stick_min[Stick_Nr],Config.stick_max[Stick_Nr]); // war 30, 270 |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) // nur bei Änderung |
{ |
chg |= (1<<Stick_Nr) ; |
//Pos_alt=Pos_Stick ; // verschoben |
} |
Stick_Nr ++ ; |
//if (Stick_Nr==1) Stick_Nr=2; // Lipo überspringen |
if (Stick_Nr==3) // Taster |
{ |
if (Config.stick_dir[Stick_Nr] > 0) |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
} |
else |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
} |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) |
{ |
chg |= (1<<Stick_Nr) ; |
} |
Stick_Nr=0; |
} |
/* |
#ifndef ohne_Lipo // MartinR |
Stick_Nr = 1; // MartinR AD-Kanal 1 überspringen wegen Lipo Überwachung |
#endif |
*/ |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 0= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
//serialPotis (); |
} |
if ((get_key_press (1 << KEY_PLUS) | get_key_long_rpt_sp ((1 << KEY_PLUS), 3))) |
//if (get_key_press (1 << KEY_PLUS)) |
//if (PINA & (1 << KEY_PLUS)) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] ++ ; else Pos_Stick[4] -- ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_max[4] ; else Pos_Stick[4] = Config.stick_min[4] ; |
} |
chg |= (1<<4) ; |
} |
else if ((get_key_press (1 << KEY_MINUS) | get_key_long_rpt_sp ((1 << KEY_MINUS), 3))) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] -- ; else Pos_Stick[4] ++ ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_min[4] ; else Pos_Stick[4] = Config.stick_max[4] ; |
} |
chg |= (1<<4) ; |
} |
if (Config.stick_neutral[4]!= 0 && Pos_Stick[4]!= 150 && (PINA &(1 << KEY_PLUS)) && (PINA &(1 << KEY_MINUS))) |
{ |
Pos_Stick[4] = 150 ; |
chg |= (1<<4) ; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
/* |
for (i=0 ; i< 4 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_frect (4, (BalkenPos+1), 118, 4, 0); // Balken löschen |
lcd_frect(62, BalkenPos, 2, 6, 1); // 0% |
} |
*/ |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
BeepTime = 200; |
BeepMuster = 0x0080; |
chg = 255; |
} |
if (chg) |
{ |
if (chg & (1<<0)); // Stick 1 |
{ |
BalkenPos = 12 + (0*8) ; |
Stick_Display = 0; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
// Stick 2 = Lipo |
if (chg & (1<<1)); // Stick 2 |
{ |
BalkenPos = 12 + (1*8) ; |
Stick_Display = 1; |
//if (serialChannelConfig & (0<<1)) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
if (Config.Lipomessung == false) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<2)); // Stick 3 |
{ |
BalkenPos = 12 + (2*8) ; |
Stick_Display = 2; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<3)); // Stick 4 = Taster |
{ |
BalkenPos = 12 + (3*8) ; |
Stick_Display = 3; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<4)); // Stick 5 = Taster vom PKT |
{ |
BalkenPos = 12 + (4*8) ; |
Stick_Display = 4; |
Balken_Zeichnen () ; |
//OCR1A = (((Pos_Stick[Stick_Display]-150)/1.6)+150) * SERVO_CORRECT; // Servostellung , 1.6=0.8*0.5 |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
chg = 0; |
serialPotis (); |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
//#ifdef HWVERSION3_9 |
//#ifndef ohne_Lipo // MartinR |
if (Config.Lipomessung == true) // MartinR: geändert |
{ |
//DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
ADC_Init(); // ADC für Lipomessung wieder aktivieren |
} |
//ADC_Init(); // ADC für Lipomessung wieder aktivieren |
//#endif |
//#endif |
} |
//-------------------------------------------------------------- |
// |
void serialPotis (void) |
{ |
uint8_t i = 0; |
memset (buffer, 0, 12); // füllt die 12+1 Byte vom buffer mit 0 |
for (i=0 ; i< 5 ; i++) |
{ |
buffer[i] = Pos_Stick[i]-150 ; |
} |
SendOutData('y', ADDRESS_FC, 1, buffer, 12); |
} |
//-------------------------------------------------------------- |
// |
void Balken_Zeichnen (void) |
{ |
// Balken löschen |
if ((Pos_Stick[Stick_Display] > Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] < 150)) // Balken links löschen |
lcd_frect ((63-((150 -Pos_alt[Stick_Display]) * 0.5)), (BalkenPos+1), (63-((150- Pos_Stick[Stick_Display]) * 0.5)), 4, 0); |
if ((Pos_Stick[Stick_Display] < Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] > 150)) // Balken rechts löschen |
lcd_frect ((63+((Pos_Stick[Stick_Display] - 150) * 0.5)), (BalkenPos+1), (63+((Pos_alt[Stick_Display] - 150) * 0.5)), 4, 0); |
// Balken zeichnen |
if (Pos_Stick[Stick_Display] >= 150) |
{ |
lcd_frect (63, (BalkenPos+1), ((Pos_Stick[Stick_Display] - 150) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((Pos_Stick[Stick_Display] - 150) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
else |
{ |
lcd_frect (63 - ((150 - Pos_Stick[Stick_Display]) * 0.5), (BalkenPos+1), ((150 - Pos_Stick[Stick_Display]) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((150 - Pos_Stick[Stick_Display]) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
// Raster zeichnen |
lcd_line(3, BalkenPos,3, (BalkenPos+6),1); // -150% |
lcd_line(23, BalkenPos,23, (BalkenPos+6),1); // -100% |
lcd_line(43, BalkenPos,43, (BalkenPos+6),1); // -50% |
lcd_line(83, BalkenPos,83, (BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
lcd_line(123,BalkenPos,123,(BalkenPos+6),1); // +150% |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/stick/stick.h |
---|
0,0 → 1,42 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _STICK_H |
#define _STICK_H |
void joystick (void); |
void serialPotis (void); |
void Balken_Zeichnen (void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/stick/stick_setup.c |
---|
0,0 → 1,279 |
/****************************************************************/ |
/* Setup für die Sticks */ |
/* 2013 Cebra */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY stick_setup.c |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 31.05.2013 OG |
//# - chg: einige redundante Menuetitel auf define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "stick_setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
#ifdef USE_JOYSTICK |
//############################################################################ |
//----------------------------- |
// Setup_Joysticks() |
//----------------------------- |
#define JSTICKS_OVERVIEW 1 |
#define JSTICKS_STICK1 2 |
#define JSTICKS_STICK2 3 |
#define JSTICKS_STICK3 4 |
#define JSTICKS_STICK4 5 |
#define JSTICKS_STICK5 6 |
#define JSTICKS_LIPO 7 |
static const char JSTICKS_OVERVIEW_de[] PROGMEM = "Übersicht"; |
static const char JSTICKS_OVERVIEW_en[] PROGMEM = "summary"; |
static const char JSTICKS_STICK1_de[] PROGMEM = "Stick 1"; |
#define JSTICKS_STICK1_en JSTICKS_STICK1_de |
static const char JSTICKS_STICK2_de[] PROGMEM = "Stick 2"; |
#define JSTICKS_STICK2_en JSTICKS_STICK2_de |
static const char JSTICKS_STICK3_de[] PROGMEM = "Stick 3"; |
#define JSTICKS_STICK3_en JSTICKS_STICK3_de |
static const char JSTICKS_STICK4_de[] PROGMEM = "Stick 4"; |
#define JSTICKS_STICK4_en JSTICKS_STICK4_de |
static const char JSTICKS_STICK5_de[] PROGMEM = "Stick 5"; |
#define JSTICKS_STICK5_en JSTICKS_STICK5_de |
static const char JSTICKS_LIPO_de[] PROGMEM = "PKT Lipomessung"; |
static const char JSTICKS_LIPO_en[] PROGMEM = "PKT Lipo measure."; |
//############################################################################ |
//----------------------------- |
// Setup_Stick() |
//----------------------------- |
#define MSTICK_MIN 1 |
#define MSTICK_MAX 2 |
#define MSTICK_TYPE 3 |
#define MSTICK_DIR 4 |
#define MSTICK_NEUT 5 |
static const char MSTICK_MIN_de[] PROGMEM = "Minimal Wert"; |
static const char MSTICK_MIN_en[] PROGMEM = "minimal value"; |
static const char MSTICK_MAX_de[] PROGMEM = "Maximal Wert"; |
static const char MSTICK_MAX_en[] PROGMEM = "maximal value"; |
static const char MSTICK_TYPE_de[] PROGMEM = "Type"; |
static const char MSTICK_TYPE_en[] PROGMEM = "type"; |
static const char MSTICK_DIR_de[] PROGMEM = "Richtung"; |
static const char MSTICK_DIR_en[] PROGMEM = "direction"; |
static const char MSTICK_NEUT_de[] PROGMEM = "Neutralisiered"; |
static const char MSTICK_NEUT_en[] PROGMEM = "neutralizing"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Joysticks_Uebersicht(void) |
{ |
lcd_cls (); |
lcd_printpns_at(0, 0, PSTR(" Joystick Setup "), 2); |
lcd_printpns_at(0, 1, PSTR("S Min Max Typ Dir N"), 0); |
for (uint8_t i = 0; i < 5; i++) { |
write_ndigit_number_u (0, 2+i,i+1, 1, 0,0); |
write_ndigit_number_u (2, 2+i,Config.stick_min[i], 3, 0,0); |
write_ndigit_number_u (6, 2+i,Config.stick_max[i], 3, 0,0); |
if (Config.stick_typ[i] == 0) lcd_printpns_at(10, 2+i, PSTR("Poti"), 0); else lcd_printpns_at(10, 2+i, PSTR("Tast"), 0); |
//if (Config.stick_typ[i] == 0) // MartinR: geändert |
//{ |
if (Config.stick_dir[i] == 0) lcd_printpns_at(15, 2+i, PSTR("Norm"), 0); else lcd_printpns_at(15, 2+i, PSTR("Rev"), 0); |
if (Config.stick_neutral[i] == 0) lcd_printpns_at(20, 2+i, PSTR("N"), 0); else lcd_printpns_at(20, 2+i, PSTR("Y"), 0); |
//} |
if (i == 1) if (Config.Lipomessung == true) lcd_printpns_at(3, 2+i, PSTR("PKT Lipomessung "), 0); |
} |
lcd_printp_at (18, 7, PSTR("OK"), 0); |
do{} |
while (!(get_key_press (1 << KEY_ENTER))); |
} |
//-------------------------------------------------------------- |
// Setup_Stick() |
//-------------------------------------------------------------- |
void Setup_Stick( uint8_t stick ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Stick n" |
//MenuCtrl_SetTitle_P( PSTR("Joystick") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSTICK_MIN , MENU_ITEM, NOFUNC , MSTICK_MIN_de , MSTICK_MIN_en ); |
MenuCtrl_PushML2_P( MSTICK_MAX , MENU_ITEM, NOFUNC , MSTICK_MAX_de , MSTICK_MAX_en ); |
MenuCtrl_PushML2_P( MSTICK_TYPE , MENU_ITEM, NOFUNC , MSTICK_TYPE_de , MSTICK_TYPE_en ); |
MenuCtrl_PushML2_P( MSTICK_DIR , MENU_ITEM, NOFUNC , MSTICK_DIR_de , MSTICK_DIR_en ); |
MenuCtrl_PushML2_P( MSTICK_NEUT , MENU_ITEM, NOFUNC , MSTICK_NEUT_de , MSTICK_NEUT_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSTICK_MIN ) { Config.stick_min[stick] = Edit_generic( Config.stick_min[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_MAX ) { Config.stick_max[stick] = Edit_generic( Config.stick_max[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_TYPE ) { Config.stick_typ[stick] = Edit_generic( Config.stick_typ[stick] ,0, 1,Sticktype,1 ,NULL,NULL); } |
if( itemid == MSTICK_DIR ) { Config.stick_dir[stick] = Edit_generic( Config.stick_dir[stick] ,0, 1,NormRev,1 ,NULL,NULL); } |
if( itemid == MSTICK_NEUT ) { Config.stick_neutral[stick]= Edit_generic( Config.stick_neutral[stick] ,0, 1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Joysticks() |
//-------------------------------------------------------------- |
void Setup_Joysticks( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Joystick" |
//MenuCtrl_SetTitle_P( PSTR("Joystick Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( JSTICKS_OVERVIEW , MENU_ITEM, NOFUNC , JSTICKS_OVERVIEW_de , JSTICKS_OVERVIEW_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK1 , MENU_SUB , NOFUNC , JSTICKS_STICK1_de , JSTICKS_STICK1_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK2 , MENU_SUB , NOFUNC , JSTICKS_STICK2_de , JSTICKS_STICK2_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK3 , MENU_SUB , NOFUNC , JSTICKS_STICK3_de , JSTICKS_STICK3_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK4 , MENU_SUB , NOFUNC , JSTICKS_STICK4_de , JSTICKS_STICK4_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK5 , MENU_SUB , NOFUNC , JSTICKS_STICK5_de , JSTICKS_STICK5_en ); |
MenuCtrl_PushML2_P( JSTICKS_LIPO , MENU_ITEM, NOFUNC , JSTICKS_LIPO_de , JSTICKS_LIPO_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == JSTICKS_OVERVIEW ) { Joysticks_Uebersicht(); } |
if( itemid == JSTICKS_STICK1 ) { Setup_Stick(0); } |
if( itemid == JSTICKS_STICK2 ) { if (Config.Lipomessung == true) |
{ |
lcd_cls (); |
lcd_printp_at(0, 3, PSTR("LiPo measure!"), MNORMAL); |
_delay_ms(1000); |
} |
else Setup_Stick(1); //Stick 2 |
} |
if( itemid == JSTICKS_STICK3 ) { Setup_Stick(2); } |
if( itemid == JSTICKS_STICK4 ) { Setup_Stick(3); } |
if( itemid == JSTICKS_STICK5 ) { Setup_Stick(4); } |
if( itemid == JSTICKS_LIPO ) { Config.Lipomessung = Edit_generic( Config.Lipomessung ,0,1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################ |
#endif // USE_JOYSTICK |
//############################################################################ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/stick/stick_setup.h |
---|
0,0 → 1,13 |
//############################################################################ |
//# HISTORY stick_setup.h |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//############################################################################ |
#ifndef _STICK_SETUP_H_ |
#define _STICK_SETUP_ |
void Setup_Joysticks(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/stick |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/timer/timer.c |
---|
0,0 → 1,856 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.c |
//# |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 29.06.2014 OG |
//# - chg: ISR( TIMER0_COMPA_vect ) - LipoCheck() wieder hinzugefuegt |
//# - chg: LipoCheck() - leichte optische Anpassung und lcd-Ausgabe an ISR angepasst |
//# - add: #include "../lipo/lipo.h" |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# - chg: ISR(TIMER0_COMPA_vect) - "PKT aus nach" (via timer_pkt_off) |
//# - chg: ISR(TIMER0_COMPA_vect) - "LCD aus nach" umgestellt von Sekunden |
//# auf Minuten (via timer_lcd_off) |
//# - chg: Code-Formattierung |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() - loescht ALLE Tasten egal ob short, long, repeat... |
//# |
//# 13.06.2013 cebra |
//# - chg: Abfrage nach Hardwaresound bei Displaybeleuchtung entfällt |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//# |
//# 24.03.2013 OG |
//# - add: UTCTime - inkrementiert wird UTCTime.seconds auf der Basis |
//# von timer_pkt_uptime (modulo 100) aktuell initialisert von |
//# der MK-NC Time durch OSD_MK_UTCTime() |
//# - add: timer_get_tidata |
//# |
//# 21.03.2013 OG |
//# - add: timer_pkt_uptime (auch in timer.h) |
//# - add: timer_osd_refresh (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_bldata (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_erdata (auch in timer.h) -> verwendet in osd.c |
//# |
//# 09.03.2013 OG |
//# - add: timer2 (auch in timer.h) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../lcd/lcd.h" |
#include "../uart/uart1.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include"../tracking/tracking.h" |
#include "../sound/pwmsine8bit.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
// |
//#if defined HWVERSION1_2W || defined HWVERSION1_2 |
//#include "HAL_HW1_2.h" |
//#endif |
// |
//#if defined HWVERSION1_3W || defined HWVERSION1_3 |
//#include "HAL_HW1_3.h" |
//#endif |
//#ifdef HWVERSION3_9 |
#include "../HAL_HW3_9.h" |
//#endif |
//---------------------- |
// generische Timer |
//---------------------- |
volatile uint16_t timer; |
volatile uint16_t timer1; |
volatile uint16_t timer2; |
volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV!) |
volatile uint16_t timer_osd_refresh; // verwendet in osd.c |
volatile uint16_t timer_get_bldata; // verwendet in osd.c |
volatile uint16_t timer_get_tidata; // verwendet in osd.c |
volatile uint16_t timer_get_displaydata; // verwendet in osd.c |
volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
volatile uint16_t abo_timer; |
uint32_t timer_lcd_off = 0; // LCD aus nach... (der Zaehler muss 32Bit sein) |
uint32_t timer_pkt_off = 0; // PKT aus nach... (der Zaehler muss 32Bit sein) |
volatile uint16_t sound_timer; |
volatile uint16_t soundpause_timer; |
volatile static unsigned int tim_main; |
volatile PKTdatetime_t UTCTime; |
volatile uint32_t timer_pkt_uptime = 0; // OG: ja, 32 bit muss so sein! |
uint8_t key_state = 0; // debounced and inverted key state: |
// bit = 1: key pressed |
uint8_t key_press = 0; // key press detect |
uint8_t key_long = 0; // key long press |
uint8_t key_rpt = 0; // key repeat |
uint8_t key_lrpt = 0; // key long press and repeat |
uint8_t key_rpts = 0; // key long press and speed repeat |
uint8_t repeat_speed = 0; |
volatile uint8_t Display_on; // LCD Flag Display on/off |
volatile uint16_t WarnCount = 0; // Zähler der LIPO Warnzeit |
volatile uint16_t WarnToggle = 0; // Togglezähler zum blinken |
volatile uint16_t WarnTime = 10; // Laenge der LIPO Warnzeit 10 Sek. |
volatile uint16_t PoffTime = 15; // Laenge der Wartezeit vor abschalten 15 Sek. |
//volatile uint16_t PoffTime = 30; // Laenge der Wartezeit vor abschalten 30 Sek. |
unsigned int BeepTime = 0; |
unsigned int BeepMuster = 0xffff; |
unsigned int BeepPrio = 0; |
volatile unsigned int CountMilliseconds = 0; |
//-------------------------------------------------------------- |
// System (100Hz) |
//-------------------------------------------------------------- |
void Timer0_Init( void ) |
{ |
timer = 0; |
TCCR0A = (1 << WGM01); |
TCCR0B = (1 << CS02) | (1 << CS00); |
OCR0A = (F_CPU / (100L * 1024L)) ; |
TIMSK0 |= (1 << OCIE0A); // enable interrupt for OCR |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void Timer1_Init (void) // Timer 1-A |
//{ |
// // löschen |
// TCCR1A = 0; |
// TCCR1B = 0; |
// TIMSK1 = 0; |
// |
// // setzen |
// TCCR1A |= (1 << COM1A1) | (1 << WGM11); |
// TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << WGM13) | (1 << WGM12); |
// |
// ICR1 = (F_CPU / 64) * 20 / 1000; |
// |
// OCR1A = 470; // ca. Servomitte |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPA_vect ) |
{ |
PORTD |= (1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPB_vect ) |
{ |
PORTD &= ~(1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Timer2_Init( void ) |
{ |
if( Config.HWSound == 1 ) |
{ // Sound PWM |
TCCR2A = 0x00; //stop |
ASSR = 0x00; //set async mode |
TCNT2 = 0x00; //setup |
OCR2A = 0xff; |
//Fast PWM 0xFF BOTTOM MAX |
//Set OC2A on Compare Match |
//clkT2S/8 (From prescaler) |
TCCR2A |= (1 << WGM20) | (1 << WGM21) |(1 << COM2A1) | (1 << COM2A0); |
TCCR2B |= (1 << CS20); |
} |
else |
{ // Displayhelligkeit |
DDRD |= (1 << DDD7); // PD7 output |
TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); // non invers |
TCCR2B |= (1 << CS20); // Prescaler 1/1 |
TIMSK2 |= (1 << OCIE2A) | (1 << OCIE2B); |
OCR2A = 255; |
} |
} |
//-------------------------------------------------------------- |
// Sound, Timer CTC |
//-------------------------------------------------------------- |
void Timer3_Init( void ) |
{ |
TCCR3A = 0x00; // stop |
TCNT3H = 0xF8; // set count |
TCNT3L = 0x00; // set count |
OCR3AH = 0x07; // set compare |
OCR3AL = 0xFF; // set compare |
TCCR3A |= (1 << WGM31); |
TCCR3B |= (1 << CS30); |
TIMSK3 |= (1 << OCIE3A); // timer interrupt sources 2=t0 compare |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_SOUND |
ISR(TIMER3_COMPA_vect) // Sound |
{ |
//void timer0_comp_isr(void){ |
//was 8 KHz 125usec sampling rate |
//now 12 KHz 83usec sampling rate |
unsigned char oldfpart; |
signed char fullsamp; |
signed int tmp; |
TCNT3 = 0; |
if( generate ) |
{ |
DDRD |= (1<<DDD7); // Port aus Ausgang |
oldfpart=waveptfpart; // remember fractional part |
waveptfpart+=freqincfpart; // add frac part of freq inc to wave pointer |
if( waveptfpart < oldfpart ) // did it walk off the end? |
{ |
waveptipart++; // yes, bump integer part |
} |
waveptipart+=freqincipart; // add int part of freq increment to wave pointer |
fullsamp=sindat[waveptipart]; // get 8 bit sin sample from table (signed) |
tmp=fullsamp*iattenfac; // cvt 7 bit x 8 = 15 bit |
OCR2A=(tmp >> 8)+128; // cvt 15 bit signed to 8 bit unsigned |
} |
else |
{ |
DDRD &= ~(1 << DDD7); // Port auf Eingang, sperrt das Rauschen |
} |
} |
#endif |
//-------------------------------------------------------------- |
// Timer-Interrupt (100 Hz) |
//-------------------------------------------------------------- |
ISR( TIMER0_COMPA_vect ) |
{ |
static uint8_t ct0 = 0; |
static uint8_t ct1 = 0; |
static uint8_t k_time_l = 0; |
static uint8_t k_time_r = 0; |
static uint8_t k_time_lr = 0; |
static uint8_t k_time_rs = 0; |
uint8_t i; |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char beeper_ein = 0; |
i = key_state ^ ~KEY_PIN; // key changed ? |
ct0 = ~(ct0 & i); // reset or count ct0 |
ct1 = ct0 ^ (ct1 & i); // reset or count ct1 |
i &= (ct0 & ct1); // count until roll over ? |
key_state ^= i; // then toggle debounced state |
key_press |= (key_state & i); // 0->1: key press detect |
if( !cnt-- ) |
{ |
cnt = 9; |
CountMilliseconds++; |
cnt_1ms++; |
} |
//-------------------------------- |
// Key pressed -> Timer Reset |
//-------------------------------- |
if( i!=0 ) // eine Taste wurde gedrueckt! -> Timer Rest und weiteres... |
{ |
if( Display_on == 0 ) // ggf. Displaylicht einschalten |
set_D_LIGHT(); |
Display_on = 1; // Flag Display on |
timer_lcd_off = 0; // Timer Reset |
timer_pkt_off = 0; // Timer Reset |
} |
//-------------------------------- |
// LCD off Timeout |
// LCD ausschalten nach n Minuten |
//-------------------------------- |
if( (Config.DisplayTimeout > 0) && (Display_on == 1) ) |
{ |
timer_lcd_off++; |
if( (timer_lcd_off/(100*60)) == Config.DisplayTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
clr_D_LIGHT(); // Displaylicht ausschalten |
Display_on = 0; // Flag Display off |
} |
} |
//-------------------------------- |
// PKT off Timeout |
// PKT ausschalten nach n Minuten |
//-------------------------------- |
if( Config.PKTOffTimeout > 0 ) |
{ |
timer_pkt_off++; |
if( (timer_pkt_off/(100*60)) == Config.PKTOffTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------- |
// PKT bei Unterspannung abschalten |
//-------------------------------- |
LipoCheck(); |
//-------------------------------- |
// Beeper |
//-------------------------------- |
if( Config.HWBeeper==1 ) |
{ |
if( BeepTime ) |
{ |
if( BeepTime > 10 ) BeepTime -= 10; |
else BeepTime = 0; |
if( BeepTime & BeepMuster ) beeper_ein = 1; |
else beeper_ein = 0; |
} |
else |
{ |
beeper_ein = 0; |
BeepMuster = 0xffff; |
BeepPrio = BeepNormal; |
} |
if( beeper_ein == 1 ) set_BEEP(); |
else clr_BEEP(); |
} |
//-------------------------------- |
// Sound |
//-------------------------------- |
#ifdef USE_SOUND |
if (sound_timer > 0) // Ton spielen |
{ |
sound_timer--; |
} |
else |
{ |
//TIMSK2 &= ~_BV(TOIE2); // Interrupt sperren, verhindert Störgeräusche |
//TCCR2A = 0x00; //stop |
generate = 0; // Sound aus |
tone_handler(); |
if (soundpause_timer > 0) |
{ |
soundpause_timer --; // Ton pause |
} |
} |
#endif |
//-------------------------------- |
// Tasten |
//-------------------------------- |
if ((key_state & LONG_MASK) == 0) // check long key function |
k_time_l = REPEAT_START; // start delay |
if (--k_time_l == 0) // long countdown |
key_long |= (key_state & LONG_MASK); |
//------ |
if ((key_state & REPEAT_MASK) == 0) // check repeat function |
k_time_r = 1; // kein delay |
if (--k_time_r == 0) |
{ |
k_time_r = REPEAT_NEXT; // repeat delay |
key_rpt |= (key_state & REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_MASK) == 0) // check repeat function |
k_time_lr = REPEAT_START; // start delay |
if (--k_time_lr == 0) |
{ |
k_time_lr = REPEAT_NEXT; // repeat delay |
key_lrpt |= (key_state & LONG_REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_SP_MASK) == 0) // check repeatX function |
k_time_rs = REPEAT_START; // start delay |
if( --k_time_rs == 0 ) // repeat countdown |
{ |
if( repeat_speed == 1 ) |
{ |
k_time_rs = REPEAT_SPEED_1; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 2 ) |
{ |
k_time_rs = REPEAT_SPEED_2; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 3 ) |
{ |
k_time_rs = REPEAT_SPEED_3; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
} |
//-------------------------------- |
// generische Timer |
//-------------------------------- |
if( timer > 0 ) timer --; |
if( timer1 > 0 ) timer1 --; |
if( timer2 > 0 ) timer2 --; |
if( timer3 > 0 ) timer3 --; |
//-------------------------------- |
// spezielle Timer |
//-------------------------------- |
if( timer_osd_refresh > 0 ) timer_osd_refresh--; // Timer fuer OSD-Screenrefresh (verwendet von osd.c) |
if( timer_get_bldata > 0 ) timer_get_bldata--; // Timer um BL-Daten zu holen (verwendet von osd.c) |
if( timer_get_tidata > 0 ) timer_get_tidata--; // Timer um Datum/Zeit vom MK zu holen (verwendet von osd.c) |
if( timer_get_displaydata > 0 ) timer_get_displaydata--; // Timer um Display vom MK zu holen (verwendet von osd.c) |
if( timer_mk_timeout > 0 ) timer_mk_timeout--; // verwendet u.a. von osd.c |
if( abo_timer > 0 ) abo_timer --; // Timer zum anfordern neuer Abo-Datenpakete wie OSD oder BL-Daten (verwendet u.a. von osd.c) |
if( timer_pktupdatecheck > 0 ) timer_pktupdatecheck--; // Timer fuer pkt.c (PKT-Update-Check) - * FUER NICHTS ANDERES! * |
if( timer_nmea_timeout > 0 ) timer_nmea_timeout--; // verwendet u.a. von osd.c |
//-------------------------------- |
//-------------------------------- |
if( Gauge_active ) // Gauge_active -> pkt.c |
{ |
if( timer_gauge > 0 ) timer_gauge--; |
if( timer_gauge==0 ) PKT_Gauge_Next(); |
} |
//-------------------------------- |
// PKT Uptime Timer |
//-------------------------------- |
timer_pkt_uptime++; |
if( timer_pkt_uptime % 100 == 0 ) // theoretisch muesste noch die Tagesgrenze (0 Uhr) implementiert werden... |
UTCTime.seconds++; |
} // end: ISR(TIMER0_COMPA_vect) |
//-------------------------------------------------------------- |
// Lowbatpin des Spannungswandlers pruefen |
// LBO des LT1308 wechselt zum Ende der Batterielaufzeit haeufig seinen Zustand in der Uebergangsphase zum LowBat |
// Die Akkuspannung schwankt auch abhaengig vom momentanen Stromverbrauch |
//-------------------------------------------------------------- |
void LipoCheck( void ) |
{ |
uint8_t lcd_xpos_save; |
uint8_t lcd_ypos_save; |
if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
{ |
set_beep( 1000, 0x0020, BeepNormal); |
lcd_xpos_save = lcd_xpos; // innerhalb einer ISR -> LCD Screenpos muss gesichert werden! |
lcd_ypos_save = lcd_ypos; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 0, MINVERS ,0 ); // Zeile 0 komplett invers |
lcd_printp_at( 0, 0, PSTR(" ** PKT LiPo! ** "), MINVERS); // und Warn-Text ausgeben |
lcd_xpos = lcd_xpos_save; // lcd Screenpos wieder herstellen |
lcd_ypos = lcd_ypos_save; |
} |
if( WarnToggle == WarnTime * 100 ) |
WarnToggle = 0; // erstmal bis hier warnen |
if( WarnToggle > 0 ) |
WarnToggle++; // weiter hochzaehlen |
if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
{ |
WarnCount = 0; |
//if (WarnCount > 0) |
// WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
} |
if( !(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
{ |
WarnCount++; // solange LBO low ist Zähler hochzählen |
if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
WarnToggle = 1; // Warnhinweis starten |
} |
if( WarnCount == (PoffTime * 100) ) |
{ |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void LipoCheck_OLD( void ) |
//{ |
// if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
// { |
// set_beep ( 1000, 0x0020, BeepNormal); |
// lcd_printp_at (0, 0, PSTR(" LIPO !!Warnung!! "), 2); |
// } |
// |
// if( WarnToggle == WarnTime * 100 ) |
// WarnToggle = 0; // erstmal bis hier warnen |
// |
// if( WarnToggle > 0 ) |
// WarnToggle++; // weiter hochzaehlen |
// |
// if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
// { |
// WarnCount = 0; |
// //if (WarnCount > 0) |
// // WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
// } |
// |
// if (!(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
// { |
// WarnCount++; // solange LBO low ist Zähler hochzählen |
// if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
// WarnToggle = 1; // Warnhinweis starten |
// } |
// |
// if( WarnCount == (PoffTime * 100) ) |
// { |
// clr_V_On(); // Spannung abschalten |
// } |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned int SetDelay( unsigned int t ) |
{ |
return( CountMilliseconds + t + 1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
char CheckDelay( unsigned int t ) |
{ |
return( ((t - CountMilliseconds) & 0x8000) >> 9 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_ms( unsigned int w ) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while( !CheckDelay(akt) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_press( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_press; // read key(s) |
key_press ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_short( uint8_t key_mask ) |
{ |
uint8_t ret; |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
ret = get_key_press (~key_state & key_mask); |
SREG = sreg; // restore status register |
return ret; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_long; // read key(s) |
key_long ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_press (get_key_rpt (key_mask)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpt; // read key(s) |
key_rpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_lrpt; // read key(s) |
key_lrpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_rpt (~key_press^key_mask); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt_sp( uint8_t key_mask, uint8_t key_speed ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpts; // read key(s) |
key_rpts ^= key_mask; // clear key(s) |
repeat_speed = key_speed; |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clear_key_all( void ) |
{ |
key_rpt = 0; // clear key(s) |
key_rpts = 0; // clear key(s) |
key_lrpt = 0; // clear key(s) |
key_long = 0; // clear key(s) |
key_press = 0; // clear key(s) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_beep( uint16_t Time, uint16_t Muster, uint8_t Prio) |
{ |
if( Config.HWBeeper == 1 ) |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // BEEPER: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) // BEEPER: hohe Prio setzen |
{ |
BeepPrio = BeepSevere; |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( Prio == BeepOff ) |
{ |
BeepPrio = BeepNormal; // BEEPER: Beep hohe Prio aus |
BeepTime = 0; |
BeepMuster = 0; |
} |
} // end: if( Config.HWBeeper==1 ) |
#ifdef USE_SOUND |
else |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // SOUND: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
playTone(900,Time/10,0); |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) |
{ |
playTone(1200,Time/10,0); |
playTone(1100,Time/10,0); |
} |
if( Prio == BeepOff ) |
{ |
playTone(0,0,0); |
} |
} |
#endif // end: #ifdef USE_SOUND |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/timer/timer.h |
---|
0,0 → 1,170 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.h |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() |
//# |
//# 07.07.2013 OG |
//# - chg: ABO_TIMEOUT verschoben nach main.h |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//############################################################################ |
#ifndef _TIMER_H |
#define _TIMER_H |
#include "../cpu.h" |
#include "../main.h" |
typedef struct |
{ |
uint32_t seconds; // seconds since nnew day |
uint8_t day; |
uint8_t month; |
uint16_t year; |
} PKTdatetime_t; |
#define KEY_ALL ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_SP_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_START 70 // after 700ms |
#define REPEAT_NEXT 15 // every 150ms |
#define REPEAT_SPEED_1 20 // every 200ms |
#define REPEAT_SPEED_2 8 // every 80ms |
#define REPEAT_SPEED_3 1 // every 10ms |
#define BeepNormal 0 // Normal Beep |
#define BeepSevere 1 // schwerer Fehler, Beep nicht unterbrechbar |
#define BeepOff 2 // Beep aus |
extern volatile uint8_t Display_on; |
extern volatile PKTdatetime_t UTCTime; |
//---------------------- |
// generische Timer |
//---------------------- |
extern volatile uint16_t timer; |
extern volatile uint16_t timer1; |
extern volatile uint16_t timer2; |
extern volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
extern volatile uint32_t timer_pkt_uptime; |
extern volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV! FUER NICHTS ANDERES!) |
extern volatile uint16_t timer_osd_refresh; |
extern volatile uint16_t timer_get_bldata; |
extern volatile uint16_t timer_get_tidata; |
extern volatile uint16_t timer_get_displaydata; |
extern volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
extern volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
extern volatile uint16_t abo_timer; |
extern volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
extern volatile uint16_t sound_timer; |
extern volatile uint16_t soundpause_timer; |
//extern volatile uint16_t WarnCount; |
//extern volatile unsigned int BeepTime; |
extern unsigned int BeepTime; |
extern unsigned int BeepMuster; |
/** |
* Get the value of the system timer counter. |
* |
* @return The value of the system timer counter. |
*/ |
uint16_t timer_get_system_count(void); |
void Timer0_Init (void); // Systeminterrupt |
void Timer1_Init (void); // Servotester |
void Timer2_Init (void); // Displayhelligkeit |
void Timer3_Init (void); // Überwachung |
uint8_t get_key_press (uint8_t key_mask); // sofort beim drücken |
uint8_t get_key_short (uint8_t key_mask); // erst beim loslassen |
uint8_t get_key_long (uint8_t key_mask); // verzögert |
uint8_t get_key_rpt (uint8_t key_mask); // mit verzögerung |
uint8_t get_key_long_rpt (uint8_t key_mask); // |
uint8_t get_key_long_rpt_sp (uint8_t key_mask, uint8_t key_speed); // mit verzögerung und 3 versch. geschw. |
void clear_key_all( void ); |
void set_beep ( uint16_t Time, uint16_t Muster, uint8_t Prio); |
extern volatile unsigned int CountMilliseconds; |
void Delay_ms(unsigned int); |
unsigned int SetDelay (unsigned int t); |
char CheckDelay (unsigned int t); |
void LipoCheck (void); // Lowbatpin des Spannungswandlers prüfen |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/ng_servo.c |
---|
0,0 → 1,236 |
/********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library.*/ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 2 */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/********************************************************************/ |
//############################################################################ |
//# HISTORY ng_servo.c |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 24.04.2013 OG |
//# - chg: umbenannt servo_test(void) nach _servo_test(void) |
//# das verursachte in meinem PKT-Projekt Errors weil die Funktion |
//# servo_test() ebenfalls ein servo.c vorhanden ist |
//############################################################################ |
#include <avr/interrupt.h> |
#include <avr/io.h> |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../HAL_HW3_9.h" |
#include "../eeprom/eeprom.h" |
//#include "config.h" |
#ifdef USE_TRACKING |
// Global variables |
uint16_t ServoPosTics; |
uint16_t ServoPeriodTics; |
// servo channel registers |
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS]; |
const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER}, |
{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}}; |
// Servo limits (depend on hardware) |
const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX}, |
{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX}, |
{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}}, |
{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX}, |
{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX}, |
{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}}; |
// Servopositionen normiert für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
const steps_pw_t steps_pw[2] = {{(uint64_t)950*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*F_CPU*1e-6/SERVO_PRESCALER + 0.5,(uint64_t)1500*F_CPU*1e-6/SERVO_PRESCALER + 0.5}, |
{(uint64_t)950*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5}}; |
// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 }; |
// functions |
void _servo_test(void) |
{ |
//Dummy |
} |
//! initializes software PWM system 16-bit Timer |
// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt |
// Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!! |
void servoInit(uint8_t servo_period) |
{ uint16_t OCValue; // set initial interrupt time |
// disble Timer/Counter1, Output Compare A Match Interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
// set the prescaler for timer1 |
if (Config.sIdxSteps == STEPS_255) { |
TCCR1B &= ~((1<<CS11) | (1<<CS10)); |
TCCR1B |= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel |
} |
else { |
TCCR1B &= ~(1<<CS12); |
TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel |
} |
// attach the software PWM service routine to timer1 output compare A |
// timerAttach(TIMER1OUTCOMPAREA_INT, servoService); |
// enable channels |
for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) { |
// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5 |
ServoChannels[channel].pin = (1 << (SERVO2 + channel)); |
} |
ServoPosTics = 0; // set PosTics |
// set PeriodTics |
ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3; |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH << 8); // read high byte of OCR1A |
OCValue += ServoPeriodTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
TIMSK1 |= (1<<OCIE1A); // enable the timer1 output compare A interrupt |
coldstart = 1; |
} |
void servoSetDefaultPos(void) |
{ |
servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN |
servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT |
} |
uint16_t pw_us(uint16_t Steps) |
{ |
return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in µs |
} |
uint16_t ServoSteps(void) |
{ |
return(ServoConst[Config.sIdxSteps].steps); |
} |
void servoSet_rev(uint8_t channel, uint8_t val) |
{ |
ServoChannels[channel].rev = val & 0x01; |
} |
void servoSet_min(uint8_t channel, uint16_t min) |
{ |
ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min; |
} |
void servoSet_max(uint8_t channel, uint16_t max) |
{ |
ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max; |
} |
void servoSet_mid(uint8_t channel, uint16_t mid) |
{ |
ServoChannels[channel].mid = mid; |
// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen |
ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \ |
(16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min; |
} |
//! turns off software PWM system |
void servoOff(void) |
{ |
// disable the timer1 output compare A interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
} |
//! set servo position on channel (raw unscaled format) |
void servoSetPositionRaw(uint8_t channel, uint16_t position) |
{ |
// bind to limits |
if (position < ServoChannels[channel].min) position = ServoChannels[channel].min; |
if (position > ServoChannels[channel].max) position = ServoChannels[channel].max; |
// set position |
ServoChannels[channel].duty = position; |
} |
//! set servo position on channel |
// input should be between 0 and ServoSteps() (entspricht Maximalausschlag) |
void servoSetPosition(uint8_t channel, uint16_t position) |
{ uint16_t pos_scaled; |
// calculate scaled position |
if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position; |
if (position < ServoConst[Config.sIdxSteps].steps/2) |
//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2) |
pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \ |
ServoConst[Config.sIdxSteps].steps; |
else |
//bei Position >= Servomittelstellung |
pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \ |
* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps; |
// set position |
servoSetPositionRaw(channel, pos_scaled); |
} |
// Umrechnung Winkel in Servoschritte |
void servoSetAngle(uint8_t servo_nr, int16_t angle) |
{ |
servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180)); |
} |
//Interruptroutine |
ISR(TIMER1_COMPA_vect) |
{ static uint8_t ServoChannel; |
uint16_t nextTics; |
uint16_t OCValue; // schedule next interrupt |
if(ServoChannel < SERVO_NUM_CHANNELS) { |
PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel |
} |
ServoChannel++; // next channel |
if(ServoChannel != SERVO_NUM_CHANNELS) { |
// loop to channel 0 if needed |
if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
// turn on new channel |
PORTD |= ServoChannels[ServoChannel].pin; |
// schedule turn off time |
nextTics = ServoChannels[ServoChannel].duty; |
} |
else { |
// ***we could save time by precalculating this |
// schedule end-of-period |
nextTics = ServoPeriodTics-ServoPosTics; |
} |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH <<8); // read high byte of OCR1A |
OCValue += nextTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
ServoPosTics += nextTics; // set our new tic position |
if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/ng_servo.h |
---|
0,0 → 1,190 |
/*********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library. */ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 4 */ |
/* */ |
/*ingroup driver_sw */ |
/*defgroup servo Interrupt-driven RC Servo Function Library (servo.c)*/ |
/*code #include "servo.h" \endcode */ |
/*par Overview */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/*which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/*********************************************************************/ |
#ifndef SERVO_H |
#define SERVO_H |
/* Servo */ |
#define SERVO_PAN 0 |
#define SERVO_TILT 1 |
#define SERVO_NUM_CHANNELS 2 // Anzahl der angeschlossen Servos max. 2!!! |
/* Servokalibrierungen derzeit zu SERVO_STEPS = 255 skaliert */ |
//prescaler 256 |
#define SERVO_I0_RIGHT 45 // default Wert, ca. 0,9ms |
#define SERVO_I0_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I0_RIGHT_MAX 100 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I0_LEFT 45 // default Wert, ca. 2,1ms |
#define SERVO_I0_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I0_LEFT_MAX 100 // SERVO_MAX - SERVO_LEFT |
#define SERVO_I0_MIDDLE SERVO_STEPS/2 |
#define SERVO_I0_MIDDLE_MIN SERVO_STEPS/2 - 25 |
#define SERVO_I0_MIDDLE_MAX SERVO_STEPS/2 + 25 |
//prescaler 64 |
#define SERVO_I1_RIGHT 180 // default Wert, ca. 0,9ms |
#define SERVO_I1_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I1_RIGHT_MAX 400 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I1_LEFT 180 // default Wert, ca. 2,1ms |
#define SERVO_I1_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I1_LEFT_MAX 400 // SERVO_MAX - SERVO_LEFT |
//#define SERVO_I1_MIDDLE ((SERVO_STEPS + 1) * 4 - 1)/2 |
#define SERVO_I1_MIDDLE_MIN ((SERVO_STEPS + 1) * 4 - 1)/2 - 100 |
#define SERVO_I1_MIDDLE_MAX ((SERVO_STEPS + 1) * 4 - 1)/2 + 100 |
#define SERVO_REV 0 // kein Reverse |
/* Test Servo */ |
#define SERVO_PERIODE 20 // default Angabe in ms |
#define SERVO_PERIODE_MIN 10 // 10ms |
#define SERVO_PERIODE_MAX 30 // 30ms |
#define SINGLE_STEP 0 // Einzelschritt aus |
#define SINGLE_STEP_MIN 0 |
#define SINGLE_STEP_MAX 20 // bei prescaler 256, sonst * 4 (von links nach rechts in 9 Abschnitte) |
// zwischen den Schritten muss Pause > der Servoperiode sein, sonst keine Aktualisierung |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// The numbers below good for parallax servos at an F_CPU of 20.000MHz. |
// Da einige Servo's auch eien Winkel von 180 grd zulassen, Wertebereich |
// entgegen den sonst üblichen. Einschränkung mit default Kalibrierung |
// auf 0,9ms (45) bis 2,1ms(45) gesetzt. Je nach Servo, entspricht einen |
// Winkel von etwa 180grd |
// Periode default 20ms |
#define SERVO_MAX 211 // 2,7 ms bei prescaler 256, bei prescaler 64 SERVO_MAX * 4 |
#define SERVO_MIN 26 // 0,33ms bei prescaler 256, bei prescaler 64 SERVO_MIN * 4 |
#define SERVO_STEPS 255 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define SERVO_PRESCALER 256 // bei prescaler 256, bei prescaler 64 SERVO_PRESCALER / 4 |
#define STEPS_255 0 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define STEPS_1023 1 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
typedef struct //Servo-Konstante je nach Prescaler |
{ |
uint16_t max; |
uint16_t min; |
uint16_t steps; |
uint16_t prescaler; |
}ServoConst_t; |
typedef struct //struct_ServoChannel |
{ |
uint8_t pin; // hardware I/O port and pin for this channel |
uint16_t duty; // PWM duty setting which corresponds to servo position |
uint8_t rev; // Parameter, wie on/off; reverse; range |
uint16_t min; // SERVO_MIN + Parameter min |
uint16_t max; // SERVO_MAX - Parameter max |
uint16_t mid_scaled; // skalierte Servomitte |
int16_t mid; // Servomitte = SERVO_STEPS/2 +/- x Schritte; bei Pescaler 256 wird nur uint8_t benötigt aber bei 64 |
}ServoChannelType; |
//uint8_t sIdxSteps; // 0 für 255 und 1 für 1023 Schritte; Prescaler 256 oder 64 |
// Define servo limits (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
}servo_limit_t; |
extern const servo_limit_t servo_limit[2][3]; |
// Define servo positions (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
}steps_pw_t; |
typedef struct { |
uint8_t rev; |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
} servo_t; |
// Servopositionen für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
extern const steps_pw_t steps_pw[2]; |
#define RIGHT 0 // Servopostionen, welche vom Einbau abhängig sind |
#define LEFT 1 |
#define MIDDLE 2 |
#define POSIDX_MAX 4 |
extern const uint8_t PosIdx[POSIDX_MAX]; // anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
// functions |
void servoSetDefaultPos(void); |
// initializes servo system |
// You must run this to begin servo control |
void servoInit(uint8_t servo_period); |
// turns off servo system |
// This stops controlling the servos and |
// returns control of the SERVOPORT to your code |
void servoOff(void); |
// set servo position on a given channel |
// servoSetPosition() commands the servo on <channel> to the position you |
// desire. The position input must lie between 0 and POSITION_MAX and |
// will be automatically scaled to raw positions between SERVO_MIN and |
// SERVO_MAX |
void servoSetPosition(uint8_t channel, uint16_t position); |
// set raw servo position on a given channel |
// Works like non-raw commands but position is not scaled. Position must |
// be between SERVO_MIN and SERVO_MAX |
void servoSetPositionRaw(uint8_t channel, uint16_t position); |
// set servo to a specific angle |
void servoSetAngle(uint8_t servo_nr, int16_t angle); |
// vor servoInit(), oder vor sei() ServoWerte mit servoSet...() initialisieren, einschließlich servoSetPosition(...)! |
void servoSet_rev(uint8_t channel, uint8_t val); |
void servoSet_min(uint8_t channel, uint16_t min); |
void servoSet_max(uint8_t channel, uint16_t max); |
void servoSet_mid(uint8_t channel, uint16_t mid); |
uint16_t pw_us(uint16_t Steps); // gibt Zeit in µs bei x Servoschritte |
uint16_t ServoSteps(void); // gibt "Konstante" derzeitiger Servoschritte zürück |
#endif /* SERVO_H */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/servo_setup.c |
---|
0,0 → 1,1577 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY servo_setup.c |
//# |
//# 27.06.2014 OG |
//# - del: #include "tools.h" |
//# - del: #include "mymath.h" |
//# |
//# 30.05.2014 OG |
//# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben; |
//# einige Optimierungen an Aufruffunktionen (unnoetige Menue- |
//# Interfacefunktionen entfernt) |
//# |
//# 13.05.2014 OG |
//# - chg: Menu_Test_Start() - Variable 'range' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt |
//# wegen Warning: variable set but not used |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(), |
//# Setup_ServoTestCont(), Setup_ServoAdjust() |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Men�steuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "servo_setup.h" |
#include "tracking.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../menu.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#ifdef USE_TRACKING |
//#include "ng_usart.h" |
//#include "ng_config.h" |
//#include "servo.h" |
//#include "tools.h" |
//#include "mk.h" |
//#include "keys.h" |
//#include "mymath.h" |
// |
//GPS_Pos_t last5pos[7]; |
//uint8_t error1 = 0; |
//NaviData_t *naviData; |
//HomePos_t MK_pos; // Home position of station |
//GPS_Pos_t currentPos; // Current position of flying object |
//int8_t satsInUse; // Number of satelites currently in use |
//uint8_t tracking = TRACKING_MIN; |
//uint8_t track_hyst = TRACKING_HYSTERESE; |
//uint8_t track_tx = 0; |
//geo_t geo; |
//int16_t anglePan, angleTilt; // Servo Winkel |
//uint8_t coldstart = 1; |
uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden |
//uint8_t FCStatusFlags; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTracking() |
//----------------------------- |
#define MSERVO_STEPS 1 |
#define MSERVO_SERVO1 2 |
#define MSERVO_SERVO2 3 |
#define MSERVO_SERVOTEST 4 |
static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte"; |
static const char MSERVO_STEPS_en[] PROGMEM = "servo steps"; |
static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1"; |
#define MSERVO_SERVO1_en MSERVO_SERVO1_de |
static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2"; |
#define MSERVO_SERVO2_en MSERVO_SERVO2_de |
static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest"; |
#define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de |
//############################################################################ |
//----------------------------- |
// Setup_ServoTest() |
//----------------------------- |
#define MSERVOT_PULS 1 |
#define MSERVOT_CONT 2 |
#define MSERVOT_SERVO 3 |
#define MSERVOT_FRAME 4 |
static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulslänge"; |
static const char MSERVOT_PULS_en[] PROGMEM = "test puls width"; |
static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd."; |
static const char MSERVOT_CONT_en[] PROGMEM = "test cont."; |
static const char MSERVOT_SERVO_de[] PROGMEM = "Servo"; |
#define MSERVOT_SERVO_en MSERVOT_SERVO_de |
static const char MSERVOT_FRAME_de[] PROGMEM = "Periode"; |
static const char MSERVOT_FRAME_en[] PROGMEM = "frame"; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTestCont() |
//----------------------------- |
#define MSERVOTC_START 1 |
#define MSERVOTC_SINGLE 2 |
#define MSERVOTC_COUNT 3 |
#define MSERVOTC_PAUSEEND 4 |
#define MSERVOTC_PAUSEINC 5 |
static const char MSERVOTC_START_de[] PROGMEM = "Start Test"; |
static const char MSERVOTC_START_en[] PROGMEM = "start test"; |
static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt"; |
static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step"; |
static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test"; |
static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test"; |
static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos."; |
static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos"; |
static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc."; |
static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc."; |
//############################################################################ |
//----------------------------- |
// Setup_ServoAdjust() |
//----------------------------- |
#define MSERVOADJ_REV 1 |
#define MSERVOADJ_LEFT 2 |
#define MSERVOADJ_RIGHT 3 |
#define MSERVOADJ_MID 4 |
static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse"; |
static const char MSERVOADJ_REV_en[] PROGMEM = "reverse"; |
static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links"; |
static const char MSERVOADJ_LEFT_en[] PROGMEM = "left"; |
static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts"; |
static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right"; |
static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte"; |
static const char MSERVOADJ_MID_en[] PROGMEM = "middle"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void draw_input_screen( void ) |
{ |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); |
} |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */ |
///* :1 springt nach max_val auf min_val und umgedreht */ |
///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */ |
///* uint16_t Change_Value_plmi(...) :R�ckgabe ge�nderter Wert */ |
///* */ |
///************************************************************************************/ |
void Servo_tmp_Original(uint8_t track) |
{ |
servoSetDefaultPos(); |
// tracking = track; // urspr�nglicher Wert Tracking aus, RSSI oder GPS |
// NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet |
// Jump_Menu(pmenu); |
} |
uint8_t Servo_tmp_on(uint8_t servo_period) |
{ |
// uint8_t tmp_tracking = tracking; |
// tracking = 0; // Servopositionierung durch tracking abschalten |
// if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten |
servoInit(servo_period); |
// lcdGotoXY(0, 0); // lcd Cursor vorpositionieren |
// return(tmp_tracking); |
return (0); |
} |
void Displ_Off_On(uint16_t val) |
{ |
if (val == 0) lcd_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_at(17, 2, strGet(ON), 0); |
} |
uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct) |
{ |
// >> Menueauswahl nach oben |
if (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) |
{ |
if (val < max_val) { |
edit = 1; |
val++; |
} |
else |
{ |
val = min_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
// >> Menueauswahl nach unten |
if (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) |
{ |
if (val > min_val) { |
val--; |
edit = 1; |
} |
else |
{ |
val = max_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
return(val); |
} |
// |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t *val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */ |
///* Bit 6 zyklische Werte�nderung aus/ein */ |
///* Bit 4-5 z.Z. ohne Funktion */ |
///* Bit 0-3 Wert xPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t Change_Value(...) :R�ckgabe ge�ndert ergibt TRUE */ |
///* */ |
///************************************************************************************/ |
//// Bei Bedarf k�nnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) �bergeben werden |
uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct) |
{ |
uint16_t tmp_val; |
tmp_val = *val; |
Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Men�punkt |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) |
*val = Change_Value_plmi(*val, min_val, max_val, 16,2, Displ_Fnct); |
if (*val == tmp_val) { |
edit = 0; |
// lcd_printp_at (0, 5, PSTR("Edit=0"), 0); |
// _delay_ms(500); |
//// return (*val); |
} |
// |
else |
{ |
edit = 1; |
// lcd_printp_at (0, 5, PSTR("Edit=1"), 0); |
// _delay_ms(500); |
} |
return (tmp_val != *val); |
} |
uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid) |
{ uint16_t range; |
if (PosProzent < 0) { |
range = mid - min; |
// if (chrxs == CHRRS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
// chrxs = CHRLS; |
//// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// } |
} |
else { |
range = max - mid; |
// if (chrxs == CHRLS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
//// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRRS; |
// } |
} |
return(range); |
} |
/************************************************************************************/ |
/* zeigt einen max. 3-stelligen Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Format_Int( uint16_t val ) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Displ_PulseWidth( uint16_t val ) |
{ |
int16_t PosProzent, range; |
servoSetPositionRaw( servo_nr, val); |
PosProzent = val - steps_pw[Config.sIdxSteps].mid; |
range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid); |
PosProzent = (int32_t)1000 * PosProzent / range; |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent ); |
} |
/************************************************************************************/ |
/* zeigt Pausenl�nge der Links-, Mittel- und Rechtsposition auf Display an */ |
/* Parameter: */ |
/* uint16_t val : Zeit in 1ms * 100 */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause(uint16_t val) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val ); |
//lcdPuts("s "); |
} |
/************************************************************************************/ |
/* zeigt aus oder Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Off_Format_Int( uint16_t val ) |
{ |
if( val == 0 ) |
{ |
lcd_printp_at( 17, 2, strGet(OFF), MNORMAL); |
} |
else |
{ |
Displ_Format_Int( val ); |
} |
} |
/************************************************************************************/ |
/* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause_Step( uint16_t val ) |
{ |
Displ_Off_Format_Int( val ); |
} |
/************************************************************************************/ |
/* zeigt zu testende Servonummer zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_ServoNr( uint16_t val ) |
{ |
Displ_Format_Int( val+1 ); |
} |
/**************************/ |
/* */ |
/* Servos-Tests */ |
/* */ |
/**************************/ |
//void Menu_Servo_Test(void) |
//{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME}; |
// |
// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global |
// servo_nr = eeprom_read_byte(&ep_servo_nr); |
// Jump_Menu(pmenu); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Frame( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.servo_frame; |
if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) ) |
{ |
Config.servo_frame = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_ServoNr( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = servo_nr; |
if( Change_Value( &tmp_val, 0, 1, Displ_ServoNr) ) |
{ // pmenu global; es gibt nur 0=Servo1, 1=Servo2 |
servo_nr = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
// Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: f�r Modelleinstellungen ohne Empf�nger |
//-------------------------------------------------------------- |
void Menu_Test_PulseWidth( void ) |
{ |
uint8_t tmp_tracking; |
uint16_t tmp_val; |
draw_input_screen(); |
// 30.05.2014 OG: macht dieser Code Sinn? |
// Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen? |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
tmp_val = steps_pw[Config.sIdxSteps].mid; |
Change_Value( &tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, Displ_PulseWidth); // pmenu global |
cli(); |
servoInit( SERVO_PERIODE ); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_SingleStep(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.single_step; |
if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) ) |
{ // pmenu global |
Config.single_step = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Repeat( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.repeat; |
if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) ) |
{ |
Config.repeat = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause; |
if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) ) |
{ |
Config.pause = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause_Step( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause_step; |
if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) ) |
{ |
Config.pause_step = tmp_val; |
} |
} |
int8_t calc_dir(uint8_t idx, int16_t *Position) |
{ uint8_t nextIdx; |
int8_t nextDir = 1; |
nextIdx = idx; |
if ((idx + 1) < POSIDX_MAX) |
nextIdx++; |
else |
nextIdx = 0; |
if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1; |
return(nextDir); |
} |
void Displ_LoopCounter( uint8_t val ) |
{ |
lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val ); |
//lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0); |
} |
// Test �ber Scalierung der Servos mit Anschlagkalibrierung |
void Menu_Test_Start(void) |
{ |
uint8_t tmp_tracking, idx, rep; |
int8_t dir; |
int16_t sPos; |
int16_t Position[3]; |
//int16_t range; |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRLS; // Flag, welche K�stchensymbole geladen |
// Displ_Title(MSG_CONTINOUS); |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
Displ_LoopCounter(Config.repeat); |
Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl m�glich |
Position[1] = ServoSteps()/2; |
Position[2] = ServoSteps(); |
// init Einzelschritt |
idx = 0; |
dir = calc_dir(idx, Position); |
sPos = Position[PosIdx[idx]]; |
idx++; |
rep = Config.repeat; |
// Test bis Ende der Wiederholungen oder irgendein Enter |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) { |
calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
//range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
// draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile |
servoSetPosition(servo_nr, sPos); |
if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht? |
sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren |
if (((dir < 0) && (sPos < Position[PosIdx[idx]])) || ((dir > 0) && (sPos > Position[PosIdx[idx]])) || !(Config.single_step)) |
sPos = Position[PosIdx[idx]]; // �berlauf bei variabler Schrittweite ber�cksichtigen oder Einzelschritt |
Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 w�rden welche �bersprungen, zus�tzlich pro Servoschritt verz�gert |
} |
else { |
dir = calc_dir(idx, Position); // Richtungs�nderung |
if (idx < (POSIDX_MAX - 1)) { |
if (idx == 0) { |
rep--; // bei jeden vollen Durchlauf Wiederholz�hler verringern |
Displ_LoopCounter(rep); |
} |
idx++; // Index f�r n�chsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge |
} |
else |
idx = 0; |
_delay_ms( Config.pause*100 ); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf) |
} |
} |
// lcdClear(); |
// if (pmenu[0] == '\0') |
// Displ_Main_Disp(); |
// else |
// return_m_pkt(strlen(pmenu)); // um bei R�cksprung auf urspr�nglichen Men�punkt zeigen oder Displ_Main_Disp() |
// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char f�r Bargraph zur�ckschreiben |
cli(); |
servoInit(SERVO_PERIODE); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
/********************************************************************************/ |
/* zeigt Servo-Anschlagposition links auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/********************************************************************************/ |
void Displ_Servo_Min(uint16_t val) |
{ |
uint16_t steps = 0; |
write_ndigit_number_s( 15, 2, val, 5, 0,0); |
servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = ServoSteps(); |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition rechts auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Max(uint16_t val) |
{ |
uint16_t steps = ServoSteps(); |
write_ndigit_number_u( 15, 2, val, 5, 0,0); |
servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = 0; |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition Mitte auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Mid( uint16_t val ) |
{ |
int16_t mid_val; |
mid_val = val - ServoSteps()/2; |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val ); |
servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die |
servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_rev( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
tmp_val = Config.servo[servo_nr].rev; |
if( Change_Value(&tmp_val , 0, 1, Displ_Off_On) ) |
{ //reverse gibt es nur 0=off, 1=on |
Config.servo[servo_nr].rev = tmp_val ; |
servoSet_rev(servo_nr, tmp_val ); |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_left( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].max; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) ) |
{ |
Config.servo[servo_nr].max = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_right( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].min; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) ) |
{ |
Config.servo[servo_nr].min = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_middle( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].mid; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) ) |
{ |
Config.servo[servo_nr].mid = tmp_val; |
} |
Servo_tmp_Original(tmp_tracking); |
} |
void Servo_NewValues(uint8_t idx_presc) |
{ |
for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) { |
if (idx_presc == STEPS_255) { // Werte umrechnen f�r Prescaler = 256 |
Config.servo[i].min /= 4; |
Config.servo[i].max /= 4; |
Config.servo[i].mid /= 4; |
} |
else { // Werte umrechnen f�r Prescaler = 64 |
Config.servo[i].min *= 4; |
Config.servo[i].max *= 4; |
Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1; |
} |
servoSet_min(i, Config.servo[i].min); |
servoSet_max(i, Config.servo[i].max); |
servoSet_mid(i, Config.servo[i].mid); |
// eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t)); |
} |
// Vorberechnung von ServoChannels[channel].duty |
servoSetDefaultPos(); // Ausgangsstellung beider Servos |
} |
/************************************************************************************/ |
/* zeigt Servoschritte zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = 255 oder 1 = 1023, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Steps(uint16_t val) |
{ |
if (val==0) |
lcd_puts_at(16,2,INTERNAT_STEPS_255,0 ); |
else |
lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Servo_Steps(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.sIdxSteps; |
if( Change_Value( &tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps) ) |
{ |
cli(); |
Config.sIdxSteps = tmp_val; |
Servo_NewValues( Config.sIdxSteps ); // hier ist der neue Index anzugeben! |
servoInit(SERVO_PERIODE); |
sei(); |
} |
} |
//-------------------------------------------------------------- |
// Setup_ServoAdjust() |
//-------------------------------------------------------------- |
void Setup_ServoAdjust( uint8_t servo ) |
{ |
servo_nr = servo; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1" |
//MenuCtrl_SetTitle_P( PSTR("Servo Adjust") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTestCont() |
//-------------------------------------------------------------- |
void Setup_ServoTestCont( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd." |
//MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en ); |
MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en ); |
MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTest() |
//-------------------------------------------------------------- |
void Setup_ServoTest( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servotest" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en ); |
MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en ); |
MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en ); |
MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTracking() |
//-------------------------------------------------------------- |
void Setup_ServoTracking( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Tracking" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); } |
if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//void start_tracking(void) |
//{ |
// #define TIMEOUT 200 // 2 sec |
// |
// uint16_t old_anglePan = 0; |
// uint16_t old_angleTilt = 0; |
// |
// //uint16_t old_hh = 0; |
// uint8_t flag; |
// uint8_t tmp_dat; |
// |
// lcd_cls (); |
// //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2); |
// |
// //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0); |
// |
// lcd_ecircle(22, 35, 16, 1); |
// lcd_ecircle(88, 35, 16, 1); |
// lcd_putc (10, 1, 0x1e, 0); // degree symbol |
// lcd_putc (20, 1, 0x1e, 0); // degree symbol |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// SwitchToNC(); |
// |
// mode = 'O'; |
// |
// // disable debug... |
// // RS232_request_mk_data (0, 'd', 0); |
// tmp_dat = 0; |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// if(error1 == 1) |
// lcd_cls(); |
// } |
// |
// GPS_Pos_t currpos; |
// currpos.Latitude = naviData->CurrentPosition.Latitude; |
// currpos.Longitude = naviData->CurrentPosition.Longitude; |
// |
// flag = 0; |
// timer = TIMEOUT; |
// abo_timer = ABO_TIMEOUT; |
// |
// coldstart = 1; |
// |
// do |
// { |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// |
////CB uint8_t FCStatusFlag = naviData->FCFlags; |
// uint8_t FCStatusFlag = naviData->FCStatusFlags; |
// //write_ndigit_number_u (0, 0, FCStatusFlag); |
// |
// Tracking_GPS(); |
// |
// //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
// |
// // alte Linien l�schen |
// //lcd_ecirc_line (22, 35, 15, old_hh, 0); |
// //old_hh = heading_home; |
// lcd_ecirc_line (22, 35, 15, old_anglePan, 0); |
// old_anglePan = anglePan; |
// lcd_ecirc_line (88, 35, 15, old_angleTilt, 0); |
// old_angleTilt = angleTilt; |
// |
// lcd_ecirc_line (22, 35, 15, anglePan, 1); |
// write_ndigit_number_u (7, 1, anglePan, 3, 0,0); |
// lcd_ecirc_line (88, 35, 15, angleTilt, 1); |
// write_ndigit_number_u (17, 1, angleTilt, 3, 0,0); |
// |
// rxd_buffer_locked = FALSE; |
// |
// if (!abo_timer) |
// { // renew abo every 3 sec |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// abo_timer = ABO_TIMEOUT; |
// } |
// } |
// |
// if (!timer) |
// { |
// OSD_Timeout(flag); |
// flag = 0; |
// } |
// } |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// //lcd_cls(); |
// //return; |
//} |
//-------------------------------------------------------------- |
// |
//void conect2at_unit(void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_ser (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_bt (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
//-------------------------------------------------------------- |
//void conect2gps_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_CONECT_GPS; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// conect2gps_ser(); |
// if((val+Offset) == 2 ) |
// conect2gps_bt(); |
// } |
//} |
//-------------------------------------------------------------- |
//void tracking_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_AT; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// test_servo_menu(); |
// if((val+Offset) == 2 ) |
// adjust_servo_menu(); |
// if((val+Offset) == 3 ) |
// show_angle(); |
// if((val+Offset) == 4 ) |
////TODO: start_tracking(); |
// if((val+Offset) == 5 ) |
// conect2at_unit(); |
// if((val+Offset) == 6 ) |
// conect2gps_menu(); |
// } |
//} |
//-------------------------------------------------------------- |
// kapeschi Ant.Treking Funktionen |
//-------------------------------------------------------------- |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ int32_t lat1, lon1, lat2, lon2; |
// int32_t d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = pos->Longitude; |
// lat2 = pos->Latitude; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator |
// // es wird jedoch in dm Meter weiter gerechnet |
// // (tlon1 - tlon2)/10) sonst uint32_t-�berlauf bei cos(0) gleich 1 |
// d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
// dlat = 1113 * (lat1 - lat2) / 10000; |
// geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
// geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
//void do_tracking(void) |
//{ //static uint8_t hysteresis = 0; |
// // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
// // (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
// |
// geo = calc_geo(&MK_pos, ¤tPos); |
// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
// //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
// //geo.bearing = MK_pos.direction; |
// //angleTilt = 0; |
// //hysteresis = 1; |
// //} |
// //else { |
// //hysteresis = 0; |
// //} |
//// |
// //// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen |
// //// 360 grd negative Winkelwerte als positive |
// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
// |
// //if (angleTilt < 0) angleTilt = 0; |
// //if (angleTilt > 180) angleTilt = 180; |
//// |
// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
// //anglePan = anglePan - 180; |
// //angleTilt = 180 - angleTilt; |
// // |
// //} |
////angleTilt = 180; |
////angleTilt = 180; |
// |
//// servoSetAngle(0, anglePan); |
//// servoSetAngle(1, angleTilt); |
//} |
/****************************************************************/ |
/* */ |
/* MK GPS Tracking */ |
/* */ |
/****************************************************************/ |
// MK OSD-Daten lesen und verifizieren |
//uint8_t OSD_Data_valid(NaviData_t **navi_data) |
//{ uint8_t ret = 0; |
//char *tx_osd = {"#co?]==EH\r"}; |
//// char interval[2] = {10, '\0'}; |
// |
//if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren |
////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
//currentPos = (*navi_data)->CurrentPosition; |
//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
//ret = 1; |
//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
//// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//satsInUse = (*navi_data)->SatsInUse; |
//} |
//} |
//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval |
//return ret; |
//} |
// |
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
// Hauptprogramm von GPS Antennen-Nachf�hrung |
//void Tracking_GPS(void) |
//{ //NaviData_t *navidata; |
// static uint8_t track_running = 0; |
// |
// if (!track_running) |
// { |
// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
// //if (OSD_Data_valid(&naviData)) { |
// if (coldstart) |
// { |
// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen |
// //if (naviData->FCFlags & FC_FLAG_MOTOR_START) |
// //{ |
// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0; |
// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0; |
// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude; |
// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude; |
// MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
// MK_pos.direction = naviData->CompassHeading; |
// coldstart = 0; |
// //} |
// //} |
// //else { |
// //do_tracking(); |
// } |
// //} |
// track_running = 0; |
// } |
// do_tracking(); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/servo_setup.h |
---|
0,0 → 1,45 |
#ifndef _TRACKING_H_ |
#define _TRACKING_H_ |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define DLEFT 0 |
#define DRIGHT 1 |
#define AltFaktor 22.5 |
#define PAN_SERVO_CORRECT 3.125 |
#define TILT_SERVO_CORRECT 3.125 |
typedef void (*Displ_Fnct_t)( uint16_t ); |
//typedef struct { |
// int32_t distance; |
// int16_t bearing; |
//}geo_t; |
//typedef struct { |
// double Home_Lon; // in degrees |
// double Home_Lat; // in degrees |
// int32_t Home_Lon7; // in 1E-7 degrees |
// int32_t Home_Lat7; // in 1E-7 degrees |
// int32_t Home_Alt; // in mm |
// // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
// int16_t direction; |
//}__attribute__((packed)) HomePos_t; |
#define INTERNAT_STEPS_255 "255 " |
#define INTERNAT_STEPS_1023 "1023" |
// kapeschi |
void Setup_ServoTracking( void ); |
void Tracking_GPS(void); |
void Tracking_NMEA(void); |
void Tracking_RSSI(void); |
void setNMEAdir(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/tracking.c |
---|
0,0 → 1,648 |
/* |
* tracking.c |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.c |
//# |
//# 27.06.2014 OG |
//# - umgestellt auf gps/gps_nmea.c |
//# - Codeformattierung |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# - del: #include "tools.h" |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_tracking() - auskommentierungen um Zugriffe auf Config.gps_UsedGPSMouse |
//# zu unterbinden. Anmerkung dazu siehe dort. |
//# |
//# 22.06.2014 OG |
//# - chg: Tracking_NMEA() - do_tracking() auskommentiert (spaeter fuer |
//# Tracking wieder geradeziehen) |
//# - Variable 'CheckGPS' hinzugefuegt weil aus anderen Sourcen (setup.c) entfernt |
//# |
//# 18.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# |
//# 01.06.2014 OG |
//# - chg: PKT_trackingMK() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# |
//# 30.05.2014 OG |
//# - chg: calc_geo()- my_itoa() ersetzt durch writex_gpspos() und lcdx_printf_at_P() |
//# weil my_itoa() nicht mehr vorhanden |
//# |
//# 13.05.2014 OG |
//# - chg: PKT_trackingBT() - Variable 'BT_WhasOn' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: PKT_trackingBT() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# |
//# 28.04.2014 OG |
//# - chg: PKT_trackingBT(), PKT_trackingMK() - OSD_Timeout() entfernt (weil es |
//# das nicht mehr gibt) und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, tracking hat niedrige Prio) |
//# - add: include "../pkt/pkt.h" |
//# |
//# 12.02.2014 OG |
//# - del: mk_param_struct entfernt |
//# |
//# 29.08.2013 Cebra |
//# - chg: falsche Parameterübergabe bei getLatitude/getLongitude |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING . NMEA Routinen werden nur noch für BT_NMEA genutzt |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
//++++++++++++++++++++++++++++++++++ |
#ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
#include "../tracking/tracking.h" |
#include "../tracking/ng_servo.h" |
#include <avr/pgmspace.h> |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../lcd/lcd.h" |
#include "../mk-data-structs.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../gps/mymath.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
uint8_t CheckGPS = true; // Patch 22.06.2014 OG: hier integriert weil ais anseren Sourcen entfernt (u.a. setup.c) |
char data_decode[RXD_BUFFER_SIZE]; |
#define DLEFT 0 |
#define DRIGHT 1 |
#define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
#define AltFaktor 22.5 |
#define TIMEOUT 200 // 2 sec |
NaviData_t *naviData; |
HomePos_t MK_pos; // Home position of station |
GPS_Pos_t currentPos; // Current position of flying object |
uint8_t tracking = TRACKING_MIN; |
uint8_t track_hyst = TRACKING_HYSTERESE; |
uint8_t track_tx =0; |
uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
geo_t geo; |
int16_t anglePan, angleTilt; |
//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
//// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ double lat1, lon1, lat2, lon2, d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = (double)pos->Longitude / 10000000.0; |
// lat2 = (double)pos->Latitude / 10000000.0; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
// // es wird jedoch in Meter weiter gerechnet |
// d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
// dlat = 111300 * (double)(lat1 - lat2); |
// // returns a value in metres http://www.kompf.de/gps/distcalc.html |
// geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
// if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
// geo.distance = sqrt(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//-------------------------------------------------------------- |
// bei Gelegenheit den Code auf gps/gps.c/gps_Deviation() aendern |
//-------------------------------------------------------------- |
geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
geo_t geo; |
lon1 = home->Home_Lon; |
lat1 = home->Home_Lat; |
lon2 = pos->Longitude; |
lat2 = pos->Latitude; |
if( !CheckGPS ) |
{ |
writex_gpspos( 0, 3, home->Home_Lat , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 3, home->Home_Lon , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
writex_gpspos( 0, 4, pos->Latitude , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 4, pos->Longitude , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
//lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
} |
// Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator |
// es wird jedoch in dm Meter weiter gerechnet |
// (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1 |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = 1113 * (lat1 - lat2) / 10000; |
geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
if( !CheckGPS ) |
{ |
lcd_printp_at (0, 5, PSTR("Bear:"), 0); |
lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing ); |
//lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance ); |
//lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (20, 5, PSTR("m"), 0); |
} |
return(geo); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void do_tracking( void ) |
{ |
static uint8_t hysteresis = 0; |
geo = calc_geo(&MK_pos, ¤tPos); |
angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance); |
if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) |
{ // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
geo.bearing = MK_pos.direction; |
if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf |
hysteresis = 1; |
} |
else |
{ |
hysteresis = 0; |
} |
// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
// 360 grd negative Winkelwerte als positive |
anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
if (angleTilt < 0) |
angleTilt = 0; |
if (angleTilt > 180) |
angleTilt = 180; |
if (anglePan >= 180) |
{ // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
anglePan = anglePan - 180; |
angleTilt = 180 - angleTilt; |
} |
servoSetAngle(0, anglePan); |
servoSetAngle(1, angleTilt); |
if (!CheckGPS) |
{ |
lcd_printp_at (0, 6, PSTR("Pan :"), 0); |
write_ndigit_number_u (6, 6, anglePan, 3, 1,0); |
lcd_printp_at (11, 6, PSTR("Tilt:"), 0); |
write_ndigit_number_u (17, 6, angleTilt, 3, 1,0); |
} |
// write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0); |
// |
// write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingBT( void ) // Tracking mit NMEA-Daten von BT-Maus |
{ |
//uint8_t BT_WhasOn = 0; |
uint8_t BT_status; |
//uint8_t flag; |
uint8_t tmp_dat; |
coldstart =1; |
//lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0); |
lcd_puts_at (0, 1,Config.gps_UsedDevName, 0); |
//set_BTOn(); |
set_Modul_On(Bluetooth); |
//BT_WhasOn = true; |
if( Config.BTIsSlave==true ) |
{ |
bt_downlink_init(); |
} |
lcd_printp_at (18, 1, PSTR(" ?? "), 0); |
BT_status = bt_connect(Config.gps_UsedMac); |
if( BT_status==true ) |
{ |
lcd_printp_at (18, 1, PSTR(" OK "), 0); |
receiveNMEA = true; |
} |
else |
{ |
lcd_printp_at (17, 1, PSTR("FAIL"), 2); |
} |
if( receiveNMEA==true ) |
{ |
lcd_cls_line( 0, 1, 20); |
lcd_printp_at( 0, 2, PSTR(" Latitude Longitude"), 2); |
lcd_printp_at( 0, 3, PSTR("H"), 0); |
lcd_printp_at( 0, 4, PSTR("M"), 0); |
bt_rx_ready = 0; |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benoetigt für Navidata Ausgabe an SV2 |
//flag = 0; |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//bt_rx_ready = 0; |
// if( !NMEA_receiveBT() ) |
// break; |
if (!NMEA_isdataready()) |
break; |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
currentPos = naviData->CurrentPosition; |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//----------------- |
// MK: Lat / Long |
//----------------- |
writex_gpspos( 2, 4, currentPos.Latitude , MNORMAL, 0,0 ); // MK: Latitude |
writex_gpspos(12, 4, currentPos.Longitude, MNORMAL, 0,0 ); // MK: Longitude |
// NMEA_GetNewData(); // neue NMEA GPGGA Daten von der BT GPA-Maus holen |
//----------------- |
// GPS-Maus: Lat / Long |
//----------------- |
writex_gpspos( 2, 3, NMEA.Latitude , MNORMAL, 0,0 ); // GPS-Maus: Latitude |
writex_gpspos(12, 3, NMEA.Longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
//do_tracking(); // das geht so noch nicht mit einer BT GPS-Maus! do_tracking(); ueberarbeiten! |
if( !CheckGPS ) |
{ |
//lcd_printp_at( 0, 2, PSTR("GPS Time: "), 0); |
lcd_puts_at( 13, 0, NMEA.Time , 2); |
lcd_printp_at( 16, 1, PSTR("Sat:"), 0); |
write_ndigit_number_u( 19, 1, NMEA.SatsInUse, 2, 1,0); |
lcd_printp_at( 0, 1, PSTR("Fix:"), 0); |
write_ndigit_number_u( 4, 1, NMEA.SatFix, 1, 1,0); |
lcd_printp_at( 6, 1, PSTR("HDOP:"), 0); |
write_ndigit_number_u_10th( 11, 1, NMEA.HDOP, 3, 0,0); |
} |
rxd_buffer_locked = FALSE; |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if (rxd_buffer_locked) |
if( !timer ) |
{ |
//OSD_Timeout(flag); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
//flag = 0; |
error = 1; |
} |
} while( !get_key_press (1 << KEY_ESC) || !receiveNMEA==true || error ==1); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if( !receiveNMEA ) |
lcd_printp_at (0, 2, PSTR("lost BT data"), 0); |
lcd_printp_at (0, 3, PSTR("GPS trennen"), 0); |
} // end: if( receiveNMEA==true ) |
else |
{ |
lcd_printp_at (0, 4, PSTR("Error at connecting"), 0); |
lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0); |
while( !get_key_press (1 << KEY_ENTER) ); |
} |
receiveNMEA = false; |
if( !bt_disconnect() ) |
lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0); |
//set_BTOff(); |
set_Modul_On(USB); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingMK( void ) // Tracking mit GPS-Daten vom Mikrokopter |
{ |
//uint8_t BT_WhasOn = 0; |
//uint8_t BT_status; |
uint8_t GPSfix=0; |
uint8_t tmp_dat; |
uint8_t toggletimer=0; |
coldstart = true; |
lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
lcd_cls_line (0,1,20); |
//lcd_printp_at (0, 3, PSTR("H"), 0); |
//lcd_printp_at (0, 4, PSTR("M"), 0); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
error = 0; |
do |
{ |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
//OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1); |
//if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//OSD_Element_SatsInUse( 18, 1, 1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//if (GPSfix == true) OSD_Element_StatusFlags( 0, 1); |
if (!(naviData->NCFlags & NC_FLAG_GPS_OK)) |
{ |
toggletimer++; |
if (toggletimer == 50) toggletimer = 0; |
if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2); |
if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0); |
rxd_buffer_locked = false; |
GPSfix = false; |
} |
else GPSfix = true; |
if( GPSfix ) |
{ |
if( coldstart ) |
{ |
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) |
{ |
MK_pos.Home_Lon = naviData->HomePosition.Longitude; |
MK_pos.Home_Lat = naviData->HomePosition.Latitude; |
MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
MK_pos.direction = naviData->CompassHeading; |
coldstart = false; |
rxd_buffer_locked = false; |
lcd_printp_at(0,1, PSTR(" "), 0); |
} |
else |
{ |
lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0); |
rxd_buffer_locked = false; |
} |
} // end: if( coldstart ) |
else |
{ |
//run |
currentPos = naviData->CurrentPosition; |
currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
do_tracking(); |
//lcd_puts_at (13, 0, NMEAtime, 2); |
//lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
//write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
//lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
//write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
//lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
//write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
rxd_buffer_locked = FALSE; |
} // run |
} |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if( rxd_buffer_locked ) |
if( !timer ) |
{ |
//OSD_Timeout(1); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
error = 1; |
} |
} while( (!get_key_press (1 << KEY_ENTER)) && (error ==0) ); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if (error ==1) |
{ |
lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
_delay_ms(2000); |
} |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_tracking(void) |
{ |
clear_key_all(); |
lcd_cls (); |
// 25.06.2014 OG: auskommentiert weil erstmal kein Config.gps_UsedGPSMouse mehr unterstuetzt |
// wird. Falls tracking.c mal richtig implementiert wird sollte man sich auf eine BT-GPS-Maus |
// konzentrieren bis Tracking einwandfrei laeuft - wenn dann noch immer Bedarf nach MK-GPS ist |
// kann man das ja wieder einbauen |
// |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT(); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK(); |
lcd_printp_at( 0,0, PSTR("Tracking Bluetooth "), MINVERS); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
PKT_trackingBT(); |
clear_key_all(); |
} |
//++++++++++++++++++++++++++++++++++ |
#endif // #ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking/tracking.h |
---|
0,0 → 1,77 |
/* |
* tracking.h |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.h |
//# |
//# 27.06.2014 OG |
//# - Entkernt: verschiedene Variablen und Funktionen geloescht |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# - add: Source-Historie |
//############################################################################ |
#ifndef TRACKING_H_ |
#define TRACKING_H_ |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// Antennen-Nachführung |
#define TRACKING_MIN 0 // aus, TRACKING_RSSI, TRACKING_GPS, TRACKING_MKCOCKPIT, TRACKING_NMEA |
#define TRACKING_MAX 4 |
// Antennen-Nachführung per RSSI |
#define TRACKING_HYSTERESE 40 // Hysterese bevor Tracking bei Richtungswechsel anspricht |
#define TRACKING_HYST_MIN 0 |
#define TRACKING_HYST_MAX 100 |
#define FC_FLAG_MOTOR_RUN 0x01 |
#define FC_FLAG_FLY 0x02 |
#define FC_FLAG_CALIBRATE 0x04 |
#define FC_FLAG_MOTOR_START 0x08 |
typedef struct { |
int32_t distance; |
int16_t bearing; |
} geo_t; |
typedef struct { |
int32_t Home_Lon; // in 1E-7 degrees |
int32_t Home_Lat; // in 1E-7 degrees |
int32_t Home_Alt; // in mm |
int16_t direction; // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
} __attribute__((packed)) HomePos_t; |
extern uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
#ifdef USE_TRACKING |
void PKT_tracking(void); |
#endif // #ifdef USE_TRACKING |
#endif // TRACKING_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/tracking |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/uart/uart1.c |
---|
0,0 → 1,638 |
/************************************************************************* |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.c,v 1.6.2.2 2009/11/29 08:56:12 Peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4.6 or higher |
Hardware: any AVR with built-in UART, |
License: GNU General Public License |
DESCRIPTION: |
An interrupt is generated when the UART has finished transmitting or |
receiving a byte. The interrupt handling routines use circular buffers |
for buffering received and transmitted data. |
The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define |
the buffer size in bytes. Note that these variables must be a |
power of 2. |
USAGE: |
Refere to the header file uart.h for a description of the routines. |
See also example test_uart.c. |
NOTES: |
Based on Atmel Application Note AVR306 |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
*************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.c |
//# |
//# 08.08.2015 CB |
//# - chg: Umbau ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - add: Erweiterung ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Funktion uart1_peek |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdbool.h> |
#include "uart1.h" |
#include "usart.h" |
#include "../main.h" |
#include "../bluetooth/bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../avr-nmea-gps-library/nmea.h" |
// |
// constants and macros |
// |
// size of RX/TX buffers |
#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1) |
#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1) |
#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK ) |
#error RX buffer size is not a power of 2 |
#endif |
#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK ) |
#error TX buffer size is not a power of 2 |
#endif |
// ATmega with two USART |
#define ATMEGA_USART1 |
#define UART1_STATUS UCSR1A |
#define UART1_CONTROL UCSR1B |
#define UART1_DATA UDR1 |
#define UART1_UDRIE UDRIE1 |
// |
// module global variables |
// |
#if defined( ATMEGA_USART1 ) |
static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE]; |
static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE]; |
static volatile unsigned char UART1_TxHead; |
static volatile unsigned char UART1_TxTail; |
static volatile unsigned char UART1_RxHead; |
static volatile unsigned char UART1_RxTmpHead; |
static volatile unsigned char UART1_RxTail; |
static volatile unsigned char UART1_LastRxError; |
static volatile unsigned char UART1_RxTmpHead;; |
volatile uint16_t UART1_RxError; |
#endif |
volatile uint8_t UART1_receiveNMEA = false; |
void SetBaudUart1(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: uart1_init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: uart1_init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
// case Baud_9600: uart1_init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_9600: uart1_init( 129); break; |
case Baud_19200: uart1_init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: uart1_init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_57600: uart1_init( 21); break; |
// case Baud_115200: uart1_init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
case Baud_115200: uart1_init( 10 ); break; |
} |
} |
// |
// these functions are only for ATmegas with two USART |
// |
#if defined( ATMEGA_USART1 ) |
#if 0 |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
// read UART status register and UART data register |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
UART1_LastRxError = lastRxError; |
} |
#else |
#warning Test neue UART ISR für NMEA |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
if (UART1_receiveNMEA) |
{ |
fusedata(data); // NMEA Daten bearbeiten |
} |
else // if (UART1_receiveNMEA) |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
} |
UART1_LastRxError = lastRxError; |
} |
////-------------------------------------------------------------- |
//// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
//// Purpose: called when the UART1 has received a character |
////-------------------------------------------------------------- |
//ISR(USART1_RX_vect) |
//{ |
// unsigned char tmphead; |
// unsigned char data; |
// unsigned char usr; |
// unsigned char lastRxError; |
// |
// static volatile uint8_t GGA; |
// static char buffer[6]; |
// static volatile uint8_t bufferindex; |
// static volatile uint8_t NMEAstart,NMEAend = false; |
// |
// // read UART status register and UART data register |
// usr = UART1_STATUS; |
// data = UART1_DATA; |
// |
// lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// |
// // calculate buffer index |
// |
// tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
// |
// if ( tmphead == UART1_RxTail ) |
// { |
// // error: receive buffer overflow |
// lastRxError = UART_BUFFER_OVERFLOW >> 8; |
// UART1_RxError++; |
// } |
// else |
// { |
// if (UART1_receiveNMEA) |
// { |
// cli(); |
// switch(data) |
// { |
// case '$': //start of a packet |
// |
// if (!NMEAstart && GGA) // falls mitten im Datensatz eine neuer kommt, warum auch immer |
// { |
// // restore index, war kein GPGGA Datensatz |
// UART1_RxHead = UART1_RxTmpHead; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// } |
// NMEAstart = true; |
// NMEAend = false; |
// GGA=false; |
// bufferindex=0; //wipe all these so they can be reused |
// |
// UART1_RxTmpHead = tmphead-1; // erstmal sichern, Start des NMEA Datensatzes |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// sei(); |
// break; |
// |
// default: //we have some of the CSV data |
// if(bufferindex<6) //dont mess up ! Dont overflow |
// { |
// buffer[bufferindex]=data; //stick the character in our buffer |
// } |
// |
// if(GGA) |
// { |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// switch ( data) |
// { |
// case '\r' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// |
// case '\n' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// } |
// |
// } |
// |
// else if(NMEAstart) //the header, erstes Komma noch nicht gekommen |
// |
// { |
// if(bufferindex<4) |
// |
// { |
// bufferindex++; //increase the position in the buffer |
// |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// } |
// else |
// |
// { |
//// cli(); |
// if(!strcmp(buffer,"GPGGA")) //the last character will be a 0(end of string) |
// |
// { |
// GGA=true; |
// NMEAstart = false; |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
//// sei(); |
// } |
// |
// else |
// |
// { |
// // restore index, war kein GPGGA Datensatz |
// |
// UART1_RxHead = UART1_RxTmpHead; |
//// commacount=0; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
//// sei(); |
// } |
// } |
// } |
// else if (!NMEAend && !GGA) UART1_RxHead = UART1_RxTmpHead; // Zeichen verwerfen |
// |
// } // end switch (data) |
// } |
// |
// else // if (UART1_receiveNMEA) |
// |
// { |
// // store new index |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
// } |
// } |
// UART1_LastRxError = lastRxError; |
//} |
#endif |
//-------------------------------------------------------------- |
// Function: UART1 Data Register Empty interrupt |
// Purpose: called when the UART1 is ready to transmit the next byte |
//-------------------------------------------------------------- |
ISR(USART1_UDRE_vect) |
{ |
unsigned char tmptail; |
if ( UART1_TxHead != UART1_TxTail) |
{ |
// calculate and store new buffer index |
tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK; |
UART1_TxTail = tmptail; |
// get one byte from buffer and write it to UART |
UART1_DATA = UART1_TxBuf[tmptail]; // start transmission |
} |
else |
{ |
// tx buffer empty, disable UDRE interrupt |
UART1_CONTROL &= ~_BV(UART1_UDRIE); |
} |
} |
//-------------------------------------------------------------- |
// Function: uart1_init() |
// Purpose: initialize UART1 and set baudrate |
// Input: baudrate using macro UART_BAUD_SELECT() |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_init(unsigned int baudrate) |
{ |
UART1_TxHead = 0; |
UART1_TxTail = 0; |
UART1_RxHead = 0; |
UART1_RxTail = 0; |
UART1_RxError = 0; |
UART1_receiveNMEA = false; //Empfang von NMEA Daten aus |
// Set baud rate |
if ( baudrate & 0x8000 ) |
{ |
UART1_STATUS = (1<<U2X1); //Enable 2x speed |
baudrate &= ~0x8000; |
} |
UBRR1H = (unsigned char)(baudrate>>8); |
UBRR1L = (unsigned char) baudrate; |
// Enable USART receiver and transmitter and receive complete interrupt |
UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1); |
// Set frame format: asynchronous, 8data, no parity, 1stop bit |
#ifdef URSEL1 |
UCSR1C = (1<<URSEL1)|(3<<UCSZ10); |
#else |
UCSR1C = (3<<UCSZ10); |
// UCSR1C = (1<<UCSZ11) | (1<<UCSZ10); // 8data Bit |
#endif |
} |
//-------------------------------------------------------------- |
// Function: uart1_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
unsigned int uart1_getc(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return UART_NO_DATA; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
UART1_RxTail = tmptail; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//-------------------------------------------------------------- |
// Function: uart1_peek() |
// Purpose: Returns the next byte (character) of incoming serial data without removing it from the internal serial buffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
int uart1_peek(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return -1; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//int HardwareSerial::peek(void) |
//{ |
// if (_rx_buffer->head == _rx_buffer->tail) { |
// return -1; |
// } else { |
// return _rx_buffer->buffer[_rx_buffer->tail]; |
// } |
//} |
//-------------------------------------------------------------- |
// Function: uart1_putc() |
// Purpose: write byte to ringbuffer for transmitting via UART |
// Input: byte to be transmitted |
// Returns: 1 on succes, 0 if remote not ready |
//-------------------------------------------------------------- |
int uart1_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK; |
while ( tmphead == UART1_TxTail ) |
{;} // wait for free space in buffer |
UART1_TxBuf[tmphead] = data; |
UART1_TxHead = tmphead; |
// enable UDRE interrupt |
UART1_CONTROL |= _BV(UART1_UDRIE); |
return (UART1_LastRxError << 8) + data; |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts() |
// Purpose: transmit string to UART1 |
// Input: string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts(const char *s ) |
{ |
while (*s) |
uart1_putc(*s++); |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts_p() |
// Purpose: transmit string from program memory to UART1 |
// Input: program memory string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts_p(const char *progmem_s ) |
{ |
register char c; |
while ( (c = pgm_read_byte(progmem_s++)) ) |
uart1_putc(c); |
} |
//-------------------------------------------------------------- |
// Function: uart1_available() |
// Purpose: Determine the number of bytes waiting in the receive buffer |
// Input: None |
// Returns: Integer number of bytes in the receive buffer |
//-------------------------------------------------------------- |
uint16_t uart1_available(void) |
{ |
return (UART_RX_BUFFER_MASK + UART1_RxHead - UART1_RxTail) % UART_RX_BUFFER_MASK; |
} |
//-------------------------------------------------------------- |
// Function: uart1_flush() |
// Purpose: Flush bytes waiting the receive buffer. Acutally ignores them. |
// Input: None |
// Returns: None |
//-------------------------------------------------------------- |
void uart1_flush(void) |
{ |
UART1_RxHead = UART1_RxTail; |
} |
/************************************************************************* |
Function: utoa() |
Purpose: convert unsigned integer to ascii |
Input: string_buffer, string_buffer_size, unsigned integer value |
Returns: first ascii character |
**************************************************************************/ |
char *utoa1(char* buffer, const unsigned int size, unsigned int value) |
{ |
char *p; |
// p points to last char |
p = buffer+size-1; |
// Set terminating Zero |
*p='\0'; |
do |
{ |
*--p = value%10 + '0'; |
value = value/10; |
} while (value!=0 && p>buffer); |
return p; |
}/* utoa */ |
#endif |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/uart/uart1.h |
---|
0,0 → 1,194 |
/************************************************************************ |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.h,v 1.8.2.1 2007/07/01 11:14:38 peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4 |
Hardware: any AVR with built-in UART, tested on AT90S8515 & ATmega8 at 4 Mhz |
License: GNU General Public License |
Usage: see Doxygen manual |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.h |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - chg: UART_RX_BUFFER_SIZE war 1024, darf aber nur max 256 sein |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 30.03.2013 CB |
//# - add: uart1_flush(void); in Headerdatei aufgenommen |
//########################################################################### |
// |
// @defgroup pfleury_uart UART Library |
// @code #include <uart.h> @endcode |
// |
// @brief Interrupt UART library using the built-in UART with transmit and receive circular buffers. |
// |
// This library can be used to transmit and receive data through the built in UART. |
// |
// An interrupt is generated when the UART has finished transmitting or |
// receiving a byte. The interrupt handling routines use circular buffers |
// for buffering received and transmitted data. |
// |
// The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE constants define |
// the size of the circular buffers in bytes. Note that these constants must be a power of 2. |
// You may need to adapt this constants to your target and your application by adding |
// CDEFS += -DUART_RX_BUFFER_SIZE=nn -DUART_RX_BUFFER_SIZE=nn to your Makefile. |
// |
// @note Based on Atmel Application Note AVR306 |
// @author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
// |
#ifndef UART_H |
#define UART_H |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
// constants and macros |
// @brief UART Baudrate Expression |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu)/((baudRate)*16l)-1) |
// @brief UART Baudrate Expression for ATmega double speed mode |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT_DOUBLE_SPEED(baudRate,xtalCpu) (((xtalCpu)/((baudRate)*8l)-1)|0x8000) |
// Size of the circular receive buffer, must be power of 2 |
#ifndef UART_RX_BUFFER_SIZE |
#define UART_RX_BUFFER_SIZE 256 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// Size of the circular transmit buffer, must be power of 2 |
#ifndef UART_TX_BUFFER_SIZE |
#define UART_TX_BUFFER_SIZE 64 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// test if the size of the circular buffers fits into SRAM |
#if ( (UART_RX_BUFFER_SIZE+UART_TX_BUFFER_SIZE) >= (RAMEND-0x60 ) ) |
#error "size of UART_RX_BUFFER_SIZE + UART_TX_BUFFER_SIZE larger than size of SRAM" |
#endif |
//global variable |
extern volatile uint16_t UART1_RxError; |
extern volatile uint8_t UART1_receiveNMEA; // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// high byte error return code of uart_getc() |
#define UART_FRAME_ERROR 0x0800 // Framing Error by UART |
#define UART_OVERRUN_ERROR 0x0400 // Overrun condition by UART |
#define UART_BUFFER_OVERFLOW 0x0200 // receive ringbuffer overflow |
#define UART_NO_DATA 0x0100 // no receive data available |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define NMEA_receive 5 // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// |
// function prototypes |
// |
// |
// @brief Initialize UART and set baudrate |
// @param baudrate Specify baudrate using macro UART_BAUD_SELECT() |
// @return none |
// |
extern void uart_init(unsigned int baudrate); |
// |
// @brief Get received byte from ringbuffer |
// |
// Returns in the lower byte the received character and in the |
// higher byte the last receive error. |
// UART_NO_DATA is returned when no data is available. |
// |
// @param void |
// @return lower byte: received byte from ringbuffer |
// @return higher byte: last receive status |
// - \b 0 successfully received data from UART |
// - \b UART_NO_DATA |
// <br>no receive data available |
// - \b UART_BUFFER_OVERFLOW |
// <br>Receive ringbuffer overflow. |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// - \b UART_OVERRUN_ERROR |
// <br>Overrun condition by UART. |
// A character already present in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped. |
// - \b UART_FRAME_ERROR |
// <br>Framing Error by UART |
// |
extern unsigned int uart_getc(void); |
// |
// @brief Put byte to ringbuffer for transmitting via UART |
// @param data byte to be transmitted |
// @return none |
// |
// @brief Set Baudrate USART1 |
extern void SetBaudUart1(uint8_t Baudrate); |
// @brief Initialize USART1 (only available on selected ATmegas) @see uart_init |
extern void uart1_init(unsigned int baudrate); |
// @brief Get received byte of USART1 from ringbuffer. (only available on selected ATmega) @see uart_getc |
extern unsigned int uart1_getc(void); |
// @brief Put byte to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_putc |
//extern void uart1_putc(unsigned char data); |
extern int uart1_putc(unsigned char data); |
// @brief Put string to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts |
extern void uart1_puts(const char *s ); |
// @brief Put string from program memory to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts_p |
extern void uart1_puts_p(const char *s ); |
// @brief Macro to automatically put a string constant into program memory |
#define uart1_puts_P(__s) uart1_puts_p(PSTR(__s)) |
extern char *utoa1(char* buffer, const unsigned int size, unsigned int value); |
extern uint16_t uart1_available(void); |
extern int uart1_peek(void); |
// @brief Flush bytes waiting the receive buffer. Acutally ignores them. |
extern void uart1_flush(void); |
#endif // UART_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/uart/usart.c |
---|
0,0 → 1,732 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.c |
//# |
//# 3.04.2015 Cebra |
//# - chg: 2 weitere /r am Ende des gesendeten Strings angefügt, damit Funktion gleich wie im MK-Tool |
//# |
//# 24.01.2014 OG |
//# - add: Debug-Code fuer die ISR-RX Funktion |
//# schaltbar via define DEBUG_FC_COMMUNICATION in main.h - der Code |
//# kann somit drin bleiben |
//# - add: Source-History |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <stdarg.h> |
#include "../main.h" |
#include "usart.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../osd/osd.h" |
uint8_t buffer[30]; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t ReceivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t RxDataLen = 0; |
volatile uint16_t stat_crc_error = 0; |
volatile uint16_t stat_overflow_error = 0; |
volatile uint8_t rx_byte; |
volatile uint8_t rxFlag = 0; |
#define UART_RXBUFSIZE 64 |
#define UART_NO_DATA 0x0100 /* no receive data available */ |
volatile static uint8_t rxbuf[UART_RXBUFSIZE]; |
volatile static uint8_t *volatile rxhead, *volatile rxtail; |
#ifdef DEBUG_FC_COMMUNICATION |
char *tmp = (char *)" "; |
#endif |
/* |
//----------------------------------------------------------------------------- |
// USART1 transmitter ISR |
ISR (USART1_TX_vect) |
{ |
static uint16_t ptr_txd1_buffer = 0; |
uint8_t tmp_tx1; |
if(!txd1_complete) // transmission not completed |
{ |
ptr_txd1_buffer++; // [0] was already sent |
tmp_tx1 = txd1_buffer[ptr_txd1_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx1 == '\r') || (ptr_txd1_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd1_buffer = 0; // reset txd pointer |
txd1_complete = TRUE; // stop transmission |
} |
UDR1 = tmp_tx1; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd1_buffer = 0; |
} |
*/ |
#ifdef USART_INT |
//----------------------------------------------------------------------------- |
// USART0 transmitter ISR |
ISR (USART_TX_vect) |
{ |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if(!txd_complete) // transmission not completed |
{ |
ptr_txd_buffer++; // [0] was already sent |
tmp_tx = txd_buffer[ptr_txd_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd_buffer = 0; // reset txd pointer |
txd_complete = TRUE; // stop transmission |
} |
UDR = tmp_tx; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd_buffer = 0; |
} |
#endif |
void SetBaudUart0(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: USART_Init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: USART_Init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
case Baud_9600: USART_Init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_19200: USART_Init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: USART_Init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: USART_Init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_115200: USART_Init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
// Macro erechnet falschen Wert (9,85 = 9) für 115200 Baud mit 20Mhz Quarz, zu grosse Abweichung |
//#warning "Baurate prüfen wenn kein 20 Mhz Quarz verwendet wird" |
case Baud_115200: USART_Init( 10 ); break; |
} |
} |
//----------------------------------------------------------------------------- |
// |
// |
//uint8_t uart_getc_nb(uint8_t *c) |
//{ |
// if (rxhead==rxtail) return 0; |
// *c = *rxtail; |
// if (++rxtail == (rxbuf + UART_RXBUFSIZE)) rxtail = rxbuf; |
// return 1; |
//} |
ISR (USART0_RX_vect) |
{ |
static uint16_t crc; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t c; |
// IdleTimer = 0; |
if (current_hardware == Wi232) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
// USART_putc (c); |
return; |
} |
if (current_hardware == MKGPS) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
return; |
} |
c = UDR; // catch the received byte |
if (OSD_active && Config.OSD_SendOSD) // Daten an SV2 senden |
uart1_putc(c); |
if (rxd_buffer_locked) |
return; // if rxd buffer is locked immediately return |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
{ |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
lcd_print( "#", MNORMAL ); // OG DEBUG |
#endif |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
} |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
{ |
if(c != '\r') // no termination character |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
else // termination character was received |
{ |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer-2]; |
crc -= rxd_buffer[ptr_rxd_buffer-1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
// compare checksum to transmitted checksum bytes |
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
{ // checksum valid |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
*tmp = rxd_buffer[2]; |
lcd_print( tmp, MNORMAL ); |
lcd_putc( 0,0, '[', MNORMAL ); |
lcd_putc( 1,0, rxd_buffer[2], MNORMAL ); |
lcd_putc( 2,0, ']', MNORMAL ); x |
if( rxd_buffer[2] != 'k' ) set_beep( 13, 0x0001, BeepNormal); |
if( rxd_buffer[2] == 'Q' ) set_beep( 150, 0x0080, BeepNormal); |
#endif |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
if (mode == rxd_buffer[2]) |
{ |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
} |
} |
else |
{ // checksum invalid |
stat_crc_error++; |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
ptr_rxd_buffer = 0; // reset rxd buffer pointer |
} |
} |
else // rxd buffer overrun |
{ |
stat_overflow_error++; |
ptr_rxd_buffer = 0; // reset rxd buffer |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
} |
//----------------------------------------------------------------------------- |
// Function: uart0_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//----------------------------------------------------------------------------- |
char USART_getc(void) |
{ |
char val; |
// while(rxhead==rxtail) ; |
if (rxhead==rxtail) |
return val=0; |
// IdleTimer = 0; |
val = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return val; |
} |
uint8_t uart_getc_nb(uint8_t *c) |
{ |
if (rxhead==rxtail) |
return 0; |
// IdleTimer = 0; |
*c = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return 1; |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void USART_Init (unsigned int baudrate) |
{ |
// set clock divider |
// #undef BAUD |
// #define BAUD baudrate |
// #include <util/setbaud.h> |
// UBRRH = UBRRH_VALUE; |
// UBRRL = UBRRL_VALUE; |
//#ifndef F_CPU |
///* In neueren Version der WinAVR/Mfile Makefile-Vorlage kann |
// F_CPU im Makefile definiert werden, eine nochmalige Definition |
// hier wuerde zu einer Compilerwarnung fuehren. Daher "Schutz" durch |
// #ifndef/#endif |
// |
// Dieser "Schutz" kann zu Debugsessions führen, wenn AVRStudio |
// verwendet wird und dort eine andere, nicht zur Hardware passende |
// Taktrate eingestellt ist: Dann wird die folgende Definition |
// nicht verwendet, sondern stattdessen der Defaultwert (8 MHz?) |
// von AVRStudio - daher Ausgabe einer Warnung falls F_CPU |
// noch nicht definiert: */ |
//#warning "F_CPU war noch nicht definiert, wird nun nachgeholt mit 4000000" |
//#define F_CPU 18432000UL // Systemtakt in Hz - Definition als unsigned long beachten |
// Ohne ergeben sich unten Fehler in der Berechnung |
//#endif |
//#define BAUD 115200UL // Baudrate |
// |
//// Berechnungen |
//#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) // clever runden |
//#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) // Reale Baudrate |
//#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) // Fehler in Promille, 1000 = kein Fehler. |
// |
// |
//#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010)) |
// #error "Systematischer Fehler der Baudrate grösser 1% und damit zu hoch!" |
//#endif |
UBRRH = (unsigned char)(baudrate>>8); |
UBRRL = (unsigned char) baudrate; |
// UBRRH = (unsigned char)(BAUD_REAL>>8); |
// UBRRL = (unsigned char) BAUD_REAL; |
#if USE_2X |
UCSRA |= (1 << U2X); // enable double speed operation |
#else |
UCSRA &= ~(1 << U2X); // disable double speed operation |
#endif |
// set 8N1 |
#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega32__) |
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0); |
#else |
UCSRC = (1 << UCSZ1) | (1 << UCSZ0); |
#endif |
UCSRB &= ~(1 << UCSZ2); |
// flush receive buffer |
while ( UCSRA & (1 << RXC) ) UDR; |
UCSRB |= (1 << RXEN) | (1 << TXEN); |
#ifdef USART_INT |
UCSRB |= (1 << RXCIE) | (1 << TXCIE); |
#else |
UCSRB |= (1 << RXCIE); |
#endif |
rxhead = rxtail = rxbuf; |
} |
//----------------------------------------------------------------------------- |
// disable the txd pin of usart |
void USART_DisableTXD (void) |
{ |
#ifdef USART_INT |
UCSRB &= ~(1 << TXCIE); // disable TX-Interrupt |
#endif |
UCSRB &= ~(1 << TXEN); // disable TX in USART |
DDRB &= ~(1 << DDB3); // set TXD pin as input |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
} |
//----------------------------------------------------------------------------- |
// enable the txd pin of usart |
void USART_EnableTXD (void) |
{ |
DDRB |= (1 << DDB3); // set TXD pin as output |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
UCSRB |= (1 << TXEN); // enable TX in USART |
#ifdef USART_INT |
UCSRB |= (1 << TXCIE); // enable TX-Interrupt |
#endif |
} |
//----------------------------------------------------------------------------- |
// short script to directly send a request thorugh usart including en- and disabling it |
// where <address> is the address of the receipient, <label> is which data set to request |
// and <ms> represents the milliseconds delay between data |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms) |
{ |
USART_EnableTXD (); // re-enable TXD pin |
unsigned char mstenth = ms/10; |
SendOutData(cmd, addr, 1, &mstenth, 1); |
// wait until command transmitted |
while (txd_complete == FALSE); |
USART_DisableTXD (); // disable TXD pin again |
} |
//----------------------------------------------------------------------------- |
// |
void USART_putc (char c) |
{ |
#ifdef USART_INT |
#else |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = c; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts (char *s) |
{ |
#ifdef USART_INT |
#else |
while (*s) |
{ |
USART_putc (*s); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts_p (const char *s) |
{ |
#ifdef USART_INT |
#else |
while (pgm_read_byte(s)) |
{ |
USART_putc (pgm_read_byte(s)); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
{ |
va_list ap; |
uint16_t pt = 0; |
uint8_t a,b,c; |
uint8_t ptr = 0; |
uint16_t tmpCRC = 0; |
uint8_t *pdata = 0; |
int len = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
va_start(ap, numofbuffers); |
if(numofbuffers) |
{ |
pdata = va_arg (ap, uint8_t*); |
len = va_arg (ap, int); |
ptr = 0; |
numofbuffers--; |
} |
while(len) |
{ |
if(len) |
{ |
a = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
a = 0; |
if(len) |
{ |
b = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
b = 0; |
if(len) |
{ |
c = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
c = 0; |
txd_buffer[pt++] = '=' + (a >> 2); |
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
txd_buffer[pt++] = '=' + ( c & 0x3f); |
} |
va_end(ap); |
for(a = 0; a < pt; a++) |
{ |
tmpCRC += txd_buffer[a]; |
} |
tmpCRC %= 4096; |
txd_buffer[pt++] = '=' + tmpCRC / 64; |
txd_buffer[pt++] = '=' + tmpCRC % 64; |
txd_buffer[pt++] = '\r'; |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_complete = FALSE; |
#ifdef USART_INT |
UDR = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
#else |
for(a = 0; a < pt; a++) |
{ |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = txd_buffer[a]; |
} |
txd_complete = TRUE; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void Decode64 (void) |
{ |
uint8_t a,b,c,d; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = ReceivedBytes - 6; |
while (len) |
{ |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
//if(ptrIn > ReceivedBytes - 3) break; |
if (len--) |
rxd_buffer[ptrOut++] = (a << 2) | (b >> 4); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((c & 0x03) << 6) | d; |
else |
break; |
} |
pRxData = &rxd_buffer[3]; |
RxDataLen = ptrOut - 3; |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToNC (void) |
{ |
if(hardware == NC) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void SwitchToWi232 (void) |
{ |
// if(hardware == NC) |
{ |
// switch to Wi232 |
current_hardware = Wi232; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToFC (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToMAG (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToGPS (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms (50); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/uart/usart.h |
---|
0,0 → 1,162 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.h |
//# |
//# 24.01.2014 OG |
//# - fix: RXD_BUFFER_LEN und TXD_BUFFER_LEN vergroessert 180 auf 230 wegen |
//# neuer FC-Version (FC v2.03) |
//# - add: Source-History |
//############################################################################ |
#ifndef _USART_H |
#define _USART_H |
//-------------------------------------------------------------- |
// |
#ifndef FALSE |
#define FALSE 0 |
#endif |
#ifndef TRUE |
#define TRUE 1 |
#endif |
// addresses |
#define ADDRESS_ANY 0 |
#define ADDRESS_FC 1 |
#define ADDRESS_NC 2 |
#define ADDRESS_MAG 3 |
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
//#define TXD_BUFFER_LEN 180 |
//#define RXD_BUFFER_LEN 180 |
#define TXD_BUFFER_LEN 230 |
#define RXD_BUFFER_LEN 230 |
// Baud rate of the USART |
#define USART_BAUD 57600 |
//#define USART_BAUD 125000 |
//-------------------------------------------------------------- |
// |
extern uint8_t buffer[30]; |
extern volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd_complete; |
extern volatile uint8_t txd1_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd1_complete; |
extern volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
extern volatile uint8_t rxd_buffer_locked; |
extern volatile uint8_t ReceivedBytes; |
extern volatile uint8_t *pRxData; |
extern volatile uint8_t RxDataLen; |
extern volatile uint16_t stat_crc_error; |
extern volatile uint16_t stat_overflow_error; |
extern volatile uint8_t rxFlag; |
extern volatile uint8_t rx_byte; |
//-------------------------------------------------------------- |
// |
void SetBaudUart0(uint8_t Baudrate); |
void USART_Init (unsigned int baudrate); |
void USART_DisableTXD (void); |
void USART_EnableTXD (void); |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms); |
void USART_putc (char c); |
void USART_puts (char *s); |
void USART_puts_p (const char *s); |
extern char USART_getc(void); |
void SendOutData (uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...); // uint8_t *pdata, uint8_t len, ... |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, uint8_t *pdata, uint8_t len); // uint8_t *pdata, uint8_t len, ... |
void Decode64 (void); |
void SwitchToNC (void); |
void SwitchToFC (void); |
void SwitchToMAG (void); |
void SwitchToGPS (void); |
void SwitchToWi232 (void); |
void debug1(void); |
uint8_t uart_getc_nb(uint8_t*); |
//-------------------------------------------------------------- |
//Anpassen der seriellen Schnittstellen Register |
#define USART_RXC_vect USART0_RX_vect |
//-------------------------------------------------------------- |
#define UCSRA UCSR0A |
#define UCSRB UCSR0B |
#define UCSRC UCSR0C |
#define UDR UDR0 |
#define UBRRL UBRR0L |
#define UBRRH UBRR0H |
// UCSRA |
#define RXC RXC0 |
#define TXC TXC0 |
#define UDRE UDRE0 |
#define FE FE0 |
#define UPE UPE0 |
#define U2X U2X0 |
#define MPCM MPCM0 |
// UCSRB |
#define RXCIE RXCIE0 |
#define TXCIE TXCIE0 |
#define UDRIE UDRIE0 |
#define TXEN TXEN0 |
#define RXEN RXEN0 |
#define UCSZ2 UCSZ02 |
#define RXB8 RXB80 |
#define TXB8 TXB80 |
// UCSRC |
#define UMSEL1 UMSEL01 |
#define UMSEL0 UMSEL00 |
#define UPM1 UPM01 |
#define UPM0 UPM00 |
#define USBS USBS0 |
#define UCSZ1 UCSZ01 |
#define UCSZ0 UCSZ00 |
#define UCPOL UCPOL0 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/i2cmaster.h |
---|
0,0 → 1,178 |
#ifndef _I2CMASTER_H |
#define _I2CMASTER_H 1 |
/************************************************************************* |
* Title: C include file for the I2C master interface |
* (i2cmaster.S or twimaster.c) |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: i2cmaster.h,v 1.10 2005/03/06 22:39:57 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device |
* Usage: see Doxygen manual |
**************************************************************************/ |
#ifdef DOXYGEN |
/** |
@defgroup pfleury_ic2master I2C Master library |
@code #include <i2cmaster.h> @endcode |
@brief I2C (TWI) Master Software Library |
Basic routines for communicating with I2C slave devices. This single master |
implementation is limited to one bus master on the I2C bus. |
This I2c library is implemented as a compact assembler software implementation of the I2C protocol |
which runs on any AVR (i2cmaster.S) and as a TWI hardware interface for all AVR with built-in TWI hardware (twimaster.c). |
Since the API for these two implementations is exactly the same, an application can be linked either against the |
software I2C implementation or the hardware I2C implementation. |
Use 4.7k pull-up resistor on the SDA and SCL pin. |
Adapt the SCL and SDA port and pin definitions and eventually the delay routine in the module |
i2cmaster.S to your target when using the software I2C implementation ! |
Adjust the CPU clock frequence F_CPU in twimaster.c or in the Makfile when using the TWI hardware implementaion. |
@note |
The module i2cmaster.S is based on the Atmel Application Note AVR300, corrected and adapted |
to GNU assembler and AVR-GCC C call interface. |
Replaced the incorrect quarter period delays found in AVR300 with |
half period delays. |
@author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
@par API Usage Example |
The following code shows typical usage of this library, see example test_i2cmaster.c |
@code |
#include <i2cmaster.h> |
#define Dev24C02 0xA2 // device address of EEPROM 24C02, see datasheet |
int main(void) |
{ |
unsigned char ret; |
i2c_init(); // initialize I2C library |
// write 0x75 to EEPROM address 5 (Byte Write) |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_write(0x75); // write value 0x75 to EEPROM |
i2c_stop(); // set stop conditon = release bus |
// read previously written value back from EEPROM address 5 |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_rep_start(Dev24C02+I2C_READ); // set device address and read mode |
ret = i2c_readNak(); // read one byte from EEPROM |
i2c_stop(); |
for(;;); |
} |
@endcode |
*/ |
#endif /* DOXYGEN */ |
/**@{*/ |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
#include <avr/io.h> |
/** defines the data direction (reading from I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_READ 1 |
/** defines the data direction (writing to I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_WRITE 0 |
/** |
@brief initialize the I2C master interace. Need to be called only once |
@param void |
@return none |
*/ |
extern void i2c_init(void); |
/** |
@brief Terminates the data transfer and releases the I2C bus |
@param void |
@return none |
*/ |
extern void i2c_stop(void); |
/** |
@brief Issues a start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_start(unsigned char addr); |
/** |
@brief Issues a repeated start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_rep_start(unsigned char addr); |
/** |
@brief Issues a start condition and sends address and transfer direction |
If device is busy, use ack polling to wait until device ready |
@param addr address and transfer direction of I2C device |
@return none |
*/ |
extern void i2c_start_wait(unsigned char addr); |
/** |
@brief Send one byte to I2C device |
@param data byte to be transfered |
@retval 0 write successful |
@retval 1 write failed |
*/ |
extern unsigned char i2c_write(unsigned char data); |
/** |
@brief read one byte from the I2C device, request more data from device |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readAck(void); |
/** |
@brief read one byte from the I2C device, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readNak(void); |
/** |
@brief read one byte from the I2C device |
Implemented as a macro, which calls either i2c_readAck or i2c_readNak |
@param ack 1 send ack, request more data from device<br> |
0 send nak, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_read(unsigned char ack); |
#define i2c_read(ack) (ack) ? i2c_readAck() : i2c_readNak(); |
/**@}*/ |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/menuctrl.c |
---|
0,0 → 1,1501 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.c |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 31.05.2014 OG |
//# - chg: MenuCtrl_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# - chg: include "../mk/mkparameters.h" geaendert auf "../mksettings/mkparameters.h" |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# - chg: _MenuCtrl_Error() - auf PKT_Popup_P() umgestellt, Layout geaendert und |
//# einen neuen Fehler (NOPARENTMENU) hinzugefuegt |
//# |
//# 09.05.2014 OG |
//# - fix: MenuCtrl_Refresh() Anzeige von deaktivierten MK-Parametern (Favoriten) |
//# Wenn MK-Paramketer-Menueeintraege deaktiviert sind werden sie vom |
//# aktuellen MK-Parameterset nicht unterstuetzt - dennoch wurde der |
//# aktuelle Wert des MK-Parameters im Menuepunkt (ganz rechts) angezeigt. |
//# Das ist jetzt behoben - es wird kein Wert angezeigt (stattdessen "***") |
//# und auch keine entsprechende editParam-Funktionen aufgerufen. |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - chg: MenuCtrl_Refresh() Keyline leicht angepasst |
//# - chg: 'not possible' Anzeige (bei deaktiviertem Item) umgestellt auf |
//# PKT_Popup_P() |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - chg: MenuCtrl_Control() |
//# -> Unterstuetzung fuer: MenuCtrl_SetMove() - Menueeintraege verschieben |
//# - chg: MenuCtrl_Refresh() - Anpassung Slider fuer MenuCtrl_SetDelete() |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# - add: _MenuCtrl_SwapItem() |
//# - add: die Datenstruktur vom Menue wurde erweitert um die Eigenschaften |
//# canMove und canDelete |
//# |
//# 17.04.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 22 auf 28 erhoeht |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 29.03.2014 OG |
//# - chg: default MENUDEFAULT_SHOWBATT auf true umgestellt (=PKT-Batterie anzeigen) |
//# - chg: MenuCtrl_Control() umgestellt auf clear_key_all() |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() - fuegt eine Trennlinie im Menue ein |
//# - chg: MenuCtrl_Refresh() Codeoptimierung bzgl. topspace/Zeile loeschen/Slider |
//# - fix: MenuCtrl_Control() es wurde zuoft ein Screen-Refresh durchgefuehrt |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - chg: MenuCtrl_Refresh() Unterstuetzung fuer MENU_PARAMEDIT |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: paramset.h und mkparameters.h |
//# |
//# 04.03.2014 OG |
//# - fix: MenuCtrl_Refresh() bei einem blauen Display (das ist wohl schneller |
//# in der Anzeige als das S/W-Display) flackerte der Menue-Slider |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# - chg: MenuCtrl_Refresh() angepasst auf MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 01.02.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 16 auf 22 fuer MK_parameters |
//# - fix: _MenuCtrl_Error() Anzeigezeit von Menu-Errors auf 8 Sekunden verlaengert |
//# und Anzeige von "menuctrl.c" |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() - Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts) |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - chg: MenuCtrl_Refresh() Anpassungen und Optimierung |
//# - add: MenuCtrl_ItemMark() |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - fix: MenuCtrl_Control() Eventcode Rueckgabe bei KEY_ENTER |
//# |
//# 22.05.2013 OG |
//# - fix: nach Aufruf einer Menuefunktion in MenuCtrl_Control() jetzt |
//# get_key_press(KEY_ALL) |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 20.05.2013 OG |
//# - chg: MenuCtrl_Control() wieder mit get_key_press(KEY_ALL) ergaenzt |
//# wenn aufgerufen mit MENUCTRL_EVENT oder erster Aufruf mit RETURN |
//# |
//# 20.05.2013 OG |
//# - chg: Space am Anfang der Titelanzeige |
//# |
//# 19.05.2013 OG |
//# - add: define MENU_SHOWLEVEL mit dem der aktuelle Menuelevel in der |
//# Titelzeile angezeigt werden kann |
//# - fix: Redraw-Logik bei MENUCTRL_RETURN |
//# - chg: MenuCtrl_Control() erweitert um PKT_Update() |
//# |
//# 18.05.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> // memset |
#include <stdarg.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../menu.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "../mksettings/mkparameters.h" |
#include "menuctrl.h" |
//############################################################################################# |
//# internal defines & structs |
//############################################################################################# |
#define MENU_SHOWLEVEL false // Anzeige der Menue-Verschachtelungstiefe in der Titelzeile |
// zeigt dem Benutzer wie weit er wieder zurueckspringen muss |
// um in das Hauptmenue zu kommen |
#define MENUDEFAULT_CYCLE false // cycle: wenn Menue am Ende dann wieder zum Anfang |
#define MENUDEFAULT_SHOWBATT true // showbatt: zeigt PKT-Batterie im Menuetitel ganz rechts |
#define MENUDEFAULT_BEEP true // beep: wenn Cursor ueber Ende/Anfang |
#define MENUCTRL_MAXMENUES 6 // Anzahl max. verschachlteter Menues |
//#define MENUCTRL_MAXITEMS 22 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
//#define MENUCTRL_MAXITEMS 28 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define MENUCTRL_MAXITEMS 34 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define ERROR_NOMENU 1 // Errorcode |
#define ERROR_MAXMENU 2 // Errorcode |
#define ERROR_MAXITEM 3 // Errorcode |
#define ERROR_NOITEM 4 // Errorcode |
#define ERROR_NOPARENTMENU 5 // Errorcode |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
const char *text; // Tastentext |
void (*func)(void); // ggf. Funktions-Pointer (0=keine Funktion) |
} menue_key_t; |
//--------------------------- |
// typedef: einzelnes Menueitem |
//--------------------------- |
typedef struct |
{ |
uint8_t id; |
uint8_t type; // MENU_ITEM, MENU_SUB, MENU_PARAMEDIT |
uint8_t disabled; // true / false (deaktiviert einen Menuepunkt) |
uint8_t mark; // true / false (markiert einen Menuepunkt Links '*') |
uint8_t markR; // true / false (markiert einen Menuepunkt Rechts) |
uint8_t textPGM; // true / false (true = text in PROGMEN; false = text im RAM) |
uint8_t textcount; // 1 .. NUM_LANG |
const char *text[NUM_LANG]; // de, en, nl (NUM_LANG aus messages.h) |
void (*func)(void); |
} menue_item_t; |
//--------------------------- |
// typedef: Menueitem Liste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; // aktives item |
uint8_t itemcount; // Anzahl Items |
uint8_t scroll_pos; // aktuelle obere Anzeigezeile |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
uint8_t topspace; // obere Leerzeilen - default=0 (Vorsicht! ggf. nur bedingt einsetzbar bei Menues die ohne scrollen auskommen!) |
uint8_t firstredraw; // true / false (flag fuer ein erstes redraw bei MENUCTRL_RETURN) |
uint8_t cycle; // true / false |
uint8_t showbatt; // true / false |
uint8_t beep; // true / false |
uint8_t canMove; // true / false |
uint8_t canDelete; // false/0 = kein entfernen; true/1 = entfernen mit Nachfrage; 2 = entfernen ohne Nachfrage |
uint8_t titlePGM; // true / false |
const char *title; // Menuetitel (oberste Zeile) (ggf. Multisprache machen?) |
uint8_t lastkey; // verwendet von GetKey() |
menue_key_t key_enter; |
menue_key_t key_enter_long; |
menue_key_t key_esc; |
menue_key_t key_esc_long; |
menue_item_t item[MENUCTRL_MAXITEMS]; |
} menue_t; |
uint8_t showlevel = MENU_SHOWLEVEL; // Anzeige Menulevel in der Titelanzeige |
int8_t midx = -1; // aktives Menue; -1 = kein Menue |
menue_t menu[MENUCTRL_MAXMENUES]; // Stack fuer n geschachltete Menues |
//############################################################################################# |
//# PRIVAT funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Error( uint8_t error ) |
{ |
const char *pStr = NULL; |
switch( error ) |
{ |
case ERROR_NOMENU: pStr = PSTR("NOMENU"); break; |
case ERROR_MAXMENU: pStr = PSTR("MAXMENU"); break; |
case ERROR_MAXITEM: pStr = PSTR("MAXITEM"); break; |
case ERROR_NOITEM: pStr = PSTR("NOITEM"); break; |
case ERROR_NOPARENTMENU: pStr = PSTR("NOPARENTMENU"); break; |
} |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error |
PKT_Popup_P( 10000, PSTR("menuctrl.c"), 0, PSTR("* ERROR *"), pStr ); // 10000 = max. 100 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
int8_t _MenuCtrl_FindItem( uint8_t itemid ) |
{ |
int8_t i; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return -1; } |
for( i=0; i<menu[midx].itemcount; i++) |
{ |
if( itemid == menu[midx].item[i].id ) |
{ |
return i; // found: return index |
} |
} |
return -1; // not found |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_CalcDisplayPos( void ) |
{ |
int8_t i; |
i = menu[midx].active - menu[midx].display_pos; |
if( i < 0 ) |
menu[midx].display_pos = menu[midx].active; |
else if( i > 5 ) |
menu[midx].display_pos = (menu[midx].active - 5); |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Beep( void ) |
{ |
if( menu[midx].beep ) |
set_beep( 25, 0xffff, BeepNormal ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// Dreieckstausch von zwei Menuepunkten. |
// Wird verwendet um Menueitems zu verschieben. |
//-------------------------------------------------------------- |
void _MenuCtrl_SwapItem( uint8_t itemindex_1, uint8_t itemindex_2 ) |
{ |
menue_item_t tmpItem; |
memcpy( &tmpItem , &menu[midx].item[itemindex_1], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_1] , &menu[midx].item[itemindex_2], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_2] , &tmpItem, sizeof(menue_item_t) ); |
} |
//############################################################################################# |
//# PUBLIC funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// MenuCtrl_Create() |
//-------------------------------------------------------------- |
void MenuCtrl_Create( void ) |
{ |
if( midx >= MENUCTRL_MAXMENUES) { _MenuCtrl_Error( ERROR_MAXMENU ); return; } |
midx++; |
memset( &menu[midx], 0, sizeof(menue_t) ); |
menu[midx].cycle = MENUDEFAULT_CYCLE; |
menu[midx].showbatt = MENUDEFAULT_SHOWBATT; |
menu[midx].beep = MENUDEFAULT_BEEP; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Destroy() |
//-------------------------------------------------------------- |
void MenuCtrl_Destroy( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
midx--; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ShowLevel( value ) |
// |
// Diese Einstellung ist Global und wirkt sich direkt auf alle |
// Menues aus! |
// Man kann das also nicht fuer einzelne Menues ein-/ausschalten. |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ShowLevel( uint8_t value ) |
{ |
showlevel = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle_P( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle_P( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = true; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String im RAM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = false; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitleFromParentItem() |
// |
// setzte anhand des uebergeordneten Menuepunktes (dem aufrufenden |
// Menuepunkt) den Titel des neuen Menues |
// |
// wird u.a. verwendet in setup.c |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitleFromParentItem( void ) |
{ |
uint8_t lang; |
const char *pStr; |
uint8_t mparent; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( midx < 1) { _MenuCtrl_Error( ERROR_NOPARENTMENU ); return; } |
mparent = midx-1; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[mparent].item[ menu[mparent].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Text vom uebergeordneten Menuepunkt |
//-------------------------- |
pStr = menu[mparent].item[ menu[mparent].active ].text[lang]; |
//-------------------------- |
// Titel setzen |
//-------------------------- |
if( menu[mparent].item[ menu[mparent].active ].textPGM ) |
{ |
MenuCtrl_SetTitle_P( pStr ); |
} |
else |
{ |
MenuCtrl_SetTitle( pStr ); |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetCycle( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetCycle( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].cycle = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetBeep( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetBeep( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].beep = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetMove( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege nach |
// oben oder nach unten zu verschieben. |
// Zum verschieden eines Menueeintrages muessen die PLUS/MINUS Tasten |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetMove( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canMove = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetDelete( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege zu loeschen. |
// Zum loeschen eines Menueeintrages muss dann die ganz rechte 'OK' Taste |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetDelete( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canDelete = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetShowBatt( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetShowBatt( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].showbatt = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTopSpace( n ) |
// |
// fuegt oben im Menue n Leerzeichen ein um Abstand vom Menuetitel |
// zu erhalten |
// |
// ACHTUNG! Das funktioniert nur mit Menues die nicht Scrollen! |
// -> also weniger als 6 Eintraege haben (je nach Anzahl von TopSpace) |
// |
// ACHTUNG! Keine Absicherung bzgl. obigem Warnpunkt! Das kann man |
// zwar machen - ist aber aktuell nicht so! |
// |
// Parameter: |
// value: 0..n Anzahl der Leerzeilen ueber dem Menue |
//-------------------------------------------------------------- |
void MenuCtrl_SetTopSpace( uint8_t n ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].topspace = n; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetKey( key, *keytext, *keyfunc ) |
// |
// Parameter: |
// key : KEY_ENTER, KEY_ENTER_LONG, KEY_ESC, KEY_ESC_LONG |
// keytext: Pointer auf Text PROGMEM oder NOTEXT (=0) |
// keyfunc: Pointer auf Funktion oder NOFUNC (=0) |
//-------------------------------------------------------------- |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein aktives Menue |
if( key == KEY_ENTER ) |
{ |
menu[midx].key_enter.active = true; |
menu[midx].key_enter.text = keytext; |
menu[midx].key_enter.func = keyfunc; |
} |
if( key == KEY_ENTER_LONG ) |
{ |
menu[midx].key_enter_long.active = true; |
menu[midx].key_enter_long.text = keytext; |
menu[midx].key_enter_long.func = keyfunc; |
} |
if( key == KEY_ESC ) |
{ |
menu[midx].key_esc.active = true; |
menu[midx].key_esc.text = keytext; |
menu[midx].key_esc.func = keyfunc; |
} |
if( key == KEY_ESC_LONG ) |
{ |
menu[midx].key_esc_long.active = true; |
menu[midx].key_esc_long.text = keytext; |
menu[midx].key_esc_long.func = keyfunc; |
} |
} |
//-------------------------------------------------------------- |
// key = MenuCtrl_GetKey() |
// |
// Rueckgabe: |
// key: z.B. KEY_ENTER oder KEY_ENTER_LONG |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetKey( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].lastkey; |
} |
//-------------------------------------------------------------- |
// menuindex = MenuCtrl_GetMenuIndex() |
// |
// gibt den aktuellen Menuindex zurueck |
// |
// Rueckgabe: |
// menuindex: <0 = keine Menue (-1) |
// =0 = Hauptmenue (Toplevel) |
// >0 = Untermenue Level |
//-------------------------------------------------------------- |
int8_t MenuCtrl_GetMenuIndex( void ) |
{ |
return midx; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemId() |
// |
// gibt die itemID des aktuell angewaehlten Menuepunktes zurueck |
// |
// Rueckgabe: |
// itemid: |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemId( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].id; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemIdByIndex( index ) |
// |
// damit koennen die aktuell vorhanden itemID's der Menuepunkte |
// ermittelt wenn im Menue via SetMove oder SetDelete Menuepunkte |
// durch den Benutzer verschoben oder geloescht wurden. |
// |
// Parameter: |
// index: von 0..itemcount-1 |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
if( (menu[midx].itemcount == 0) || (index > menu[midx].itemcount-1) ) return 0; // index ausserhalb aktiver Menuepunkte |
return menu[midx].item[ index ].id; |
} |
//-------------------------------------------------------------- |
// num = MenuCtrl_GetItemCount() |
// |
// gibt die Anzahl der Menuepunkte zurueck |
// |
// Rueckgabe: |
// num: Anzahl der Menuepunkte |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemCount( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].itemcount; |
} |
//-------------------------------------------------------------- |
// textPtr = MenuCtrl_GetItemText() |
// |
// gibt einen Pointer auf den Text des aktuellen Menuepunktes zurueck |
// |
// ACHTUNG! Der Pointer kann auf RAM oder auch auf PGM zeigen! |
// |
// Um herauszufinden auf welchen Bereich der Pointer zeigt kann |
// MenuCtrl_IsItemTextPGM() verwendet werden. |
// |
// Rueckgabe: |
// textPtr: in RAM oder PGM |
//-------------------------------------------------------------- |
const char * MenuCtrl_GetItemText( void ) |
{ |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[ menu[midx].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
return menu[midx].item[ menu[midx].active ].text[lang]; |
} |
//-------------------------------------------------------------- |
// isPGM = MenuCtrl_IsItemTextPGM() |
// |
// gibt true/false zurueck je nachdem der Text von MenuCtrl_GetItemText() |
// im Progmem (=true) oder im RAM (=false) ist |
// |
// Rueckgabe: |
// isPGM: true / false (true = text in PROGMEN; false = text im RAM) |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_IsItemTextPGM( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].textPGM; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemSelect( itemid ) |
// |
// aktiviert einen Menuepunkt / setzt den Menue-Cursor darauf |
// |
// die Funktion ist nicht durchgetestet, funktioniert aber |
// in MKSettings_Menu() / mkparameters.c |
// |
// es kann sein das es zu Problemen kommt wenn das Menue |
// scrollt (mehr als 6 Menueeintraege) |
// |
// Parameter: |
// itemid: ID des Menuitems |
//-------------------------------------------------------------- |
void MenuCtrl_ItemSelect( uint8_t itemid ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].active = i; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemActive( itemid, value ) |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].disabled = !value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMark( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem '*' am Anfang (Links). |
// Wird u.a. von BT_SelectDevice() (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].mark = value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMarkR( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts). |
// Wird u.a. von ... (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].markR = value; |
} |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// fuer: MenuCtrl_PushML_P(), MenuCtrl_Push() |
//-------------------------------------------------------------- |
void _MenuCtrl_Push( uint8_t useprogmem, uint8_t numlang, uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), ... ) |
{ |
va_list ap; |
uint8_t idx; |
uint8_t i; |
if( midx < 0 ) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( menu[midx].itemcount >= MENUCTRL_MAXITEMS ) { _MenuCtrl_Error( ERROR_MAXITEM ); return; } |
//if( numlang > NUM_LANG ) { _MenuCtrl_Error( ERROR_MAXLANG ); return; } |
idx = menu[midx].itemcount; |
menu[midx].item[idx].id = itemid; |
menu[midx].item[idx].type = itemtype; // MENU_ITEM, MENU_SUB |
menu[midx].item[idx].mark = false; |
menu[midx].item[idx].func = menufunc; |
menu[midx].item[idx].textPGM = useprogmem; |
menu[midx].item[idx].textcount= numlang; |
va_start(ap, menufunc); |
for( i=0; i<numlang; i++) |
{ |
menu[midx].item[idx].text[i] = va_arg( ap, const char *); |
} |
va_end(ap); |
menu[midx].itemcount++; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2_P( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// PROGMEN Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// RAM Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf RAM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
// Spezialfunktion fuer mkparameters.c ! |
// |
// -> siehe: mkparameters.c / Menu_EditCategory() |
// |
// ACHTUNG! |
// Die externe Variable paramEditItem muss in mkparameters.c |
// vor Aufruf dieser Funktion richtig initialisiert sein! |
// Ich habe darauf verzichtet hier nochmal ein find_paramEditItem() |
// aufzurufen um Speicherplatz zu sparen. |
//-------------------------------------------------------------- |
void MenuCtrl_PushParamEdit( uint8_t paramID ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 2, paramID, MENU_PARAMEDIT, NOFUNC, paramEditItem.title_de, paramEditItem.title_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparator() |
// |
// fuegt eine Trennlinie im Menue ein |
// (die itemID ist dabei 0) |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparator( void ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, 0, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparatorID( itemId ) |
// |
// fuegt eine Trennlinie im Menue ein mit einer eigenen itemID |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparatorID( uint8_t itemid ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, itemid, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Refresh( mode ) |
// |
// Parameter: |
// mode: MENU_REDRAW || MENU_REFRESH |
//-------------------------------------------------------------- |
void MenuCtrl_Refresh( uint8_t mode ) |
{ |
uint8_t y; |
uint8_t item; |
uint8_t sh; |
uint8_t sy; |
uint8_t actchar; |
uint8_t markchar; |
uint8_t markcharR; |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein Menue vorhanden |
if( mode == MENU_REDRAW ) |
{ |
//-------------------------- |
// Clear |
//-------------------------- |
lcd_frect( 0, 8, 6, 47, 0); // Clear: ganz linke Spalte des Sliders |
lcd_frect( 127-2, 0, 2, 63, 0); // Clear: ganz rechts 2 Pixel |
//-------------------------- |
// Titel |
//-------------------------- |
if( menu[midx].title != 0 ) |
{ |
if( showlevel ) |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19s"), menu[midx].title ); |
writex_ndigit_number_u( 0, 0, midx, 1, 0, MINVERS, 1,0); // Menuelevel zeigen |
} |
else |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20s"), menu[midx].title ); |
} |
} |
else |
lcd_frect( 0, 0, 128, 7, 1); // Titel: Leerzeile |
//-------------------------- |
// Keyline Beschriftung |
//-------------------------- |
lcdx_cls_row( 7, MNORMAL, 0); |
PKT_KeylineUpDown( 1, 7, 0,0); |
lcd_printp_at( 12, 7, strGet(KEYLINE4), MNORMAL); |
// Taste: KEY_ENTER |
if( menu[midx].key_enter.active && menu[midx].key_enter.text != 0 ) |
{ |
lcdx_printf_at_P( 17, 7, MNORMAL, 0,0 , PSTR("%4S"), menu[midx].key_enter.text ); |
} |
// Taste: KEY_ESC |
if( menu[midx].key_esc.active && menu[midx].key_esc.text != 0 ) |
{ |
lcdx_printf_at_P( 12, 7, MNORMAL, 0,0 , PSTR("%5S"), menu[midx].key_esc.text ); |
} |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Zeilen |
//-------------------------- |
for( y=0; y<6; y++) |
{ |
item = y + menu[midx].display_pos; |
if( item < menu[midx].itemcount ) |
{ |
//-------------------------- |
// char: aktuelle Auswahl |
//-------------------------- |
actchar = ' '; |
if( item == menu[midx].active ) |
{ |
if( menu[midx].item[item].disabled ) actchar = 0x15; // aktuelle Auswahl: Pfeil disabled |
else actchar = 0x1d; // aktuelle Auswahl: Pfeil |
} |
//-------------------------- |
// char: markiert |
//-------------------------- |
markchar = ' '; |
if( menu[midx].item[item].mark ) markchar = '*'; |
//-------------------------- |
// char: markiert RECHTS |
//-------------------------- |
markcharR = ' '; |
if( menu[midx].item[item].type == MENU_SUB ) markcharR = 0x1d; |
if( menu[midx].item[item].markR ) markcharR = SYMBOL_CHECK; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[item].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Ausgabe: RAM oder PROGMEM |
//-------------------------- |
if( MENU_SEPARATOR == menu[midx].item[item].type ) |
{ |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%18S") , actchar, markchar, PSTR("") ); |
lcd_frect( 6*3, ((y+1)*8)+3, 124-(6*3), 0, 1); // Trennlinie |
} |
else if( MENU_PARAMEDIT == menu[midx].item[item].type ) |
{ |
// SPEZIELL fuer mkparameters.c - zeigt die paramID-Bezeichnung inkl. dem aktuellen WERT an! |
strcpy_P( mkparam_strValueBuffer, PSTR("***")); // Vorbelegung von mkparam_strValueBuffer |
if( !menu[midx].item[item].disabled ) // nur wenn Menuepunkt nicht deaktiviert (disabled = paramID nicht vorhanden) |
{ // -> ermittle den aktuellen Wert der paramID fuer Anzeige |
find_paramEditItem( menu[midx].item[item].id ); // paramID suchen (Ergebnis in Variable 'paramEditItem') |
paramEditItem.editfunc( paramEditItem.paramID, MKPARAM_SHORTVALUE ); // Wert von paramID in mkparam_strValueBuffer darstellen |
} |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%14S %3s") , actchar, markchar, menu[midx].item[item].text[lang], mkparam_strValueBuffer ); |
} |
else |
{ |
// Anmerkung zu menu[midx].topspace: NUR fuer Menues OHNE scrollen!! |
if( menu[midx].item[item].textPGM ) |
{ |
// String im PGM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16S%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17S ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
else |
{ |
// String im RAM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16s%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17s ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
} |
} // end: if( item < menu[midx].itemcount ) |
//-------------------------- |
// evtl. Leerzeile loeschen |
//-------------------------- |
if( (y < menu[midx].topspace) || (item >= menu[midx].itemcount+menu[midx].topspace) ) |
{ |
lcd_frect( 6, (y+1)*8, 128-6, 7, 0); // Zeile loeschen |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 45 // Finetuning der Slider-Hoehe |
lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear |
if( menu[midx].itemcount > 6 ) // Slider nur bei mehr als 6 Menueitems |
{ |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 6) / menu[midx].itemcount; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = 8+(menu[midx].display_pos * (SLIDERH-sh)) / (menu[midx].itemcount-6); // Slider: Position |
//lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear (flackert auf blauen Display!) |
//lcd_frect( 0, 8+1, 1, sy-(8), 0); // Slider: Clear / oben (nicht mehr notwendig da Aenderung am Refresh) |
//lcd_frect( 0, sy+1+sh+1, 1, SLIDERH-(sh-sy+1), 0); // Slider: Clear / unten (nicht mehr notwendig da Aenderung am Refresh) |
lcd_frect( 0, sy+1, 1, sh, 1); // Slider: Draw |
} |
} |
//-------------------------------------------------------------- |
// event = MenuCtrl_Control( ctrlmode ) |
// |
// Parameter: |
// ctrlmode: MENUCTRL_EVENT || MENUCTRL_RETURN |
// |
// Rueckgabe: |
// event: MENUEVENT_NONE || MENUEVENT_ITEM || MENUEVENT_KEY |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ) |
{ |
uint8_t menu_event; |
uint8_t refreshmode; |
uint8_t item; |
uint8_t wahl; |
uint8_t i; |
uint8_t goUp = false; |
uint8_t goDown = false; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein Menue vorhanden |
if( menu[midx].itemcount == 0) { _MenuCtrl_Error( ERROR_NOITEM ); return 0; } // kein Menueitem vorhanden |
// bei MENUCTRL_RETURN muss man ggf. selber fuer ein Redraw sorgen |
// (beim ersten Aufruf wird es jedoch dennoch erzwungen) |
if( ctrlmode == MENUCTRL_EVENT || !menu[midx].firstredraw ) |
{ |
MenuCtrl_Refresh( MENU_REDRAW ); |
menu[midx].firstredraw = true; |
} |
clear_key_all(); |
do |
{ |
menu_event = MENUEVENT_NONE; |
menu[midx].lastkey = KEY_NONE; |
refreshmode = false; |
//-------------------------- |
// Pruefe PKT Update oder |
// andere PKT-Aktion |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
refreshmode = MENU_REDRAW; |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Cursor: nach oben (UP) |
//-------------------------- |
//+++++ |
// UP - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_MINUS) ) |
{ |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active-1 ); |
goUp = true; |
} |
else _MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
if( get_key_short(1 << KEY_MINUS) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus AKTIV |
//+++++ |
// UP - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus NICHT aktiv |
//+++++ |
// UP - goUp |
//+++++ |
if( goUp ) // up |
{ |
refreshmode = MENU_REFRESH; |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
menu[midx].active--; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = menu[midx].itemcount-1; |
} |
else |
{ |
_MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
// Cursor: nach unten (DOWN) |
//-------------------------- |
//+++++ |
// DOWN - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active+1 ); |
goDown = true; |
} |
else _MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
if( get_key_short(1 << KEY_PLUS) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus AKTIV |
//+++++ |
// DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
// DOWN - goDown |
//+++++ |
if( goDown ) // down |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
menu[midx].active++; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = 0; |
} |
else |
{ |
_MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
//-------------------------- |
item = menu[midx].active; |
goUp = false; |
goDown = false; |
//-------------------------- |
// KEY_ENTER |
//-------------------------- |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
// todo: keyfunc |
if( menu[midx].item[item].type == MENU_SEPARATOR ) |
{ |
// do nothing |
} |
else if( menu[midx].item[item].disabled ) |
{ |
set_beep( 200, 0x00ff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_NOTPOSSIBLE),0,0,0); // "nicht möglich!" |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].item[item].func != NOFUNC ) |
{ |
menu[midx].item[item].func(); |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].key_enter.func != NOFUNC ) |
{ |
menu[midx].key_enter.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu[midx].lastkey = KEY_ENTER; |
menu_event = MENUEVENT_ITEM; |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (Deletemodus AKTIV) |
// |
// loescht einen Menueeintrag |
//-------------------------- |
if( menu[midx].canDelete && get_key_long(1 << KEY_ENTER) && (menu[midx].itemcount > 0) ) |
{ |
wahl = 1; |
if( menu[midx].canDelete == 1 ) // ==1 -> mit Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 0, strGet(STR_MENUCTRL_DELASK),PSTR(""),0,0); // "Eintrag entfernen?" |
lcd_printp_at( 12, 7, strGet(YESNO), MINVERS); |
wahl = 0; |
while( !wahl ) |
{ |
if( get_key_short(1<<KEY_ESC) ) wahl = 1; // "Ja" |
if( get_key_short(1<<KEY_ENTER) ) wahl = 2; // "Nein" |
} |
refreshmode = MENU_REDRAW; |
} |
if( wahl==1 ) |
{ |
for( i=menu[midx].active; i<menu[midx].itemcount-1; i++) |
{ |
_MenuCtrl_SwapItem( i, i+1 ); |
} |
menu[midx].itemcount--; |
if( (menu[midx].display_pos > 0) && (menu[midx].display_pos+6 > menu[midx].itemcount) ) |
{ |
menu[midx].display_pos--; |
menu[midx].active--; |
} |
if( menu[midx].active >= menu[midx].itemcount ) |
menu[midx].active = menu[midx].itemcount-1; |
_MenuCtrl_CalcDisplayPos(); |
refreshmode = MENU_REDRAW; |
if( menu[midx].canDelete == 2 ) // ==2 -> direkt loeschen, ohne Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_DELITEM),0,0,0); // "Eintrag entfernt!" |
} |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (NICHT Deletemodus) |
//-------------------------- |
if( !menu[midx].canDelete && menu[midx].key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_enter_long.func != NOFUNC ) |
{ |
menu[midx].key_enter_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ENTER_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC_LONG |
//-------------------------- |
if( menu[midx].key_esc_long.active && get_key_long(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc_long.func != NOFUNC ) |
{ |
menu[midx].key_esc_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC |
//-------------------------- |
if( get_key_short(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc.func != NOFUNC ) |
{ |
menu[midx].key_esc.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC; |
} |
} |
if( refreshmode ) |
{ |
MenuCtrl_Refresh( refreshmode ); |
clear_key_all(); |
} |
} |
while ( menu_event == MENUEVENT_NONE && ctrlmode == MENUCTRL_EVENT ); |
return menu_event; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/menuctrl.h |
---|
0,0 → 1,138 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.h |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() |
//# - add: MENU_SEPARATOR |
//# - chg: Wert von MENU_REFRESH und MENU_REDRAW |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: define MENU_PARAMEDIT |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - add: MenuCtrl_ItemMark() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 17.05.2013 OG - NEU |
//############################################################################ |
#ifndef MENUCTRL_H |
#define MENUCTRL_H |
#define MENU_ITEM 1 // Typ des Menueitems: normaler Eintrag ohne ">" am Ende |
#define MENU_SUB 2 // Typ des Menueitems: Submenue - es wird noch ein ">" angezeigt |
#define MENU_SEPARATOR 3 // nur fuer MenuCtrl_PushSeparator() fuer (verwendet von mkparameters.c) |
#define MENU_PARAMEDIT 4 // nur fuer MenuCtrl_PushParamEdit() fuer (verwendet von mkparameters.c) |
#define NOFUNC 0 // keine Funktion uebergeben |
#define NOTEXT 0 // kein Text uebergeben |
#define MENUEVENT_NONE 0 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_ITEM 1 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_KEY 2 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUCTRL_EVENT 0 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung nur bei einem Event |
#define MENUCTRL_RETURN 1 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung immer |
#define MENU_REFRESH 1 // MenuCtrl_Refresh() |
#define MENU_REDRAW 2 // MenuCtrl_Refresh() |
void MenuCtrl_Create( void ); |
void MenuCtrl_Destroy( void ); |
void MenuCtrl_ShowLevel( uint8_t value ); |
void MenuCtrl_SetTitle( const char *title ); |
void MenuCtrl_SetTitle_P( const char *title ); |
void MenuCtrl_SetTitleFromParentItem( void ); |
void MenuCtrl_SetCycle( uint8_t value ); |
void MenuCtrl_SetShowBatt( uint8_t value ); |
void MenuCtrl_SetBeep( uint8_t value ); |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ); |
void MenuCtrl_SetTopSpace( uint8_t value ); |
void MenuCtrl_SetMove( uint8_t value ); |
void MenuCtrl_SetDelete( uint8_t value ); |
int8_t MenuCtrl_GetMenuIndex( void ); |
uint8_t MenuCtrl_GetItemId( void ); |
uint8_t MenuCtrl_GetKey( void ); |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ); |
uint8_t MenuCtrl_GetItemCount( void ); |
const char * MenuCtrl_GetItemText( void ); |
uint8_t MenuCtrl_IsItemTextPGM( void ); |
void MenuCtrl_ItemSelect( uint8_t itemid ); |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ); |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_PushParamEdit( uint8_t itemid ); |
void MenuCtrl_PushSeparator( void ); |
void MenuCtrl_PushSeparatorID( uint8_t itemid ); |
void MenuCtrl_Refresh( uint8_t mode ); |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ); |
#endif // MENUCTRL_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/scrollbox.c |
---|
0,0 → 1,570 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.c |
//# |
//# 31.05.2014 OG |
//# - chg: ScrollBox_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 29.03.2014 OG |
//# - chg: ScrollBox_Show() umgestellt auf clear_key_all() |
//# |
//# 22.05.2013 OG |
//# - fix: include pkt/pkt.h |
//# |
//# 19.05.2013 OG |
//# - chg: ScrollBox_Show() erweitert um PKT_CtrlHook() um u.a. PKT-Updates |
//# zu ermoeglichen |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() - Variante fuer 'format' im RAM |
//# - chg: ScrollBox_Push_P() -> keine Rueckgabe mehr (void) |
//# - chg: auf xprintf umgestellt (siehe utils/xstring.c) |
//# - add: verschiedene Beschreibungen ueber Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
//#define USE_SCROLLBOX_DEBUG // Debug-Funktionen einkompilieren/aktivieren (gesendet wird an uart1) |
#include "../cpu.h" |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdarg.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" // xprintf |
#include "scrollbox.h" |
// Debug |
#ifdef USE_SCROLLBOX_DEBUG |
#include "../uart/uart1.h" |
#include <util/delay.h> |
#endif |
#define SCROLLBOX_ALLOC_MEM // dynamische RAM-Belegen (malloc) verwenden? (Standardeinstellung: ja) |
#define SCROLLBOX_LINESIZE 20+1 // Ausgabezeile der ScrollBox (20 Chars + 0x00) |
#define SCROLLBOX_KEYSIZE 4+1 // Textlabel einer Taste (4 Chars + 0x00) |
#define SCROLLBOX_W 20 // Textbreite |
#define SCROLLBOX_H 7 // Textzeilen |
#define MSBLINE 10 // drawmode-code fuer: Linie |
//----------------------------------------------------------- |
// statischer Speicher wenn nicht SCROLLBOX_ALLOC_MEM |
// verwendet wird |
//----------------------------------------------------------- |
#ifndef SCROLLBOX_ALLOC_MEM |
#define SCROLLBOX_BUFFER_SIZE 2048 // 2 KByte - ram-size in bytes |
char scrollbox_buffer[SCROLLBOX_BUFFER_SIZE]; |
#endif |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
char text[SCROLLBOX_KEYSIZE]; // Tastentext |
} scrollbox_key_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_line_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t mode; // drawmode: MNORMAL, MINVERSE oder MSBLINE |
char line[SCROLLBOX_LINESIZE]; // malloc: lines * 21 bytes (20 chars + 0) |
} scrollbox_line_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_t |
//----------------------------------------------------------- |
typedef struct |
{ |
scrollbox_line_t *buffer; // malloc: lines * 22 bytes (21 chars + 0) |
uint8_t maxlines; // max. reservierte Lines (malloc buffer) |
uint8_t lines; // Anzahl gepushte lines |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
scrollbox_key_t key_enter; |
scrollbox_key_t key_enter_long; |
} scrollbox_t; |
//----------------------------------------------------------- |
// Buffer & Co. |
//----------------------------------------------------------- |
scrollbox_t scrollbox; |
char buffer_sbline[SCROLLBOX_LINESIZE]; |
//############################################################################################# |
//############################################################################################# |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#ifdef USE_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// nur wenn define: USE_SCROLLBOX_DEBUG |
//-------------------------------------------------------------- |
void ScrollBox_Debug( void ) |
{ |
char s[33]; |
uint8_t i,j; |
char *p; |
scrollbox_line_t *sboxp; |
uint32_t h; |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: BEGIN\r\n\r\n") ); |
ltoa( scrollbox.buffer, s, 16); |
uart1_puts_p( PSTR("address: buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
ltoa( scrollbox_buffer, s, 16); |
uart1_puts_p( PSTR("address: scrollbox_buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
itoa( sizeof(scrollbox_line_t), s, 10); |
uart1_puts_p( PSTR("sizeof(scrollbox_line_t) = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.lines, s, 10); |
uart1_puts_p( PSTR("scrollbox.lines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.maxlines, s, 10); |
uart1_puts_p( PSTR("scrollbox.maxlines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.display_pos, s, 10); |
uart1_puts_p( PSTR("scrollbox.display_pos = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
for(i=0; i<scrollbox.lines; i++) |
{ |
itoa( i, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
//sboxp = scrollbox.buffer + sizeof(scrollbox_line_t)*i; |
sboxp = scrollbox.buffer + i; |
ltoa( sboxp, s, 16); // hex |
uart1_puts( "[0x" ); |
uart1_puts( s ); |
uart1_puts( "]" ); |
ltoa( sboxp, s, 10); // dec |
uart1_puts( "[" ); |
uart1_puts( s ); |
uart1_puts( "] " ); |
itoa( sboxp->mode, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
p = sboxp->line; |
if( sboxp->mode != 10 ) |
{ |
uart1_puts( p ); |
} |
uart1_puts( "|\r\n" ); |
} |
//-------------------------------------------------------------- |
// Hier wird fuer eine gewisse Zeit (insgesamt 2500 mal) permanent die Zeile 23 ausgegeben. |
// Wenn ich das richtig sehe schreibe ich hierbei nicht ;-) |
// Aber was sagt das Empfangsterminal.... |
//-------------------------------------------------------------- |
sboxp = scrollbox.buffer + 23; |
for( h=0; h<10; h++) |
{ |
itoa( h, s, 10); |
uart1_puts( s ); |
uart1_puts( "# " ); |
uart1_puts( sboxp->line ); |
uart1_puts( "|\r\n" ); |
_delay_ms(10); |
} |
//-------------------------------------------------------------- |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: END\r\n") ); |
uart1_puts( "##########################################\r\n\r\n" ); |
} |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#endif // USER_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// ok = ScrollBox_Create( uint8_t maxlines ) |
// |
// Rueckgabe: |
// true = ok |
// false = kein Speicher |
// |
// - reserviert speicher fuer max. maxlines |
// - initialisiert scrollbox-Datenstruktur |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Create( uint8_t maxlines ) |
{ |
scrollbox.buffer = 0; |
#ifdef SCROLLBOX_ALLOC_MEM |
scrollbox.buffer = malloc( maxlines * sizeof(scrollbox_line_t)); |
//scrollbox.buffer = calloc( maxlines, sizeof(scrollbox_line_t) ); |
#else |
scrollbox.buffer = (scrollbox_line_t *) scrollbox_buffer; |
#endif |
if( !scrollbox.buffer ) // kein RAM mehr |
{ |
lcd_cls(); |
lcd_printp_at( 1, 3, PSTR("Error: ScrollBox"), MNORMAL); |
lcd_printp_at( 1, 4, PSTR("No more RAM"), MNORMAL); |
set_beep ( 500, 0x3333, BeepNormal); |
while (!get_key_short (1 << KEY_ESC)); |
return false; |
} |
scrollbox.maxlines = maxlines; |
scrollbox.lines = 0; |
scrollbox.display_pos = 0; |
scrollbox.key_enter.active = false; |
scrollbox.key_enter_long.active = false; |
return true; |
} |
//-------------------------------------------------------------- |
// ScrollBox_Destroy() |
// |
// gibt reservierten Speicher wieder frei |
// Wichtig! Da malloc verwendet wird! |
//-------------------------------------------------------------- |
void ScrollBox_Destroy( void ) |
{ |
if( scrollbox.buffer ) |
{ |
#ifdef SCROLLBOX_ALLOC_MEM |
free( scrollbox.buffer ); |
#endif |
} |
scrollbox.buffer = 0; |
} |
//-------------------------------------------------------------- |
// ScrollBox_PushLine() |
// |
// fuegt eine Trennlinie hinzu |
//-------------------------------------------------------------- |
void ScrollBox_PushLine( void ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
sboxp->mode = MSBLINE; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// _scrollbox_push(...) |
// |
// intern fuer: ScrollBox_Push_P(), ScrollBox_Push() |
// Beschreibung siehe dort |
//-------------------------------------------------------------- |
void _scrollbox_push( uint8_t useprogmem, uint8_t mode, const char *format, va_list ap ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
_xvsnprintf( useprogmem, buffer_sbline, SCROLLBOX_LINESIZE, format, ap); |
strncpyfill( sboxp->line, buffer_sbline, SCROLLBOX_LINESIZE); // copy: buffer_sbline zur scrollbox |
sboxp->mode = mode; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push_P( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im PROGMEM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( true, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im RAM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( false, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// key: KEY_ENTER, KEY_ENTER_LONG (keine weiteren bisher!) |
//-------------------------------------------------------------- |
void ScrollBox_SetKey( uint8_t key, const char *keytext ) |
{ |
if( key == KEY_ENTER ) |
{ |
scrollbox.key_enter.active = true; |
strncpyfill( scrollbox.key_enter.text, keytext, SCROLLBOX_KEYSIZE); |
} |
if( key == KEY_ENTER_LONG ) |
{ |
scrollbox.key_enter_long.active = true; |
strncpyfill( scrollbox.key_enter_long.text, keytext, SCROLLBOX_KEYSIZE); |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Refresh() |
// |
// zeigt die ScrollBox - wird normalerweise von ScrollBox_Show() |
// automatisch durchgefuehrt |
//-------------------------------------------------------------- |
void ScrollBox_Refresh( void ) |
{ |
uint8_t y; |
uint8_t sh; |
uint8_t sy; |
scrollbox_line_t *sboxp; |
//-------------------------- |
// Text |
//-------------------------- |
for( y=0; y<7; y++) |
{ |
sboxp = scrollbox.buffer + (scrollbox.display_pos + y); |
if( y + scrollbox.display_pos < scrollbox.lines ) |
{ |
if( sboxp->mode == MSBLINE ) |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
lcd_line ( (21-SCROLLBOX_W)*6, (y*8)+3, 125, (y*8)+3, 1); // line |
} |
else |
{ |
lcd_puts_at( 21-SCROLLBOX_W, y, sboxp->line, sboxp->mode); |
} |
//p += sizeof(scrollbox_line_t); |
} |
else // clear |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 55 // Finetuning der Slider-Hoehe |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 7) / scrollbox.lines; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = (scrollbox.display_pos * (SLIDERH-sh)) / (scrollbox.lines-7); // Slider: Position |
lcd_frect( 0, 0, 1, SLIDERH, 0); // Slider: Clear |
lcd_frect( 0, sy, 1, sh, 1); // Slider: draw // Slider: Draw |
//-------------------------- |
// Keyline |
//-------------------------- |
PKT_KeylineUpDown( 1, 7, 0,0); // Keyline: Up / Down |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline: Ende |
if( scrollbox.key_enter.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter.text , MNORMAL); |
else if ( scrollbox.key_enter_long.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter_long.text, MNORMAL); |
else lcd_printp_at(17, 7, PSTR(" ") , MNORMAL); |
//ScrollBox_Debug(); |
} |
//-------------------------------------------------------------- |
// key = ScrollBox_Show() |
// |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Show( void ) |
{ |
uint8_t keyexit = 0; |
clear_key_all(); |
if( scrollbox.buffer ) |
{ |
ScrollBox_Refresh(); |
do |
{ |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
//-------------------------- |
// scrollen: nach unten |
//-------------------------- |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2 )) // down |
{ |
if( scrollbox.display_pos < ( scrollbox.lines - SCROLLBOX_H) ) |
{ |
scrollbox.display_pos++; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am unteren Ende angelangt |
} |
} |
//-------------------------- |
// scrollen: nach oben |
//-------------------------- |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2 )) // up |
{ |
if( scrollbox.display_pos > 0 ) |
{ |
scrollbox.display_pos--; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am oberen Ende angelangt |
} |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter.active && get_key_short(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER; |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER_LONG; |
} |
} while( !get_key_press(1 << KEY_ESC) && (keyexit == 0) ); |
} |
//get_key_press(KEY_ALL); // ersetzt durch obiges clear_key_all() |
return keyexit; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/scrollbox.h |
---|
0,0 → 1,49 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.h |
//# |
//# 17.05.2013 OG |
//# - del: defines KEY_xyz_LONG verschoben nach HAL_HW3_9.h |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() |
//# - chg: ScrollBox_Push_P() (keine Rueckgabe mehr) |
//# - add: include <stdarg.h> |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _SCROLLBOX_H |
#define _SCROLLBOX_H |
#include <stdarg.h> // Notwendig! (OG) |
uint8_t ScrollBox_Create( uint8_t maxlines ); |
void ScrollBox_Destroy( void ); |
void ScrollBox_PushLine( void ); |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ); |
void ScrollBox_Push( uint8_t mode, const char *format, ... ); |
void ScrollBox_SetKey( uint8_t key, const char *keytext ); |
void ScrollBox_Refresh( void ); |
uint8_t ScrollBox_Show( void ); |
//void ScrollBox_Debug( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/twi_slave.c |
---|
0,0 → 1,373 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.c |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : This is sample driver to AVRs TWI module. |
* It is interupt driveren. All functionality is controlled through |
* passing information to and from functions. Se main.c for samples |
* of how to use the driver. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
#if defined(__ICCAVR__) |
#include "ioavr.h" |
#include "inavr.h" |
#else |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#endif |
#include "twi_slave.h" |
// Emulate GCC ISR() statement in IAR |
#if defined(__ICCAVR__) |
#define PRAGMA(x) _Pragma( #x ) |
#define ISR(vec) PRAGMA( vector=vec ) __interrupt void handler_##vec(void) |
#endif |
static unsigned char TWI_buf[TWI_BUFFER_SIZE]; // Transceiver buffer. Set the size in the header file |
static unsigned char TWI_msgSize = 0; // Number of bytes to be transmitted. |
static unsigned char TWI_state = TWI_NO_STATE; // State byte. Default set to TWI_NO_STATE. |
// This is true when the TWI is in the middle of a transfer |
// and set to false when all bytes have been transmitted/received |
// Also used to determine how deep we can sleep. |
static volatile unsigned char TWI_busy = 0; |
union TWI_statusReg_t TWI_statusReg = {0}; // TWI_statusReg is defined in TWI_Slave.h |
/**************************************************************************** |
Call this function to set up the TWI slave to its initial standby state. |
Remember to enable interrupts from the main application after initializing the TWI. |
Pass both the slave address and the requrements for triggering on a general call in the |
same byte. Use e.g. this notation when calling this function: |
TWI_Slave_Initialise( (TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) ); |
The TWI module is configured to NACK on any requests. Use a TWI_Start_Transceiver function to |
start the TWI. |
****************************************************************************/ |
void twi_slave_init( unsigned char TWI_ownAddress ) |
{ |
TWAR = TWI_ownAddress; // Set own TWI slave address. Accept TWI General Calls. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins. |
(0<<TWIE)|(0<<TWINT)| // Disable TWI Interupt. |
(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Do not ACK on any requests, yet. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to test if the TWI_ISR is busy transmitting. |
****************************************************************************/ |
unsigned char twi_slave_busy( void ) |
{ |
return TWI_busy; |
} |
/**************************************************************************** |
Call this function to fetch the state information of the previous operation. The function will hold execution (loop) |
until the TWI_ISR has completed with the previous operation. If there was an error, then the function |
will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_status( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI has completed the transmission. |
return ( TWI_state ); // Return error state. |
} |
/**************************************************************************** |
Call this function to send a prepared message, or start the Transceiver for reception. Include |
a pointer to the data to be sent if a SLA+W is received. The data will be copied to the TWI buffer. |
Also include how many bytes that should be sent. Note that unlike the similar Master function, the |
Address byte is not included in the message buffers. |
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation, |
then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start_with_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char temp; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_msgSize = msgSize; // Number of data to transmit. |
for ( temp = 0; temp < msgSize; temp++ ) // Copy data that may be transmitted if the TWI Master requests data. |
{ |
TWI_buf[ temp ] = msg[ temp ]; |
} |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 1; |
} |
/**************************************************************************** |
Call this function to start the Transceiver without specifing new transmission data. Useful for restarting |
a transmission, or just starting the transceiver for reception. The driver will reuse the data previously put |
in the transceiver buffers. The function will hold execution (loop) until the TWI_ISR has completed with the |
previous operation, then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to read out the received data from the TWI transceiver buffer. I.e. first call |
TWI_Start_Transceiver to get the TWI Transceiver to fetch data. Then Run this function to collect the |
data when they have arrived. Include a pointer to where to place the data and the number of bytes |
to fetch in the function call. The function will hold execution (loop) until the TWI_ISR has completed |
with the previous operation, before reading out the data and returning. |
If there was an error in the previous transmission the function will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char i; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
if( TWI_statusReg.lastTransOK ) // Last transmission completed successfully. |
{ |
for ( i=0; i<msgSize; i++ ) // Copy data from Transceiver buffer. |
{ |
msg[ i ] = TWI_buf[ i ]; |
} |
TWI_statusReg.RxDataInBuf = FALSE; // Slave Receive data has been read from buffer. |
} |
return( TWI_statusReg.lastTransOK ); |
} |
// ********** Interrupt Handlers ********** // |
/**************************************************************************** |
This function is the Interrupt Service Routine (ISR), and called when the TWI interrupt is triggered; |
that is whenever a TWI event has occurred. This function should not be called directly from the main |
application. |
****************************************************************************/ |
ISR(TWI_vect) |
{ |
static unsigned char TWI_bufPtr; |
switch (TWSR) |
{ |
case TWI_STX_ADR_ACK: // Own SLA+R has been received; ACK has been returned |
// case TWI_STX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
case TWI_STX_DATA_ACK: // Data byte in TWDR has been transmitted; ACK has been received |
TWDR = TWI_buf[TWI_bufPtr++]; |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_STX_DATA_NACK: // Data byte in TWDR has been transmitted; NACK has been received. |
// I.e. this could be the end of the transmission. |
if (TWI_bufPtr == TWI_msgSize) // Have we transceived all expected data? |
{ |
TWI_statusReg.lastTransOK = TRUE; // Set status bits to completed successfully. |
} |
else // Master has sent a NACK before all data where sent. |
{ |
TWI_state = TWSR; // Store TWI State as errormessage. |
} |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Answer on next address match |
(0<<TWWC); // |
TWI_busy = 0; // Transmit is finished, we are not busy anymore |
break; |
case TWI_SRX_GEN_ACK: // General call address has been received; ACK has been returned |
// case TWI_SRX_GEN_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
TWI_statusReg.genAddressCall = TRUE; |
case TWI_SRX_ADR_ACK: // Own SLA+W has been received ACK has been returned |
// case TWI_SRX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
// Dont need to clear TWI_S_statusRegister.generalAddressCall due to that it is the default state. |
TWI_statusReg.RxDataInBuf = TRUE; |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Expect ACK on this transmission |
(0<<TWWC); |
TWI_busy = 1; |
break; |
case TWI_SRX_ADR_DATA_ACK: // Previously addressed with own SLA+W; data has been received; ACK has been returned |
case TWI_SRX_GEN_DATA_ACK: // Previously addressed with general call; data has been received; ACK has been returned |
// TODO: What is this? Seems to be no bounds checking! |
TWI_buf[TWI_bufPtr++] = TWDR; |
TWI_statusReg.lastTransOK = TRUE; // Set flag transmission successfull. |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Send ACK after next reception |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_SRX_STOP_RESTART: // A STOP condition or repeated START condition has been received while still addressed as Slave |
// Enter not addressed mode and listen to address match |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Enable interrupt and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Wait for new address match |
(0<<TWWC); // |
TWI_busy = 0; // We are waiting for a new address match, so we are not busy |
break; |
case TWI_SRX_ADR_DATA_NACK: // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
case TWI_SRX_GEN_DATA_NACK: // Previously addressed with general call; data has been received; NOT ACK has been returned |
case TWI_STX_DATA_ACK_LAST_BYTE: // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// case TWI_NO_STATE // No relevant state information available; TWINT = �0� |
case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition |
TWI_state = TWSR; //Store TWI State as errormessage, operation also clears noErrors bit |
TWCR = (1<<TWSTO)|(1<<TWINT); //Recover from TWI_BUS_ERROR, this will release the SDA and SCL pins thus enabling other devices to use the bus |
break; |
default: |
TWI_state = TWSR; // Store TWI State as errormessage, operation also clears the Success bit. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Acknowledge on any new requests. |
(0<<TWWC); // |
TWI_busy = 0; // Unknown status, so we wait for a new address match that might be something we can handle |
} |
} |
/* |
void example(){ |
unsigned char messageBuf[TWI_BUFFER_SIZE]; |
unsigned char TWI_slaveAddress; |
// LED feedback port - connect port B to the STK500 LEDS |
DDRB = 0xFF; // Set to ouput |
PORTB = 0x55; // Startup pattern |
// Own TWI slave address |
TWI_slaveAddress = 0x10; |
// Initialise TWI module for slave operation. Include address and/or enable General Call. |
TWI_Slave_Initialise( (unsigned char)((TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) )); |
SEI(); |
// Start the TWI transceiver to enable reseption of the first command from the TWI Master. |
TWI_Start_Transceiver(); |
// This example is made to work together with the AVR315 TWI Master application note. In adition to connecting the TWI |
// pins, also connect PORTB to the LEDS. The code reads a message as a TWI slave and acts according to if it is a |
// general call, or an address call. If it is an address call, then the first byte is considered a command byte and |
// it then responds differently according to the commands. |
// This loop runs forever. If the TWI is busy the execution will just continue doing other operations. |
for(;;) |
{ |
#ifdef POWER_MANAGEMENT_ENABLED |
// Sleep while waiting for TWI transceiver to complete or waiting for new commands. |
// If we have data in the buffer, we can't enter sleep because we have to take care |
// of it first. |
// If the transceiver is busy, we enter idle mode because it will wake up by all TWI |
// interrupts. |
// If the transceiver not is busy, we can enter power-down mode because next receive |
// should be a TWI address match and it wakes the device up from all sleep modes. |
if( ! TWI_statusReg.RxDataInBuf ) { |
if(TWI_Transceiver_Busy()) { |
MCUCR = (1<<SE)|(0<<SM2)|(0<<SM1)|(0<<SM0); // Enable sleep with idle mode |
} else { |
MCUCR = (1<<SE)|(0<<SM2)|(1<<SM1)|(0<<SM0); // Enable sleep with power-down mode |
} |
SLEEP(); |
} else { |
NOP(); // There is data in the buffer, code below takes care of it. |
} |
#else // No power management |
// Here you can add your own code that should be run while waiting for the TWI to finish |
NOP(); // Put own code here. |
#endif |
// Check if the TWI Transceiver has completed an operation. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
// Check if the last operation was successful |
if ( TWI_statusReg.lastTransOK ) |
{ |
// Check if the last operation was a reception |
if ( TWI_statusReg.RxDataInBuf ) |
{ |
TWI_Get_Data_From_Transceiver(messageBuf, 2); |
// Check if the last operation was a reception as General Call |
if ( TWI_statusReg.genAddressCall ) |
{ |
// Put data received out to PORTB as an example. |
PORTB = messageBuf[0]; |
} |
else // Ends up here if the last operation was a reception as Slave Address Match |
{ |
// Example of how to interpret a command and respond. |
// TWI_CMD_MASTER_WRITE stores the data to PORTB |
if (messageBuf[0] == TWI_CMD_MASTER_WRITE) |
{ |
PORTB = messageBuf[1]; |
} |
// TWI_CMD_MASTER_READ prepares the data from PINB in the transceiver buffer for the TWI master to fetch. |
if (messageBuf[0] == TWI_CMD_MASTER_READ) |
{ |
messageBuf[0] = PINB; |
TWI_Start_Transceiver_With_Data( messageBuf, 1 ); |
} |
} |
} |
else // Ends up here if the last operation was a transmission |
{ |
NOP(); // Put own code here. |
} |
// Check if the TWI Transceiver has already been started. |
// If not then restart it to prepare it for new receptions. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
TWI_Start_Transceiver(); |
} |
} |
else // Ends up here if the last operation completed unsuccessfully |
{ |
TWI_Act_On_Failure_In_Last_Transmission( TWI_Get_State_Info() ); |
} |
} |
} |
}*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/twi_slave.h |
---|
0,0 → 1,121 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.h |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : Header file for TWI_slave.c |
* Include this file in the application. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
/**************************************************************************** |
TWI Status/Control register definitions |
****************************************************************************/ |
#define TWI_BUFFER_SIZE 4 // Reserves memory for the drivers transceiver buffer. |
// Set this to the largest message size that will be sent including address byte. |
/**************************************************************************** |
Global definitions |
****************************************************************************/ |
union TWI_statusReg_t // Status byte holding flags. |
{ |
unsigned char all; |
struct |
{ |
unsigned char lastTransOK:1; |
unsigned char RxDataInBuf:1; |
unsigned char genAddressCall:1; // TRUE = General call, FALSE = TWI Address; |
unsigned char unusedBits:5; |
}; |
}; |
extern union TWI_statusReg_t TWI_statusReg; |
/**************************************************************************** |
Function definitions |
****************************************************************************/ |
void twi_slave_init( unsigned char ); |
unsigned char twi_slave_busy( void ); |
unsigned char twi_slave_get_status( void ); |
void twi_slave_start_with_data( unsigned char * , unsigned char ); |
void twi_slave_start( void ); |
unsigned char twi_slave_get_data( unsigned char *, unsigned char ); |
/**************************************************************************** |
Bit and byte definitions |
****************************************************************************/ |
#define TWI_READ_BIT 0 // Bit position for R/W bit in "address byte". |
#define TWI_ADR_BITS 1 // Bit position for LSB of the slave address bits in the init byte. |
#define TWI_GEN_BIT 0 // Bit position for LSB of the general call bit in the init byte. |
#define TRUE 1 |
#define FALSE 0 |
/**************************************************************************** |
TWI State codes |
****************************************************************************/ |
// General TWI Master staus codes |
#define TWI_START 0x08 // START has been transmitted |
#define TWI_REP_START 0x10 // Repeated START has been transmitted |
#define TWI_ARB_LOST 0x38 // Arbitration lost |
// TWI Master Transmitter staus codes |
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received |
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received |
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received |
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received |
// TWI Master Receiver staus codes |
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received |
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received |
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted |
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted |
// TWI Slave Transmitter staus codes |
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned |
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received |
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received |
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// TWI Slave Receiver staus codes |
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned |
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned |
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned |
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave |
// TWI Miscellaneous status codes |
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = �0� |
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/twimaster.c |
---|
0,0 → 1,202 |
/************************************************************************* |
* Title: I2C master library using hardware TWI interface |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: twimaster.c,v 1.3 2005/07/02 11:14:21 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device with hardware TWI |
* Usage: API compatible with I2C Software Library i2cmaster.h |
**************************************************************************/ |
#include <inttypes.h> |
#include <compat/twi.h> |
#include <i2cmaster.h> |
/* define CPU frequency in Mhz here if not defined in Makefile */ |
#ifndef F_CPU |
#define F_CPU 16000000UL |
#endif |
/* I2C clock in Hz */ |
#define SCL_CLOCK 400000L |
/************************************************************************* |
Initialization of the I2C bus interface. Need to be called only once |
*************************************************************************/ |
void i2c_init(void) |
{ |
/* initialize TWI clock: 100 kHz clock, TWPS = 0 => prescaler = 1 */ |
TWSR = 0; /* no prescaler */ |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ |
}/* i2c_init */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
return 0 = device accessible, 1= failed to access device |
*************************************************************************/ |
unsigned char i2c_start(unsigned char address) |
{ |
uint8_t twst; |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) return 1; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed and ACK/NACK has been received |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1; |
return 0; |
}/* i2c_start */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
If device is busy, use ack polling to wait until device is ready |
Input: address and transfer direction of I2C device |
*************************************************************************/ |
void i2c_start_wait(unsigned char address) |
{ |
uint8_t twst; |
while ( 1 ) |
{ |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) continue; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) |
{ |
/* device busy, send stop condition to terminate write operation */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
continue; |
} |
//if( twst != TW_MT_SLA_ACK) return 1; |
break; |
} |
}/* i2c_start_wait */ |
/************************************************************************* |
Issues a repeated start condition and sends address and transfer direction |
Input: address and transfer direction of I2C device |
Return: 0 device accessible |
1 failed to access device |
*************************************************************************/ |
unsigned char i2c_rep_start(unsigned char address) |
{ |
return i2c_start( address ); |
}/* i2c_rep_start */ |
/************************************************************************* |
Terminates the data transfer and releases the I2C bus |
*************************************************************************/ |
void i2c_stop(void) |
{ |
/* send stop condition */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
}/* i2c_stop */ |
/************************************************************************* |
Send one byte to I2C device |
Input: byte to be transfered |
Return: 0 write successful |
1 write failed |
*************************************************************************/ |
unsigned char i2c_write( unsigned char data ) |
{ |
uint8_t twst; |
// send data to the previously addressed device |
TWDR = data; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits |
twst = TW_STATUS & 0xF8; |
if( twst != TW_MT_DATA_ACK) return 1; |
return 0; |
}/* i2c_write */ |
/************************************************************************* |
Read one byte from the I2C device, request more data from device |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readAck(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readAck */ |
/************************************************************************* |
Read one byte from the I2C device, read is followed by a stop condition |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readNak(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readNak */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/xutils.c |
---|
0,0 → 1,765 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ xutils.c - erweiterte String-Funktionen, xprintf (Info nach History) |
//+ und weitere Hilfsfunktionen wie z.B. UTCdatetime2local() |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY xutils.c |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P(), _strncpyat() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# - add: buffer sprintf_buffer[] |
//# - chg: PGMBUFF_SIZE und ARGBUFF_SIZE von 80 auf 40 geaendert |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() entfernt Leerzeichen auf der rechten Seite |
//# |
//# 14.05.2013 OG |
//# - chg: Kommentarkopf von UTCdatetime2local() aktualisiert |
//# |
//# 05.05.2013 OG |
//# - chg: UTCdatetime2local() auf Config.timezone/summertime umgestellt |
//# - add: include eeprom.h |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.c |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# - fix: _xvsnprintf() Darstellung kleiner negativer Nachkommazahlen (-1<z<0) |
//# |
//# 29.04.2013 OG |
//# - chg: Doku zu xprintf Ergaenzt bei 'bekannte Einschraenkungen' |
//# - chg: includes reduziert auf das Notwendige |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
//############################################################################ |
//# xprintf |
//# |
//# Diese Variante von printf ist angepasst auf das PKT: |
//# |
//# - Unterstuetzung von Festkomma-Integer Anzeige |
//# - Overflow-Anzeige durch '*' wenn eine Zahl die Format-Maske sprengt |
//# - progmen wird optional unterstuetz fuer den format-String als auch fuer |
//# String Argumente |
//# - strikte Einhaltung von Laengen einer Format-Maske |
//# (ggf. wird die Ausgabe gekuerzt) |
//# |
//# In diesem Source sind nur die Basis-xprintf zum Erzeugen von formatierten |
//# Strings. Die direkten Ausgabefunktionen auf den Screen sind in lcd.c |
//# als lcdx_printf_at() und lcdx_printf_at_P(). |
//# |
//# FORMAT ANGABEN: |
//# |
//# %d: dezimal int signed (Rechtsbuendige Ausgabe) (Wandlung via itoa) |
//# "%d" arg: 1234 -> "1234" |
//# "%5d" arg: 1234 -> " 1234" |
//# "%5.2d" arg: 1234 -> " 12.34" |
//# "%05d" arg: 123 -> "00123" |
//# "%3.2d" arg: -13 -> " -0.13" |
//# |
//# %u: dezimal int unsigned (Rechtsbuendige Ausgabe) (Wandlung via utoa) |
//# wie %d jedoch mittels utoa |
//# |
//# %h: hex int unsigned -> Hex-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%4h" |
//# %o: octal int unsigned -> Octal-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%2o" |
//# %b: binary int unsigned -> Binaer-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%8b" |
//# |
//# %ld, %lu, %lh, %lo, %lb: |
//# wie die obigen jedoch fuer long-zahlen (via ltoa, ultoa) |
//# nur wenn define USE_XPRINTF_LONG gesetzt ist! |
//# Bespiele: "%ld", "%5.6ld" usw. |
//# |
//# %s: String aus RAM -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %S: String aus progmem -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# |
//# %c: einzelnes char -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %%: Ausgabe von "%" |
//# |
//# |
//# BEISPIELE: |
//# |
//# vorhanden in: osd/osdata.c, lcd/lcd.c |
//# |
//# |
//# BEKANNTE EINSCHRAENKUNGEN: |
//# |
//# 1. padding '0' mit negativen Zahlen wenn padding aktiv ist ergibt z.B. "00-1.5" |
//# 2. der Entwickler muss auf eine korrekte Format-Maske achten. Andernfalls |
//# kommt es zu nicht vorhersehbaren Ausgaben. |
//# 3. nicht in ISR's verwenden - dazu muss ggf. eine Anpassung durchgefuert werden |
//# |
//# KOMPILIER OPTIONEN |
//# |
//# define: USE_XPRINTF_LONG |
//# Unterstuetzung von long int / unsigned long int ('l' Modifier in Maske) |
// Wird in der main.h gesetzt |
//############################################################################ |
#define USE_XPRINTF_LONG // Unterstuetzung von long integer - Achtung! Muss fuer PKT gesetzt sein |
// da in Sourcen verwendet! |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#define PGMBUFF_SIZE 40 // max. 40 chars bei format-String aus progmem |
#define ARGBUFF_SIZE 40 // buffer fuer xprintf Parameter (strings, itoa, utoa) (fuer ltoa ggf. anpassen) |
#define SPRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer xsnprintf(), xsnprintf_P() |
char sprintf_buffer[SPRINTF_BUFFER_SIZE]; |
//---------------------- |
// xprintf parser data |
//---------------------- |
typedef struct |
{ |
uint8_t parse; // true / false |
char cmd; // char: d u h o b s S c % |
uint8_t prelen; // Vorkomma |
uint8_t declen; // decimal (Nachkomma) |
uint8_t point; // true / false |
uint8_t uselong; // true / false |
uint8_t mask; // true / false |
char pad; // ' ' oder '0' |
} xprintf_t; |
//---------------------- |
// Buffers |
//---------------------- |
char cmddigit[7]; |
char argbuff[ARGBUFF_SIZE]; |
char pgmbuff[PGMBUFF_SIZE]; |
//#################################################################################### |
//--------------------------------------------------------------------- |
// Basisfunktion von xprintf |
// Doku: siehe oben |
//--------------------------------------------------------------------- |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ) |
{ |
const char *p_fmt; // pointer auf den format-String |
char *p_dst; // pointer auf den destination buffer |
const char *p_str; // pointer auf einen arg-String wenn ein String ausgegeben werden soll |
char *p_cmddigit = 0; // pointer auf den Digit-Buffer fuer eine Maske (Laenge/Nachkommastellen) |
const char *p_fmtbuff; // pointer auf den format-Buffer (fuer progmem) |
const char *p_argbuff; // pointer auf den argbuffer |
uint8_t fmtlen, arglen, overflow; |
uint8_t i,j,dec; |
uint8_t dstcnt; |
uint8_t minus; |
xprintf_t cmd; // parser Daten |
p_fmtbuff = format; |
if( useprogmem ) // format von progmem in's RAM kopieren |
{ |
strncpy_P( pgmbuff, format, PGMBUFF_SIZE); |
pgmbuff[PGMBUFF_SIZE-1] = 0; |
p_fmtbuff = pgmbuff; |
} |
cmd.parse = false; |
p_fmt = p_fmtbuff-1; // -1 -> wird am Anfang der Schleife korrigiert |
p_dst = buffer; |
dstcnt = 0; |
do |
{ |
if( dstcnt >= n ) // max. Anzahl von Zeichen fuer Ziel 'buffer' erreicht? |
break; |
p_fmt++; // naechstes Zeichen von format |
//######################## |
//# 1. PARSE |
//######################## |
//------------------------ |
// START: parse cmd |
//------------------------ |
if( cmd.parse == false && *p_fmt == '%' ) |
{ |
memset( &cmd, 0, sizeof(xprintf_t) ); // init |
cmd.parse = true; |
cmd.pad = ' '; |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// NO parse: copy char |
//------------------------ |
if( cmd.parse == false ) |
{ |
*p_dst = *p_fmt; |
p_dst++; |
dstcnt++; |
continue; |
} |
//------------------------ |
// set: pad (eine '0' ganz am Anfang der Formatmaske) |
//------------------------ |
if( cmd.parse == true && *p_fmt == '0' && p_cmddigit == cmddigit ) |
{ |
cmd.pad = '0'; |
continue; |
} |
//------------------------ |
// set: vor/nach-kommastellen |
//------------------------ |
if( cmd.parse == true && *p_fmt >= '0' && *p_fmt <= '9' ) |
{ |
*p_cmddigit = *p_fmt; |
p_cmddigit++; |
continue; |
} |
//------------------------ |
// set: point |
//------------------------ |
if( cmd.parse == true && *p_fmt == '.' && cmd.point == false ) |
{ |
cmd.point = true; |
*p_cmddigit = 0; |
cmd.prelen = atoi( cmddigit ); |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// set: uselong |
//------------------------ |
#ifdef USE_XPRINTF_LONG |
if( cmd.parse == true && *p_fmt == 'l' ) |
{ |
cmd.uselong = true; |
continue; |
} |
#endif |
//------------------------ |
// END: parse cmd |
//------------------------ |
if( cmd.parse == true && (*p_fmt == 'd' || *p_fmt == 'u' || *p_fmt == 'x' || *p_fmt == 'b' || *p_fmt == 'o' || |
*p_fmt == 's' || *p_fmt == 'S' || *p_fmt == 'c' || *p_fmt == '%') ) |
{ |
cmd.cmd = *p_fmt; |
cmd.parse = false; |
*p_cmddigit = 0; |
if( cmd.point == false ) cmd.prelen = atoi(cmddigit); |
else cmd.declen = atoi(cmddigit); |
if( cmd.point || cmd.prelen>0 ) |
cmd.mask = true; |
} |
//######################## |
//# 2. EXECUTE |
//######################## |
//------------------------ |
// exec cmd: "d,u,x,b,o" |
//------------------------ |
if( cmd.cmd == 'd' || cmd.cmd == 'u' || cmd.cmd == 'x' || cmd.cmd == 'b' || cmd.cmd == 'o' ) |
{ |
if( cmd.uselong ) |
{ |
#ifdef USE_XPRINTF_LONG |
switch(cmd.cmd) |
{ |
case 'd': ltoa ( va_arg(ap, long) , argbuff, 10); break; // LONG dezimal int signed |
case 'u': ultoa( va_arg(ap, unsigned long), argbuff, 10); break; // LONG dezimal int unsigned |
case 'x': ultoa( va_arg(ap, unsigned long), argbuff, 16); break; // LONG hex int unsigned |
case 'b': ultoa( va_arg(ap, unsigned long), argbuff, 2); break; // LONG binary int unsigned |
case 'o': ultoa( va_arg(ap, unsigned long), argbuff, 8); break; // LONG octal int unsigned |
} |
#endif |
} |
else |
{ |
switch(cmd.cmd) |
{ |
case 'd': itoa( va_arg(ap, int) , argbuff, 10); break; // dezimal int signed |
case 'u': utoa( va_arg(ap, unsigned int), argbuff, 10); break; // dezimal int unsigned |
case 'x': utoa( va_arg(ap, unsigned int), argbuff, 16); break; // hex int unsigned |
case 'b': utoa( va_arg(ap, unsigned int), argbuff, 2); break; // binary int unsigned |
case 'o': utoa( va_arg(ap, unsigned int), argbuff, 8); break; // octal int unsigned |
} |
} |
minus = (argbuff[0] == '-'); |
arglen = strlen(argbuff); |
fmtlen = cmd.prelen + cmd.declen + (cmd.point ? 1 : 0); |
arglen = strlen(argbuff); |
overflow = cmd.mask && // Zahl zu gross -> "*" anzeigen statt der Zahl |
(arglen > cmd.prelen + cmd.declen || cmd.prelen < 1+minus); |
if( overflow ) // overflow: Zahl passt nicht in Maske |
{ // -> zeige '*.*' |
for( i=0; (i < fmtlen) && (dstcnt < n); i++) |
{ |
if( cmd.point && i==cmd.prelen ) *p_dst = '.'; |
else *p_dst = '*'; |
p_dst++; |
dstcnt++; |
} |
} |
else // else: if( overflow ) |
{ |
if( !cmd.mask ) // keine Maske: alles von der Zahl ausgeben |
fmtlen = arglen; |
p_argbuff = argbuff; |
if( minus ) // wenn Zahl negativ: merken und auf dem argbuff nehmen |
{ |
p_argbuff++; |
arglen--; |
} |
//----------------- |
// die Zahl wird 'Rueckwaerts' uebertragen |
//----------------- |
dec = -1; // wird am Anfang der Schleife auf 0 gesetzt |
j = 1; // zaehler fuer argbuff |
for( i=1; i<=fmtlen; i++ ) |
{ |
dec++; // Zaehler Dizmalstellen |
if( dstcnt+fmtlen-i <= n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( cmd.point && (dec == cmd.declen) ) // Dezimalpunkt setzen |
{ |
p_dst[fmtlen-i] = '.'; |
continue; |
} |
if( j <= arglen ) // Ziffer uebertragen aus argbuff |
{ |
p_dst[fmtlen-i] = p_argbuff[arglen-j]; |
j++; |
continue; |
} |
if( cmd.declen > 0 && // Nachkomma und 1. Vorkommastelle ggf. auf '0' |
(dec < cmd.declen || dec == cmd.declen+1) ) // setzen wenn die Zahl zu klein ist |
{ |
p_dst[fmtlen-i] = '0'; |
continue; |
} |
if( minus && ( (cmd.pad == ' ') || // ggf. Minuszeichen setzen |
(cmd.pad != ' ' && i == fmtlen) ) ) // Minuszeichen bei '0'-padding an erster Stelle setzen |
{ |
minus = false; |
p_dst[fmtlen-i] = '-'; |
continue; |
} |
p_dst[fmtlen-i] = cmd.pad; // padding ' ' oder '0' |
} // end: if( dstcnt+fmtlen-i <= n ) |
} |
p_dst += fmtlen; |
dstcnt += fmtlen; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "s", "S", "c" |
//------------------------ |
if( cmd.cmd == 's' || cmd.cmd == 'S' || cmd.cmd == 'c' ) |
{ |
switch(cmd.cmd) |
{ |
case 's': p_str = va_arg( ap, char *); // String aus dem RAM |
break; |
case 'S': strncpy_P( argbuff, va_arg( ap, char *), ARGBUFF_SIZE); // String liegt im progmem -> in's RAM kopieren |
argbuff[ARGBUFF_SIZE-1] = 0; |
p_str = argbuff; |
break; |
case 'c': argbuff[0] = va_arg( ap, int); // einzelnes char |
argbuff[1] = 0; |
p_str = argbuff; |
break; |
} |
fmtlen = cmd.prelen; |
arglen = strlen(p_str); |
if( !cmd.mask ) // keine Maske: alles vom String ausgeben |
fmtlen = arglen; |
for( i=0; i<fmtlen; i++) |
{ |
if( dstcnt < n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( *p_str ) // char uebertragen |
{ |
*p_dst = *p_str; |
p_str++; |
} |
else // padding |
{ |
*p_dst = ' '; |
} |
p_dst++; |
} |
dstcnt++; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "%" |
//------------------------ |
if( cmd.cmd == '%' ) |
{ |
*p_dst = '%'; |
p_dst++; |
dstcnt++; |
continue; |
} |
} while( (dstcnt < n) && *p_fmt ); |
*(p_dst + (dstcnt < n ? 0 : -1)) = 0; // terminierende 0 im Ausgabebuffer setzen |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( false, buffer, n, format, ap); |
va_end(ap); |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( true, buffer, n, format, ap); |
va_end(ap); |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf_P( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//-------------------------------------------------------------- |
// kopiert einen String von src auf dst mit fester Laenge und |
// ggf. Space paddings rechts |
// |
// - fuellt ggf. den dst-String auf size Laenge mit Spaces |
// - setzt Terminierung's 0 bei dst auf Position size |
//-------------------------------------------------------------- |
void strncpyfill( char *dst, const char *src, size_t size) |
{ |
uint8_t i; |
uint8_t pad = false; |
for( i=0; i<size; i++) |
{ |
if(*src == 0) pad = true; |
if( pad ) *dst = ' '; |
else *dst = *src; |
src++; |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// entfernt rechte Leerzeichen aus einem String |
//-------------------------------------------------------------- |
void strrtrim( char *dst) |
{ |
char *p; |
p = dst + strlen(dst) - 1; |
while( (p != dst) && (*p == ' ') ) p--; |
if( (*p != ' ') && (*p != 0) ) p++; |
*p = 0; |
} |
//-------------------------------------------------------------- |
// INTERN - fuer strncpyat(), strncpyat_P() |
//-------------------------------------------------------------- |
void _strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount, uint8_t progmem) |
{ |
uint8_t i; |
if( progmem ) |
strncpy_P( dst, src, size); |
else |
strncpy( dst, src, size); |
for( i=0; i<size; i++) |
{ |
if( *dst == 0) return; |
if( *dst == sepchar) |
{ |
sepcharcount--; |
if( sepcharcount==0 ) |
{ |
*dst = 0; |
return; |
} |
} |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// strncpyat( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in PROGMEM |
//-------------------------------------------------------------- |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, false); |
} |
//-------------------------------------------------------------- |
// strncpyat_P( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in RAM |
//-------------------------------------------------------------- |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, true); |
} |
//-------------------------------------------------------------- |
// UTCdatetime2local( PKTdatetime_t *dtbuffer, PKTdatetime_t dt ) |
// |
// konvertiert die UTC-Time 'dt' in die lokale Zeit und speichert |
// dieses in 'dtbuffer' ab. |
// |
// Parameter: |
// |
// dtdst: Pointer Destination (PKTdatetime_t) (Speicher muss alloziiert sein!) |
// dtsrc: Pointer Source (PKTdatetime_t) |
// |
// Hinweise: |
// |
// Schaltjahre (bzw. der 29.02.) werden nicht unterstuetzt |
//-------------------------------------------------------------- |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ) |
{ |
int8_t timeoffset; |
int32_t v; |
int8_t diff; |
// 01 02 03 04 05 06 07 08 09 10 11 12 Monat |
int8_t daymonth[] = {31,28,31,30,31,30,31,31,30,31,30,31}; |
//-------------------------- |
// timezone: Einstellbereich -12 .. 0 .. 12 (+1 = Berlin) |
// summertime: Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) |
//-------------------------- |
timeoffset = Config.timezone + Config.summertime; |
//timeoffset = 2; // solange noch nicht in PKT-Config: Berlin, Sommerzeit |
memcpy( dtdst, dtsrc, sizeof(PKTdatetime_t) ); // copy datetime to destination |
//-------------------------- |
// Zeitzonenanpassung |
//-------------------------- |
if( dtdst->year != 0 && dtdst->month >= 1 && dtdst->month <= 12 ) // nur wenn gueltiges Datum vorhanden |
{ |
//-------------------------- |
// 1. Sekunden |
//-------------------------- |
v = (int32_t)dtdst->seconds; |
v += timeoffset*3600; // Stunden korrigieren |
diff = 0; |
if( v > 86400 ) // Tagesueberschreitung? (86400 = 24*60*60 bzw. 24 Stunden) |
{ |
v -= 86400; |
diff++; // inc: Tag |
} |
else if( v < 0 ) // Tagesunterschreitung? |
{ |
v += 86400; |
diff--; // dec: Tag |
} |
dtdst->seconds = (uint32_t)v; // SET: seconds |
//-------------------------- |
// 2. Tag |
//-------------------------- |
v = (int32_t)dtdst->day; |
v += diff; |
diff = 0; |
if( v > daymonth[dtdst->month-1] ) // Monatsueberschreitung? |
{ |
v = 1; // erster Tag des Monats |
diff++; // inc: Monat |
} |
else if( v < 1 ) // Monatsunterschreitung? |
{ |
if( dtdst->month > 1 ) |
v = daymonth[dtdst->month-1-1]; // letzter Tag des vorherigen Monats |
else |
v = 31; // letzter Tag im Dezember des vorherigen Jahres |
diff--; // dec: Monat |
} |
dtdst->day = (uint8_t)v; // SET: day |
//-------------------------- |
// 3. Monat |
//-------------------------- |
v = (int32_t)dtdst->month; |
v += diff; |
diff = 0; |
if( v > 12 ) // Jahresueberschreitung? |
{ |
v = 1; |
diff++; // inc: Jahr |
} |
else if( v < 1 ) // Jahresunterschreitung? |
{ |
v = 12; |
diff--; // dec: Jahr |
} |
dtdst->month = (uint8_t)v; // SET: month |
//-------------------------- |
// 4. Jahr |
//-------------------------- |
dtdst->year += diff; // SET: year |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils/xutils.h |
---|
0,0 → 1,64 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY xutils.h |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.h |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
#ifndef _XUTILS_H |
#define _XUTILS_H |
#include <stdarg.h> |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ); |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ); |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ); |
char *buffered_sprintf( const char *format, ... ); |
char *buffered_sprintf_P( const char *format, ... ); |
void strncpyfill( char *dst, const char *src, size_t size); |
void strrtrim( char *dst); |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/utils |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+maniacbug-mighty-1284p-68ed99c |
+ |
+Arduino |
+ |
+_doc |
+ |
+maniacbug-mighty-1284p-68ed99c.zip |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/waypoints/waypoints.c |
---|
0,0 → 1,362 |
/*#######################################################################################*/ |
/* !!! THIS IS NOT FREE SOFTWARE !!! */ |
/*#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 2008 Ingo Busker, Holger Buss |
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY |
// + FOR NON COMMERCIAL USE ONLY |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdint.h> |
#include <string.h> |
#include <stdbool.h> |
//#include "91x_lib.h" |
#include "waypoints.h" |
#include "../mk-data-structs.h" |
#include "../uart/uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../main.h" |
#include <util/delay.h> |
#ifdef USE_WAYPOINTS |
// the waypoints list |
NaviData_t *NaviData; |
//Point_t PointList[MAX_LIST_LEN]; |
uint8_t WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount |
uint8_t POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount |
uint8_t WPCount = 0; // number of waypoints |
uint8_t PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN |
uint8_t POICount = 0; |
uint8_t WPActive = false; |
uint8_t PointList_Init(void) |
{ |
return PointList_Clear(); |
} |
uint8_t PointList_Clear(void) |
{ |
uint8_t i; |
WPIndex = 0; // real list position are 1 ,2, 3 ... |
POIIndex = 0; // real list position are 1 ,2, 3 ... |
WPCount = 0; // no waypoints |
POICount = 0; |
PointCount = 0; // no contents |
WPActive = false; |
NaviData->WaypointNumber = WPCount; |
NaviData->WaypointIndex = 0; |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
Config.PointList[i].Position.Status = INVALID; |
Config.PointList[i].Position.Latitude = 0; |
Config.PointList[i].Position.Longitude = 0; |
Config.PointList[i].Position.Altitude = 0; |
Config.PointList[i].Heading = 361; // invalid value |
Config.PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered |
Config.PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
Config.PointList[i].Type = POINT_TYPE_INVALID; |
Config.PointList[i].Event_Flag = 0; // future implementation |
Config.PointList[i].AltitudeRate = 0; // no change of setpoint |
} |
return true; |
} |
uint8_t PointList_GetCount(void) |
{ |
return PointCount; // number of points in the list |
} |
Point_t* PointList_GetAt(uint8_t index) |
{ |
if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1])); // return pointer to this waypoint |
else return(NULL); |
} |
uint8_t PointList_SetAt(Point_t* pPoint) |
{ |
// if index is in range |
if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN)) |
{ |
// check list entry before update |
switch(Config.PointList[pPoint->Index-1].Type) |
{ |
case POINT_TYPE_INVALID: // was invalid |
switch(pPoint->Type) |
{ |
default: |
case POINT_TYPE_INVALID: |
// nothing to do |
break; |
case POINT_TYPE_WP: |
WPCount++; |
PointCount++; |
break; |
case POINT_TYPE_POI: |
POICount++; |
PointCount++; |
break; |
} |
break; |
case POINT_TYPE_WP: // was a waypoint |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
WPCount--; |
PointCount--; |
break; |
default: |
case POINT_TYPE_WP: |
//nothing to do |
break; |
case POINT_TYPE_POI: |
POICount++; |
WPCount--; |
break; |
} |
break; |
case POINT_TYPE_POI: // was a poi |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
POICount--; |
PointCount--; |
break; |
case POINT_TYPE_WP: |
WPCount++; |
POICount--; |
break; |
case POINT_TYPE_POI: |
default: |
// nothing to do |
break; |
} |
break; |
} |
memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry |
NaviData->WaypointNumber = WPCount; |
return pPoint->Index; |
} |
else return(0); |
} |
// returns the pointer to the first waypoint within the list |
Point_t* PointList_WPBegin(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
if(WPActive == false) return(NULL); |
POIIndex = 0; // set invalid POI |
if(PointCount > 0) |
{ |
// search for first wp in list |
for(i = 0; i <MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
WPIndex = i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP in the list |
{ |
NaviData->WaypointIndex = 1; |
// update index to POI |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // some points in the list but no WP found |
{ |
NaviData->WaypointIndex = 0; |
//Check for an existing POI |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
POIIndex = i + 1; |
break; |
} |
} |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns the last waypoint |
Point_t* PointList_WPEnd(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
POIIndex = 0; // set invalid |
if(WPActive == false) return(NULL); |
if(PointCount > 0) |
{ |
// search backward! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
WPIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP within the list |
{ |
NaviData->WaypointIndex = WPCount; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // list contains some points but no WP in the list |
{ |
// search backward for a POI! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
POIIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
NaviData->WaypointIndex = 0; |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns a pointer to the next waypoint or NULL if the end of the list has been reached |
Point_t* PointList_WPNext(void) |
{ |
uint8_t wp_found = 0; |
if(WPActive == false) return(NULL); |
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list |
{ |
uint8_t i; |
for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs |
{ |
wp_found = i+1; |
break; |
} |
} |
} |
if(wp_found) |
{ |
WPIndex = wp_found; // update list position |
NaviData->WaypointIndex++; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint |
} |
else |
{ // no next wp found |
NaviData->WaypointIndex = 0; |
POIIndex = 0; |
return(NULL); |
} |
} |
void PointList_WPActive(uint8_t set) |
{ |
if(set) |
{ |
WPActive = true; |
PointList_WPBegin(); // uopdates POI index |
} |
else |
{ |
WPActive = false; |
POIIndex = 0; // disable POI also |
} |
} |
Point_t* PointList_GetPOI(void) |
{ |
return PointList_GetAt(POIIndex); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/waypoints/waypoints.h |
---|
0,0 → 1,56 |
#ifndef _WAYPOINTS_H |
#define _WAYPOINTS_H |
//#include "ubx.h" |
#include "../mk-data-structs.h" |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
//typedef struct |
//{ |
// int32_t Longitude; // in 1E-7 deg |
// int32_t Latitude; // in 1E-7 deg |
// int32_t Altitude; // in mm |
// uint8_t Status;// validity of data |
//} __attribute__((packed)) GPS_Pos_t; |
//typedef struct |
//{ |
// GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
// int16_t Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
// uint8_t ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
// uint8_t HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// uint8_t Event_Flag; // future implementation |
// uint8_t Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
// uint8_t Type; // typeof Waypoint |
// uint8_t WP_EventChannelValue; // |
// uint8_t AltitudeRate; // rate to change the setpoint |
// uint8_t reserve[8]; // reserve |
//} __attribute__((packed)) Point_t; |
// Init List, return TRUE on success |
uint8_t PointList_Init(void); |
// Clear List, return TRUE on success |
uint8_t PointList_Clear(void); |
// Returns number of points in the list |
uint8_t PointList_GetCount(void); |
// return pointer to point at position |
Point_t* PointList_GetAt(uint8_t index); |
// set a point in the list at index, returns its index on success, else 0 |
uint8_t PointList_SetAt(Point_t* pPoint); |
// goto the first WP in the list and return pointer to it |
Point_t* PointList_WPBegin(void); |
// goto the last WP in the list and return pointer to it |
Point_t* PointList_WPEnd(void); |
// goto next WP in the list and return pointer to it |
Point_t* PointList_WPNext(void); |
// enables/disables waypoint function |
void PointList_WPActive(uint8_t set); |
// returns pointer to actual POI |
Point_t* PointList_GetPOI(void); |
#endif // _WAYPOINTS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wi232/Wi232.c |
---|
0,0 → 1,451 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.c |
//# |
//# 15.06.2014 OG |
//# -chg: umgestellt auf PKT_Progress_Init() / PKT_Progress_Next() |
//# |
//# 10.06.2014 OG |
//# KOMPLETTE SANIERUNG ab v3.80cX5 |
//# (ehemaliger Code in v3.80cX4 oder frueher) |
//# - add: Wi232_ConfigPC() - ehemals in connect.c als Port_USB2CFG_Wi() |
//# |
//# 06.06.2014 OG |
//# - add: Wi232_Write_Progress() |
//# - chg: Codeformattierung |
//# |
//# 05.06.2014 OG |
//# -chg: Codeformattierung |
//# |
//# 14.05.2014 OG |
//# - chg: InitErrorWi232(), InitWi232() - umgestellt auf dezimale Anzeige des |
//# Fehlercodes damit der einfacher im Source zu identifizieren ist |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../main.h" |
#include "../wi232/Wi232.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
//############################################################################ |
static const char STR_WI232INIT[] PROGMEM = "Wi.232 Init"; |
//############################################################################ |
//############################################################################ |
//-------------------------------------------------------------- |
// lError = WriteWi232( Wi232Register, RegisterValue) |
// |
// set Register to Wi232, Register, Value |
// |
// Returns: |
// 0 = ACK, FF = NAK |
//-------------------------------------------------------------- |
uint8_t Wi232_Write( uint8_t Wi232Register, uint8_t RegisterValue ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
unsigned int v; |
if( RegisterValue < 0x80 ) |
{ |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(Wi232Register); |
USART_putc(RegisterValue); |
} |
else // RegisterValue >= 0x80 |
{ |
USART_putc(0xff); |
USART_putc(0x03); |
USART_putc(Wi232Register); |
USART_putc(0xfe); |
USART_putc(RegisterValue - 0x80); |
} |
do |
{ |
v = USART_getc(); // ACK erwartet |
_delay_ms(100); |
tc++; |
} while( (v == 0) && (tc != timeout) ); |
if( v != 0x06 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
//PKT_Message_P( PSTR("Wi.232 NAK"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen (wird nicht mehr angezeigt da uebernommen von hoeheren Funktionen) |
return 0xFF; // return: ERROR |
} |
return 0; // return: OK (v==0x06) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Wi232_Write_Progress( uint8_t Wi232Register, uint8_t RegisterValue, uint8_t errorcode ) |
{ |
uint8_t nError; |
// nError = 0: ok |
// nError > 0: Fehler |
nError = Wi232_Write( Wi232Register, RegisterValue ); |
PKT_Progress_Next(); // Progressbar |
if( nError ) |
{ |
nError = errorcode; |
} |
return nError; |
} |
//-------------------------------------------------------------- |
// Registervalue = Wi232_Read( Wi232Register ) |
// |
// send Readcommand to Wi232, |
// |
// Returns: |
// Registervalue: 0 = timeout 0xFF = Syntaxerror |
//-------------------------------------------------------------- |
uint8_t Wi232_Read( uint8_t Wi232Register ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
uint8_t v; |
v = USART_getc(); // Zeichen loeschen |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(0xfe); |
USART_putc(Wi232Register); |
_delay_ms(50); |
//lcd_printpns_at (0, 2, PSTR("read Wi232"),0); |
do |
{ |
v = USART_getc(); //ACK erwartet |
_delay_ms(100); |
tc++; |
} while( v==0 && tc!=timeout ); |
if( tc == timeout ) |
return 0; // Timeout |
if( v != 0x06 ) |
return 0xFF; // Syntaxerror |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Register*/ |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Value*/ |
// lcd_print_hex(v,0); |
return v; |
} |
//-------------------------------------------------------------- |
// foundbaud = Wi232_CheckBaud( Baudrate ) |
// |
// RUECKGABE: |
// =0: Error (Wi.232 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint16_t Wi232_CheckBaud( uint8_t Baudrate ) // Suche Wi232 mit entsprechender Baudrate |
{ |
uint8_t RegisterWi232; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
SetBaudUart0( Baudrate ); // UART auf angegebene Baudrate setzen |
RegisterWi232 = Wi232_Read( regDiscover ); |
//--------------------------------------------------- |
// Hinweis zu RegisterWi232 nach regDiscover |
// == 0 : "no Wi.232 found" |
// == 0xFF: "Wi.232 Syntaxerror" |
// |
// >0 && < 0xff: Wi.232 Version (Wi.232 gefunden) |
//--------------------------------------------------- |
if( RegisterWi232 > 0 && RegisterWi232 < 0xFF ) |
{ |
return Baudrate; // Wi.232 gefunden |
} |
return 0; // Wi.232 nicht gefunden bei gegebener Baudrate |
} |
//-------------------------------------------------------------- |
// InitWI232( InitBaudrate ) |
// |
// set Wi232Register for Mikrokopter |
// |
// Returns: |
// 0 = ACK, FF = NAK ????????? |
//-------------------------------------------------------------- |
void Wi232_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
uint8_t InitBaudrate = Config.PKT_Baudrate; |
//---------------------------- |
// 1. Kopter ausgeschaltet? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, strGet(STR_SWITCHOFFMK), NULL, strGet(STR_ATTENTION), STR_WI232INIT) ) |
{ |
return; |
} |
//---------------------------- |
// 2. Wi.232 suchen |
//---------------------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
Change_Output( Uart02Wi ); // Verbindung zu Wi232 herstellen |
SwitchToWi232(); // Serielle Kanaele Wi232 mit USB verbinden |
set_WI232CMD(); |
_delay_ms(100); |
//_delay_ms(200); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_115200 ); |
if( !foundbaud ) // gefunden? |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 not found"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
} |
//---------------------------- |
// 3. Wi.232 initialisieren |
//---------------------------- |
if( foundbaud > 0 ) |
{ |
//-------------- |
// Screen |
//-------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_WI232INIT ); |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 18, 0, 0,0); // 18 Progress Steps |
// DEBUG - hier kann ggf. die _gefundene_ Baudrate vom Wi.232 angezeigt werden |
//lcdx_printf_center_P( 6, MNORMAL, 0,-4, PSTR("(found %lu Baud)"), Baud_to_uint32(foundbaud) ); // zeige Baudrate |
// DEBUG - und hier auf welche Baudrate das Wi.232 gesetzt werden soll |
//lcdx_printf_center_P( 7, MNORMAL, 0,-2, PSTR("(set %lu Baud)"), Baud_to_uint32(Config.PKT_Baudrate) ); // zeige Baudrate |
SetBaudUart0( foundbaud ); |
// wenn sich ein EEPROM-Wert ändert wird auch das Ram beschrieben damit die Änderung sofort wirksam wird |
//-------------------------------------------------------------------------------------------------------------------- |
// + Errorcode |
// | |
// | |
if( !nError ) nError = Wi232_Write_Progress( regNVTXCHANNEL,Config.WiTXRXChannel, 1); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regTXCHANNEL,Config.WiTXRXChannel , 2); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVRXCHANNEL,Config.WiTXRXChannel, 3); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regRXCHANNEL,Config.WiTXRXChannel , 4); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVSLPMODE ,Sleep_Awake , 5); // Sleepmode |
if( !nError ) nError = Wi232_Write_Progress( regNVPWRMODE,WbModeP15 , 6); // Transceiver Mode/Powermode |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: TWaitTime16 (=0x10 // 16 ms Delay (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVTXTO,Config.WiTXTO , 7); // UART Timeout |
if( !nError ) nError = Wi232_Write_Progress( regTXTO,Config.WiTXTO , 8); // UART Timeout |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: UartMTU64 (=64 // default=64, valid 1-144) |
if( !nError ) nError = Wi232_Write_Progress( regNVUARTMTU,Config.WiUartMTU , 9); // UART Buffer |
if( !nError ) nError = Wi232_Write_Progress( regUARTMTU,Config.WiUartMTU ,10); // UART Buffer |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: NetMode_Normal (=0x01 // Normalmode (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVNETMODE,Config.WiNetworkMode ,11); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNETMODE,Config.WiNetworkMode ,12); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNVUSECRC ,CRC_Enable ,13); // CRC |
if( !nError ) nError = Wi232_Write_Progress( regNVCSMAMODE,CSMA_En ,14); // CSMA |
if( !nError ) nError = Wi232_Write_Progress( regNVNETGRP,Config.WiNetworkGroup ,15); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNETGRP,Config.WiNetworkGroup ,16); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNVDATARATE,InitBaudrate ,17); // Baudrate |
_delay_ms(200); |
if( !nError ) nError = Wi232_Write_Progress( regDATARATE,InitBaudrate ,18); // Baudrate |
_delay_ms(250); |
//_delay_ms(6000); // DEBUG |
if( nError ) |
{ |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("Wi.232 Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
} |
else |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
WriteWiInitFlag(); // erfolgreiches Init merken |
} |
} |
clr_WI232CMD(); |
SetBaudUart0( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// Wi232_ConfigPC() |
// |
// Connect USB direct to Wi.232 in Progmode |
//-------------------------------------------------------------- |
void Wi232_ConfigPC( void ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titelzeile Invers |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, strGet(STR_WI232CONFIG) ); // Titeltext "Wi.232 Konfig." |
lcdx_printp_center( 2, strGet(STR_USBCONNECTED), MNORMAL, 0,-2); // "mit USB verbunden" |
lcdx_printp_center( 4, strGet(STR_SEEMKWIKI) , MNORMAL, 0,0); // "siehe MK-Wiki:" |
lcdx_printp_center( 5, PSTR("\"RadioTronix\"") , MNORMAL, 0,2); // "RadioTronix" |
lcd_rect_round( 0, 28, 127, 24, 1, R2); // Rahmen um Text |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline "Ende" |
Change_Output(USB2Wi); |
set_WI232CMD(); // Port D6 = CMD |
while(!get_key_press(1 << KEY_ESC)) |
{ |
show_Lipo(); |
} |
clr_WI232CMD(); // Port D6 = CMD |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
// 10.06.2014 OG: das Folgende war im orginalen Port_USB2CFG_Wi() nicht |
// vorhanden - das hatte den Effekt, dass wenn man diese |
// Funktion aufgerufen und wieder beendet hatte das |
// PKT-Updatetool kein PKT-Update mehr via PKT_CtrlHook() |
// durchfuehren konnte. Die USB Umschaltung war wohl nicht sauber. |
// |
uart1_init( UART_BAUD_SELECT(USART_BAUD,F_CPU) ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
SetBaudUart1( Config.PKT_Baudrate ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wi232/Wi232.h |
---|
0,0 → 1,192 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.h |
//# |
//# 10.06.2014 OG |
//# - add: Wi232_Initalize() |
//# - add: Wi232_ConfigPC() |
//# - del: InitWi232() |
//# |
//# 08.06.2014 OG |
//# - chg: InitWi232() - Parameteraenderung |
//# - del: Wi232_New_Baudrate |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef WI232_H_ |
#define WI232_H_ |
// Non-volatile Registers |
// Name Address Description Default |
#define regNVTXCHANNEL 0x00 // Transmit channel setting ## 0 ## |
#define regNVRXCHANNEL 0x01 // Receive channel setting ## 0 ## |
#define regNVPWRMODE 0x02 // Operating mode settings ## +13 dBm widebandmode ## |
#define regNVDATARATE 0x03 // UART data rate ## 2400bps ## |
#define regNVNETMODE 0x04 // Network mode (Normal/Slave) ## Normal ## |
#define regNVTXTO 0x05 // Transmit wait timeout ## ~16ms ## |
#define regNVNETGRP 0x06 // Network group ID ## 0x00 ## |
#define regNVUSECRC 0x08 // Enable/Disable CRC ## Enabled ## |
#define regNVUARTMTU 0x09 // Minimum transmission unit. ## 64 bytes ## |
#define regNVSHOWVER 0x0A // Enable/Disable start-up message ## Enabled ## |
#define regNVCSMAMODE 0x0B // Enable/Disable CSMA ## Enabled ## |
#define regNVSLPMODE 0x0D // Power state of module ## Awake ## |
#define regNVACKONWAKE 0x0E // Send ACK character to host on wake |
// Non-volatile Read Only Registers |
// Name Address Description |
#define regMAC0 0x22 // These registers form the unique 48-bit MAC address. |
#define regMAC1 0x23 // MAC |
#define regMAC2 0x24 // MAC |
#define regOUI0 0x25 // MAC |
#define regOUI1 0x26 // MAC |
#define regOUI2 0x27 // MAC |
#define regDiscover 0x78 // Versionsregister |
// Volatile Read/Write Registers |
// Name Address Description |
#define regTXCHANNEL 0x4B // Transmit channel setting |
#define regRXCHANNEL 0x4C // Receive channel setting |
#define regPWRMODE 0x4D // Operating mode settings |
#define regDATARATE 0x4E // UART data rate |
#define regNETMODE 0x4F // Network mode (Normal or Slave) |
#define regTXTO 0x50 // Transmit wait timeout |
#define regNETGRP 0x51 // Network group ID |
#define regUSECRC 0x53 // Enable/Disable CRC |
#define regUARTMTU 0x54 // Minimum transmission unit. |
#define regSHOWVER 0x55 // Enable/Disable start-up message |
#define regCSMAMODE 0x56 // Enable/disable CSMA |
#define regSLPMODE 0x58 // Power state of module |
#define regACKONWAKE 0x59 // Send ACK character to host on wake |
// Wideband Channels |
// regNVTXCHAN (0x00) regTXCHAN (0x4B) |
// Channel Number Frequency |
#define wChan0 0x00 // 868.300 MHz |
#define wChan1 0x01 // 868.95 MHz ## MK ## |
// Narrowband Channels |
// regNVRXCHAN (0x01) regRXCHAN (0x4C) |
// Channel Number Frequency |
#define nChan0 0x00 // 868.225 MHz |
#define nChan1 0x01 // 868.375 MHz ## MK ## |
#define nChan2 0x02 // 868.850 MHz |
#define nChan3 0x03 // 869.050 MHz |
#define nChan4 0x04 // 869.525 MHz |
#define nChan5 0x05 // 869.850 MHz |
// Power Mode |
// regNVPWRMODE (0x02) regPWRMODE (0x4D) |
// PM1 PM1 PM0 Mode |
#define NbModeN0 0x00 // 0 0 0 Narrowband Mode 0dBm power setting (typical) |
#define WbModeP5 0x01 // 0 0 1 Wideband Mode +5dBm power setting (typical) |
#define WbModeP10 0x02 // 0 1 0 Wideband Mode +10dBm power setting (typical) |
#define WbModeP15 0x03 // 0 1 1 Wideband Mode +15dBm power setting (typical) ## MK ## |
#define WbModeN0 0x04 // 1 0 0 Wideband Mode 0dBm power setting (typical) |
#define NbModeP5 0x05 // 1 0 1 Narrowband Mode +5dBm power setting (typical) |
#define NbModeP10 0x06 // 1 1 0 Narrowband Mode +10dBm power setting (typical) |
#define NbModeP15 0x07 // 1 1 1 Narrowband Mode +15dBm power setting (typical) |
// Wi232 UART Baudrate |
// regNVDATARATE (0x03) regDATARATE (0x4E) |
// Baud Rate BR2 BR1 BR0 |
#define Wi232_2400 Baud_2400 // 0 0 0* (default 2400) |
#define Wi232_9600 Baud_9600 // 0 0 1 |
#define Wi232_19200 Baud_19200 // 0 1 0 |
#define Wi232_38400 Baud_38400 // 0 1 1 |
#define Wi232_57600 Baud_57600 // 1 0 0 ## MK ## |
#define Wi232_115200 Baud_115200 // 1 0 1 |
#define Wi232_10400 0x06 // 1 1 0 |
#define Wi232_31250 0x07 // 1 1 1 |
// NetworkMode |
// regNVNETMODE (0x04) regNETMODE (0x4F) |
#define NetMode_Slave 0x00 // Slavemode |
#define NetMode_Normal 0x01 // Normalmode (default) |
// Transmit Wait Timeout |
// regNVTXTO (0x05) regTXTO (0x50) |
#define TWaitTimeFull 0x00 // full Buffer required |
#define TWaitTime16 0x10 // 16 ms Delay (default) |
// Network Group |
// regNVNETGRP (0x06) regNETGRP (0x51) |
#define NetWorkGroup 66 // default = 0, valid 0-127 ## MK = 66 ## |
// CRC Control |
// regNVUSECRC (0x08) regUSECRC (0x53) |
#define CRC_Disable 0x00 // no CRC check |
#define CRC_Enable 0x01 // CRC check (default) |
// UART minimum transmission unit |
// regNVUARTMTU (0x09) regUARTMTU (0x54) |
#define UartMTU64 64 // default=64, valid 1-144 |
// Verbose mode |
// regNVSHOWVER (0x0A) |
#define ShowVers_Dis 0x00 // do not show Startupmessage ## MK = 66 ## |
#define ShowVers_En 0x01 // show Startupmessage (default) |
// CSMA enable |
// regNVCSMAMODE (0x0B) regCSMAMODE (0x56) |
#define CSMA_Dis 0x00 // disable CSMA Carrier-sense multiple access |
#define CSMA_En 0x01 // enable CSMA (default) |
// Sleep control |
// regNVSLPMODE (0x0D) regSLPMODE (0x58) |
#define Sleep_Awake 0x00 // Sleepmode = Awake (default) |
#define Sleep 0x01 // Sleepmode = Sleep |
#define Sleep_Stby 0x02 // Sleepmode = Standby |
// ACK on Wake |
// regNVACKONWAKE (0x0D) regACKONWAKE (0x59) |
#define ACKwake_Dis 0x00 // disable ACK on Wake |
#define ACKwake_En 0x01 // enable ACK on Wake (default) |
//---------------------------------- |
// EXPORT |
//---------------------------------- |
void Wi232_Initalize( void ); |
void Wi232_ConfigPC( void ); |
#endif // WI232_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wifly/PKT_WiFlyHQ.c |
---|
0,0 → 1,1406 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/** |
* @file WiFly RN-XV Library |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.c |
//# |
//# 03.06.2014 OG |
//# - fix: find_WiFly() - Benutzer kann Suche nach WiFly jetzt abbrechen |
//# (z.b. weil er kein WiFly angesteckt hat) |
//# von dieser Aenderungen betroffen sind auch noch einige Codechanges |
//# an anderen Funktionen. |
//# |
//# 15.04.2014 OG |
//# - chg: wifly_init() - Logik bei der Versionsverifikation von WIFLY_ADHOC |
//# geaendert da v2.38 ansonsten ggf. den Bach runter gegangen waere. |
//# AKTUELL UNGETESTET DA KEIN WIFLY ADHOC VORHANDEN! |
//# - add: Wifly_update() ein paar Kommentare im Code |
//# - add: KOMMENTAR zu define DEBUG_WIFLY ! (lesenswert!) |
//# |
//# 14.04.2014 Cebra |
//# - add: Defines für getestete WiFly Softwareversionen |
//# - add: wifly_init bei der Versionsabfrage erweitert |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: define DELAY_BEFORE_SAVE |
//# - add: define DELAY_BEFORE_REBOOT |
//# |
//# 02.04.2014 OG |
//# WIP: etliche weitere Aenderungen.... |
//# - chg: getVersion(): Rueckgabe/Logik geaendert |
//# - add: define DELAY_AFTER_MODUL_ON |
//# |
//# 01.04.2014 SH |
//# - chg: Nicht benötigte Progmem Strings auskommentiert |
//# |
//# 11.03.2014 SH |
//# - add: neue Funktionen wl_reset() und wl_update() |
//# - chg: wl_init() wurde komplett auf den neuen AP Modus geändert, die Init Routine vom Ad Hoc Modus befindet sich ausgeklammert darunter |
//# - chg: getVersion wurde angepasst + neuer Wert für resp_Version[] PROGMEM = "RN-171\r\n" anstatt WiFly |
//# - chg: setSSID() und setPassphrase() wurden um weiteren Parameter ergänzt |
//# - chg: createAdhocNetwork angepasst da Funktion setSSID() enthalten |
//# - del: ausgeklammerte Funktionen getConnection, getDHCPMode, getSpaceReplace, getopt_Asc und join |
//# |
//# 12.02.2014 OG |
//# - chg: readTimeout() Auskommentiert: "unused variable 'ind'" |
//# - chg: wl_init() Auskommentiert: "unused variable 'dhcpMode'" |
//# |
//# 02.07.2013 cebra |
//# - new: Routinen für WiFly WLAN-Modul |
//# |
//########################################################################### |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "PKT_WiFlyHQ.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../timer/timer.h" |
#include "../lcd/lcd.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//---------------------------------------------------- |
// DEBUG |
// |
// Wenn 'DEBUG_WIFLY' eingeschalte wird dann werden die Ausgaben |
// des WiFly auf dem PKT-Screen angezeigt - fast wie bei einem |
// Terminal Programm! |
// |
// Man kann dadurch genaueres erkennen wie die Kommunikation |
// ablaeuft und mit welchen Strings beispielsweise das WiFly |
// ein Ok quittiert. |
//---------------------------------------------------- |
//#define DEBUG_WIFLY |
//---------------------------------------------------- |
// unterstützte WiFly Softwareversionen |
//---------------------------------------------------- |
#define Adhoc_Version1 232 // getestet Version 2.32 |
#define Adhoc_Version2 238 // getestet Version 2.38 |
#define Apmode_Version1 441 // getestet Version 4.41 |
//---------------------------------------------------- |
// Einstellungen |
//---------------------------------------------------- |
static const char WIFLY_IP_ADDRESS[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_PORT[] PROGMEM = "2000"; |
static const char WIFLY_IP_GATEWAY[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_NETMASK[] PROGMEM = "255.255.0.0"; |
static const char WIFLY_VERSION_ADHOC[] PROGMEM = "2.38.3"; |
//static const char WIFLY_VERSION_ADHOC[] PROGMEM = "v2.32"; |
static const char WIFLY_VERSION_APMODE[] PROGMEM = "4.41"; |
//---------------------------------------------------- |
// Einsstellungen fuer Delay's |
//---------------------------------------------------- |
//#define WIFLY_DEFAULT_TIMEOUT 500 /* 500 milliseconds */ |
#define WIFLY_DEFAULT_TIMEOUT 100 // n milliseconds |
//---------------------------------------------------- |
//---------------------------------------------------- |
uint8_t progress_max = 0; |
uint8_t progress_act = 0; |
char progress_buffer[22]; |
char buffer[20]; |
char wifly_version_string[12]; |
uint16_t wifly_version; |
//---------------------------------------------------- |
// Texte |
//---------------------------------------------------- |
static const char STR_WIFLY_NOT_FOUND[] PROGMEM = "WiFly not found"; |
static const char RECEIVE_AOK[] PROGMEM = "AOK"; |
#define REPLACE_SPACE_CHAR '$' // ersetzt Spaces in einem String-Parameter des WiFly - '$' ist WiFly default (0x24) fuer SSID und Pwd |
//static const char [] PROGMEM = ""; |
//---------------------------------------------------- |
//############################################################################# |
//# |
//############################################################################# |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_begin( void ) |
{ |
lcd_cls(); |
lcd_setpos(0,0); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_wait( void ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) && !get_key_press(1 << KEY_ENTER) ); |
lcd_print_LF(); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_message( uint8_t *text ) |
{ |
lcd_print_LF(); |
lcd_print( text, MINVERS ); |
debug_wait(); |
lcd_print_LF(); |
} |
//############################################################################# |
//# |
//############################################################################# |
/** Read the next character from the WiFly serial interface. |
* Waits up to timeout milliseconds to receive the character. |
* @param chp pointer to store the read character in |
* @param timeout the number of milliseconds to wait for a character |
* @retval true - character read |
* @retval false - timeout reached, character not read |
*/ |
bool readTimeout( char *chp, uint16_t timeout) |
{ |
char ch; |
timer2 = timeout; |
while( timer2>0 ) |
{ |
if( uart1_available() > 0 ) |
{ |
ch = uart1_getc(); |
*chp = ch; |
#ifdef DEBUG_WIFLY |
lcd_print_char( ch, MNORMAL); |
#endif |
return (true); |
} |
} |
return (false); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void flushRx( int timeout ) |
{ |
char ch; |
while( readTimeout( &ch, timeout) ); |
} |
/** |
* Read characters from the WiFly and match them against the |
* progmem string. Ignore any leading characters that don't match. Keep |
* reading, discarding the input, until the string is matched |
* or until no characters are received for the timeout duration. |
* @param str The string to match, in progmem. |
* @param timeout fail if no data received for this period (in milliseconds). |
* @retval true - a match was found |
* @retval false - no match found, timeout reached |
*/ |
bool match_P( const char *str, uint16_t timeout) |
{ |
const char *match = str; |
char ch, ch_P; |
ch_P = pgm_read_byte(match); |
if( ch_P == '\0' ) return true; /* Null string always matches */ |
while( readTimeout( &ch, timeout) ) |
{ |
// DEBUG |
//lcd_print_char( ch, MNORMAL ); |
if( ch == ch_P ) match++; |
else |
{ |
match = str; // Restart match |
if( ch == pgm_read_byte(match) ) match++; |
} |
ch_P = pgm_read_byte( match ); |
if( ch_P == '\0' ) return (true); |
} |
return (false); |
} |
//############################################################################# |
//# progress |
//############################################################################# |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_begin( uint8_t maxsteps ) |
{ |
progress_max = maxsteps; |
progress_act = 0; |
} |
//-------------------------------------------------------------- |
// wait4key: true / false |
//-------------------------------------------------------------- |
void progress_end( uint8_t wait4key ) |
{ |
progress_max = 0; |
progress_act = 0; |
if( wait4key ) |
{ |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
} |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_show( const char *msg ) |
{ |
int8_t yoffs = 3; |
#ifdef DEBUG_WIFLY |
return; |
#endif |
if( progress_max ) |
{ |
progress_act++; |
lcdx_printf_center_P( 4, MNORMAL, 0,yoffs, PSTR("%d/%d"), progress_act, progress_max ); |
if( msg ) |
{ |
strncpyat_P( progress_buffer, msg, 20, ' ', 2); |
lcd_frect( 2, (5*8)+yoffs, 124, 7, 0); // Zeile 5 loeschen |
lcdx_print_center( 5, (uint8_t *)progress_buffer, MNORMAL, 0,yoffs); // Text zentriert; String im RAM |
} |
lcd_rect_round( 0, 28+yoffs, 127, 22, 1, R2); // Rahmen |
_delay_ms(700); // fuer Anzeige (Entschleunigung) |
} |
} |
//-------------------------------------------------------------- |
// gibt zentriert einen Text und loescht vorher die Zeile |
//-------------------------------------------------------------- |
void MsgCenter_P( uint8_t y, const char *text, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_row( y, MNORMAL, yoffs ); |
lcdx_printp_center( y, text, MNORMAL, 0, yoffs ); |
} |
//############################################################################# |
//# sendcmd |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void cmd_showerror( const char *cmdstr) |
{ |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( cmdstr, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t find_Prompt() |
{ |
char c; |
uint8_t state = 0; |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( state == 0 && (c == '>') ) { state = 1; continue; } |
if( state == 1 && (c == ' ') ) return true; |
if( state == 1 ) break; // Fehler: nicht vorgesehen |
} |
return false; |
} |
//-------------------------------------------------------------- |
// nicht verwendet... |
//-------------------------------------------------------------- |
uint8_t find_Prompt2() |
{ |
char c; |
timer2 = 800; // 8 Sekunden max. suchen |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) && timer2 ) |
{ |
if( c == '>' ) return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Das ist erstmal so uebernommen - ob das alles sein muss?? |
// |
// Put the WiFly into command mode |
// |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t cmd_EnterCommandMode( void ) |
{ |
uint8_t retry = 2; |
int8_t result = 0; // false |
while( retry && !result ) |
{ |
//_delay_ms(250); |
uart1_puts_p( PSTR("$$$") ); |
//_delay_ms(250); |
_delay_ms(100); |
uart1_putc('\r'); |
if( get_key_short(1 << KEY_ESC) ) // Abbruch durch Benutzer? |
{ |
return -1; // -1 = Abbruch Benutzer |
} |
result = find_Prompt(); // ggf. result = true |
if( !result ) |
{ |
retry--; |
_delay_ms(250); |
} |
} |
return result; |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// cmdstr : PROGMEM |
// strvalue : RAM |
// match_receive: PROGMEM |
// findprompt : true/false |
//-------------------------------------------------------------- |
uint8_t cmdsend( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
uint8_t result = true; |
const char *p; |
char c; |
progress_show( cmdstr ); |
//-------------------- |
// Senden... |
//-------------------- |
uart1_puts_p( cmdstr ); |
if( strvalue != NULL ) |
{ |
uart1_putc(' '); |
p = strvalue; |
while( *p ) |
{ |
c = *p; |
if( c == ' ' ) c = REPLACE_SPACE_CHAR; |
uart1_putc( c ); |
p++; |
} |
} |
uart1_putc('\r'); |
//-------------------- |
// Antwort... |
//-------------------- |
if( match_receive ) |
{ |
//if( !match_P( p, WIFLY_DEFAULT_TIMEOUT) ) |
if( !match_P( match_receive, 2000) ) |
{ |
cmd_showerror( cmdstr ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
result = false; |
} |
// DEBUG |
//lcdx_printf_at_P( 2, 7, MINVERS, 0,0, PSTR("%d"), result ); |
} |
#ifdef DEBUG_WIFLY |
debug_wait(); |
#endif |
// TODO: Kommandos die keinen Prompt zurueckliefern? (evtl. 'reset'?) |
//uart1_putc('\r'); // fuer cmd_Join ein extra Return... (bei den anderen Kommandos stoert es nicht) |
if( result && findprompt) |
{ |
result = find_Prompt(); |
} |
return( result ); |
} |
//-------------------------------------------------------------- |
// cmdsend_P() |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t cmdsend_P( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
strncpy_P( buffer, strvalue, 20); |
return cmdsend( cmdstr, buffer, match_receive, findprompt ); |
} |
//-------------------------------------------------------------- |
// Version wird anhand des Prompt's ermittelt |
// Implementiert als State-Machine |
//-------------------------------------------------------------- |
uint8_t cmd_GetVersion( void ) |
{ |
char c; |
uint8_t state = 0; |
char *dst = wifly_version_string; |
uint8_t n = 0; |
wifly_version = 0; |
uart1_putc('\r'); // erzwinge Prompt |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( (state == 0) && (c == '<') ) |
{ |
state = 1; |
continue; |
} |
if( (state == 1) && (c != '>') ) |
{ |
// TODO: Ende vom Prompt im Fehlerfall catchen... |
if( n+1 >= sizeof(wifly_version_string) ) |
break; |
*dst = c; |
dst++; |
n++; |
continue; |
} |
if( (state == 1) && (c == '>') ) // Versionsstring-Ende erreicht |
{ |
*dst = 0; |
state = 2; |
break; |
} |
} |
if( state == 2 ) |
{ |
//uint16_t wifly_version; |
wifly_version = (wifly_version_string[0]-'0') * 100; |
wifly_version += (wifly_version_string[2]-'0') * 10; |
wifly_version += (wifly_version_string[3]-'0') * 1; |
} |
else |
{ |
cmd_showerror( PSTR("get version") ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
} |
return (state == 2); |
} |
//-------------------------------------------------------------- |
// cmd_Set() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_Set( const char *cmdstr ) |
{ |
return cmdsend( cmdstr, 0, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : RAM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr( const char *cmdstr, const char *str ) |
{ |
return cmdsend( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr_P( const char *cmdstr, const char *str ) |
{ |
return cmdsend_P( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetNum() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// value : Dezimal |
//-------------------------------------------------------------- |
uint8_t cmd_SetNum( const char *cmdstr, const uint16_t value ) |
{ |
utoa( value, buffer, 10 ); |
return cmd_SetStr( cmdstr, buffer); |
} |
//-------------------------------------------------------------- |
// cmd_Reset() |
//-------------------------------------------------------------- |
uint8_t cmd_Reset( void ) |
{ |
uint8_t result = true; |
//_delay_ms( 500 ); |
result = cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), false ); |
_delay_ms( 1000 ); |
return result; |
// return cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Save() |
//-------------------------------------------------------------- |
uint8_t cmd_Save( void ) |
{ |
//_delay_ms( DELAY_BEFORE_SAVE ); |
_delay_ms( 200 ); |
return cmdsend( PSTR("save"), 0, PSTR("Storing"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Join() |
//-------------------------------------------------------------- |
uint8_t cmd_Join( const char *ssid ) |
{ |
uint8_t result = true; |
result = cmdsend( PSTR("join"), ssid, PSTR("Associated"), false ); |
if( result ) uart1_putc('\r');; |
if( result ) result = find_Prompt(); |
return result; |
} |
//-------------------------------------------------------------- |
// cmd_Reboot() |
//-------------------------------------------------------------- |
uint8_t cmd_Reboot( void ) |
{ |
//_delay_ms( DELAY_BEFORE_REBOOT ); |
_delay_ms( 300 ); |
cmdsend( PSTR("reboot"), 0, 0, false ); |
_delay_ms( 2000 ); |
return true; |
} |
//############################################################################# |
//# WiFly suchen |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void find_WiFly_searchmsg( const char *text, uint8_t baud ) |
{ |
lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,0, PSTR("%S %lu Baud"), text, Baud_to_uint32(baud) ); // Text zentriert |
} |
//-------------------------------------------------------------- |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t find_WiFly_baud( uint8_t baud ) |
{ |
int8_t result = 1; |
find_WiFly_searchmsg( strGet(STR_SEARCH), baud ); |
// Anmerkung 06.04.2014 OG: |
// SetBaudUart1() (in uart/uart1.c) setzt nur die Baudrate des Uart. |
// Config.PKT_Baudrate wird durch SetBaudUart1() NICHT geaendert! |
SetBaudUart1( baud ); |
result = cmd_EnterCommandMode(); |
if( result > 0 ) |
{ |
return baud; // Baud gefunden, ok! |
} |
return result; // nicht gefunden oder Abbruch durch Benutzer |
} |
//-------------------------------------------------------------- |
// ok = find_WiFly() |
// |
// Sucht das WiFly mit verschiedenen Baudraten. |
// |
// Wenn das WiFly mit von Config.PKT_Baudrate (= 57600) abweichender |
// Baudrate gefunden wird (z.B. 9600 Baud) dann wird das WiFly |
// automatisch auf 57600 Baud umkonfiguriert damit nachfolgende |
// Funktionen das nicht mehr beachten muessen. |
// |
// RUECKGABE: |
// ok = true : WiFly gefunden und ist auf 57600 Baud |
// ok = false: kein WiFly gefunden |
// |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
// |
// Anmerkung: |
// #define Baud_9600 1 |
// #define Baud_19200 2 |
// #define Baud_38400 3 |
// #define Baud_57600 4 |
// #define Baud_115200 5 |
// #define Baud_4800 6 |
// #define Baud_2400 7 |
//-------------------------------------------------------------- |
int8_t find_WiFly( void ) // search Wifly with all Baudrates |
{ |
int8_t result = 1; // 1 == true |
int8_t baud = 0; |
//inCommandMode = false; |
//exitCommand = 0; |
set_Modul_On( Wlan ); // WiFly einschalten |
timer2 = 60; // Delay damit das WiFly booten kann |
while( timer2 ); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline: "Ende" |
//------------------------ |
// suche WiFly mit versch. Baudraten |
//------------------------ |
if( baud==0 ) baud = find_WiFly_baud( Baud_57600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_9600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_115200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_38400 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_19200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_4800 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_2400 ); |
lcdx_cls_row( 7, MNORMAL, 0); // Keyline loeschen |
//------------------------ |
// Abbruch durch Benutzer? |
//------------------------ |
if( baud < 0 ) |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
return -1; // -1 = Abbruch Benutzer |
} |
//------------------------ |
// kein WiFly gefunden... |
//------------------------ |
if( baud==0 ) // WiFly nicht gefunden :-( |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( STR_WIFLY_NOT_FOUND, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
return false; // 0 = nicht gefunden |
} |
//------------------------ |
// WiFly gefunden! |
//------------------------ |
find_WiFly_searchmsg( strGet(STR_FOUND), baud ); // Anzeige: "gefunden..." |
//------------------------ |
// WiFly Baud ok? |
// -> ggf. WiFly Baud umkonfigurieren |
//------------------------ |
if( Config.PKT_Baudrate != baud ) |
{ |
find_WiFly_searchmsg( strGet(STR_SET), Config.PKT_Baudrate ); // Anzeige: "setze..." |
if( result ) result = cmd_Set( PSTR("set u b 57600") ); // -> etwas unsauber hier einfach 57600 zu setzen und nicht Config.PKT_Baudrate |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); // true = uart-baud setzen |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
if( result ) result = cmd_EnterCommandMode(); |
} |
return result; |
} |
//############################################################################# |
//# |
//############################################################################# |
//-------------------------------------------------------------- |
// zeigt die WiFly-Version an |
//-------------------------------------------------------------- |
uint8_t wifly_version_show( void ) |
{ |
int8_t result = 1; // 1 = true |
ShowTitle_P( PSTR("WiFly Version"), true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); // Ergebnis in 'wifly_version_string' |
set_Modul_On( USB ); |
if( result ) |
{ |
// TODO: |
// Anzeige ob WiFly-Version kompatibel zum gewaehlten |
// Mode (AP-Mode, AdHoc) ist |
//lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcd_rect_round( 0, 8*4+3, 127, 8*2, 1, R2); // Anzeige-Rahmen |
lcdx_printf_center_P( 5, MNORMAL, 0,0, PSTR("Version: v%s"), wifly_version_string ); |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
else |
{ |
PKT_Message_P( PSTR("Version FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t _ask( uint8_t keyline ) |
{ |
uint8_t ask = 0; |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
lcd_printp_at( 12, 7, strGet(keyline), MNORMAL); // Keyline |
if( keyline == ENDE ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !ask ) // warte auf Ja/Nein |
{ |
if( get_key_press(1 << KEY_ENTER) ) ask = 1; // "Ja" |
if( get_key_press(1 << KEY_ESC) ) ask = 2; // "Nein" |
}; |
return( ask==1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_init_perform( uint8_t wiflymode ) //AP Mode |
{ |
uint8_t result = true; |
#ifndef DEBUG_WIFLY |
MsgCenter_P( 2, PSTR("INIT"), MNORMAL, 0 ); |
#endif |
//--------------- |
// WIFLY_ADHOC |
//--------------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
progress_begin( 9 ); |
//if( result ) result = cmd_Set( PSTR("set u m 1") ); // ist das notwendig? - (turn off echo) |
//if( result ) result = cmd_Set( PSTR("set sys printlvl 0") ); // ist das notwendig? |
//if( result ) result = cmd_Set( PSTR("set comm remote 0") ); // ist das notwendig? |
if( result ) result = cmd_Set( PSTR("set wlan join 4") ); // 4 = AdHoc |
if( result ) result = cmd_Set( PSTR("set ip dhcp 0") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_SetStr( PSTR("set wlan ssid"), Config.Wlan_SSID ); |
if( result ) result = cmd_SetNum( PSTR("set wlan chan"), Config.Wlan_Channel ); |
} |
//--------------- |
// WIFLY_APMODE |
//--------------- |
if( wiflymode == WIFLY_APMODE ) |
{ |
progress_begin( 13 ); |
if( result ) result = cmd_Set( PSTR("set wlan join 7") ); |
if( result ) result = cmd_Set( PSTR("set ip dhcp 4") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_Set( PSTR("set wlan rate 3") ); |
if( result ) result = cmd_SetStr( PSTR("set apmode ssid") , Config.Wlan_SSID ); |
if( result ) result = cmd_SetStr( PSTR("set apmode pass") , Config.Wlan_Password ); |
if( result ) result = cmd_SetNum( PSTR("set wlan channel"), Config.Wlan_Channel ); |
if( result ) result = cmd_Set( PSTR("set ip protocol 0x3") ); |
if( result ) result = cmd_Set( PSTR("set apmode link_monitor 0") ); |
} |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
return( result ); |
} |
//-------------------------------------------------------------- |
// ok = wifly_init( wiflymode ) |
// |
// initialisiert das WiFly fuer AP-Mode oder AdHoc |
// |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_init( uint8_t wiflymode ) //AP Mode |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
uint8_t versionerror = false; |
char *title = (char *)PSTR("Init WiFly"); |
char *strMode; |
if( wiflymode == WIFLY_ADHOC ) strMode = (char *)PSTR("AdHoc"); |
else strMode = (char *)PSTR("AP-Mode"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Version pruefen |
//----------------------------- |
if( result ) |
{ |
//-------- |
// ADHOC |
//-------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
if( (wifly_version != Adhoc_Version1) // 2.32 |
&& (wifly_version != Adhoc_Version2) // 2.38 |
) |
{ |
versionerror = true; |
} |
} |
//-------- |
// APMODE |
//-------- |
if( wiflymode == WIFLY_APMODE && wifly_version != Apmode_Version1 ) versionerror = true; |
if( versionerror ) |
{ |
set_Modul_On( USB ); |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -1, PSTR("ERROR") ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("Version no %S!"), strMode ); // "Version no AP-Mode" / "AdHoc" |
ask = _ask( ENDE ); |
return false; |
} |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); // WiFly Versionsanzeige |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: OK") , strMode ); // "AP-Mode" oder "AdHoc" |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?") , title ); // "Init WiFly?" |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Init durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
result = wifly_init_perform( wiflymode ); |
} |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Init FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Init OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return( result ); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_update( uint8_t wiflymode ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Update WiFly"); |
const char *pStr; |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
if( wiflymode == WIFLY_ADHOC ) pStr = WIFLY_VERSION_ADHOC; |
else pStr = WIFLY_VERSION_APMODE; |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("%S: v%s"), strGet(STR_VON), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: v%S"), strGet(STR_NACH), pStr ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Update durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
progress_begin( 16 ); |
} |
//---------------------------- |
// 1. Reset |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("Reset"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
if( result ) result = find_WiFly(); |
//---------------------------- |
// 2. mit Heimnetz verbinden |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("verbinde Heimnetz"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ip dhcp 1") ); |
if( result ) result = cmd_SetStr( PSTR("set wlan phrase"), Config.Wlan_HomePassword ); |
if( result ) result = cmd_Set( PSTR("set wlan channel 0") ); |
if( result ) result = cmd_Join( Config.Wlan_HomeSSID ); |
//---------------------------- |
// 3. FTP Download & Update |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("FTP Update"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ftp address 198.175.253.161") ); |
if( result ) result = cmd_Set( PSTR("set ftp dir public") ); |
if( result ) result = cmd_Set( PSTR("set ftp user roving") ); |
if( result ) result = cmd_Set( PSTR("set ftp pass Pass123") ); |
if( result ) result = cmd_Set( PSTR("set ftp timeout 800") ); |
//if( result ) result = cmd_Set( PSTR("del config") ); |
if( result ) |
{ |
//----- |
// das kann einiges gekuerzt werden wenn das denn zuverlaessig laufen wuerde... |
// |
// hier wird auf v4.40 geprueft da laut Refrenz-Manual erst seit dieser Version |
// "cupdate" unterstuetzt wird. Bei einem Downgraden von v4.4 auf eine aeltere |
// Version ist ein clean des Dateisystems ('c') erforderlich da sonst das WiFly |
// nur via Hardware-Taster an GPIO9 wiedre zum Leben erweckt werden kann. |
// |
// Anmerkung: evtl. wuer auch ein "del config" reichen - das ist aber nicht |
// hinreichend getestet |
//----- |
if( wifly_version >= 440 ) |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp cupdate wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp cupdate wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
else |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp update wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp update wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
} |
if( result ) result = cmd_Reset(); |
if( result ) { timer2 = 50; while(timer2); }; |
/* |
//----- |
// Baustelle / Experimente bzgl. FTP-Update |
//----- |
//debug_message("FTP END"); |
//debug_message("RESET"); |
//if( result ) result = cmd_Reboot(); |
if( result ) { timer2 = 100; while(timer2); }; |
if( result ) { timer2 = 200; while(timer2); }; |
debug_message("FINDPR"); |
uart1_putc('\r'); |
if( result ) result = find_Prompt(); |
if( result ) debug_message("FINDPR END: 1"); |
else debug_message("FINDPR END: 0"); |
*/ |
//progress_end( true ); // Progress abschalten & warten |
progress_end( false ); // Progress abschalten |
//---------------------------- |
// 4. Init |
//---------------------------- |
/* |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) set_Modul_On( USB ); |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) ShowTitle_P( title, true ); |
if( result ) result = result = find_WiFly(); |
if( result ) result = wifly_init_perform( wiflymode ); |
*/ |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Update FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Update OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_reset( void ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Reset WiFly"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
ShowTitle_P( title, true ); |
if( result ) progress_begin( 2 ); |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
set_Modul_On( USB ); |
if( result ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("Reset OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
else |
{ |
PKT_Message_P( PSTR("Reset FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//############################################################################# |
//# TEST / DEBUG |
//############################################################################# |
//-------------------------------------------------------------- |
// nur zum testen! |
//-------------------------------------------------------------- |
uint8_t wifly_test( void ) |
{ |
uint8_t result = true; |
return result; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wifly/PKT_WiFlyHQ.h |
---|
0,0 → 1,89 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/* Release history |
* |
* Version Date Description |
* 0.1 25-Mar-2012 First release. |
* 0.2 09-Apr-2012 Added features to support http servers. |
* - added an httpserver.ino example. |
* - added sendChunk() and sendChunkln() to send chunked HTTP bodies. |
* - added terminal() method for simple terminal access via debug stream |
* - replaced getFreeMemory() with simpler version that works with 0 bytes |
* - turned peek buffer into a circular buffer to fix bug with detecting |
* *CLOS* and *OPEN* after a partial match. |
* - Added new TCP connection detection via *OPEN* match from available(). |
* isConnected() can now be polled until a client connects. |
* - made the match() function public, handy for matching text in a stream. |
* - Added a getProtocol() function to get current set of protocols. |
* 0.3 21-Apr-2012 Added createAdhocNetwork() to create an Ad Hoc WiFi network. |
* Optimised the setopt() and getopt() function so they handle |
* integer conversions and refactored all of the set and get functions. |
* Added a multMatch_P() function to match serial data against multiple |
* progmem strings. |
* Added failure detection to the join() function to quickly detect |
* a failure rather than relying on a timeout. |
* Added setJoin() and getJoin() function for access to the wlan join parameter. |
* Refactored getres() to use the new multiMatch_P() function. |
* |
*/ |
/** |
* @mainpage WiFlyHQ WiFly RN-XV Arduino library |
* |
* This library provides functions for setting up and managing the WiFly module, |
* sending UDP packets, opening TCP connections and sending and receiving data |
* over the TCP connection. |
* |
* @author Harlequin-Tech |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.h |
//# |
//# 13.04.2014 OG |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 02.04.2014 OG |
//# - add: Versionshistorie ergaenzt |
//########################################################################### |
#ifndef _WIFLYHQ_H_ |
#define _WIFLYHQ_H_ |
// Todo: Strings verfuegbar machen für connect.c |
//extern const char WIFLY_IP_ADDRESS[]; |
//extern const char WIFLY_IP_PORT[]; |
uint8_t wifly_update( uint8_t wiflymode ); |
uint8_t wifly_reset(void); |
uint8_t wifly_version_show( void ); |
uint8_t wifly_init( uint8_t wiflymode ); |
uint8_t wifly_test( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wifly/wifly_setup.c |
---|
0,0 → 1,424 |
/* |
* wifly_setup.c |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY wifly_setup.c |
//# |
//# 08.06.2014 OG |
//# - chg: Setup_WiFly() - Tipptext fuer Module-Installed ergaenzt |
//# |
//# 04.06.2014 OG |
//# - chg: Menuetext angepasst; Menue-Separator hinzugefuegt |
//# |
//# 31.05.2014 OG |
//# - chg: Setup's auf geaendertes Edit_String() umgestellt (Param 'Text' entfernt) |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 30.03.2014 OG |
//# - chg: Text von WL_INSTALLED geaendert von "Modul eingebaut?" auf "Modul vorhanden?" |
//# -> das Modul wird aufgesteckt und nicht eingebaut... |
//# - chg: ein paar englische Texte geaendert |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 11.03.2014 SH |
//# - add: neue Menüpunkte Reset, Version und Update |
//# - chg: Menüpunkt Passwort wurde wieder aktiviert |
//# - chg: beim Menüpunkt Kanal fängt die Auswahl jetzt bei 0 an (Modul sucht sich automatisch den besten Kanal) |
//# |
//# 12.02.2014 OG |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'z'" |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'i'" |
//# |
//# 02.07.2013 cebra |
//# - new: wifly_setup(): Setup für WiFly WLAN-Modul |
//########################################################################### |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
//#include <avr/pgmspace.h> |
//#include <string.h> |
//#include <util/delay.h> |
//#include <string.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../setup/setup.h" |
#include "../wifly/PKT_WiFlyHQ.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//char buffer[20]; // fuer Versionsabfrage |
//----------------------------- |
// Setup_WiFly() (WLAN) |
//----------------------------- |
#define WL_INSTALLED 1 |
#define WL_SSID 2 |
#define WL_PASSWORD 3 |
#define WL_CHANNEL 4 |
#define WL_INIT 5 |
#define WL_RESET 6 |
#define WL_VERSION 7 |
#define WL_UPDATE 8 |
#define WL_PCCONFIG 9 |
#define WL_WPA 10 |
#define WL_HOMESSID 11 |
#define WL_HOMEPASSWORD 12 |
#define WL_TEST 99 |
static const char WL_INSTALLED_de[] PROGMEM = "WiFly Modus"; |
static const char WL_INSTALLED_en[] PROGMEM = "WiFly mode"; |
static const char WL_SSID_de[] PROGMEM = "SSID"; |
#define WL_SSID_en WL_SSID_de |
static const char WL_PASSWORD_de[] PROGMEM = "Passwort"; |
static const char WL_PASSWORD_en[] PROGMEM = "Password"; |
static const char WL_HOMESSID_de[] PROGMEM = "Home SSID"; |
#define WL_HOMESSID_en WL_HOMESSID_de |
static const char WL_HOMEPASSWORD_de[] PROGMEM = "Home Passwort"; |
static const char WL_HOMEPASSWORD_en[] PROGMEM = "Home Password"; |
static const char WL_CHANNEL_de[] PROGMEM = "Kanal"; |
static const char WL_CHANNEL_en[] PROGMEM = "Channel"; |
static const char WL_VERSION_de[] PROGMEM = "zeige Version"; |
static const char WL_VERSION_en[] PROGMEM = "show version"; |
static const char WL_UPDATE_de[] PROGMEM = "Update"; |
#define WL_UPDATE_en WL_UPDATE_de |
static const char WL_PCCONFIG_de[] PROGMEM = "WiFly einschalten"; |
static const char WL_PCCONFIG_en[] PROGMEM = "WiFly power on"; |
static const char WL_WPA_de[] PROGMEM = "Sicherheit"; |
static const char WL_WPA_en[] PROGMEM = "Security"; |
//############################################################################ |
//-------------------------------------------------------------- |
// Setup_WiFly_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob WiFly ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WiFly_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
if( Config.UseWL == WIFLY_APMODE ) MenuCtrl_SetTitle_P( PSTR("WiFly AP-Mode")); |
else if( Config.UseWL == WIFLY_ADHOC ) MenuCtrl_SetTitle_P( PSTR("WiFly AdHoc")); |
else MenuCtrl_SetTitle_P( PSTR("WiFly")); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( WL_INSTALLED , MENU_ITEM, NOFUNC , WL_INSTALLED_de , WL_INSTALLED_en ); |
if( Config.UseWL ) |
{ |
//MenuCtrl_Push_P( WL_TEST , MENU_ITEM, NOFUNC , PSTR("TEST") ); |
MenuCtrl_PushML2_P( WL_VERSION , MENU_ITEM, NOFUNC , WL_VERSION_de , WL_VERSION_en ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_SSID , MENU_ITEM, NOFUNC , WL_SSID_de , WL_SSID_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_PushML2_P( WL_PASSWORD, MENU_ITEM, NOFUNC , WL_PASSWORD_de , WL_PASSWORD_en ); |
MenuCtrl_PushML2_P( WL_CHANNEL , MENU_ITEM, NOFUNC , WL_CHANNEL_de , WL_CHANNEL_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AP-Mode") ); |
else |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AdHoc") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_HOMESSID , MENU_ITEM, NOFUNC , WL_HOMESSID_de , WL_HOMESSID_en ); |
MenuCtrl_PushML2_P( WL_HOMEPASSWORD, MENU_ITEM, NOFUNC , WL_HOMEPASSWORD_de , WL_HOMEPASSWORD_en ); |
MenuCtrl_Push_P( WL_UPDATE , MENU_ITEM, NOFUNC , PSTR("UPDATE: WiFly") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_Push_P( WL_RESET , MENU_ITEM, NOFUNC , PSTR("RESET: WiFly") ); |
MenuCtrl_PushML2_P( WL_PCCONFIG , MENU_ITEM, &Port_WiFly2Wi , WL_PCCONFIG_de , WL_PCCONFIG_en ); |
//MenuCtrl_PushML2_P( WL_WPA , MENU_ITEM, NOFUNC , WL_WPA_de , WL_WPA_en ); |
if( Config.UseWL == WIFLY_ADHOC ) |
{ |
// aktuell kann es zu Problemen kommen wenn das WiFly |
// auf v2.38 gedowngraded wird (WiFly nicht mehr ansprechbar) |
// -> deshalb: deaktiviert |
MenuCtrl_ItemActive( WL_UPDATE, false ); |
} |
} |
} |
//-------------------------------------------------------------- |
// Setup_WiFly() |
//-------------------------------------------------------------- |
void Setup_WiFly( void ) |
{ |
uint8_t itemid; |
uint8_t UseWL; |
char string[20]; |
Setup_WiFly_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// TEST |
//-------------------- |
/* |
if( itemid == WL_TEST ) |
{ |
lcd_cls(); |
ShowTitle_P( PSTR("TEST"), true ); |
wifly_test(); |
} |
*/ |
//-------------------- |
// Wlan_INSTALLED |
//-------------------- |
if( itemid == WL_INSTALLED ) |
{ |
UseWL = Config.UseWL; |
Config.UseWL = Edit_generic( Config.UseWL, 0, 2, WlanMode, 1, strGet(STR_EXTSV2MODULE),NULL); |
if( UseWL != Config.UseWL ) // hat Benutzer Einstellung geaendert? |
{ |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wlan vorhanden / nicht vorhanden |
Setup_WiFly_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// WL_VERSION |
//-------------------- |
if( itemid == WL_VERSION ) |
{ |
wifly_version_show(); |
} |
//-------------------- |
// WL_SSID |
//-------------------- |
if( itemid == WL_SSID ) |
{ |
strncpyfill( string, Config.Wlan_SSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_SSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_PASSWORD |
//-------------------- |
if( itemid == WL_PASSWORD) |
{ |
strncpyfill( string, Config.Wlan_Password, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_Password, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMESSID |
//-------------------- |
if( itemid == WL_HOMESSID ) |
{ |
strncpyfill( string, Config.Wlan_HomeSSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomeSSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMEPASSWORD |
//-------------------- |
if( itemid == WL_HOMEPASSWORD) |
{ |
strncpyfill( string, Config.Wlan_HomePassword, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomePassword, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_CHANNEL |
//-------------------- |
if( itemid == WL_CHANNEL ) |
{ |
Config.Wlan_Channel = Edit_generic( Config.Wlan_Channel, 0,13,Show_uint3,1 ,NULL,NULL); |
} |
//-------------------- |
// WL_INIT |
//-------------------- |
if( itemid == WL_INIT ) |
{ |
wifly_init( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_RESET |
//-------------------- |
if( itemid == WL_RESET ) |
{ |
wifly_reset(); |
} |
//-------------------- |
// Wlan_UPDATE |
//-------------------- |
if( itemid == WL_UPDATE ) |
{ |
wifly_update( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_WPA |
//-------------------- |
//if( itemid == WL_WPA ) |
//{ |
// Config.Wlan_WPA = Edit_generic(Config.Wlan_WPA,0,1,WL3,WlanSecurity,1); |
//} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_WiFly() |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wifly/wifly_setup.h |
---|
0,0 → 1,18 |
/* |
* wifly_setup.h |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
#ifndef WIFLY_SETUP_H_ |
#define WIFLY_SETUP_H_ |
#define WIFLY_APMODE 1 |
#define WIFLY_ADHOC 2 |
void Setup_WiFly( void ); |
#endif /* WIFLY_SETUP_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/wifly |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
+ |
+_doc |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Koptertool3.9 |
+ |
+nmealib |
+ |
+Resourcen |
+ |
+TinyGPS |
+ |
+LICENSE.TXT |
+ |
+Source_PKT384e_TEST!.zip |
+ |
+Source_PKT384a.zip |
+ |
+mkparameters.c |
+ |
+DIFF_PKT384a.txt |
+ |
+DIFF_PKT384e.txt |
+ |
+DIFF_PKT384d.txt |
+ |
+.refactorings |
+ |
+.settings |
+ |
+.directory |
+ |
+.cproject |
+ |
+.project |
Added: svn:mergeinfo |
Merged /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2:r2203-2209 |
/Transportables_Koptertool/PKT/devSettings/EclipseProject/.cproject |
---|
0,0 → 1,600 |
<?xml version="1.0" encoding="UTF-8" standalone="no"?> |
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> |
<storageModule moduleId="org.eclipse.cdt.core.settings"> |
<cconfiguration id="de.innot.avreclipse.configuration.app.debug.2000056965"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="de.innot.avreclipse.configuration.app.debug.2000056965" moduleId="org.eclipse.cdt.core.settings" name="Debug"> |
<macros> |
<stringMacro name="MKVERSION" type="VALUE_TEXT" value="088n"/> |
</macros> |
<externalSettings/> |
<extensions> |
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/> |
<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
</extensions> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<configuration artifactName="${ProjName}" buildArtefactType="de.innot.avreclipse.buildArtefactType.app" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=de.innot.avreclipse.buildArtefactType.app,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" description="" id="de.innot.avreclipse.configuration.app.debug.2000056965" name="Debug" parent="de.innot.avreclipse.configuration.app.debug"> |
<folderInfo id="de.innot.avreclipse.configuration.app.debug.2000056965." name="/" resourcePath=""> |
<toolChain id="de.innot.avreclipse.toolchain.winavr.app.debug.1236844690" name="AVR-GCC Toolchain" superClass="de.innot.avreclipse.toolchain.winavr.app.debug"> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objcopy.flash.app.debug.1198076823" name="Generate HEX file for Flash memory" superClass="de.innot.avreclipse.toolchain.options.toolchain.objcopy.flash.app.debug"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objcopy.eeprom.app.debug.2002583651" name="Generate HEX file for EEPROM" superClass="de.innot.avreclipse.toolchain.options.toolchain.objcopy.eeprom.app.debug"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objdump.app.debug.1365066380" name="Generate Extended Listing (Source + generated Assembler)" superClass="de.innot.avreclipse.toolchain.options.toolchain.objdump.app.debug"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.size.app.debug.1548907584" name="Print Size" superClass="de.innot.avreclipse.toolchain.options.toolchain.size.app.debug"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.avrdude.app.debug.668179531" name="AVRDude" superClass="de.innot.avreclipse.toolchain.options.toolchain.avrdude.app.debug"/> |
<targetPlatform id="de.innot.avreclipse.targetplatform.winavr.app.debug.456217481" name="AVR Cross-Target" superClass="de.innot.avreclipse.targetplatform.winavr.app.debug"/> |
<builder buildPath="${workspace_loc:/GPL_Mikrokoptertool/Debug}" id="de.innot.avreclipse.target.builder.winavr.app.debug.14002817" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="AVR GNU Make Builder" superClass="de.innot.avreclipse.target.builder.winavr.app.debug"/> |
<tool id="de.innot.avreclipse.tool.assembler.winavr.app.debug.1155822459" name="AVR Assembler" superClass="de.innot.avreclipse.tool.assembler.winavr.app.debug"> |
<option id="de.innot.avreclipse.assembler.option.debug.level.1714829156" name="Generate Debugging Info" superClass="de.innot.avreclipse.assembler.option.debug.level"/> |
<inputType id="de.innot.avreclipse.tool.assembler.input.2027459826" superClass="de.innot.avreclipse.tool.assembler.input"/> |
</tool> |
<tool id="de.innot.avreclipse.tool.compiler.winavr.app.debug.1161577200" name="AVR Compiler" superClass="de.innot.avreclipse.tool.compiler.winavr.app.debug"> |
<option id="de.innot.avreclipse.compiler.option.debug.level.932022619" name="Generate Debugging Info" superClass="de.innot.avreclipse.compiler.option.debug.level"/> |
<option id="de.innot.avreclipse.compiler.option.optimize.1406693815" name="Optimization Level" superClass="de.innot.avreclipse.compiler.option.optimize" value="de.innot.avreclipse.compiler.optimize.size" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.omitfcpu.1875189497" name="Omit F_CPU" superClass="de.innot.avreclipse.compiler.option.omitfcpu" value="false" valueType="boolean"/> |
<option id="de.innot.avreclipse.compiler.option.optimize.other.2011741999" name="Other Optimization Flags" superClass="de.innot.avreclipse.compiler.option.optimize.other" value="-mcall-prologues" valueType="string"/> |
<option id="de.innot.avreclipse.compiler.option.std.408793995" name="Language Standard" superClass="de.innot.avreclipse.compiler.option.std" value="de.innot.avreclipse.compiler.option.std.c99" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.otherflags.433698861" name="Other flags" superClass="de.innot.avreclipse.compiler.option.otherflags" value="-Wstrict-prototypes -Wformat" valueType="string"/> |
<inputType id="de.innot.avreclipse.compiler.winavr.input.667334442" name="C Source Files" superClass="de.innot.avreclipse.compiler.winavr.input"/> |
</tool> |
<tool id="de.innot.avreclipse.tool.cppcompiler.app.debug.1906599067" name="AVR C++ Compiler" superClass="de.innot.avreclipse.tool.cppcompiler.app.debug"> |
<option id="de.innot.avreclipse.cppcompiler.option.debug.level.1755945135" name="Generate Debugging Info" superClass="de.innot.avreclipse.cppcompiler.option.debug.level"/> |
<option id="de.innot.avreclipse.cppcompiler.option.optimize.28999345" name="Optimization Level" superClass="de.innot.avreclipse.cppcompiler.option.optimize"/> |
</tool> |
<tool id="de.innot.avreclipse.tool.linker.winavr.app.debug.2019441965" name="AVR C Linker" superClass="de.innot.avreclipse.tool.linker.winavr.app.debug"> |
<option id="de.innot.avreclipse.linker.option.otherlinkargs.1199235149" name="Other Arguments" superClass="de.innot.avreclipse.linker.option.otherlinkargs" value="-lm" valueType="string"/> |
<inputType id="de.innot.avreclipse.tool.linker.input.260453186" name="OBJ Files" superClass="de.innot.avreclipse.tool.linker.input"> |
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/> |
<additionalInput kind="additionalinput" paths="$(LIBS)"/> |
</inputType> |
</tool> |
<tool id="de.innot.avreclipse.tool.cpplinker.app.debug.2042927031" name="AVR C++ Linker" superClass="de.innot.avreclipse.tool.cpplinker.app.debug"/> |
<tool id="de.innot.avreclipse.tool.archiver.winavr.base.762685064" name="AVR Archiver" superClass="de.innot.avreclipse.tool.archiver.winavr.base"/> |
<tool id="de.innot.avreclipse.tool.objdump.winavr.app.debug.1107051151" name="AVR Create Extended Listing" superClass="de.innot.avreclipse.tool.objdump.winavr.app.debug"/> |
<tool id="de.innot.avreclipse.tool.objcopy.flash.winavr.app.debug.1112080996" name="AVR Create Flash image" superClass="de.innot.avreclipse.tool.objcopy.flash.winavr.app.debug"/> |
<tool id="de.innot.avreclipse.tool.objcopy.eeprom.winavr.app.debug.423591241" name="AVR Create EEPROM image" superClass="de.innot.avreclipse.tool.objcopy.eeprom.winavr.app.debug"/> |
<tool id="de.innot.avreclipse.tool.size.winavr.app.debug.563536542" name="Print Size" superClass="de.innot.avreclipse.tool.size.winavr.app.debug"/> |
<tool id="de.innot.avreclipse.tool.avrdude.app.debug.1219663509" name="AVRDude" superClass="de.innot.avreclipse.tool.avrdude.app.debug"/> |
</toolChain> |
</folderInfo> |
<sourceEntries> |
<entry excluding="TinyGPS|Koptertool3.9|followme/_old_source|bluetooth/_old_source|mkparameters.c|mk/_old_source|mksettings/_old_source|mksettings/_FC_eeprom_Versions_Archiv|stick/_old_source|setup/_old_source|wifly/_old_source|mk/_FC_eeprom_Versions_Archiv|mk/_OLD_Source|wifly/_OLD_Source|parameter.c|parameter201f.c|parameter091.c|setup/setup_new.c|lcd/font8x6_old.c|lcd/Font8x8.c|wifly/IPAddress.cpp|timer/Kopie von timer.c|wifly/WiFlyHQ.c|pkt.c|tools.c|scrollbox.c|diff|messages_new.c|lcd/lcd|osd/osd2|tracking/servo.c|parameter087.c|Resourcen|Jerome|Tracking|Release|Koptertool 3.9|Koptertool 3.2|Koptertool 1.2 644P|Koptertool 1.2|Hardware1.3 Wi232|Koptertool 1.3|Koptertool 1.2 644|Debug|Hardware 1.2 644,Wi232|DatenblÀtter|voltmeter.c|Datenblätter|font8X6.c|Wegpunkte|FC087|V0.86a|volt.c|pwm.c|touchscreen.c|Harald|Bluetooth.c|motortestI2C.c|AVRSMS_com.c|AVRSMS_api.c|AVRSMS_tools.c|AVRSMS_zip.c|Atmel|Altastation|jeti.h|Hardware|Stefan Schulze|font8x8.h|jeti.c|Fehler|settings.c|Bootloader|displ_val.c|usart1.c|Backup" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/> |
</sourceEntries> |
</configuration> |
</storageModule> |
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> |
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> |
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"> |
<doc-comment-owner id="org.eclipse.cdt.ui.doxygen"> |
<path value=""/> |
</doc-comment-owner> |
</storageModule> |
</cconfiguration> |
<cconfiguration id="de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632" moduleId="org.eclipse.cdt.core.settings" name="Koptertool3.9"> |
<macros> |
<stringMacro name="testvariable" type="VALUE_TEXT" value="1"/> |
<stringMacro name="AVRTARGETMCU" type="VALUE_TEXT" value="atmega1284p"/> |
<stringMacro name="MKVERSION" type="VALUE_TEXT" value="088n"/> |
<stringMacro name="Softwareversion" type="VALUE_TEXT" value="3.6.5g"/> |
<stringMacro name="MCU" type="VALUE_TEXT" value="atmega1284p"/> |
<stringMacro name="HWVERSION" type="VALUE_TEXT" value="#define HWVERSION3_9"/> |
<stringMacro name="BuildArtifactFileBaseName" type="VALUE_TEXT" value="${ProjName}"/> |
</macros> |
<externalSettings/> |
<extensions> |
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/> |
<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
</extensions> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<configuration artifactName="${ProjName}" buildArtefactType="de.innot.avreclipse.buildArtefactType.app" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=de.innot.avreclipse.buildArtefactType.app,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" description="88n" errorParsers="org.eclipse.cdt.core.MakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GLDErrorParser" id="de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632" name="Koptertool3.9" parent="de.innot.avreclipse.configuration.app.release" postannouncebuildStep="" postbuildStep="" preannouncebuildStep="" prebuildStep=""> |
<folderInfo id="de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632." name="/" resourcePath=""> |
<toolChain errorParsers="" id="de.innot.avreclipse.toolchain.winavr.app.release.1606310366" name="AVR-GCC Toolchain" resourceTypeBasedDiscovery="true" superClass="de.innot.avreclipse.toolchain.winavr.app.release"> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objcopy.flash.app.release.1069287281" name="Generate HEX file for Flash memory" superClass="de.innot.avreclipse.toolchain.options.toolchain.objcopy.flash.app.release"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objcopy.eeprom.app.release.1395198777" name="Generate HEX file for EEPROM" superClass="de.innot.avreclipse.toolchain.options.toolchain.objcopy.eeprom.app.release" value="true" valueType="boolean"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.objdump.app.release.1943135107" name="Generate Extended Listing (Source + generated Assembler)" superClass="de.innot.avreclipse.toolchain.options.toolchain.objdump.app.release" value="true" valueType="boolean"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.size.app.release.2109904530" name="Print Size" superClass="de.innot.avreclipse.toolchain.options.toolchain.size.app.release"/> |
<option id="de.innot.avreclipse.toolchain.options.toolchain.avrdude.app.release.1992664981" name="AVRDude" superClass="de.innot.avreclipse.toolchain.options.toolchain.avrdude.app.release"/> |
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF" id="de.innot.avreclipse.targetplatform.winavr.app.release.1528737441" name="AVR Cross-Target" superClass="de.innot.avreclipse.targetplatform.winavr.app.release"/> |
<builder buildPath="${workspace_loc:/GPL_Mikrokoptertool/Release}" enableAutoBuild="true" errorParsers="org.eclipse.cdt.core.MakeErrorParser" id="de.innot.avreclipse.target.builder.winavr.app.release.1862080332" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="AVR GNU Make Builder" parallelBuildOn="true" parallelizationNumber="10" superClass="de.innot.avreclipse.target.builder.winavr.app.release"/> |
<tool command="avr-gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG}${OUTPUT_PREFIX}${OUTPUT} ${INPUTS}" errorParsers="org.eclipse.cdt.core.GASErrorParser" id="de.innot.avreclipse.tool.assembler.winavr.app.release.1852553503" name="AVR Assembler" superClass="de.innot.avreclipse.tool.assembler.winavr.app.release"> |
<option id="de.innot.avreclipse.assembler.option.debug.level.1242179784" name="Generate Debugging Info" superClass="de.innot.avreclipse.assembler.option.debug.level" value="de.innot.avreclipse.assembler.option.debug.level.none" valueType="enumerated"/> |
<option id="de.innot.avreclipse.asm.option.warnings.nowarn.1608753415" name="Suppress warnings (-W)" superClass="de.innot.avreclipse.asm.option.warnings.nowarn" value="false" valueType="boolean"/> |
<inputType id="de.innot.avreclipse.tool.assembler.input.396233498" superClass="de.innot.avreclipse.tool.assembler.input"/> |
</tool> |
<tool command="avr-gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG}${OUTPUT_PREFIX}${OUTPUT} ${INPUTS}" errorParsers="org.eclipse.cdt.core.GCCErrorParser" id="de.innot.avreclipse.tool.compiler.winavr.app.release.1087602969" name="AVR Compiler" superClass="de.innot.avreclipse.tool.compiler.winavr.app.release"> |
<option id="de.innot.avreclipse.compiler.option.debug.level.2016051807" name="Generate Debugging Info" superClass="de.innot.avreclipse.compiler.option.debug.level" value="de.innot.avreclipse.compiler.option.debug.level.none" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.optimize.1652449165" name="Optimization Level" superClass="de.innot.avreclipse.compiler.option.optimize" value="de.innot.avreclipse.compiler.optimize.size" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.omitfcpu.232644227" name="Omit F_CPU" superClass="de.innot.avreclipse.compiler.option.omitfcpu" value="false" valueType="boolean"/> |
<option id="de.innot.avreclipse.compiler.option.otherflags.87881368" name="Other flags" superClass="de.innot.avreclipse.compiler.option.otherflags" value="" valueType="string"/> |
<option id="de.innot.avreclipse.compiler.option.std.1420800047" name="Language Standard" superClass="de.innot.avreclipse.compiler.option.std" value="de.innot.avreclipse.compiler.option.std.gnu99" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.def.174580993" name="Define Syms (-D)" superClass="de.innot.avreclipse.compiler.option.def" valueType="definedSymbols"> |
<listOptionValue builtIn="false" value="__AVR_ATmega1284P__"/> |
</option> |
<option id="de.innot.avreclipse.compiler.option.debug.type.1335593774" name="Debug Info Format" superClass="de.innot.avreclipse.compiler.option.debug.type" value="de.innot.avreclipse.compiler.option.debug.level.stabsplus" valueType="enumerated"/> |
<option id="de.innot.avreclipse.compiler.option.optimize.other.266970312" name="Other Optimization Flags" superClass="de.innot.avreclipse.compiler.option.optimize.other" value="-mcall-prologues -mno-interrupts -ffunction-sections -D__DELAY_BACKWARD_COMPATIBLE__" valueType="string"/> |
<option id="de.innot.avreclipse.compiler.option.warnerr.796997598" name="Warnings as Errors (-Werror)" superClass="de.innot.avreclipse.compiler.option.warnerr" value="false" valueType="boolean"/> |
<inputType id="de.innot.avreclipse.compiler.winavr.input.1885366176" name="C Source Files" superClass="de.innot.avreclipse.compiler.winavr.input"/> |
</tool> |
<tool id="de.innot.avreclipse.tool.cppcompiler.app.release.1241414384" name="AVR C++ Compiler" superClass="de.innot.avreclipse.tool.cppcompiler.app.release"> |
<option id="de.innot.avreclipse.cppcompiler.option.debug.level.519795832" name="Generate Debugging Info" superClass="de.innot.avreclipse.cppcompiler.option.debug.level" value="de.innot.avreclipse.cppcompiler.option.debug.level.none" valueType="enumerated"/> |
<option id="de.innot.avreclipse.cppcompiler.option.optimize.25088117" name="Optimization Level" superClass="de.innot.avreclipse.cppcompiler.option.optimize" value="de.innot.avreclipse.cppcompiler.optimize.size" valueType="enumerated"/> |
</tool> |
<tool command="avr-gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG}${OUTPUT_PREFIX}${OUTPUT} ${INPUTS}" errorParsers="org.eclipse.cdt.core.GLDErrorParser" id="de.innot.avreclipse.tool.linker.winavr.app.release.191727013" name="AVR C Linker" superClass="de.innot.avreclipse.tool.linker.winavr.app.release"> |
<option id="de.innot.avreclipse.linker.option.libs.745778707" name="Libraries (-l)" superClass="de.innot.avreclipse.linker.option.libs" valueType="libs"> |
<listOptionValue builtIn="false" value="m"/> |
</option> |
<option id="de.innot.avreclipse.linker.option.otherlinkargs.623435598" name="Other Arguments" superClass="de.innot.avreclipse.linker.option.otherlinkargs" value="-s -mrelax " valueType="string"/> |
<option id="de.innot.avreclipse.linker.option.nosyms.299444960" name="Omit Symbols (-s)" superClass="de.innot.avreclipse.linker.option.nosyms" value="false" valueType="boolean"/> |
<option id="de.innot.avreclipse.linker.option.libpath.1529109468" name="Libraries Path (-L)" superClass="de.innot.avreclipse.linker.option.libpath"/> |
<inputType id="de.innot.avreclipse.tool.linker.input.1916909513" name="OBJ Files" superClass="de.innot.avreclipse.tool.linker.input"> |
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/> |
<additionalInput kind="additionalinput" paths="$(LIBS)"/> |
</inputType> |
</tool> |
<tool id="de.innot.avreclipse.tool.cpplinker.app.release.1777649897" name="AVR C++ Linker" superClass="de.innot.avreclipse.tool.cpplinker.app.release"/> |
<tool id="de.innot.avreclipse.tool.archiver.winavr.base.499276008" name="AVR Archiver" superClass="de.innot.avreclipse.tool.archiver.winavr.base"/> |
<tool command="-avr-objdump" commandLinePattern="${COMMAND} ${FLAGS} ${INPUTS} >${OUTPUT}" errorParsers="" id="de.innot.avreclipse.tool.objdump.winavr.app.release.768856626" name="AVR Create Extended Listing" superClass="de.innot.avreclipse.tool.objdump.winavr.app.release"/> |
<tool command="-avr-objcopy" commandLinePattern="${COMMAND} ${FLAGS} ${INPUTS} ${OUTPUT}" errorParsers="" id="de.innot.avreclipse.tool.objcopy.flash.winavr.app.release.1655349728" name="AVR Create Flash image" superClass="de.innot.avreclipse.tool.objcopy.flash.winavr.app.release"/> |
<tool command="-avr-objcopy" commandLinePattern="${COMMAND} ${FLAGS} ${INPUTS} ${OUTPUT}" errorParsers="" id="de.innot.avreclipse.tool.objcopy.eeprom.winavr.app.release.698767043" name="AVR Create EEPROM image" superClass="de.innot.avreclipse.tool.objcopy.eeprom.winavr.app.release"/> |
<tool command="-avr-size" commandLinePattern="${COMMAND} ${FLAGS} ${INPUTS}" errorParsers="" id="de.innot.avreclipse.tool.size.winavr.app.release.506627432" name="Print Size" superClass="de.innot.avreclipse.tool.size.winavr.app.release"/> |
<tool id="de.innot.avreclipse.tool.avrdude.app.release.1974837550" name="AVRDude" superClass="de.innot.avreclipse.tool.avrdude.app.release"/> |
</toolChain> |
</folderInfo> |
<sourceEntries> |
<entry excluding="TinyGPS|Koptertool3.9|followme/_old_source|bluetooth/_old_source|mkparameters.c|mk/_old_source|mksettings/_old_source|mksettings/_FC_eeprom_Versions_Archiv|stick/_old_source|setup/_old_source|wifly/_old_source|mk/_FC_eeprom_Versions_Archiv|mk/_OLD_Source|wifly/_OLD_Source|parameter.c|parameter201f.c|parameter091.c|setup/setup_new.c|lcd/font8x6_old.c|lcd/Font8x8.c|wifly/IPAddress.cpp|timer/Kopie von timer.c|wifly/WiFlyHQ.c|pkt.c|tools.c|scrollbox.c|diff|messages_new.c|lcd/lcd|osd/osd2|tracking/servo.c|parameter087.c|Resourcen|Jerome|Tracking|Release|Koptertool 3.9|Koptertool 3.2|Koptertool 1.2 644P|Koptertool 1.2|Hardware1.3 Wi232|Koptertool 1.3|Koptertool 1.2 644|Debug|Hardware 1.2 644,Wi232|DatenblÀtter|voltmeter.c|Datenblätter|font8X6.c|Wegpunkte|FC087|V0.86a|volt.c|pwm.c|touchscreen.c|Harald|Bluetooth.c|motortestI2C.c|AVRSMS_com.c|AVRSMS_api.c|AVRSMS_tools.c|AVRSMS_zip.c|Atmel|Altastation|jeti.h|Hardware|Stefan Schulze|font8x8.h|jeti.c|Fehler|settings.c|Bootloader|displ_val.c|usart1.c|Backup" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/> |
</sourceEntries> |
</configuration> |
</storageModule> |
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> |
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> |
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"> |
<doc-comment-owner id="org.eclipse.cdt.ui.doxygen"> |
<path value=""/> |
</doc-comment-owner> |
</storageModule> |
</cconfiguration> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<project id="GPL_Mikrokoptertool.de.innot.avreclipse.project.winavr.elf_2.1.0.1917146692" name="AVR Cross Target Application" projectType="de.innot.avreclipse.project.winavr.elf_2.1.0"/> |
</storageModule> |
<storageModule moduleId="refreshScope" versionNumber="1"> |
<resource resourceType="PROJECT" workspacePath="/GPL_PKT_V3_6_9b_FC090e"/> |
</storageModule> |
<storageModule moduleId="scannerConfiguration"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959.623565166;de.innot.avreclipse.configuration.app.release.799832959.623565166.;de.innot.avreclipse.tool.compiler.winavr.app.release.695648390;de.innot.avreclipse.compiler.winavr.input.71411405"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959.1517645312;de.innot.avreclipse.configuration.app.release.799832959.1517645312.;de.innot.avreclipse.tool.compiler.winavr.app.release.130890171;de.innot.avreclipse.compiler.winavr.input.106986704"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632;de.innot.avreclipse.configuration.app.release.799832959.623565166.2071059632.;de.innot.avreclipse.tool.compiler.winavr.app.release.1087602969;de.innot.avreclipse.compiler.winavr.input.1885366176"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959;de.innot.avreclipse.configuration.app.release.799832959."> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959;de.innot.avreclipse.configuration.app.release.799832959.;de.innot.avreclipse.tool.compiler.winavr.app.release.1810413415;de.innot.avreclipse.compiler.winavr.input.2125782100"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.debug.2000056965;de.innot.avreclipse.configuration.app.debug.2000056965."> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959.1517645312;de.innot.avreclipse.configuration.app.release.799832959.1517645312."> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.debug.2000056965;de.innot.avreclipse.configuration.app.debug.2000056965.;de.innot.avreclipse.tool.compiler.winavr.app.debug.1161577200;de.innot.avreclipse.compiler.winavr.input.667334442"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="de.innot.avreclipse.configuration.app.release.799832959.623565166;de.innot.avreclipse.configuration.app.release.799832959.623565166."> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC"/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
</storageModule> |
</cproject> |
/Transportables_Koptertool/PKT/devSettings/EclipseProject/.project |
---|
0,0 → 1,88 |
<?xml version="1.0" encoding="UTF-8"?> |
<projectDescription> |
<name>GPL_PKT_V3_85d</name> |
<comment></comment> |
<projects> |
</projects> |
<buildSpec> |
<buildCommand> |
<name>org.eclipse.mylyn.wikitext.ui.wikiTextValidationBuilder</name> |
<arguments> |
</arguments> |
</buildCommand> |
<buildCommand> |
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name> |
<arguments> |
<dictionary> |
<key>?name?</key> |
<value></value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.append_environment</key> |
<value>true</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.autoBuildTarget</key> |
<value>all</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.buildArguments</key> |
<value>-j10</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.buildCommand</key> |
<value>make</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.buildLocation</key> |
<value>${workspace_loc:/GPL_Mikrokoptertool/Release}</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key> |
<value>clean</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.contents</key> |
<value>org.eclipse.cdt.make.core.activeConfigSettings</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.enableAutoBuild</key> |
<value>true</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.enableCleanBuild</key> |
<value>true</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.enableFullBuild</key> |
<value>true</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.fullBuildTarget</key> |
<value>all</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.stopOnError</key> |
<value>true</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key> |
<value>true</value> |
</dictionary> |
</arguments> |
</buildCommand> |
<buildCommand> |
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name> |
<triggers>full,incremental,</triggers> |
<arguments> |
</arguments> |
</buildCommand> |
</buildSpec> |
<natures> |
<nature>org.eclipse.cdt.core.cnature</nature> |
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature> |
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature> |
<nature>de.innot.avreclipse.core.avrnature</nature> |
<nature>org.eclipse.mylyn.wikitext.ui.wikiTextNature</nature> |
</natures> |
</projectDescription> |
/Transportables_Koptertool/PKT/devSettings/EclipseProject/avr-gcc_5.2.1_patch/msys-1.0.dll |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/Transportables_Koptertool/PKT/devSettings/EclipseProject/avr-gcc_5.2.1_patch/readme.txt |
---|
0,0 → 1,5 |
Wenn beim Kompilieren auf win7 oder win8.1 der Fehler 0xc0000142 auftritt, dann kann das Kopieren der msys-1.0.dll nach |
utils\bin directory (WinAVR-Verzeichnis) |
helfen... |
/Transportables_Koptertool/PKT/trunk/10DOF/bmp085.c |
---|
0,0 → 1,224 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY bmp085.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für BMP085/BMP180 Luftdrucksensor, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "bmp085.h" |
//path to i2c fleury lib |
#include BMP085_I2CFLEURYPATH |
/* |
* i2c write |
*/ |
void bmp085_writemem(uint8_t reg, uint8_t value) { |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_write(value); |
i2c_stop(); |
} |
/* |
* i2c read |
*/ |
void bmp085_readmem(uint8_t reg, uint8_t buff[], uint8_t bytes) { |
uint8_t i =0; |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_rep_start(BMP085_ADDR | I2C_READ); |
for(i=0; i<bytes; i++) { |
if(i==bytes-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
#if BMP085_FILTERPRESSURE == 1 |
#define BMP085_AVARAGECOEF 21 |
static long k[BMP085_AVARAGECOEF]; |
long bmp085_avaragefilter(long input) { |
uint8_t i=0; |
long sum=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
k[i] = k[i+1]; |
} |
k[BMP085_AVARAGECOEF-1] = input; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
sum += k[i]; |
} |
return (sum /BMP085_AVARAGECOEF) ; |
} |
#endif |
/* |
* read calibration registers |
*/ |
void bmp085_getcalibration() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
bmp085_readmem(BMP085_REGAC1, buff, 2); |
bmp085_regac1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC2, buff, 2); |
bmp085_regac2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC3, buff, 2); |
bmp085_regac3 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC4, buff, 2); |
bmp085_regac4 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC5, buff, 2); |
bmp085_regac5 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC6, buff, 2); |
bmp085_regac6 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGB1, buff, 2); |
bmp085_regb1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGB2, buff, 2); |
bmp085_regb2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMB, buff, 2); |
bmp085_regmb = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMC, buff, 2); |
bmp085_regmc = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMD, buff, 2); |
bmp085_regmd = ((int)buff[0] <<8 | ((int)buff[1])); |
} |
/* |
* get raw temperature as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawtemperature() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
long ut,x1,x2; |
//read raw temperature |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADTEMPERATURE); |
_delay_ms(5); // min. 4.5ms read Temp delay |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 2); |
ut = ((long)buff[0] << 8 | ((long)buff[1])); //uncompensated temperature value |
//calculate raw temperature |
x1 = ((long)ut - bmp085_regac6) * bmp085_regac5 >> 15; |
x2 = ((long)bmp085_regmc << 11) / (x1 + bmp085_regmd); |
bmp085_rawtemperature = x1 + x2; |
} |
/* |
* get raw pressure as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawpressure() { |
uint8_t buff[3]; |
memset(buff, 0, sizeof(buff)); |
long up,x1,x2,x3,b3,b6,p; |
unsigned long b4,b7; |
#if BMP085_AUTOUPDATETEMP == 1 |
bmp085_getrawtemperature(); |
#endif |
//read raw pressure |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADPRESSURE+(BMP085_MODE << 6)); |
_delay_ms(2 + (3<<BMP085_MODE)); |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 3); |
up = ((((long)buff[0] <<16) | ((long)buff[1] <<8) | ((long)buff[2])) >> (8-BMP085_MODE)); // uncompensated pressure value |
//calculate raw pressure |
b6 = bmp085_rawtemperature - 4000; |
x1 = (bmp085_regb2* (b6 * b6) >> 12) >> 11; |
x2 = (bmp085_regac2 * b6) >> 11; |
x3 = x1 + x2; |
b3 = (((((long)bmp085_regac1) * 4 + x3) << BMP085_MODE) + 2) >> 2; |
x1 = (bmp085_regac3 * b6) >> 13; |
x2 = (bmp085_regb1 * ((b6 * b6) >> 12)) >> 16; |
x3 = ((x1 + x2) + 2) >> 2; |
b4 = (bmp085_regac4 * (uint32_t)(x3 + 32768)) >> 15; |
b7 = ((uint32_t)up - b3) * (50000 >> BMP085_MODE); |
p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; |
x1 = (p >> 8) * (p >> 8); |
x1 = (x1 * 3038) >> 16; |
x2 = (-7357 * p) >> 16; |
bmp085_rawpressure = p + ((x1 + x2 + 3791) >> 4); |
#if BMP085_FILTERPRESSURE == 1 |
bmp085_rawpressure = bmp085_avaragefilter(bmp085_rawpressure); |
#endif |
} |
/* |
* get celsius temperature |
*/ |
double bmp085_gettemperature() { |
bmp085_getrawtemperature(); |
double temperature = ((bmp085_rawtemperature + 8)>>4); |
temperature = temperature /10; |
return temperature; |
} |
/* |
* get pressure |
*/ |
int32_t bmp085_getpressure() { |
bmp085_getrawpressure(); |
return bmp085_rawpressure + BMP085_UNITPAOFFSET; |
} |
/* |
* get altitude |
*/ |
double bmp085_getaltitude() { |
bmp085_getrawpressure(); |
return ((1 - pow(bmp085_rawpressure/(double)101325, 0.1903 )) / 0.0000225577) + BMP085_UNITMOFFSET; |
} |
/* |
* init bmp085 |
*/ |
void bmp085_init() { |
#if BMP085_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
bmp085_getcalibration(); //get calibration data |
bmp085_getrawtemperature(); //update raw temperature, at least the first time |
#if BMP085_FILTERPRESSURE == 1 |
//initialize the avarage filter |
uint8_t i=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
bmp085_getrawpressure(); |
} |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/bmp085.h |
---|
0,0 → 1,74 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- this library is a porting of the bmp085driver 0.4 ardunio library |
http://code.google.com/p/bmp085driver/ |
Notes: |
- to compile with avrgcc you may define -lm flag for some math functions |
*/ |
#ifndef BMP085_H_ |
#define BMP085_H_ |
#include <stdio.h> |
#include <avr/io.h> |
#define BMP085_ADDR (0x77<<1) //0x77 default I2C address |
#define BMP085_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define BMP085_I2CINIT 1 //init i2c |
//registers |
#define BMP085_REGAC1 0xAA |
#define BMP085_REGAC2 0xAC |
#define BMP085_REGAC3 0xAE |
#define BMP085_REGAC4 0xB0 |
#define BMP085_REGAC5 0xB2 |
#define BMP085_REGAC6 0xB4 |
#define BMP085_REGB1 0xB6 |
#define BMP085_REGB2 0xB8 |
#define BMP085_REGMB 0xBA |
#define BMP085_REGMC 0xBC |
#define BMP085_REGMD 0xBE |
#define BMP085_REGCONTROL 0xF4 //control |
#define BMP085_REGCONTROLOUTPUT 0xF6 //output 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB |
#define BMP085_REGREADTEMPERATURE 0x2E //read temperature |
#define BMP085_REGREADPRESSURE 0x34 //read pressure |
//modes |
#define BMP085_MODEULTRALOWPOWER 0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5 |
#define BMP085_MODESTANDARD 1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4 |
#define BMP085_MODEHIGHRES 2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3 |
#define BMP085_MODEULTRAHIGHRES 3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25 |
//autoupdate temperature enabled |
#define BMP085_AUTOUPDATETEMP 1 //autoupdate temperature every read |
//setup parameters |
#define BMP085_MODE BMP085_MODEULTRAHIGHRES //define a mode |
#define BMP085_UNITPAOFFSET 0 //define a unit offset (pa) |
#define BMP085_UNITMOFFSET 0 //define a unit offset (m) |
//avarage filter |
#define BMP085_FILTERPRESSURE 1 //avarage filter for pressure |
//variables |
int bmp085_regac1, bmp085_regac2, bmp085_regac3, bmp085_regb1, bmp085_regb2, bmp085_regmb, bmp085_regmc, bmp085_regmd; |
unsigned int bmp085_regac4, bmp085_regac5, bmp085_regac6; |
long bmp085_rawtemperature, bmp085_rawpressure; |
//functions |
extern void bmp085_init(); |
extern int32_t bmp085_getpressure(); |
extern double bmp085_getaltitude(); |
extern double bmp085_gettemperature(); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/hmc5883l.c |
---|
0,0 → 1,131 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY hmc5883l.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für HMC5883L Compass, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "hmc5883l.h" |
//path to i2c fleury lib |
#include HMC5883L_I2CFLEURYPATH |
static double hmc5883l_scale = 0; |
/* |
* init the hmc5883l |
*/ |
void hmc5883l_init() { |
#if HMC5883L_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
//set scale |
hmc5883l_scale = 0; |
uint8_t regValue = 0x00; |
#if HMC5883L_SCALE == HMC5883L_SCALE088 |
regValue = 0x00; |
hmc5883l_scale = 0.73; |
#elif HMC5883L_SCALE == HMC5883L_SCALE13 |
regValue = 0x01; |
hmc5883l_scale = 0.92; |
#elif HMC5883L_SCALE == HMC5883L_SCALE19 |
regValue = 0x02; |
hmc5883l_scale = 1.22; |
#elif HMC5883L_SCALE == HMC5883L_SCALE25 |
regValue = 0x03; |
hmc5883l_scale = 1.52; |
#elif HMC5883L_SCALE == HMC5883L_SCALE40 |
regValue = 0x04; |
hmc5883l_scale = 2.27; |
#elif HMC5883L_SCALE == HMC5883L_SCALE47 |
regValue = 0x05; |
hmc5883l_scale = 2.56; |
#elif HMC5883L_SCALE == HMC5883L_SCALE56 |
regValue = 0x06; |
hmc5883l_scale = 3.03; |
#elif HMC5883L_SCALE == HMC5883L_SCALE81 |
regValue = 0x07; |
hmc5883l_scale = 4.35; |
#endif |
//setting is in the top 3 bits of the register. |
regValue = regValue << 5; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_CONFREGB); |
i2c_write(regValue); |
i2c_stop(); |
//set measurement mode |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_MODEREG); |
i2c_write(HMC5883L_MEASUREMODE); |
i2c_stop(); |
} |
/* |
* get raw data |
*/ |
void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) { |
uint8_t i = 0; |
uint8_t buff[6]; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_DATAREGBEGIN); |
i2c_stop(); |
i2c_start_wait(HMC5883L_ADDR | I2C_READ); |
for(i=0; i<6; i++) { |
if(i==6-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
*mxraw = (int16_t)((buff[0] << 8) | buff[1]); |
*mzraw = (int16_t)((buff[2] << 8) | buff[3]); |
*myraw = (int16_t)((buff[4] << 8) | buff[5]); |
} |
/* |
* get scaled data |
*/ |
void hmc5883l_getdata(double *mx, double *my, double *mz) { |
int16_t mxraw = 0; |
int16_t myraw = 0; |
int16_t mzraw = 0; |
hmc5883l_getrawdata(&mxraw, &myraw, &mzraw); |
#if HMC5883L_CALIBRATED == 1 |
float mxt = mxraw - HMC5883L_OFFSETX; |
float myt = myraw - HMC5883L_OFFSETY; |
float mzt = mzraw - HMC5883L_OFFSETZ; |
*mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt; |
*my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt; |
*mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt; |
#else |
*mx = mxraw * hmc5883l_scale; |
*my = myraw * hmc5883l_scale; |
*mz = mzraw * hmc5883l_scale; |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/hmc5883l.h |
---|
0,0 → 1,71 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- HMC5883L Triple Axis Magnetometer Arduino Library |
http://bildr.org/2012/02/hmc5883l_arduino/ |
*/ |
#ifndef HMC5883L_H_ |
#define HMC5883L_H_ |
//definitions |
#define HMC5883L_ADDR (0x1E<<1) //device address |
//i2c settings |
#define HMC5883L_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define HMC5883L_I2CINIT 1 //init i2c |
//registers |
#define HMC5883L_CONFREGA 0x00 |
#define HMC5883L_CONFREGB 0x01 |
#define HMC5883L_MODEREG 0x02 |
#define HMC5883L_DATAREGBEGIN 0x03 |
//setup measurement mode |
#define HMC5883L_MEASURECONTINOUS 0x00 |
#define HMC5883L_MEASURESINGLESHOT 0x01 |
#define HMC5883L_MEASUREIDLE 0x03 |
#define HMC5883L_MEASUREMODE HMC5883L_MEASURECONTINOUS |
//setup scale |
#define HMC5883L_SCALE088 1 //0.88 |
#define HMC5883L_SCALE13 2 //1.3 |
#define HMC5883L_SCALE19 3 //1.9 |
#define HMC5883L_SCALE25 4 //2.5 |
#define HMC5883L_SCALE40 5 //4.0 |
#define HMC5883L_SCALE47 6 //4.7 |
#define HMC5883L_SCALE56 7 //5.6 |
#define HMC5883L_SCALE81 8 //8.1 |
#define HMC5883L_SCALE HMC5883L_SCALE13 |
#define HMC5883L_CALIBRATED 1 //enable this if this magn is calibrated |
//calibration values |
#if HMC5883L_CALIBRATED == 1 |
#define HMC5883L_OFFSETX -99.7 |
#define HMC5883L_OFFSETY -154.0 |
#define HMC5883L_OFFSETZ -22.7 |
#define HMC5883L_GAINX1 0.952017 |
#define HMC5883L_GAINX2 0.00195895 |
#define HMC5883L_GAINX3 0.0139661 |
#define HMC5883L_GAINY1 0.00195895 |
#define HMC5883L_GAINY2 0.882824 |
#define HMC5883L_GAINY3 0.00760243 |
#define HMC5883L_GAINZ1 0.0139661 |
#define HMC5883L_GAINZ2 0.00760243 |
#define HMC5883L_GAINZ3 0.995365 |
#endif |
//functions |
extern void hmc5883l_init(); |
extern void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw); |
extern void hmc5883l_getdata(double *mx, double *my, double *mz); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/mpu6050.c |
---|
0,0 → 1,694 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 |
#include <math.h> //include libm |
#endif |
//path to i2c fleury lib |
#include "i2cmaster.h" |
volatile uint8_t buffer[14]; |
/* |
* read bytes from chip register |
*/ |
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data) { |
uint8_t i = 0; |
int8_t count = 0; |
if(length > 0) { |
//request register |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); |
_delay_us(10); |
//read data |
i2c_start(MPU6050_ADDR | I2C_READ); |
for(i=0; i<length; i++) { |
count++; |
if(i==length-1) |
data[i] = i2c_readNak(); |
else |
data[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
return count; |
} |
/* |
* read 1 byte from chip register |
*/ |
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) { |
return mpu6050_readBytes(regAddr, 1, data); |
} |
/* |
* write bytes to chip register |
*/ |
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data) { |
if(length > 0) { |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (uint8_t i = 0; i < length; i++) { |
i2c_write((uint8_t) data[i]); |
} |
i2c_stop(); |
} |
} |
/* |
* write 1 byte to chip register |
*/ |
void mpu6050_writeByte(uint8_t regAddr, uint8_t data) { |
return mpu6050_writeBytes(regAddr, 1, &data); |
} |
/* |
* read bits from chip register |
*/ |
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { |
// 01101001 read byte |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 010 masked |
// -> 010 shifted |
int8_t count = 0; |
if(length > 0) { |
uint8_t b; |
if ((count = mpu6050_readByte(regAddr, &b)) != 0) { |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
b &= mask; |
b >>= (bitStart - length + 1); |
*data = b; |
} |
} |
return count; |
} |
/* |
* read 1 bit from chip register |
*/ |
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data) { |
uint8_t b; |
uint8_t count = mpu6050_readByte(regAddr, &b); |
*data = b & (1 << bitNum); |
return count; |
} |
/* |
* write bit/bits to chip register |
*/ |
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { |
// 010 value to write |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 00011100 mask byte |
// 10101111 original value (sample) |
// 10100011 original & ~mask |
// 10101011 masked | value |
if(length > 0) { |
uint8_t b = 0; |
if (mpu6050_readByte(regAddr, &b) != 0) { //get current data |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
data <<= (bitStart - length + 1); // shift data into correct position |
data &= mask; // zero all non-important bits in data |
b &= ~(mask); // zero all important bits in existing byte |
b |= data; // combine data with existing byte |
mpu6050_writeByte(regAddr, b); |
} |
} |
} |
/* |
* write one bit to chip register |
*/ |
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data) { |
uint8_t b; |
mpu6050_readByte(regAddr, &b); |
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); |
mpu6050_writeByte(regAddr, b); |
} |
#if MPU6050_GETATTITUDE == 2 |
/* |
* write word/words to chip register |
*/ |
void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data) { |
if(length > 0) { |
uint8_t i = 0; |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (i = 0; i < length * 2; i++) { |
i2c_write((uint8_t)(data[i++] >> 8)); // send MSB |
i2c_write((uint8_t)data[i]); // send LSB |
} |
i2c_stop(); |
} |
} |
/* |
* set a chip memory bank |
*/ |
void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank) { |
bank &= 0x1F; |
if (userBank) bank |= 0x20; |
if (prefetchEnabled) bank |= 0x40; |
mpu6050_writeByte(MPU6050_RA_BANK_SEL, bank); |
} |
/* |
* set memory start address |
*/ |
void mpu6050_setMemoryStartAddress(uint8_t address) { |
mpu6050_writeByte(MPU6050_RA_MEM_START_ADDR, address); |
} |
/* |
* read a memory block |
*/ |
void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
for (uint16_t i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
// read the chunk of data as specified |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i); |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
} |
/* |
* write a memory block |
*/ |
uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
uint8_t *verifyBuffer = 0; |
uint8_t *progBuffer = 0; |
uint16_t i; |
uint8_t j; |
if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
for (i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
if (useProgMem) { |
// write the chunk of data as specified |
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
// write the chunk of data as specified |
progBuffer = (uint8_t *)data + i; |
} |
mpu6050_writeBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer); |
// verify data if needed |
if (verify && verifyBuffer) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); |
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { |
free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 0; // uh oh. |
} |
} |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
if (verify) free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* write a dmp configuration set |
*/ |
uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem) { |
uint8_t *progBuffer = 0; |
uint8_t success, special; |
uint16_t i, j; |
if (useProgMem) { |
progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary |
} |
// config set data is a long string of blocks with the following structure: |
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] |
uint8_t bank, offset, length; |
for (i = 0; i < dataSize;) { |
if (useProgMem) { |
bank = pgm_read_byte(data + i++); |
offset = pgm_read_byte(data + i++); |
length = pgm_read_byte(data + i++); |
} else { |
bank = data[i++]; |
offset = data[i++]; |
length = data[i++]; |
} |
// write data or perform special action |
if (length > 0) { |
// regular block of data to write |
if (useProgMem) { |
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); |
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
progBuffer = (uint8_t *)data + i; |
} |
success = mpu6050_writeMemoryBlock(progBuffer, length, bank, offset, 1, 0); |
i += length; |
} else { |
// special instruction |
// NOTE: this kind of behavior (what and when to do certain things) |
// is totally undocumented. This code is in here based on observed |
// behavior only, and exactly why (or even whether) it has to be here |
// is anybody's guess for now. |
if (useProgMem) { |
special = pgm_read_byte(data + i++); |
} else { |
special = data[i++]; |
} |
if (special == 0x01) { |
// enable DMP-related interrupts |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, 1); //setIntZeroMotionEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, 1); //setIntFIFOBufferOverflowEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, 1); //setIntDMPEnabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x32); // single operation |
success = 1; |
} else { |
// unknown special command |
success = 0; |
} |
} |
if (!success) { |
if (useProgMem) free(progBuffer); |
return 0; // uh oh |
} |
} |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* get the fifo count |
*/ |
uint16_t mpu6050_getFIFOCount(void) { |
mpu6050_readBytes(MPU6050_RA_FIFO_COUNTH, 2, (uint8_t *)buffer); |
return (((uint16_t)buffer[0]) << 8) | buffer[1]; |
} |
/* |
* read fifo bytes |
*/ |
void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length) { |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, length, data); |
} |
/* |
* get the interrupt status |
*/ |
uint8_t mpu6050_getIntStatus(void) { |
mpu6050_readByte(MPU6050_RA_INT_STATUS, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* reset fifo |
*/ |
void mpu6050_resetFIFO(void) { |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} |
/* |
* get gyro offset X |
*/ |
int8_t mpu6050_getXGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset X |
*/ |
void mpu6050_setXGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Y |
*/ |
int8_t mpu6050_getYGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Y |
*/ |
void mpu6050_setYGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Z |
*/ |
int8_t mpu6050_getZGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Z |
*/ |
void mpu6050_setZGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
#endif |
/* |
* set sleep disabled |
*/ |
void mpu6050_setSleepDisabled() { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0); |
} |
/* |
* set sleep enabled |
*/ |
void mpu6050_setSleepEnabled(void) { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1); |
} |
/* |
* test connectino to chip |
*/ |
uint8_t mpu6050_testConnection(void) { |
mpu6050_readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, (uint8_t *)buffer); |
if(buffer[0] == 0x34) |
return 1; |
else |
return 0; |
} |
/* |
* initialize the accel and gyro |
*/ |
void mpu6050_init(void) { |
//allow mpu6050 chip clocks to start up |
_delay_ms(100); |
//set sleep disabled |
mpu6050_setSleepDisabled(); |
//wake up delay needed sleep disabled |
_delay_ms(10); |
//set clock source |
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) |
// as the clock reference for improved stability |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set sampe rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz |
//set gyro range |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS); |
//set accel range |
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS); |
#if MPU6050_GETATTITUDE == 1 |
#error "Do not enable timer 0 it is in use elsewhere!" |
//MPU6050_TIMER0INIT |
#endif |
} |
//can not accept many request if we alreay have getattitude requests |
/* |
* get raw data |
*/ |
void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
*ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
*ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
*az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
*gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
*gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
*gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
} |
/* |
* get raw data converted to g and deg/sec values |
*/ |
void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
*axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
*ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
*azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
*gxds = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN; |
*gyds = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN; |
*gzds = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN; |
#else |
*axg = (double)(ax)/MPU6050_AGAIN; |
*ayg = (double)(ay)/MPU6050_AGAIN; |
*azg = (double)(az)/MPU6050_AGAIN; |
*gxds = (double)(gx)/MPU6050_GGAIN; |
*gyds = (double)(gy)/MPU6050_GGAIN; |
*gzds = (double)(gz)/MPU6050_GGAIN; |
#endif |
} |
#if MPU6050_GETATTITUDE == 1 |
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; |
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
/* |
* Mahony update function (for 6DOF) |
*/ |
void mpu6050_mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) { |
float norm; |
float halfvx, halfvy, halfvz; |
float halfex, halfey, halfez; |
float qa, qb, qc; |
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
// Normalise accelerometer measurement |
norm = sqrt(ax * ax + ay * ay + az * az); |
ax /= norm; |
ay /= norm; |
az /= norm; |
// Estimated direction of gravity and vector perpendicular to magnetic flux |
halfvx = q1 * q3 - q0 * q2; |
halfvy = q0 * q1 + q2 * q3; |
halfvz = q0 * q0 - 0.5f + q3 * q3; |
// Error is sum of cross product between estimated and measured direction of gravity |
halfex = (ay * halfvz - az * halfvy); |
halfey = (az * halfvx - ax * halfvz); |
halfez = (ax * halfvy - ay * halfvx); |
// Compute and apply integral feedback if enabled |
if(mpu6050_mahonytwoKiDef > 0.0f) { |
integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq); // integral error scaled by Ki |
integralFBy += mpu6050_mahonytwoKiDef * halfey * (1.0f / mpu6050_mahonysampleFreq); |
integralFBz += mpu6050_mahonytwoKiDef * halfez * (1.0f / mpu6050_mahonysampleFreq); |
gx += integralFBx; // apply integral feedback |
gy += integralFBy; |
gz += integralFBz; |
} else { |
integralFBx = 0.0f; // prevent integral windup |
integralFBy = 0.0f; |
integralFBz = 0.0f; |
} |
// Apply proportional feedback |
gx += mpu6050_mahonytwoKpDef * halfex; |
gy += mpu6050_mahonytwoKpDef * halfey; |
gz += mpu6050_mahonytwoKpDef * halfez; |
} |
// Integrate rate of change of quaternion |
gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); // pre-multiply common factors |
gy *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
gz *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
qa = q0; |
qb = q1; |
qc = q2; |
q0 += (-qb * gx - qc * gy - q3 * gz); |
q1 += (qa * gx + qc * gz - q3 * gy); |
q2 += (qa * gy - qb * gz + q3 * gx); |
q3 += (qa * gz + qb * gy - qc * gx); |
// Normalise quaternion |
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
q0 /= norm; |
q1 /= norm; |
q2 /= norm; |
q3 /= norm; |
} |
/* |
* update quaternion |
*/ |
void mpu6050_updateQuaternion(void) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
double axg = 0; |
double ayg = 0; |
double azg = 0; |
double gxrs = 0; |
double gyrs = 0; |
double gzrs = 0; |
//get raw data |
while(1) { |
mpu6050_readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, (uint8_t *)buffer); |
if(buffer[0]) |
break; |
_delay_us(10); |
} |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
gxrs = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN*0.01745329; //degree to radians |
#else |
axg = (double)(ax)/MPU6050_AGAIN; |
ayg = (double)(ay)/MPU6050_AGAIN; |
azg = (double)(az)/MPU6050_AGAIN; |
gxrs = (double)(gx)/MPU6050_GGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy)/MPU6050_GGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz)/MPU6050_GGAIN*0.01745329; //degree to radians |
#endif |
//compute data |
mpu6050_mahonyUpdate(gxrs, gyrs, gzrs, axg, ayg, azg); |
} |
/* |
* update timer for attitude |
*/ |
ISR(TIMER0_OVF_vect) { |
mpu6050_updateQuaternion(); |
} |
/* |
* get quaternion |
*/ |
void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz) { |
*qw = q0; |
*qx = q1; |
*qy = q2; |
*qz = q3; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1); |
*pitch = -asin(2*q1*q3 + 2*q0*q2); |
*roll = atan2(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 - 1); |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/mpu6050.h |
---|
0,0 → 1,174 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- most of the code is a port of the arduino mpu6050 library by Jeff Rowberg |
https://github.com/jrowberg/i2cdevlib |
- Mahony complementary filter for attitude estimation |
http://www.x-io.co.uk |
*/ |
#ifndef MPU6050_H_ |
#define MPU6050_H_ |
#include <avr/io.h> |
#include "mpu6050registers.h" |
#ifdef __cplusplus |
extern "C" { |
#endif |
//i2c settings |
#define MPU6050_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define MPU6050_I2CINIT 0 //init i2c |
//definitions |
#define MPU6050_ADDR (0x68 <<1) //device address - 0x68 pin low (GND), 0x69 pin high (VCC) |
//enable the getattitude functions |
//because we do not have a magnetometer, we have to start the chip always in the same position |
//then to obtain your object attitude you have to apply the aerospace sequence |
//0 disabled |
//1 mahony filter |
//2 dmp chip processor |
#define MPU6050_GETATTITUDE 2 |
//definitions for raw data |
//gyro and acc scale |
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 |
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 |
#define MPU6050_GYRO_LSB_250 131.0 |
#define MPU6050_GYRO_LSB_500 65.5 |
#define MPU6050_GYRO_LSB_1000 32.8 |
#define MPU6050_GYRO_LSB_2000 16.4 |
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000 |
#endif |
#define MPU6050_ACCEL_LSB_2 16384.0 |
#define MPU6050_ACCEL_LSB_4 8192.0 |
#define MPU6050_ACCEL_LSB_8 4096.0 |
#define MPU6050_ACCEL_LSB_16 2048.0 |
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16 |
#endif |
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
#define MPU6050_AXOFFSET 0 |
#define MPU6050_AYOFFSET 0 |
#define MPU6050_AZOFFSET 0 |
#define MPU6050_AXGAIN 16384.0 |
#define MPU6050_AYGAIN 16384.0 |
#define MPU6050_AZGAIN 16384.0 |
#define MPU6050_GXOFFSET -42 |
#define MPU6050_GYOFFSET 9 |
#define MPU6050_GZOFFSET -29 |
#define MPU6050_GXGAIN 16.4 |
#define MPU6050_GYGAIN 16.4 |
#define MPU6050_GZGAIN 16.4 |
#endif |
//definitions for attitude 1 function estimation |
#if MPU6050_GETATTITUDE == 1 |
#error "GETATTITUDE == 1 is not supported!" |
//setup timer0 overflow event and define madgwickAHRSsampleFreq equal to timer0 frequency |
//timerfreq = (FCPU / prescaler) / timerscale |
// timerscale 8-bit = 256 |
// es. 61 = (16000000 / 1024) / 256 |
#define MPU6050_TIMER0INIT TCCR0B |=(1<<CS02)|(1<<CS00); TIMSK0 |=(1<<TOIE0); |
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz |
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain |
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain |
#endif |
#if MPU6050_GETATTITUDE == 2 |
//dmp definitions |
//packet size |
#define MPU6050_DMP_dmpPacketSize 42 |
//define INT0 rise edge interrupt |
#define MPU6050_DMP_INT0SETUP EICRA |= (1<<ISC01) | (1<<ISC00) |
//define enable and disable INT0 rise edge interrupt |
#define MPU6050_DMP_INT0DISABLE EIMSK &= ~(1<<INT0) |
#define MPU6050_DMP_INT0ENABLE EIMSK |= (1<<INT0) |
extern volatile uint8_t mpu6050_mpuInterrupt; |
#endif |
//functions |
extern void mpu6050_init(void); |
extern uint8_t mpu6050_testConnection(void); |
#if MPU6050_GETATTITUDE == 0 |
extern void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); |
extern void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds); |
#endif |
extern void mpu6050_setSleepDisabled(void); |
extern void mpu6050_setSleepEnabled(void); |
extern int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data); |
extern void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data); |
extern void mpu6050_writeByte(uint8_t regAddr, uint8_t data); |
extern int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data); |
extern void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); |
extern void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data); |
#if MPU6050_GETATTITUDE == 1 |
extern void mpu6050_updateQuaternion(void); |
extern void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double *pitch, double *roll, double *yaw); |
#endif |
#if MPU6050_GETATTITUDE == 2 |
extern void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data); |
extern void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank); |
extern void mpu6050_setMemoryStartAddress(uint8_t address); |
extern void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address); |
extern uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem); |
extern uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem); |
extern uint16_t mpu6050_getFIFOCount(void); |
extern void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length); |
extern uint8_t mpu6050_getIntStatus(void); |
extern void mpu6050_resetFIFO(); |
extern int8_t mpu6050_getXGyroOffset(void); |
extern void mpu6050_setXGyroOffset(int8_t offset); |
extern int8_t mpu6050_getYGyroOffset(void); |
extern void mpu6050_setYGyroOffset(int8_t offset); |
extern int8_t mpu6050_getZGyroOffset(void); |
extern void mpu6050_setZGyroOffset(int8_t offset); |
//base dmp |
extern uint8_t mpu6050_dmpInitialize(void); |
extern void mpu6050_dmpEnable(void); |
extern void mpu6050_dmpDisable(void); |
extern void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw); |
extern uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz); |
#endif |
#ifdef __cplusplus |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/mpu6050dmp6.c |
---|
0,0 → 1,495 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050dmp6.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, DMP-Routinen für IMU, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
/* |
* This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
*/ |
//to enable get roll, pitch and yaw function we include the math function |
//if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
//${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 2 |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include <math.h> //include libm |
#define MPU6050_DMP_CODE_SIZE 1929 |
#define MPU6050_DMP_CONFIG_SIZE 192 |
#define MPU6050_DMP_UPDATES_SIZE 47 |
volatile uint8_t mpu6050_mpuInterrupt = 0; |
uint8_t *dmpPacketBuffer; |
uint16_t mpu6050_fifoCount = 0; |
uint8_t mpu6050_mpuIntStatus = 0; |
uint8_t mpu6050_fifoBuffer[64]; |
/* ================================================================================================ * |
| Default MotionApps v2.0 42-byte FIFO packet structure: | |
| | |
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| | |
| [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
* ================================================================================================ */ |
// this block of memory gets written to the MPU on start-up, and it seems |
// to be volatile memory, so it has to be done each time (it only takes ~1 |
// second though) |
const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
// bank 0, 256 bytes |
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
// bank 1, 256 bytes |
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
// bank 2, 256 bytes |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
// bank 3, 256 bytes |
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
// bank 4, 256 bytes |
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
// bank 5, 256 bytes |
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
// bank 6, 256 bytes |
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
// bank 7, 138 bytes (remainder) |
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
}; |
const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
// BANK OFFSET LENGTH [DATA] |
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
// SPECIAL 0x01 = enable interrupts |
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate |
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
// It is important to make sure the host processor can keep up with reading and processing |
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
}; |
const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
0x01, 0xB2, 0x02, 0xFF, 0xFF, |
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
0x01, 0x6A, 0x02, 0x06, 0x00, |
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
0x01, 0x62, 0x02, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
}; |
/* |
* initialize mpu6050 dmp |
*/ |
uint8_t mpu6050_dmpInitialize() { |
//setup interrupt |
MPU6050_DMP_INT0SETUP; |
//reset |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
_delay_ms(30);//wait after reset |
//disable sleep mode |
mpu6050_setSleepDisabled(); |
//set memorybank to 0 |
mpu6050_setMemoryBank(0, 0, 0); |
//get X/Y/Z gyro offsets |
int8_t xgOffset = mpu6050_getXGyroOffset(); |
int8_t ygOffset = mpu6050_getYGyroOffset(); |
int8_t zgOffset = mpu6050_getZGyroOffset(); |
//setting slave 0 address to 0x7F |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
//disabling I2C Master mode |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
//setting slave 0 address to 0x68 (self) |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
//resetting I2C Master control |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
_delay_ms(20); |
//load DMP code into memory banks |
if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
//set clock source |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
//set DMP and FIFO_OFLOW interrupts enabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
//set sample rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
//set external frame sync to TEMP_OUT_L[0] |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set gyro sensitivity to +/- 2000 deg/sec |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
//set DMP configuration bytes (function unknown) |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
//clear OTP Bank flag |
mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
//set X/Y/Z gyro offsets to previous values |
//xgOffset = 0; |
//ygOffset = 0; |
zgOffset = 90; |
mpu6050_setXGyroOffset(xgOffset); |
mpu6050_setYGyroOffset(ygOffset); |
mpu6050_setZGyroOffset(zgOffset); |
//set X/Y/Z gyro user offsets to zero |
mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
//writing final memory update 1/7 (function unknown) |
uint8_t dmpUpdate[16], j; |
uint16_t pos = 0; |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 2/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reset FIFO |
mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
//reading FIFO count |
uint8_t fifoCount = mpu6050_getFIFOCount(); |
uint8_t fifoBuffer[128]; |
//current FIFO count |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
//setting motion detection threshold to 2 |
mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
//setting zero-motion detection threshold to 156 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
//setting motion detection duration to 80 |
mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
//setting zero-motion detection duration to 0 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
//reset FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
//enabling FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
//enabling DMP |
mpu6050_dmpEnable(); |
//resetting DMP |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//writing final memory update 3/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 4/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 5/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reading FIFO data...")); |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//reading final memory update 6/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//reading FIFO data |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//writing final memory update 7/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//disabling DMP (you turn it on later) |
mpu6050_dmpDisable(); |
//resetting FIFO and clearing INT status one last time |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} else { |
return 2; // configuration block loading failed |
} |
} else { |
return 1; // main binary block loading failed |
} |
return 0; // success |
} |
/* |
* enable dmp |
*/ |
void mpu6050_dmpEnable() { |
MPU6050_DMP_INT0ENABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
} |
/* |
* disable dmp |
*/ |
void mpu6050_dmpDisable() { |
MPU6050_DMP_INT0DISABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
} |
/* |
* get quaternion from packet |
*/ |
void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
if (packet == 0) packet = dmpPacketBuffer; |
*qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
*qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
*qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
*qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
*pitch = -asin(2*qx*qz + 2*qw*qy); |
*roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
} |
/* |
* get quaternion and wait |
*/ |
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
//reset interrupt |
mpu6050_mpuInterrupt = 0; |
//check for overflow |
mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
//reset |
mpu6050_resetFIFO(); |
} else if (mpu6050_mpuIntStatus & 0x02) { |
//wait for correct available data length, should be a VERY short wait |
while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
//read a packet from FIFO |
mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
//get quaternion |
mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
return 1; |
} |
return 0; |
} |
/* |
* on interrupt set data availabe |
*/ |
ISR (INT0_vect) { |
mpu6050_mpuInterrupt = 1; |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/mpu6050registers.h |
---|
0,0 → 1,365 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#ifndef MPU6050REGISTERS_H_ |
#define MPU6050REGISTERS_H_ |
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN |
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN |
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN |
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS |
#define MPU6050_RA_XA_OFFS_L_TC 0x07 |
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS |
#define MPU6050_RA_YA_OFFS_L_TC 0x09 |
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS |
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B |
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR |
#define MPU6050_RA_XG_OFFS_USRL 0x14 |
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR |
#define MPU6050_RA_YG_OFFS_USRL 0x16 |
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR |
#define MPU6050_RA_ZG_OFFS_USRL 0x18 |
#define MPU6050_RA_SMPLRT_DIV 0x19 |
#define MPU6050_RA_CONFIG 0x1A |
#define MPU6050_RA_GYRO_CONFIG 0x1B |
#define MPU6050_RA_ACCEL_CONFIG 0x1C |
#define MPU6050_RA_FF_THR 0x1D |
#define MPU6050_RA_FF_DUR 0x1E |
#define MPU6050_RA_MOT_THR 0x1F |
#define MPU6050_RA_MOT_DUR 0x20 |
#define MPU6050_RA_ZRMOT_THR 0x21 |
#define MPU6050_RA_ZRMOT_DUR 0x22 |
#define MPU6050_RA_FIFO_EN 0x23 |
#define MPU6050_RA_I2C_MST_CTRL 0x24 |
#define MPU6050_RA_I2C_SLV0_ADDR 0x25 |
#define MPU6050_RA_I2C_SLV0_REG 0x26 |
#define MPU6050_RA_I2C_SLV0_CTRL 0x27 |
#define MPU6050_RA_I2C_SLV1_ADDR 0x28 |
#define MPU6050_RA_I2C_SLV1_REG 0x29 |
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A |
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B |
#define MPU6050_RA_I2C_SLV2_REG 0x2C |
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D |
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E |
#define MPU6050_RA_I2C_SLV3_REG 0x2F |
#define MPU6050_RA_I2C_SLV3_CTRL 0x30 |
#define MPU6050_RA_I2C_SLV4_ADDR 0x31 |
#define MPU6050_RA_I2C_SLV4_REG 0x32 |
#define MPU6050_RA_I2C_SLV4_DO 0x33 |
#define MPU6050_RA_I2C_SLV4_CTRL 0x34 |
#define MPU6050_RA_I2C_SLV4_DI 0x35 |
#define MPU6050_RA_I2C_MST_STATUS 0x36 |
#define MPU6050_RA_INT_PIN_CFG 0x37 |
#define MPU6050_RA_INT_ENABLE 0x38 |
#define MPU6050_RA_DMP_INT_STATUS 0x39 |
#define MPU6050_RA_INT_STATUS 0x3A |
#define MPU6050_RA_ACCEL_XOUT_H 0x3B |
#define MPU6050_RA_ACCEL_XOUT_L 0x3C |
#define MPU6050_RA_ACCEL_YOUT_H 0x3D |
#define MPU6050_RA_ACCEL_YOUT_L 0x3E |
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F |
#define MPU6050_RA_ACCEL_ZOUT_L 0x40 |
#define MPU6050_RA_TEMP_OUT_H 0x41 |
#define MPU6050_RA_TEMP_OUT_L 0x42 |
#define MPU6050_RA_GYRO_XOUT_H 0x43 |
#define MPU6050_RA_GYRO_XOUT_L 0x44 |
#define MPU6050_RA_GYRO_YOUT_H 0x45 |
#define MPU6050_RA_GYRO_YOUT_L 0x46 |
#define MPU6050_RA_GYRO_ZOUT_H 0x47 |
#define MPU6050_RA_GYRO_ZOUT_L 0x48 |
#define MPU6050_RA_EXT_SENS_DATA_00 0x49 |
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A |
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B |
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C |
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D |
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E |
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F |
#define MPU6050_RA_EXT_SENS_DATA_07 0x50 |
#define MPU6050_RA_EXT_SENS_DATA_08 0x51 |
#define MPU6050_RA_EXT_SENS_DATA_09 0x52 |
#define MPU6050_RA_EXT_SENS_DATA_10 0x53 |
#define MPU6050_RA_EXT_SENS_DATA_11 0x54 |
#define MPU6050_RA_EXT_SENS_DATA_12 0x55 |
#define MPU6050_RA_EXT_SENS_DATA_13 0x56 |
#define MPU6050_RA_EXT_SENS_DATA_14 0x57 |
#define MPU6050_RA_EXT_SENS_DATA_15 0x58 |
#define MPU6050_RA_EXT_SENS_DATA_16 0x59 |
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A |
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B |
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C |
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D |
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E |
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F |
#define MPU6050_RA_EXT_SENS_DATA_23 0x60 |
#define MPU6050_RA_MOT_DETECT_STATUS 0x61 |
#define MPU6050_RA_I2C_SLV0_DO 0x63 |
#define MPU6050_RA_I2C_SLV1_DO 0x64 |
#define MPU6050_RA_I2C_SLV2_DO 0x65 |
#define MPU6050_RA_I2C_SLV3_DO 0x66 |
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 |
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 |
#define MPU6050_RA_MOT_DETECT_CTRL 0x69 |
#define MPU6050_RA_USER_CTRL 0x6A |
#define MPU6050_RA_PWR_MGMT_1 0x6B |
#define MPU6050_RA_PWR_MGMT_2 0x6C |
#define MPU6050_RA_BANK_SEL 0x6D |
#define MPU6050_RA_MEM_START_ADDR 0x6E |
#define MPU6050_RA_MEM_R_W 0x6F |
#define MPU6050_RA_DMP_CFG_1 0x70 |
#define MPU6050_RA_DMP_CFG_2 0x71 |
#define MPU6050_RA_FIFO_COUNTH 0x72 |
#define MPU6050_RA_FIFO_COUNTL 0x73 |
#define MPU6050_RA_FIFO_R_W 0x74 |
#define MPU6050_RA_WHO_AM_I 0x75 |
#define MPU6050_TC_PWR_MODE_BIT 7 |
#define MPU6050_TC_OFFSET_BIT 6 |
#define MPU6050_TC_OFFSET_LENGTH 6 |
#define MPU6050_TC_OTP_BNK_VLD_BIT 0 |
#define MPU6050_VDDIO_LEVEL_VLOGIC 0 |
#define MPU6050_VDDIO_LEVEL_VDD 1 |
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 |
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 |
#define MPU6050_CFG_DLPF_CFG_BIT 2 |
#define MPU6050_CFG_DLPF_CFG_LENGTH 3 |
#define MPU6050_EXT_SYNC_DISABLED 0x0 |
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 |
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 |
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 |
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 |
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 |
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 |
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 |
#define MPU6050_DLPF_BW_256 0x00 |
#define MPU6050_DLPF_BW_188 0x01 |
#define MPU6050_DLPF_BW_98 0x02 |
#define MPU6050_DLPF_BW_42 0x03 |
#define MPU6050_DLPF_BW_20 0x04 |
#define MPU6050_DLPF_BW_10 0x05 |
#define MPU6050_DLPF_BW_5 0x06 |
#define MPU6050_GCONFIG_FS_SEL_BIT 4 |
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 |
#define MPU6050_GYRO_FS_250 0x00 |
#define MPU6050_GYRO_FS_500 0x01 |
#define MPU6050_GYRO_FS_1000 0x02 |
#define MPU6050_GYRO_FS_2000 0x03 |
#define MPU6050_ACONFIG_XA_ST_BIT 7 |
#define MPU6050_ACONFIG_YA_ST_BIT 6 |
#define MPU6050_ACONFIG_ZA_ST_BIT 5 |
#define MPU6050_ACONFIG_AFS_SEL_BIT 4 |
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 |
#define MPU6050_ACCEL_FS_2 0x00 |
#define MPU6050_ACCEL_FS_4 0x01 |
#define MPU6050_ACCEL_FS_8 0x02 |
#define MPU6050_ACCEL_FS_16 0x03 |
#define MPU6050_DHPF_RESET 0x00 |
#define MPU6050_DHPF_5 0x01 |
#define MPU6050_DHPF_2P5 0x02 |
#define MPU6050_DHPF_1P25 0x03 |
#define MPU6050_DHPF_0P63 0x04 |
#define MPU6050_DHPF_HOLD 0x07 |
#define MPU6050_TEMP_FIFO_EN_BIT 7 |
#define MPU6050_XG_FIFO_EN_BIT 6 |
#define MPU6050_YG_FIFO_EN_BIT 5 |
#define MPU6050_ZG_FIFO_EN_BIT 4 |
#define MPU6050_ACCEL_FIFO_EN_BIT 3 |
#define MPU6050_SLV2_FIFO_EN_BIT 2 |
#define MPU6050_SLV1_FIFO_EN_BIT 1 |
#define MPU6050_SLV0_FIFO_EN_BIT 0 |
#define MPU6050_MULT_MST_EN_BIT 7 |
#define MPU6050_WAIT_FOR_ES_BIT 6 |
#define MPU6050_SLV_3_FIFO_EN_BIT 5 |
#define MPU6050_I2C_MST_P_NSR_BIT 4 |
#define MPU6050_I2C_MST_CLK_BIT 3 |
#define MPU6050_I2C_MST_CLK_LENGTH 4 |
#define MPU6050_CLOCK_DIV_348 0x0 |
#define MPU6050_CLOCK_DIV_333 0x1 |
#define MPU6050_CLOCK_DIV_320 0x2 |
#define MPU6050_CLOCK_DIV_308 0x3 |
#define MPU6050_CLOCK_DIV_296 0x4 |
#define MPU6050_CLOCK_DIV_286 0x5 |
#define MPU6050_CLOCK_DIV_276 0x6 |
#define MPU6050_CLOCK_DIV_267 0x7 |
#define MPU6050_CLOCK_DIV_258 0x8 |
#define MPU6050_CLOCK_DIV_500 0x9 |
#define MPU6050_CLOCK_DIV_471 0xA |
#define MPU6050_CLOCK_DIV_444 0xB |
#define MPU6050_CLOCK_DIV_421 0xC |
#define MPU6050_CLOCK_DIV_400 0xD |
#define MPU6050_CLOCK_DIV_381 0xE |
#define MPU6050_CLOCK_DIV_364 0xF |
#define MPU6050_I2C_SLV_RW_BIT 7 |
#define MPU6050_I2C_SLV_ADDR_BIT 6 |
#define MPU6050_I2C_SLV_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV_EN_BIT 7 |
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 |
#define MPU6050_I2C_SLV_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV_GRP_BIT 4 |
#define MPU6050_I2C_SLV_LEN_BIT 3 |
#define MPU6050_I2C_SLV_LEN_LENGTH 4 |
#define MPU6050_I2C_SLV4_RW_BIT 7 |
#define MPU6050_I2C_SLV4_ADDR_BIT 6 |
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV4_EN_BIT 7 |
#define MPU6050_I2C_SLV4_INT_EN_BIT 6 |
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 |
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 |
#define MPU6050_MST_PASS_THROUGH_BIT 7 |
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 |
#define MPU6050_MST_I2C_LOST_ARB_BIT 5 |
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 |
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 |
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 |
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 |
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 |
#define MPU6050_INTCFG_INT_LEVEL_BIT 7 |
#define MPU6050_INTCFG_INT_OPEN_BIT 6 |
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 |
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 |
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 |
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 |
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 |
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 |
#define MPU6050_INTMODE_ACTIVEHIGH 0x00 |
#define MPU6050_INTMODE_ACTIVELOW 0x01 |
#define MPU6050_INTDRV_PUSHPULL 0x00 |
#define MPU6050_INTDRV_OPENDRAIN 0x01 |
#define MPU6050_INTLATCH_50USPULSE 0x00 |
#define MPU6050_INTLATCH_WAITCLEAR 0x01 |
#define MPU6050_INTCLEAR_STATUSREAD 0x00 |
#define MPU6050_INTCLEAR_ANYREAD 0x01 |
#define MPU6050_INTERRUPT_FF_BIT 7 |
#define MPU6050_INTERRUPT_MOT_BIT 6 |
#define MPU6050_INTERRUPT_ZMOT_BIT 5 |
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 |
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 |
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 |
#define MPU6050_INTERRUPT_DMP_INT_BIT 1 |
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 |
// TODO: figure out what these actually do |
// UMPL source code is not very obivous |
#define MPU6050_DMPINT_5_BIT 5 |
#define MPU6050_DMPINT_4_BIT 4 |
#define MPU6050_DMPINT_3_BIT 3 |
#define MPU6050_DMPINT_2_BIT 2 |
#define MPU6050_DMPINT_1_BIT 1 |
#define MPU6050_DMPINT_0_BIT 0 |
#define MPU6050_MOTION_MOT_XNEG_BIT 7 |
#define MPU6050_MOTION_MOT_XPOS_BIT 6 |
#define MPU6050_MOTION_MOT_YNEG_BIT 5 |
#define MPU6050_MOTION_MOT_YPOS_BIT 4 |
#define MPU6050_MOTION_MOT_ZNEG_BIT 3 |
#define MPU6050_MOTION_MOT_ZPOS_BIT 2 |
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 |
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 |
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 |
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 |
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 |
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 |
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 |
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 |
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 |
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 |
#define MPU6050_DETECT_FF_COUNT_BIT 3 |
#define MPU6050_DETECT_FF_COUNT_LENGTH 2 |
#define MPU6050_DETECT_MOT_COUNT_BIT 1 |
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 |
#define MPU6050_DETECT_DECREMENT_RESET 0x0 |
#define MPU6050_DETECT_DECREMENT_1 0x1 |
#define MPU6050_DETECT_DECREMENT_2 0x2 |
#define MPU6050_DETECT_DECREMENT_4 0x3 |
#define MPU6050_USERCTRL_DMP_EN_BIT 7 |
#define MPU6050_USERCTRL_FIFO_EN_BIT 6 |
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 |
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 |
#define MPU6050_USERCTRL_DMP_RESET_BIT 3 |
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 |
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 |
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 |
#define MPU6050_PWR1_DEVICE_RESET_BIT 7 |
#define MPU6050_PWR1_SLEEP_BIT 6 |
#define MPU6050_PWR1_CYCLE_BIT 5 |
#define MPU6050_PWR1_TEMP_DIS_BIT 3 |
#define MPU6050_PWR1_CLKSEL_BIT 2 |
#define MPU6050_PWR1_CLKSEL_LENGTH 3 |
#define MPU6050_CLOCK_INTERNAL 0x00 |
#define MPU6050_CLOCK_PLL_XGYRO 0x01 |
#define MPU6050_CLOCK_PLL_YGYRO 0x02 |
#define MPU6050_CLOCK_PLL_ZGYRO 0x03 |
#define MPU6050_CLOCK_PLL_EXT32K 0x04 |
#define MPU6050_CLOCK_PLL_EXT19M 0x05 |
#define MPU6050_CLOCK_KEEP_RESET 0x07 |
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 |
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 |
#define MPU6050_PWR2_STBY_XA_BIT 5 |
#define MPU6050_PWR2_STBY_YA_BIT 4 |
#define MPU6050_PWR2_STBY_ZA_BIT 3 |
#define MPU6050_PWR2_STBY_XG_BIT 2 |
#define MPU6050_PWR2_STBY_YG_BIT 1 |
#define MPU6050_PWR2_STBY_ZG_BIT 0 |
#define MPU6050_WAKE_FREQ_1P25 0x0 |
#define MPU6050_WAKE_FREQ_2P5 0x1 |
#define MPU6050_WAKE_FREQ_5 0x2 |
#define MPU6050_WAKE_FREQ_10 0x3 |
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 |
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 |
#define MPU6050_BANKSEL_MEM_SEL_BIT 4 |
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 |
#define MPU6050_WHO_AM_I_BIT 6 |
#define MPU6050_WHO_AM_I_LENGTH 6 |
#define MPU6050_DMP_MEMORY_BANKS 8 |
#define MPU6050_DMP_MEMORY_BANK_SIZE 256 |
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/sensor.c |
---|
0,0 → 1,30 |
/* |
* sensor.c |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY sensor.c |
//# |
//# 19.09.2015 cebra |
//# - add: Routinen für das GY-87 Sensorboard zur Berechnung der Kompasswerte etc. |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include "mpu6050.h" |
#include "sensor.h" |
void Init_Sensors(void) |
{ |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF/sensor.h |
---|
0,0 → 1,15 |
/* |
* sensor.h |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
#ifndef SENSOR_H_ |
#define SENSOR_H_ |
#endif /* 10DOF_SENSOR_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/10DOF |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+_doc |
/Transportables_Koptertool/PKT/trunk/HAL_HW3_9.c |
---|
0,0 → 1,562 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.c |
//# |
//# |
//# 10.11.2014 cebra |
//# - chg: Displaybeleuchtung aktiv einschalten beim Start |
//# |
//# 14.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf PKT_TitlePKTlipo(false) da eine |
//# richtige LiPo-Anzeige im Startbildschirm zu diesem Zeitpunkt noch |
//# nicht moeglich ist |
//# - chg: InitHWPorts() - bt_init() umgestellt auf BTM222_Initalize() |
//# |
//# 11.06.2014 OG |
//# - fix: InitHWPorts() einstellen der Sprache geht jetzt wieder |
//# |
//# 10.06.2014 OG |
//# - chg: InitHWPorts() - umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - chg: InitHWPorts() - Aufruf von InitWi232() geaendert |
//# |
//# 28.05.2014 OG |
//# - chg: InitHWPorts() - Bearbeiten der Sprache auskommentiert weil sich |
//# Edit_generic() geaendert hat und notwendige Texte aus einem Menue |
//# bezieht welches hier in der HAL nicht vorhanden ist |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 24.03.2014 OG |
//# - chg: InitHWPorts(): den Begriff "WiFly" bzw. "WiFly Patch" geaendert |
//# zu "SV2 Patch" |
//# |
//# 21.02.2014 OG |
//# - chg: InitHWPorts() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: InitHWPorts() Anzeige vom Startscreen ueberarbeitet |
//# - chg: Code-Formatierung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING vor Servo_init |
//# |
//# 07.07.2013 OG |
//# - add: DEBUG_SV2_EXTENSION in InitHWPorts() bzgl. Anzeige von "A:.. B:.." usw. |
//# |
//# 26.06.2013 OG |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 26.06.2013 Cebra |
//# - add: Modulumschaltung für WiFlypatch umgebaut |
//# - chg: Wartezeit nach Einschalten von BT auf 3 Sekunden verlängert |
//# |
//# 22.06.2013 cebra |
//# - chg: Fehler in CheckPCB führte zu fehlerhafter Lipomessung, alle Pins von Port A waren auf Eingang |
//# |
//# 13.06.2013 cebra |
//# - chg: Displaybeleuchtung ohne Helligkeitssteuerung |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 28.04.2013 OG |
//# - add: Unterstuetzung von define USE_FONT_BIG (Layoutanpassung) |
//# |
//# 12.04.2013 Cebra |
//# - chg: Fehler bei der Umschaltung Wi232/Kabelverbindung beim Start des PKT |
//########################################################################### |
#ifndef HAL_HW3_9_C_ |
#define HAL_HW3_9_C_ |
#include "cpu.h" |
#include <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include "main.h" |
//#if defined HWVERSION3_9 |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "uart/uart1.h" |
#include "timer/timer.h" |
#include "wi232/Wi232.h" |
//#include "i2cmaster.h" |
#include "bluetooth/bluetooth.h" |
#include "bluetooth/error.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "pkt/pkt.h" |
#include "setup/setup.h" |
#include "osd/osd.h" |
#include "sound/pwmsine8bit.h" |
#include "tracking/ng_servo.h" |
volatile uint8_t USBBT; |
volatile uint8_t U02SV2; |
volatile uint8_t PCB_Version; |
volatile uint8_t PCB_SV2RxTxPatch; |
uint8_t PortA; |
uint8_t PortB; |
uint8_t PortC; |
uint8_t PortD; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void CheckPCB(void) |
{ |
// |
DDRA = 0x00; |
DDRB = 0x00; |
DDRC = 0x00; |
DDRD = 0x00; |
PortA = PINA; |
PortB = PINB; |
PortC = PINC; |
PortD = PIND; |
DDRC = 0x00; |
PORTC &= ~(1<<PORTC7); // Pullup setzen |
if (PINC & (1<<PINC7) ) PCB_Version = PKT39m; else PCB_Version = PKT39x; |
PORTC &= ~(1<<PORTC0); // Pullup setzen |
if (PINC & (1<<PINC0) ) PCB_SV2RxTxPatch = false; else PCB_SV2RxTxPatch = true; |
PORTC |= (1<<PORTC0); // Pullup aus |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void InitHWPorts( void ) // Initialisierung der Hardware für die jeweilige Leiterplattenversion |
{ |
int8_t yoffs; |
CheckPCB(); // Version der Leiterplatte prüfen |
if( PCB_SV2RxTxPatch ) |
{ |
DDRC |= (1<<DDC0)|(1<<DDC1); // Ausgang: C0 Reset BT, C1 Reset WiFly |
PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
} |
PORTA |= (1<<PORTA4)|(1<<PORTA5)|(1<<PORTA6)|(1<<PORTA7); // Enable Pull Up for the 4 keys |
DDRA &= ~(1<<DDA4); // Eingang: A4 auf Low setzen (Power On) |
if( Config.Lipomessung == true ) |
{ |
DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
} |
DDRB = 0xFF; // alles Ausgaenge |
PORTC |= (1<<PORTC4)|(1<<PORTC7); // Enable Pull Up for LBO + Summer |
DDRC |= (1<<DDC2)|(1<<DDC3)|(1<<DDC5)|(1<<DDC6|(1<<DDC7)); // Ausgang: Led2,Rs232Switch,Summer |
DDRC &= ~(1<<DDC4); // Eingang: LowBat LTC1308 |
if( PCB_SV2RxTxPatch ) clr_USBOn(); |
else _BTOn(); // Erstmal USB dektivieren, damit beim versehentlichen Einschalten USB im PC ruhig bleibt |
DDRD |= (1<<DDD4)|(1<<DDD5)|(1<<DDD6)|(1<<DDD7); // Ausgang: PiepserTest, Servo, Wi232-CMD und Beleuchtung |
PORTD |= (1<<PORTD6); // Wi232-CMD auf High schalten |
set_D_LIGHT(); // Displaybeleuchtung an |
set_V_On(); // Spannung mit T3 halten |
Timer0_Init(); // system |
Display_on = 1; |
USART_Init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
uart1_init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); // USB |
// I2C_Init(1); |
// i2c_init(); |
LCD_Init(0); // muss vor "ReadParameter" stehen |
ReadParameter(); // aktuelle Werte aus EEProm auslesen |
#ifdef USE_TRACKING |
servoInit(SERVO_PERIODE); |
#endif |
//servoSetDefaultPos(); |
sei(); |
Old_Baudrate = Config.PKT_Baudrate; |
SetBaudUart1( Config.PKT_Baudrate ); |
SetBaudUart0( Config.PKT_Baudrate ); |
// 10.11.2015 cebra |
// TODO cebra: diese Stelle muss bei HW_Sound überarbeitet werden |
// if( (Config.HWSound==0) && (PCB_Version == PKT39m) ) |
// { |
// Timer2_Init(); // Displaybeleuchtung oder Soundmodul |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
// } |
#ifdef USE_SOUND |
if (Config.HWSound==1) InitSound(); |
#endif |
LCD_Init(1); |
set_beep ( 400, 0x0080, BeepNormal); |
OSD_active = false; // keine OSD Ausgabe |
ADC_Init(); // ADC für Lipomessung// MartinR: verschoben |
//------------------------- |
// beim ersten Start (oder nach Reset) Sprache abfragen |
//------------------------- |
if( Config.DisplayLanguage > NUM_LANG ) |
{ |
Config.DisplayLanguage = 1; |
//Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage, 0, 1, Language,1, NULL,NULL); // OG: ?? hierbei wird die falsche Sprache angezeigt - warum? |
Config.DisplayLanguage = Edit_generic( 2, 0, 1, Language,1, NULL,NULL); |
WriteParameter(); |
} |
//------------------------- |
// Anzeige Start-Screen |
//------------------------- |
PKT_TitlePKTlipo( false ); // Titel mit PKT-Version anzeigen (und clearscreen) |
yoffs = 11; |
if( PCB_SV2RxTxPatch ) |
{ |
lcdx_printp_center( 2, PSTR("SV2 Patch"), MNORMAL, 0,7); // "SV2 Patch" (ehemals "Wifly Patch") |
yoffs = 5; |
} |
if( PCB_Version==PKT39m ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9m (09.2011)"), MNORMAL, 0,yoffs); |
} |
if( PCB_Version==PKT39x ) |
{ |
lcdx_printp_center( 1, PSTR("HW 3.9x (11.2012)"), MNORMAL, 0,yoffs); |
} |
lcdx_printp_center( 5, strGet(BOOT1), MNORMAL, 0,2); // "Taste 1 Sekunde" |
lcdx_printp_center( 6, strGet(BOOT2), MNORMAL, 0,4); // "festhalten!" |
lcd_rect_round( 0, 5*8-2, 127, 2*8+8, 1, R2); // Rahmen |
//------------------------- |
//------------------------- |
#ifdef DEBUG_SV2_EXTENSION |
lcd_printpns_at (0, 7, PSTR("A:"),0); |
lcd_print_hex(PortA,0); |
lcd_printpns_at (5, 7, PSTR("B:"),0); |
lcd_print_hex(PortB,0); |
lcd_printpns_at (10, 7, PSTR("C:"),0); |
lcd_print_hex(PortC,0); |
lcd_printpns_at (15, 7, PSTR("D:"),0); |
lcd_print_hex(PortD,0); |
#endif |
//_delay_ms(800); |
_delay_ms(900); |
if (PINA & (1<<PINA7)) // Spannung eingeschaltet lassen |
clr_V_On(); |
_delay_ms(100); |
set_beep ( 400, 0x0080, BeepNormal); |
get_key_press(KEY_ALL); |
#ifdef USE_SOUND |
if (Config.HWSound==1); |
{ |
playTone(505,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(535,100,Config.Volume); |
playTone(525,100,Config.Volume); |
playTone(515,100,Config.Volume); |
playTone(505,100,Config.Volume); |
} |
#endif |
//lcd_cls(); // 20.02.2014 OG |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
// _delay_ms(250); |
// servoSetPosition(1,0); |
// _delay_ms(250); |
// servoSetPosition(0,0); |
// _delay_ms(250); |
// servoSetPosition(1,400); |
// _delay_ms(250); |
// servoSetPosition(0,400); |
set_BTOff(); |
//set_Modul_On(USB); |
if( (Config.UseWi == true) && (Config.WiIsSet == false) ) |
{ |
Wi232_Initalize(); // wenn Wi232 nicht initialisiert ist, dann jetzt tun |
} |
#ifdef USE_BLUETOOTH |
if ((Config.UseBT == true) && (Config.BTIsSet == false)) // BT ausschalten |
{ |
//lcd_cls(); |
BTM222_Initalize(); |
// set_USBOn(); |
} |
bt_start(); |
#endif // end: #ifdef USE_BLUETOOTH |
if( (Config.UseWi == true) && (Config.U02SV2 == 0) ) |
{ |
Change_Output(Uart02Wi); // Verbindung zu Wi232 herstellen |
} |
else |
{ |
Change_Output(Uart02FC); // Verbindung zu SV" (Kabel) herstellen |
} |
} // InitHWPorts() |
//#define set_BEEP() (PORTC &= ~(1 << Summer)) // Summer |
//#define clr_BEEP() (PORTC |= (1 << Summer)) |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC &= ~(1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD &= ~(1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_BEEP(void) |
{ |
if (PCB_Version == PKT39m) |
PORTC |= (1 << Summer); // Summer |
if (PCB_Version == PKT39x) |
PORTD |= (1 << SummerV2); // Summer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_D_LIGHT(void) /* Displaybeleuchtung ein*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM einschalten |
// TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); |
// TCCR2B |= (1 << CS20); |
// } |
// else |
clr_DISPLAYLIGHTV2(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clr_D_LIGHT(void) /* Displaybeleuchtung aus*/ |
{ |
if (PCB_Version == PKT39m) |
{ |
// if (Config.HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHT(); |
} |
if (PCB_Version == PKT39x) |
{ |
// if (HWSound==0) |
// { |
// // PWM ausschalten |
// TCCR2A = 0; |
// TCCR2B = 0; |
// } |
// else |
set_DISPLAYLIGHTV2(); |
} |
} |
uint8_t BTIsOn=0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_Modul_On(uint8_t Modul) // Umschaltfunktionen für Leiterplatte 3.9x mit WiFlypatch |
{ |
if (PCB_SV2RxTxPatch) PORTC &= ~((1<<BT_Reset)|(1<<WiFly_Reset)|(1<<USB_Reset)); // Erst alle Module ausschalten |
switch (Modul) |
{ |
case USB: if (PCB_SV2RxTxPatch) set_USBOn(); |
else set_USBOn(); |
BTIsOn = 0; |
break; |
case Bluetooth: if (PCB_SV2RxTxPatch) set_BTM222On(); |
else _BTOn(); |
if (BTIsOn == 0) |
{ |
BTIsOn = 1; |
_delay_ms(3000); // wait for BTM222 initialisation |
} |
break; |
case Wlan: set_WiFlyOn(); |
BTIsOn = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOn(void) |
{ |
if (BTIsOn == 0) |
{ |
if (PCB_SV2RxTxPatch) set_Modul_On(Bluetooth); |
else |
{ |
_BTOn(); |
BTIsOn = 1; |
_delay_ms(3000); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_BTOff(void) |
{ |
if( PCB_SV2RxTxPatch ) |
set_Modul_On(USB); |
else |
{ |
set_USBOn(); |
BTIsOn = 0; |
} |
} |
#endif |
//#endif // HAL_HW3_9_C_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/HAL_HW3_9.h |
---|
0,0 → 1,194 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY HAL_HW3_9.h |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 26.06.2013 Cebra |
//# - add: defines für WyFlypatch Port C0 + C1 |
//# |
//# 17.05.2013 OG |
//# - add: defines: KEY_ENTER_LONG, KEY_ESC_LONG, KEY_PLUS_LONG, KEY_MINUS_LONG |
//############################################################################ |
#ifndef HAL_HW3_9_H_ |
#define HAL_HW3_9_H_ |
//#define PIEPSER_NERVT // Summer zu Testzwecken ganz Ausschalten |
#define PKT39m 1 |
#define PKT39x 2 |
#define USB 1 |
#define Bluetooth 2 |
#define Wlan 3 |
// Hardware 3.9 Portbelegung |
#define KEY_PIN PINA // Port A als Input |
#define Touch0 PA0 // Pin 37 |
#define Touch1 PA1 // Pin 36 |
#define Touch2 PA2 // Pin 35 |
#define Touch3 PA3 // Pin 34 |
#define KEY_EXT PINA3 // MartinR |
#define Key1 PA4 // Pin 33 |
#define Key2 PA5 // Pin 32 |
#define Key3 PA6 // Pin 31 |
#define Key4 PA7 // Pin 30 |
#define USB2Wi PB0 // Pin 40 aktiv low > IC5 |
#define VoltageHold PB1 // Pin 41 High = Spannung T3 halten |
#define Display_Reset PB2 // Pin 42 |
#define Display_A0 PB3 // Pin 43 |
#define Display_CS PB4 // Pin 44 |
#define Display_SI PB5 // Pin 1 |
#define LED1 PB6 // Pin 2 Low = LED1 (nicht benutzbar wegen SCL |
#define Display_SCL PB7 // Pin 3 |
#define I2C_SCL PC0 // Pin 19 SCL |
#define I2C_CDA PC1 // Pin 20 SDA |
#define BT_Reset PC0 // Pin 19 Reseteingang BTM222 / WiyFlypatch |
#define WiFly_Reset PC1 // Pin 20 Reseteingang WyFly / WiyFlypatch |
#define USB2FC PC2 // Pin 21 aktiv low > IC5 |
#define USB_BT PC3 // Pin 22 high = USB, Low = Bluetooth, LED2 |
#define USB_Reset PC3 // Pin 22 Reseteingang USB FT232 / WiFlypatch |
#define LowBat PC4 // Pin 23 Low Bat Warning Lipo PKT,Input |
#define Uart02Wi PC5 // Pin 24 aktiv Low > IC4 |
#define Uart02FC PC6 // Pin 25 aktiv Low > IC4 |
#define Summer PC7 // Pin 26 Low = Summer |
#define DisplaybeleuchtungV2 PC7 // Pin 26 High = Display-LED PCB 3.9x |
#define Uart0RxD PD0 // Pin 9 über IC4 =Wi | SV2 |
#define Uart0TxD PD1 // Pin 10 über IC4 =Wi | SV2 |
#define Uart1RxD PD2 // Pin 11 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define Uart1TxD PD3 // Pin 12 direkt = USB, BTM, über IC5 = Wi | SV2 |
#define SERVO2 PD4 // Pin 13 PWM Servo 2 |
#define SERVO1 PD5 // Pin 14 PWM Servo 1 |
#define Wi232_CMD PD6 // Pin 15 aktiv Low = Wi232 CMD |
#define Displaybeleuchtung PD7 // Pin 16 High = Display-LED PCB 3.9m |
#define SummerV2 PD7 // Pin 16 Low = Summer, PCB 3.9x |
#define KEY_ENTER Key1 |
#define KEY_ESC Key2 |
#define KEY_PLUS Key3 |
#define KEY_MINUS Key4 |
#define KEY_NONE 0 // keine Taste (menuctrl) |
#define KEY_ENTER_LONG KEY_ENTER+100 // for long press keys used in scrollbox & menuctrl |
#define KEY_ESC_LONG KEY_ESC+100 |
#define KEY_PLUS_LONG KEY_PLUS+100 |
#define KEY_MINUS_LONG KEY_MINUS+100 |
// |= schaltet Ausgang auf HIGH |
// &= ~ schaltet Ausgang auf LOW |
#define set_reset() (PORTB |= (1 << Display_Reset)) |
#define clr_reset() (PORTB &= ~(1 << Display_Reset)) |
#define set_A0() (PORTB |= (1 << Display_A0)) |
#define clr_A0() (PORTB &= ~(1 << Display_A0)) |
#define set_cs() (PORTB |= (1 << Display_CS)) |
#define clr_cs() (PORTB &= ~(1 << Display_CS)) |
#define set_si() (PORTB |= (1 << Display_SI)) |
#define clr_si() (PORTB &= ~(1 << Display_SI)) |
#define set_scl() (PORTB |= (1 << Display_SCL)) |
#define clr_scl() (PORTB &= ~(1 << Display_SCL)) |
#define _BTOn() (PORTC &= ~(1 << USB_BT)) // Bluetooth ein |
#define set_USBOn() (PORTC |= (1 << USB_Reset)) // USB ein |
#define clr_USBOn() (PORTC &= ~(1 << USB_Reset)) // USB aus |
#define set_BTM222On() (PORTC |= (1 << BT_Reset)) // BT ein |
#define clr_BTM222On() (PORTC &= ~(1 << BT_Reset)) // BT aus |
#define set_WiFlyOn() (PORTC |= (1 << WiFly_Reset)) // WiFly ein |
#define clr_WiFlyOn() (PORTC &= ~(1 << WiFly_Reset)) // WiFly aus |
#define clr_V_On() (PORTB &= ~(1 << VoltageHold)) // Spannung mit T3 halten |
#define set_V_On() (PORTB |= (1 << VoltageHold)) |
#define set_USB2FC() (PORTC &= ~(1 << USB2FC)) // USB mit FC-Kabel verbinden |
#define clr_USB2FC() (PORTC |= (1 << USB2FC)) |
#define set_USB2Wi() (PORTB &= ~(1 << USB2Wi)) // USB mit Wi232 verbinden |
#define clr_USB2Wi() (PORTB |= (1 << USB2Wi)) |
#define set_Uart02FC() (PORTC &= ~(1 << Uart02FC)) // Uart0 mit FC-Kabel verbinden |
#define clr_Uart02FC() (PORTC |= (1 << Uart02FC)) |
#define set_Uart02Wi() (PORTC &= ~(1 << Uart02Wi)) // Uart0 mit Wi232 verbinden |
#define clr_Uart02Wi() (PORTC |= (1 << Uart02Wi)) |
#define set_DISPLAYLIGHT() (PORTD &= ~(1 << Displaybeleuchtung)) // Displaybeleuchtung PCB3.9m |
#define clr_DISPLAYLIGHT() (PORTD |= (1 << Displaybeleuchtung)) |
#define set_DISPLAYLIGHTV2() (PORTC &= ~(1 << DisplaybeleuchtungV2)) // Displaybeleuchtung PCB3.9x |
#define clr_DISPLAYLIGHTV2() (PORTC |= (1 << DisplaybeleuchtungV2)) |
#define set_WI232CMD() (PORTD &= ~(1 << Wi232_CMD)) // Wi232 Programmierpin |
#define clr_WI232CMD() (PORTD |= (1 << Wi232_CMD)) |
void set_D_LIGHT(void); /* Displaybeleuchtung ein*/ |
void clr_D_LIGHT(void); /* Displaybeleuchtung aus */ |
void set_BEEP(void); /* Beeper ein*/ |
void clr_BEEP(void); /* Beeper aus*/ |
void InitHWPorts(void); |
void set_BTOn(void); /* Bluetooth einschalten*/ |
void set_BTOff(void); /* Bluetooth einschalten*/ |
void set_Modul_On(uint8_t Modul); |
extern volatile uint8_t PCB_Version; |
extern volatile uint8_t PCB_SV2RxTxPatch; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/avr-nmea-gps-library/nmea.c |
---|
0,0 → 1,525 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY nmea.c |
//# |
//# 08.08.2015 CB |
//# - add: Einführung eines neuen Interruptbasierten NMEA Parsers |
//# cpp-Code auf c geändert und an PKT angepasst |
//# |
//############################################################################ |
/* URSPRUNG: |
File: nmea.cpp |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include "nmea.h" |
#include "../timer/timer.h" |
//#include <stdio.h> |
//#include <stdlib.h> |
//#include <util/delay.h> |
#include <stdbool.h> |
#define MAX_POWER 10 |
#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
static const int32_t powers[MAX_POWER] PROGMEM = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
uint8_t m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
m_bFlagDataReady = false; // valid GPS fix and data available, user can call reader functions |
volatile uint32_t res_nNMEAcounter = 0; // NMEA Datensatzzähler |
volatile uint32_t res_nCRCerror = 0; // zählt die CRC Errors beim dekodieren |
uint8_t receiveNMEA = false; |
char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// will be set to true for characters between $ and * only |
bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
int m_nChecksum ; // numeric checksum, computed for a given sentence |
bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
int index_received_checksum ; // used to parse received checksum |
// word cutting variables |
int m_nWordIdx , // the current word in a sentence |
m_nPrevIdx, // last character index where we did a cut |
m_nNowIdx ; // current character index |
// globals to store parser results |
int32_t res_fLongitude; // GPRMC and GPGGA |
int32_t res_fLatitude; // GPRMC and GPGGA |
unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
char Time[9]; // GPRMC and GPGGA |
uint8_t res_nSatellitesUsed; // GPGGA |
uint8_t res_nGPSfix; // GPGGA |
int16_t res_nHDOP; // GPGGA |
int16_t res_fAltitude; // GPGGA |
float res_fSpeed; // GPRMC |
int16_t res_fBearing; // GPRMC |
NMEA_GPGGA_t NMEA; // Variable mit der Struktur für die NMEA Daten für PKT |
// the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// aux functions |
int digit2dec(char hexdigit); |
float string2float(char* s); |
int mstrcmp(const char *s1, const char *s2); |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c) |
{ |
if (c == '$') { |
m_bFlagRead = true; |
// init parser vars |
m_bFlagComputedCks = false; |
m_nChecksum = 0; |
// after getting * we start cuttings the received m_nChecksum |
m_bFlagReceivedCks = false; |
index_received_checksum = 0; |
// word cutting variables |
m_nWordIdx = 0; m_nPrevIdx = 0; m_nNowIdx = 0; |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
} |
if (m_bFlagRead) { |
// check ending |
if (c == '\r' || c== '\n') { |
// catch last ending item too |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
// cut received m_nChecksum |
tmp_szChecksum[index_received_checksum] = 0; |
// sentence complete, read done |
m_bFlagRead = false; |
// parse |
parsedata(); |
} else { |
// computed m_nChecksum logic: count all chars between $ and * exclusively |
if (m_bFlagComputedCks && c == '*') m_bFlagComputedCks = false; |
if (m_bFlagComputedCks) m_nChecksum ^= c; |
if (c == '$') m_bFlagComputedCks = true; |
// received m_nChecksum |
if (m_bFlagReceivedCks) { |
tmp_szChecksum[index_received_checksum] = c; |
index_received_checksum++; |
} |
if (c == '*') m_bFlagReceivedCks = true; |
// build a word |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = c; |
if (c == ',') { |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
m_nPrevIdx = m_nNowIdx; |
} |
else m_nNowIdx++; |
} |
} |
return m_nWordIdx; |
} |
/* |
* parse internal tmp_ structures, fused by pushdata, and set the data flag when done |
*/ |
void parsedata(void) { |
int received_cks = 16*digit2dec(tmp_szChecksum[0]) + digit2dec(tmp_szChecksum[1]); |
//uart1.Send("seq: [cc:%X][words:%d][rc:%s:%d]\r\n", m_nChecksum,m_nWordIdx, tmp_szChecksum, received_cks); |
// check checksum, and return if invalid! |
if (m_nChecksum != received_cks) { |
m_bFlagDataReady = false; |
res_nCRCerror = res_nCRCerror+1; |
return; |
} |
/* $GPGGA |
* $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh |
* ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A, |
* |
* WORDS: |
* 1 = UTC of Position |
* 2 = Latitude |
* 3 = N or S |
* 4 = Longitude |
* 5 = E or W |
* 6 = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix) |
* 7 = Number of satellites in use [not those in view] |
* 8 = Horizontal dilution of position |
* 9 = Antenna altitude above/below mean sea level (geoid) |
* 10 = Meters (Antenna height unit) |
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. |
* -geoid is below WGS-84 ellipsoid) |
* 12 = Meters (Units of geoidal separation) |
* 13 = Age in seconds since last update from diff. reference station |
* 14 = Diff. reference station ID# |
* 15 = Checksum |
*/ |
if (mstrcmp(tmp_words[0], "$GPGGA") == 0) { |
// Check GPS Fix: 0=no fix, 1=GPS fix, 2=Dif. GPS fix |
res_nGPSfix = atoi(&tmp_words[6][0]); |
NMEA.SatFix = res_nGPSfix; |
if (tmp_words[6][0] == '0') { |
// clear data |
res_fLatitude = 0; |
res_fLongitude = 0; |
// m_bFlagDataReady = false; |
// return; |
} |
// parse time |
res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
NMEA_getTime (tmp_words[1]); |
// parse latitude and longitude in NMEA format |
// res_fLatitude = string2float(tmp_words[2]); |
// res_fLongitude = string2float(tmp_words[4]); |
// get decimal format |
// if (tmp_words[3][0] == 'S') res_fLatitude *= -1.0; |
// if (tmp_words[5][0] == 'W') res_fLongitude *= -1.0; |
// float degrees = trunc(res_fLatitude / 100.0f); |
// float minutes = res_fLatitude - (degrees * 100.0f); |
// res_fLatitude = degrees + minutes / 60.0f; |
// degrees = trunc(res_fLongitude / 100.0f); |
// minutes = res_fLongitude - (degrees * 100.0f); |
// res_fLongitude = degrees + minutes / 60.0f; |
res_fLatitude = NMEA_calcLatitude(tmp_words[2],tmp_words[3]); |
res_fLongitude = NMEA_calcLongitude(tmp_words[4],tmp_words[5]); |
NMEA.Latitude = res_fLatitude; |
NMEA.Longitude = res_fLongitude; |
// parse number of satellites |
res_nSatellitesUsed = (int)string2float(tmp_words[7]); |
NMEA.SatsInUse = atoi(tmp_words[7]); |
// parse HDOP |
res_nHDOP = NMEA_floatStrToInt(tmp_words[8],1); |
NMEA.HDOP = res_nHDOP; |
// parse altitude |
// res_fAltitude = string2float(tmp_words[9]); |
res_fAltitude = NMEA_floatStrToInt( tmp_words[9], 1); |
NMEA.Altitude = res_fAltitude; |
// data ready |
m_bFlagDataReady = true; |
res_nNMEAcounter = res_nNMEAcounter +1; |
NMEA.Counter = res_nNMEAcounter; |
} |
// 8.8.2015 CB wird zur Zeit nicht benötigt |
/* $GPRMC |
* note: a siRF chipset will not support magnetic headers. |
* $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh |
* ex: $GPRMC,230558.501,A,4543.8901,N,02112.7219,E,1.50,181.47,230213,,,A*66, |
* |
* WORDS: |
* 1 = UTC of position fix |
* 2 = Data status (V=navigation receiver warning) |
* 3 = Latitude of fix |
* 4 = N or S |
* 5 = Longitude of fix |
* 6 = E or W |
* 7 = Speed over ground in knots |
* 8 = Track made good in degrees True, Bearing This indicates the direction that the device is currently moving in, |
* from 0 to 360, measured in azimuth. |
* 9 = UT date |
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course) |
* 11 = E or W |
* 12 = Checksum |
*/ |
// if (mstrcmp(tmp_words[0], "$GPRMC") == 0) { |
// // Check data status: A-ok, V-invalid |
// if (tmp_words[2][0] == 'V') { |
// // clear data |
// res_fLatitude = 0; |
// res_fLongitude = 0; |
//// m_bFlagDataReady = false; |
// return; |
// } |
//// // parse time |
//// res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
//// res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
//// res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
//// // parse latitude and longitude in NMEA format |
//// res_fLatitude = string2float(tmp_words[3]); |
//// res_fLongitude = string2float(tmp_words[5]); |
//// // get decimal format |
//// if (tmp_words[4][0] == 'S') res_fLatitude *= -1.0; |
//// if (tmp_words[6][0] == 'W') res_fLongitude *= -1.0; |
//// float degrees = trunc(res_fLatitude / 100.0f); |
//// float minutes = res_fLatitude - (degrees * 100.0f); |
//// res_fLatitude = degrees + minutes / 60.0f; |
//// degrees = trunc(res_fLongitude / 100.0f); |
//// minutes = res_fLongitude - (degrees * 100.0f); |
//// res_fLongitude = degrees + minutes / 60.0f; |
// //parse speed |
// // The knot (pronounced not) is a unit of speed equal to one nautical mile (1.852 km) per hour |
// res_fSpeed = NMEA_floatStrToInt(tmp_words[7],1); |
// res_fSpeed /= 1.852; // convert to km/h |
// // parse bearing |
// res_fBearing = NMEA_floatStrToInt(tmp_words[8],1); |
//// // parse UTC date |
//// res_nUTCDay = digit2dec(tmp_words[9][0]) * 10 + digit2dec(tmp_words[9][1]); |
//// res_nUTCMonth = digit2dec(tmp_words[9][2]) * 10 + digit2dec(tmp_words[9][3]); |
//// res_nUTCYear = digit2dec(tmp_words[9][4]) * 10 + digit2dec(tmp_words[9][5]); |
// |
// // data ready |
// res_nNMEAcounter = res_nNMEAcounter +1; |
// NMEA.Counter = res_nNMEAcounter; |
// m_bFlagDataReady = true; |
// } |
} |
//-------------------------------------------------------------- |
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLatitude( const char *s, const char *NS) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(0,5,NS,0,0,0); |
deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
min = NMEA_floatStrToInt( &s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *NS == 'S' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLongitude( const char *s, const char *WE) |
{ |
int32_t deg; |
int32_t min; |
//lcdx_puts_at(10,5,WE,0,0,0); |
deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
min = NMEA_floatStrToInt( &s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
deg = deg * 10000000 + min; |
if( *WE == 'W' ) |
{ |
deg = -deg; |
} |
return deg; |
} |
//-------------------------------------------------------------- |
void NMEA_getTime( const char *s) |
{ |
uint8_t sem = 0; |
uint8_t i; |
for( i=0; i<6; i++ ) |
{ |
NMEA.Time[sem++] = s[i]; |
if( i==1 || i==3) |
NMEA.Time[sem++] = ':'; |
} |
NMEA.Time[sem] = '\0'; |
} |
//-------------------------------------------------------------- |
/* |
* returns base-16 value of chars '0'-'9' and 'A'-'F'; |
* does not trap invalid chars! |
*/ |
int digit2dec(char digit) |
{ |
if ((int)digit >= 65) |
return ((int)digit - 55); |
else |
return ((int)digit - 48); |
} |
//-------------------------------------------------------------- |
/* returns base-10 value of zero-terminated string |
* that contains only chars '+','-','0'-'9','.'; |
* does not trap invalid strings! |
*/ |
float string2float(char* s) { |
long integer_part = 0; |
float decimal_part = 0.0; |
float decimal_pivot = 0.1; |
bool isdecimal = false, isnegative = false; |
char c; |
while ( ( c = *s++) ) { |
// skip special/sign chars |
if (c == '-') { isnegative = true; continue; } |
if (c == '+') continue; |
if (c == '.') { isdecimal = true; continue; } |
if (!isdecimal) { |
integer_part = (10 * integer_part) + (c - 48); |
} |
else { |
decimal_part += decimal_pivot * (float)(c - 48); |
decimal_pivot /= 10.0; |
} |
} |
// add integer part |
decimal_part += (float)integer_part; |
// check negative |
if (isnegative) decimal_part = - decimal_part; |
return decimal_part; |
} |
//-------------------------------------------------------------- |
// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
//-------------------------------------------------------------- |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ) |
{ |
char *endPtr; |
int32_t v; |
v = strtol(s, &endPtr, 10); |
if( *endPtr == '.' ) |
{ |
for (s = endPtr + 1; *s && power1; s++) |
{ |
v = v * 10 + (*s - '0'); |
--power1; |
} |
} |
if( power1 ) |
{ |
// Table to avoid multiple multiplications |
v = v * getPower(power1); |
} |
return v; |
} |
//-------------------------------------------------------------- |
int mstrcmp(const char *s1, const char *s2) |
{ |
while((*s1 && *s2) && (*s1 == *s2)) |
s1++,s2++; |
return *s1 - *s2; |
} |
//-------------------------------------------------------------- |
bool NMEA_isdataready() { |
return m_bFlagDataReady; |
} |
int NMEA_getHour() { |
return res_nUTCHour; |
} |
int NMEA_getMinute() { |
return res_nUTCMin; |
} |
int NMEA_getSecond() { |
return res_nUTCSec; |
} |
int NMEA_getDay() { |
return res_nUTCDay; |
} |
int NMEA_getMonth() { |
return res_nUTCMonth; |
} |
int NMEA_getYear() { |
return res_nUTCYear; |
} |
int32_t NMEA_getLatitude() { |
return res_fLatitude; |
} |
int32_t NMEA_getLongitude() { |
return res_fLongitude; |
} |
uint8_t NMEA_getSatellites() { |
return res_nSatellitesUsed; |
} |
uint8_t NMEA_getGPSfix() { |
return res_nGPSfix; |
} |
int16_t NMEA_getHDOP() { |
return res_nHDOP; |
} |
int16_t NMEA_getAltitude() { |
return res_fAltitude; |
} |
float NMEA_getSpeed() { |
return res_fSpeed; |
} |
int16_t NMEA_getBearing() { |
return res_fBearing; |
} |
uint32_t NMEA_getNMEAcounter() { |
return res_nNMEAcounter; |
} |
uint32_t NMEA_getCRCerror() { |
return res_nCRCerror; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/avr-nmea-gps-library/nmea.h |
---|
0,0 → 1,135 |
/* |
File: nmea.h |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
NMEA GPS content parser |
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
http://www.pocketmagic.net |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
#include <string.h> |
//#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <math.h> |
#include <stdio.h> |
//--------------------------------- |
#define NMEA_TIMEOUT 500 // Timeout 5 sek. |
//--------------------------------- |
typedef struct |
{ |
uint32_t Counter; // Zaehler empfangene GPGGA Pakete |
int32_t Latitude; // in 1E-7 deg |
int32_t Longitude; // in 1E-7 deg |
int16_t Altitude; // Hoehe in Meter |
uint8_t SatFix; // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix, 6=Estimated) |
uint8_t SatsInUse; // Number of satelites currently in use |
int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser |
char Time[9]; // "hh:mm:ss" |
} NMEA_GPGGA_t; |
//--------------------------------- |
// export vars |
//--------------------------------- |
extern NMEA_GPGGA_t NMEA; |
extern uint8_t receiveNMEA; |
extern volatile uint32_t res_nNMEAcounter; |
// /*class NMEA { |
// |
// private: |
// bool m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
// m_bFlagDataReady; // valid GPS fix and data available, user can call reader functions |
// char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
// tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
// |
// // will be set to true for characters between $ and * only |
// bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
// int m_nChecksum ; // numeric checksum, computed for a given sentence |
// bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
// int index_received_checksum ; // used to parse received checksum |
// |
// // word cutting variables |
// int m_nWordIdx , // the current word in a sentence |
// m_nPrevIdx, // last character index where we did a cut |
// m_nNowIdx ; // current character index |
// |
// // globals to store parser results |
// float res_fLongitude; // GPRMC and GPGGA |
// float res_fLatitude; // GPRMC and GPGGA |
// unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
// res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
// int res_nSatellitesUsed; // GPGGA |
// float res_fAltitude; // GPGGA |
// float res_fSpeed; // GPRMC |
// float res_fBearing; // GPRMC |
// |
// // the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// // aux functions |
// int digit2dec(char hexdigit); |
// float string2float(char* s); |
// int mstrcmp(const char *s1, const char *s2); |
// |
// public: |
// // constructor, initing a few variables |
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c); |
void parsedata(void); |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ); |
int32_t NMEA_calcLatitude( const char *s, const char *NS); |
int32_t NMEA_calcLongitude( const char *s, const char *WE); |
void NMEA_getTime( const char *s); |
// READER functions: retrieving results, call isdataready() first |
bool NMEA_isdataready(); |
int NMEA_getHour(); |
int NMEA_getMinute(); |
int NMEA_getSecond(); |
int NMEA_getDay(); |
int NMEA_getMonth(); |
int NMEA_getYear(); |
int32_t NMEA_getLatitude(); |
int32_t NMEA_getLongitude(); |
uint8_t NMEA_getSatellites(); |
uint8_t NMEA_getGPSfix(); |
int16_t NMEA_getHDOP(); |
int16_t NMEA_getAltitude(); |
float NMEA_getSpeed(); |
int16_t NMEA_getBearing(); |
uint32_t NMEA_getNMEAcounter(); |
uint32_t NMEA_getCRCerror(); |
// }; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/bluetooth.c |
---|
0,0 → 1,1760 |
/** |
* source for the Bluetooth driver |
* @file bluetooth.c |
* @author Linus Lotz<lotz@in.tum.de> |
* @author Salomon Sickert |
*/ |
//############################################################################ |
//# HISTORY bluetooth.c |
//# |
//# |
//# 02.08.2015 CB |
//# - chg: bt_showsettings(), angepasst und vollendet, zur Zeit nur im Mastermodus des BTM222 nutzbar |
//# |
//# 30.06.2014 OG |
//# - chg: bt_discover() wieder auf (berechnete) Progressbar umgestellt |
//# |
//# 26.06.2014 OG |
//# - del: bt_receiveNMEA() - jetzt in gps/nmea.c |
//# - chg: bt_discover(), bt_searchDevice() - stark geaendert; Timeout geht jetzt |
//# nach Zeit und Abbruch durch Benutzer moeglich |
//# - fix: BT-Devicesuche - wenn bei einer Suche Devices gefunden wurden und bei |
//# einer nachfolgenden Suche weniger (z.B. weil ein Device ausgeschaltet wurde) |
//# dann wurden zuviele Devices angezeigt; fixed |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover(), bt_searchDevice() |
//# |
//# 22.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 21.06.2014 CB |
//# - fix: Fehler in "copy_DevName", zwei Leerzeichen am Anfang beendeten sofort den Namen, Namen war dann leer |
//# - fix: Umbau der Timeoutfunktion bei der Devicesuche, wenn kein BT Device in Reichweite wurde die Suche nicht beendet |
//# - add: Progressbar bei BT Device Suche |
//# |
//# 19.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - etliche Aenderungen und Funktionsumbenennungen - vorallem rund um den |
//# Bereich BT-Init |
//# - viel Code-Layout (noch nicht abgeschlossen) |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 2013 Cebra |
//# - Erweiterung um BT Local-ID Abfrage |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung fuer WiFlypatch geaendert |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() - korrigiert ggf. fehlehaften BT-Namen in Config.bt_name |
//# |
//# 24.06.2013 OG |
//# - chg: bt_init(): Code-Layout, Code-Struktur, ggf. Debug-Ausgaben |
//# - chg: bt_searchmodul(): Code-Layout und Ausgabe |
//# - chg: send_cmd(): Handling von BT_SET_NAME & Code-Layout |
//# - add: define DEBUG_BT |
//# |
//# 23.05.2013 OG |
//# - chg: Timeout reduziert in bt_disconnect() (trennen der BT-Verbindung) |
//# - fix: copy_DevName() Range angepasst und 0 Terminierung |
//# |
//# 24.04.2013 OG |
//# - chg: Auskommentiert: #define __DELAY_BACKWARD_COMPATIBLE__ |
//# das wird bereits vom Compiler-Symbol dieses PKT-Projektes definiert |
//# und erzeugt nur zusaetzliche Warnings |
//# |
//# 03.04.2013 Cebra |
//# - chg: Init Routinen BT überarbeitet, es kam zu falschen Baudrateneinstellungen |
//# beim BTM-222 ohne Hinweis, mehr Displayinformationen beim Init |
//############################################################################ |
#include <string.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdlib.h> |
#include "bluetooth.h" |
#include "../main.h" |
//---------------------------------------------------------------------------- |
#ifdef USE_BLUETOOTH // 30.01.2014 OG |
// brutal hier den Source ausklammern, damit man an anderen Stellen des PKT |
// den BT-Code durch den Compiler entdeckt |
// ansonsten kann es passieren, dass einiges an BT-Code unenddeckt einkompiliert wird |
// -> bei den ersten Test's waren das immerhin ca. 10 KByte durch unenddeckten Code... |
//---------------------------------------------------------------------------- |
//#define DEBUG_BT |
//#ifdef HWVERSION3_9 |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "fifo.h" |
#include "error.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../utils/xutils.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "../utils/scrollbox.h" |
//############################################################################ |
//############################################################################ |
//#define BT_CMD_TIMEOUT_MS 2000 |
#define BT_CMD_TIMEOUT_MS 500 // 15.06.2014 OG: Timeout reduziert von 2000 auf 500 |
static const char STR_BTM222INIT[] PROGMEM = "BTM222 Init"; |
//#define SaveMem |
// |
// Baudrate for the UART-connection to the BTM-222 on SQUIRREL |
// |
#define SQUIRREL |
#ifdef SQUIRREL |
#define UART_BAUD_RATE 19200 |
#endif |
#ifdef NUT |
#define UART_BAUD_RATE 19200 |
#endif |
typedef enum { |
BT_RAW, |
BT_DATA, |
BT_CMD, |
BT_NOECHO, |
BT_NOANSWER |
} communication_mode_t; |
typedef enum { |
BT_TEST, // AT |
BT_CONNECT, // ATA |
BT_DISCONNECT, // ATH |
BT_CLEAR_ADDRESS, // ATD0 |
BT_SET_ADDRESS, // ATD=_____ |
BT_GET_LOCALID, // ATB? Inquire the Local BD address |
BT_FIND_DEVICES, // ATF? |
BT_DISABLE_AUTOCONNECT, // ATO1 |
BT_ENABLE_AUTOCONNECT, // ATO0 |
BT_SET_MASTER, // ATR0 |
BT_SET_SLAVE, // ATR1 |
BT_SET_PIN, // ATP=1234 |
BT_SET_2400, // ATL* Baudrate 2400 |
BT_SET_4800, // ATL0 Baudrate 4800 |
BT_SET_9600, // ATL1 Baudrate 9600 |
BT_SET_19200, // ATL2 Baudrate 19200 |
BT_SET_38400, // ATL3 Baudrate 38400 |
BT_SET_57600, // ATL4 Baudrate 57600 |
BT_SET_115200, // ATL5 Baudrate 115200 |
BT_SET_NOANSWER, // ATQ1 Rückmeldungen aus |
BT_SET_NOECHO, // ATE0 ECHO deaktivieren |
BT_SET_ANSWER, // ATQ0 Rückmeldungen |
BT_SET_ECHO, // ATE1 ECHO aktivieren |
BT_SET_DEFAULT, // Defaultwerte setzen |
BT_SET_NAME, // Devicename |
BT_SET_DISPWRDOWN, // disable auto Powerdown |
BT_SHOW_SETTINGS // ATI1 Listing all setting value |
} bt_cmd_t; |
//TODO: FIFO Groesse |
#define IN_FIFO_SIZE 512 |
char localID[15]="12345678901234"; |
static uint8_t bt_buffer[IN_FIFO_SIZE]; |
fifo_t in_fifo; |
char bt_rx_buffer[RXD_BUFFER_SIZE]; |
volatile uint8_t bt_rx_len; |
volatile uint8_t bt_rx_ready = 0; |
uint8_t EchoAnswerOn=false; //Merkzelle |
static bt_mode_t bt_mode = BLUETOOTH_SLAVE; |
static communication_mode_t comm_mode = BT_CMD; |
uint8_t i = 0; |
uint8_t bt_devicecount = 0; |
uint8_t bt_rxerror = 0; |
uint8_t bt_frameerror = 0; |
uint8_t bt_overrun = 0; |
uint8_t bt_bufferoverflow = 0; |
device_info device_list[NUTS_LIST]; |
// Set a timeout of Y ms and a Conditon X, which have to be true while timeout |
#define while_timeout(X, Y) for(uint16_t __timeout = 0; __timeout++ <= Y && (X); Delay_MS(Y ? 1 : 0)) |
//-------------------------------------------------------------- |
// entfernt ungueltige Zeichen und Leerzeichen (rechts) aus dem |
// BT-Namen die evtl. durch Default's reingekommen sind. |
// Falls ungueltige Zeichen ('.') wird der BT-Name auf "PKT" |
// gesetzt. |
//-------------------------------------------------------------- |
void bt_fixname(void) |
{ |
char *p; |
p = Config.bt_name; |
while( *p!=0 && *p!='.' ) p++; // suche '.' |
if( *p=='.' || Config.bt_name[0]==' ' || Config.bt_name[0]==0) // wenn '.' gefunden oder Anfang mit ' ' oder kein String |
strcpy_P(Config.bt_name, PSTR("PKT")); // -> dann BT-Name auf "PKT" setzen |
strrtrim( Config.bt_name ); // ggf. Leerzeichen rechts loeschen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_MS(int count) |
{ |
for (int i = 0; i < count; i++) |
_delay_ms(1); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_start(void) |
{ |
if( Config.BTIsSlave == true ) EchoAnswerOn = false; |
else EchoAnswerOn = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void uart_receive(void) |
{ |
unsigned int uart_data; |
while( !fifo_is_full(&in_fifo) ) |
{ |
uart_data = uart1_getc(); |
//USART_puts("."); |
if (!(uart_data==UART_NO_DATA)) USART_putc(uart_data); |
switch( uart_data & 0xFF00 ) |
{ |
// Framing Error detected, i.e no stop bit detected |
case UART_FRAME_ERROR: |
bt_rxerror++; |
bt_frameerror++; |
return; |
// Overrun, a character already presend in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped |
// |
case UART_OVERRUN_ERROR: |
bt_rxerror++; |
bt_overrun++; |
return; |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// |
case UART_BUFFER_OVERFLOW: |
bt_rxerror++; |
bt_bufferoverflow++; |
return; |
// UART Inputbuffer empty, nothing to do |
case UART_NO_DATA: |
return; |
default: |
fifo_write( &in_fifo, uart_data); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void uart_send(const char *data, const uint8_t length) |
{ |
#ifdef SND_DEBUG |
debug_pgm(PSTR("bt_uart_send")); |
if (comm_mode == BT_CMD) debug_pgm(PSTR("bt_uart_send:BT_CMD")); else debug_pgm(PSTR("bt_uart_send:Wrong comm-mode")); |
if (EchoAnswerOn == true) debug_pgm(PSTR("bt_uart_send:EchoAnswer ON")); else debug_pgm(PSTR("bt_uart_send:EchoAnswer OFF")); |
#endif |
char echo; |
// lcd_printp_at (i++, 1, PSTR("."), 0); |
for (uint8_t i = 0; i < length; i++) |
{ |
#ifdef SND_DEBUG |
USART_putc((data[i])); //test |
#endif |
// debug_pgm(PSTR("bt_init_S")); |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR("UART: Remote not ready")); |
#endif |
return; |
} |
if (comm_mode == BT_RAW) |
_delay_ms(50); |
if (comm_mode == BT_DATA) |
_delay_ms(1); |
if ((comm_mode == BT_CMD) && (EchoAnswerOn == true)) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: get Echo")); |
#endif |
uint8_t x = 0; |
for (; x < 3; x++) |
{ |
while_timeout(fifo_is_empty(&in_fifo), 200) |
// for(uint16_t __timeout = 0; __timeout++ <= 200 && (fifo_is_empty(&in_fifo)); _delay_ms(200 ? 1 : 0)) |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
if (echo != data[i]) { |
if (uart1_putc(data[i]) == 0) |
{ |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UART: Remote not ready")); |
#endif |
return; |
} |
} |
else |
break; |
} |
#ifdef DEBUG |
if (x == 3) |
{ |
error_putc(data[i]); |
error_pgm(PSTR("BT: WRONG ECHO")); |
//_delay_ms(2000); |
} |
#endif |
} |
else |
{ |
{ |
uart_receive(); |
fifo_read(&in_fifo, &echo); |
} |
#ifdef SND_DEBUG |
warn_pgm(PSTR ("UARTsend: skip Echo")); |
#endif |
} |
} |
} |
//-------------------------------------------------------------- |
// send_cmd( command, *data) |
//-------------------------------------------------------------- |
static uint16_t send_cmd( const bt_cmd_t command, const char *data) |
{ |
uint16_t CommandDelay; // nach BTM222 Kommandos verschiedene Verzögerungszeiten bevor es weitergehen kann |
uint8_t i; |
char full_command[20]; // Maximum command size |
CommandDelay = 100; // Default = 100ms |
switch( command ) |
{ |
case BT_SET_PIN: strcpy_P( full_command, PSTR("ATP=")); |
for( i = 0; i < bt_pin_length; i++) |
{ |
full_command[i+4] = Config.bt_pin[i]; |
} |
full_command[(bt_pin_length+4)] = 0; |
break; |
case BT_SET_DEFAULT: strcpy_P( full_command, PSTR("ATZ0")); |
CommandDelay = 3000; |
break; |
case BT_SET_2400: strcpy_P( full_command, PSTR("ATL*")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL*")); |
#endif |
break; |
case BT_SET_4800: strcpy_P( full_command, PSTR("ATL0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL0")); |
#endif |
break; |
case BT_SET_9600: strcpy_P( full_command, PSTR("ATL1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL1")); |
#endif |
break; |
case BT_SET_19200: strcpy_P( full_command, PSTR("ATL2")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL2")); |
#endif |
break; |
case BT_SET_38400: strcpy_P( full_command, PSTR("ATL3")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL3")); |
#endif |
break; |
case BT_SET_57600: strcpy_P( full_command, PSTR("ATL4")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL4")); |
#endif |
break; |
case BT_SET_115200: strcpy_P( full_command, PSTR("ATL5")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATL5")); |
#endif |
break; |
case BT_SET_NOANSWER: strcpy_P( full_command, PSTR("ATQ1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ1")); |
#endif |
break; |
case BT_SET_NOECHO: strcpy_P( full_command, PSTR("ATE0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE0")); |
#endif |
break; |
case BT_SET_ANSWER: strcpy_P( full_command, PSTR("ATQ0")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATQ0")); |
#endif |
break; |
case BT_SET_ECHO: strcpy_P( full_command, PSTR("ATE1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATE1")); |
#endif |
break; |
case BT_TEST: strcpy_P( full_command, PSTR("AT")); |
#ifdef DEBUG |
debug_pgm(PSTR("AT")); |
#endif |
break; |
case BT_CONNECT: strcpy_P( full_command, PSTR("ATA")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATA")); |
#endif |
break; |
case BT_DISCONNECT: strcpy_P( full_command, PSTR("ATH")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATH")); |
#endif |
break; |
case BT_CLEAR_ADDRESS: strcpy_P( full_command, PSTR("ATD0")); |
break; |
case BT_SET_ADDRESS: strcpy_P( full_command, PSTR("ATD=")); |
memcpy( (full_command + strlen(full_command)), data, 12); |
full_command[16] = 0; |
#ifdef DEBUG |
debug_pgm(PSTR("ATLD=")); |
#endif |
break; |
case BT_FIND_DEVICES: strcpy_P( full_command, PSTR("ATF?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATF?")); |
#endif |
break; |
case BT_DISABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO1")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO1")); |
#endif |
break; |
case BT_ENABLE_AUTOCONNECT: |
strcpy_P( full_command, PSTR("ATO0")); |
CommandDelay = 3500; |
#ifdef DEBUG |
debug_pgm(PSTR("ATO0")); |
#endif |
break; |
case BT_SET_MASTER: strcpy_P( full_command, PSTR("ATR0")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR0")); |
#endif |
break; |
case BT_SET_SLAVE: strcpy_P( full_command, PSTR("ATR1")); |
CommandDelay = 3000; |
#ifdef DEBUG |
debug_pgm(PSTR("ATR1")); |
#endif |
break; |
case BT_SET_NAME: strcpy_P( full_command, PSTR("ATN=") ); |
strcat( full_command, Config.bt_name); |
//lcd_print_at( 0, 7, full_command, 0); // DEBUG |
//lcdx_printf_at( 17, 7, MNORMAL, 0,0, "%3d", strlen(full_command) ); |
//_delay_ms(6000); |
#ifdef DEBUG |
debug_pgm(PSTR("ATN=")); |
#endif |
break; |
case BT_SET_DISPWRDOWN: strcpy_P(full_command, PSTR("ATS1")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATS1")); |
#endif |
break; |
case BT_GET_LOCALID: strcpy_P(full_command, PSTR("ATB?")); |
#ifdef DEBUG |
debug_pgm(PSTR("ATB?")); |
#endif |
break; |
case BT_SHOW_SETTINGS: strcpy_P(full_command, PSTR("ATI1")); |
break; |
default: warn_pgm(PSTR("CMD UNK")); |
CommandDelay = 0; |
return false; |
} |
strcat_P( full_command, PSTR("\r") ); |
// throw away your television |
uart_receive(); |
fifo_clear( &in_fifo ); |
// send command |
uart_send( full_command, strlen(full_command)); |
// get response |
#ifdef DEBUG |
debug_pgm(PSTR("send_cmd:get Response")); |
#endif |
if( !EchoAnswerOn ) |
{ |
_delay_ms(CommandDelay); |
uart1_flush(); |
fifo_clear(&in_fifo); |
return true; |
} |
else |
{ |
while_timeout(true, BT_CMD_TIMEOUT_MS) |
{ |
uart_receive(); |
if( command == BT_GET_LOCALID ) |
{ |
_delay_ms(CommandDelay); |
return bt_getID(); |
} |
if( fifo_strstr_pgm( &in_fifo, PSTR("OK\r\n")) ) |
{ |
_delay_ms(CommandDelay); |
return true; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("ERROR\r\n")) ) |
{ |
return false; |
} |
} // end: while_timeout() |
} |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static void clean_line(void) |
{ |
while_timeout(true, 50) |
uart_receive(); |
fifo_strstr_pgm( &in_fifo, PSTR("\r\n") ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
static communication_mode_t update_comm_mode( uint16_t timeout_ms ) |
{ |
while_timeout(true, timeout_ms) |
{ |
uart_receive(); |
if( fifo_strstr_pgm(&in_fifo, PSTR("DISCONNECT")) ) |
{ |
clean_line(); |
comm_mode = BT_CMD; |
send_cmd(BT_TEST, NULL); |
return comm_mode; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("CONNECT")) ) |
{ |
_delay_ms(100); // don't delete this, else there will be no success!!!!!!!!! |
comm_mode = BT_DATA; |
return comm_mode; |
} |
if( fifo_strstr_pgm (&in_fifo, PSTR("Time out,Fail to connect!")) ) |
{ |
clean_line(); |
#ifdef DEBUG |
debug_pgm(PSTR("CONNECT FAILED\n")); |
#endif |
send_cmd(BT_TEST, NULL); |
comm_mode = BT_CMD; |
return comm_mode; |
} |
} |
return comm_mode; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_set_EchoAnswer( uint8_t on ) |
{ |
comm_mode = BT_CMD; // Set comm_mode to CMD |
EchoAnswerOn = false; |
if( on ) |
{ |
send_cmd( BT_SET_ECHO , NULL); |
send_cmd( BT_SET_ANSWER , NULL); |
send_cmd( BT_TEST , NULL); |
// EchoAnswerOn = true; |
/* |
if( !send_cmd( BT_TEST , NULL) ) |
{ |
//lcd_printp_at (0, 6, PSTR("Error set Echo"), MNORMAL); |
//_delay_ms(3000); |
} |
*/ |
} |
else |
{ |
send_cmd( BT_SET_NOECHO , NULL); |
send_cmd( BT_SET_NOANSWER, NULL); |
// EchoAnswerOn = false; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t BTM222_SetBaud( uint8_t baudrate ) |
{ |
bt_cmd_t btcommand = BT_SET_57600; |
uint8_t lOk = true; |
comm_mode = BT_CMD; // Set comm_mode to CMD |
uart1_flush(); |
fifo_clear( &in_fifo ); |
bt_set_EchoAnswer(true); |
if( !send_cmd(BT_TEST, NULL) ) // Test mit PKT_Baudrate |
{ |
return false; |
} |
else |
{ |
switch( baudrate ) |
{ |
case Baud_2400: btcommand = BT_SET_2400; break; |
case Baud_4800: btcommand = BT_SET_4800; break; |
case Baud_9600: btcommand = BT_SET_9600; break; |
case Baud_19200: btcommand = BT_SET_19200; break; |
case Baud_38400: btcommand = BT_SET_38400; break; |
case Baud_57600: btcommand = BT_SET_57600; break; |
case Baud_115200: btcommand = BT_SET_115200; break; |
} |
if( !(send_cmd( btcommand, NULL)) ) lOk = false; // Fehler |
SetBaudUart1( baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
send_cmd( BT_TEST, NULL); |
if( !(send_cmd(BT_TEST, NULL)) ) lOk = false; // Fehler |
bt_set_EchoAnswer(false); |
} |
return lOk; |
} |
//-------------------------------------------------------------- |
// foundbaud = BTM222_CheckBaud( Baudrate ) |
// |
// suche BTM222 mit entsprechender Baudrate |
// |
// RUECKGABE: |
// =0: Error (BTM222 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint8_t BTM222_CheckBaud( uint8_t Baudrate ) |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
comm_mode = BT_CMD; // Set comm_mode to CMD |
SetBaudUart1( Baudrate ); |
uart1_flush(); |
fifo_clear( &in_fifo ); |
EchoAnswerOn = false; |
send_cmd( BT_TEST , NULL); |
send_cmd( BT_SET_ANSWER, NULL); |
send_cmd( BT_SET_ECHO , NULL); |
EchoAnswerOn = true; |
if( send_cmd(BT_TEST, NULL) ) |
{ |
return Baudrate; // BTM222 Baudrate gefunden |
} |
return 0; // 0 = nicht gefunden |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void BTM222_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
// ggf. rechte Leerzeichen oder ungueltige Zeichen |
// von bt_name entfernen |
bt_fixname(); |
//---------------------------- |
// 1. Frage: initialisieren? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, PSTR("initialisieren?"), NULL, STR_BTM222INIT, STR_BTM222INIT) ) |
{ |
return; |
} |
//------------------------------------ |
// 2. BTM222 suchen |
//------------------------------------ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Old_Baudrate) ); // "suche nnn Baud" |
set_Modul_On( Bluetooth ); |
EchoAnswerOn = false; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = BTM222_CheckBaud( Baud_115200 ); |
if( !foundbaud ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 not found!"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
Config.UseBT = false; // Modul in Konfig sperren, verhindert Init beim Start |
} |
//------------------------------------ |
// 2. Initialisieren |
//------------------------------------ |
if( foundbaud > 0 ) |
{ |
PKT_Title_P( STR_BTM222INIT, true, true ); // Titel: mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_BTM222INIT ); // Headline |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 7, 0, 0,0); // 7 Progress Steps |
comm_mode = BT_CMD; // Set comm_mode to CMD |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_CLEAR_ADDRESS, NULL) ) nError = 1; // Clear remote address |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_SLAVE, NULL) ) nError = 2; // Set BTM to SLAVE |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_PIN, NULL) ) nError = 3; // Set BTM PIN |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_NAME, NULL) ) nError = 4; // Set BTM Name |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_DISPWRDOWN, NULL) ) nError = 5; |
if( !nError && PKT_Progress_Next() && !send_cmd( BT_GET_LOCALID, NULL) ) nError = 6; // MAC ermitteln |
//if( !nError && PKT_Progress_Next() && !send_cmd( BT_SET_ANSWER, NULL) ) nError = 7; |
if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Config.PKT_Baudrate)) nError = 8; // Set BTM Baudrate |
//if( !nError && PKT_Progress_Next() && !BTM222_SetBaud( Baud_38400)) nError = 8; // * TEST/DEBUG * Set BTM Baudrate |
SetBaudUart1( Config.PKT_Baudrate ); // PKT wieder auf Original Baudrate setzen |
if( !nError ) |
{ |
bt_set_EchoAnswer( false ); |
bt_mode = BLUETOOTH_SLAVE; |
WriteBTInitFlag(); // Init merken |
WriteBTSlaveFlag(); |
bt_start(); |
set_Modul_On( USB ); |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("BTM222 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
return; |
} |
else |
{ |
set_Modul_On(USB); |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("BTM Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
return; |
} |
} |
SetBaudUart1( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
set_Modul_On( USB ); |
clear_key_all(); |
return; |
} |
//###################################################################################### |
//###################################################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_set_mode( const bt_mode_t mode ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: set Mode")); |
#endif |
//if (update_comm_mode(0) == BT_DATA) // 30.1.2012 CB |
// return false; |
if( mode == bt_mode ) |
return true; |
if( mode == BLUETOOTH_MASTER ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_SET_MASTER, NULL) ) |
{ |
bt_mode = BLUETOOTH_MASTER; |
send_cmd(BT_DISABLE_AUTOCONNECT, NULL); |
WriteBTMasterFlag(); |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Master is set")); |
#endif |
} |
} |
if( mode == BLUETOOTH_SLAVE ) |
{ |
comm_mode = BT_CMD; |
bt_set_EchoAnswer(true); |
if( send_cmd(BT_ENABLE_AUTOCONNECT, NULL) ) |
{ |
bt_mode = BLUETOOTH_SLAVE; |
send_cmd(BT_SET_SLAVE, NULL); |
WriteBTSlaveFlag(); |
bt_set_EchoAnswer(false); |
comm_mode = BT_CMD; |
#ifdef DEBUG |
debug_pgm(PSTR("bt_setmode: Slave is set")); |
#endif |
} |
} |
// if (bt_mode == BLUETOOTH_MASTER) debug_pgm(PSTR("bt_mode:BLUETOOTH_MASTER ")); |
// if (bt_mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("bt_mode:BLUETOOTH_SLAVE")); |
// if (mode == BLUETOOTH_MASTER) debug_pgm(PSTR("mode:BLUETOOTH_MASTER ")); |
// if (mode == BLUETOOTH_SLAVE) debug_pgm(PSTR("mode:BLUETOOTH_SLAVE")); |
return (mode == bt_mode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_receive( void *data, uint8_t length, uint16_t timeout_ms ) |
{ |
uint8_t rec_length = 0; |
uint8_t i = 0; |
//while_timeout(true, timeout_ms); |
for( uint16_t __timeout = 0; __timeout++ <= true && (timeout_ms); _delay_ms(true ? 1 : 0)) |
{ |
if( i == length ) |
return true; |
uart_receive(); |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( update_comm_mode(0) != BT_DATA) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("not connected")); |
#endif |
return false; |
} |
if( timeout_ms == 0 ) // We have a connection |
timeout_ms += 2000; |
if( fifo_is_empty(&in_fifo) ) |
continue; |
if( !rec_length ) // Find starting point of packet |
{ |
fifo_read( &in_fifo, (char *)&rec_length); |
if( rec_length != length ) |
{ |
rec_length = 0; |
} |
else |
{ |
timeout_ms += 2000; |
} |
} |
else |
{ |
fifo_read( &in_fifo, (char *)data + i); |
i++; |
} |
} |
return false; |
} |
#ifndef SaveMem |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_send( void *data, const uint8_t length ) |
{ |
if (update_comm_mode(0) == BT_CMD) |
return false; |
uart_send((const char *)&length, 1); |
uart_send((char *)data, length); |
return (update_comm_mode(0) == BT_DATA); |
} |
#ifdef SQUIRREL |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_connect( const char *address ) |
{ |
fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart_receive(); |
fifo_clear(&in_fifo); |
// Maybe we already disconnected??? |
if (BT_DATA == update_comm_mode(0)) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("We are still connected...")); |
#endif |
return false; |
} |
// send_cmd(BT_TEST, NULL); |
/* |
if (!send_cmd(BT_DISABLE_AUTOCONNECT, address)) |
return false; |
*/ |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("SET_ADD")); |
#endif |
if (!send_cmd(BT_SET_ADDRESS, address)) |
return false; |
send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm (PSTR ("CONNECT")); |
#endif |
if (!send_cmd(BT_CONNECT, NULL)) |
return false; |
#ifdef DEBUG |
debug_pgm (PSTR ("WAIT FOR COMM")); |
#endif |
return (BT_DATA == update_comm_mode(20000)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint16_t bt_disconnect(void) |
{ |
uint8_t RetVal = false; |
for( uint8_t i = 0; i < 4; i++) // Switch to online cmd mode |
{ |
const char plus = '+'; |
uart_send(&plus, 1); |
_delay_ms(1000); |
} |
comm_mode = BT_CMD; |
EchoAnswerOn = true; |
uart1_flush(); // noch nicht verarbeitete Daten löschen |
fifo_clear(&in_fifo); |
if( !send_cmd(BT_DISCONNECT, NULL) ) |
RetVal = false; |
if( BT_CMD == update_comm_mode(2000) ) // neue Position um Disconnect-Antwort zu finden |
{ |
fifo_clear(&in_fifo); |
RetVal = true; |
} |
if( !send_cmd(BT_CLEAR_ADDRESS, NULL) ) |
RetVal = false; |
// if( BT_CMD == update_comm_mode(2000) ) // 31.7.2015 CB ursprüngliche Position, wurde verschoben |
// { |
// fifo_clear(&in_fifo); |
// return true; |
// } |
#ifdef DEBUG |
debug_pgm(PSTR("Still in DATA??")); |
#endif |
return RetVal; |
} |
/* |
BTM-222 Softwareversion 4.35 |
Inquiry Results: |
111111111222222222233333333334 |
01234567890123456789012345678901234567890 |
1 LE091452 0024-2C-BEB0CA |
2 E71 c 0024-7C-3EC9B9 |
BTM-222 Softwareversion 6.26 |
Inquiry Results: |
1 E71 c 0024-7C-3EC9B9 N.A. |
2 LE091452 0024-2C-BEB0CA N.A. |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_mac(const char *src, char *dst) |
{ |
uint8_t off = 0; |
for (uint8_t i = 0; i < 40; i++) |
{ |
if (src[i] == '-') |
if (src[i+3] == '-')// MAC Adresse suchen |
{ |
off = i-4; |
break; |
} |
} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_localID(const char *src, char *dst) |
{ |
uint8_t off = 0; |
//for (uint8_t i = 0; i < 40; i++) |
//{ |
// if (src[i] == '-') if (src[i+3] == '-')// MAC Adresse suchen |
// { |
// off = i-4; |
// break; |
// } |
//} |
for (uint8_t i = 0; i < 14; i++) |
{ |
if (src[i + off] == '-') |
off++; |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void copy_DevName(const char *src, char *dst) |
{ |
uint8_t off = 0; |
uint8_t i = 0; |
for (i = 0; i < 18; i++) |
{ |
if (src[i] == ' ' && src[i+1] == ' ') |
{ |
dst[i] = 0; //Stringende |
break; // nach zwei Leerzeichen ist der Name zuende |
} |
dst[i] = src[i + off]; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t bt_discover( void ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t devcount = 0; |
uint16_t Timeout = 10000; |
uint8_t progress = 0; |
uint8_t keyesc = false; |
#define BT_SEARCHTIME 7500 // = 75 Sekunden BT-Geraete suchen (60 werden vom BTM222 min. dafuer veranschlagt + 15 Sekunden Sicherheit) |
#define PROGRESSUPDATE 100 // = 1 Sekunde - jede Sekunde die Progressbar updaten bei BT-Inquiry |
//PKT_Progress_Init( max, yoffs, width,height) |
PKT_Progress_Init( 4 + ((BT_SEARCHTIME/PROGRESSUPDATE)+1), 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
//------------------------ |
//------------------------ |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 2 |
send_cmd( BT_TEST, NULL); |
fifo_clear( &in_fifo ); |
if( !send_cmd( BT_FIND_DEVICES, NULL) ) |
return 0; |
PKT_Progress_Next(); // Progressbar: Extra 3 |
//------------------------ |
//------------------------ |
do |
{ |
uart_receive(); |
Timeout--; |
_delay_ms(1); |
//write_ndigit_number_u( 0,7,fifo_getcount( &in_fifo) ,5,0,0); |
if( fifo_is_full(&in_fifo) ) |
break; |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
keyesc = true; |
break; |
} |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
fifo_clear( &in_fifo ); |
//------------------------ |
//------------------------ |
if( Timeout!=0 && !keyesc ) |
{ |
// 25.06.2014 OG: umgestellt auf zeitliches Timeout anstatt einer Zaehlschleife |
timer3 = BT_SEARCHTIME; // 75 Sekunden BT Device suchen |
while( timer3 && (!fifo_search( &in_fifo, PSTR("Inquiry End")))) |
{ |
if( (timer3 / PROGRESSUPDATE) != progress ) // Progressbar: next step |
{ |
progress = (timer3 / PROGRESSUPDATE); |
PKT_Progress_Next(); |
} |
uart_receive(); |
_delay_ms(2); // 25.06.2014 OG: diese Verzoegerung muss aktuell sein - sonst werden scheinbar keine Geraete gefunden :-( |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
//keyesc = true; |
break; |
} |
} |
} |
PKT_Progress_Next(); // Progressbar: Extra 4 |
//set_beep( 300, 0xffff, BeepNormal ); _delay_ms(3000); // Test um das Ende der Progressbar zu ueberpruefen |
//------------------------ |
//------------------------ |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
// write_ndigit_number_u( 0,6,devcount ,5,0,0); |
// set_beep( 300, 0xffff, BeepNormal ); |
// _delay_ms(1000); |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
if( strstr_P( buffer, PSTR("Inquiry End"))) |
{ |
break; |
} |
copy_DevName( &buffer[3], device_list[devcount].DevName); |
copy_mac( &buffer[3], device_list[devcount].mac); |
devcount++; |
if( devcount >= NUTS_LIST ) |
{ |
break; |
} |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
return devcount; |
} |
//-------------------------------------------------------------- |
uint8_t bt_showsettings( void ) |
{ |
char buffer[512]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t Timeout = 100; |
bt_rxerror = 0; |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
uart1_flush(); |
PKT_Progress_Init( 4 , 0, 0,0); // Anzahl der gesamten Progress Steps |
PKT_Progress_Next(); // Progressbar: Extra 1 |
if( !bt_set_mode( BLUETOOTH_MASTER) ) |
return 0; |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("1 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("1 E A off") ); |
PKT_Progress_Next(); // Progressbar: Extra 2 |
EchoAnswerOn = true; |
if( !send_cmd( BT_SHOW_SETTINGS, NULL) ) |
ScrollBox_Push_P( MNORMAL, PSTR("Command Error") ); |
PKT_Progress_Next(); // Progressbar: Extra 3 |
do |
{ |
uart_receive(); |
// write_ndigit_number_u( 0,6,fifo_getcount( &in_fifo) ,5,0,0); |
Timeout--; |
_delay_ms(2); |
if( fifo_is_full(&in_fifo) ) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("Fifo is full") ); |
break; |
} |
} while(Timeout != 0 ); |
// } while( (Timeout != 0) && (!fifo_search(&in_fifo, PSTR("+++"))) ); |
PKT_Progress_Next(); // Progressbar: Extra 4 |
while( !fifo_is_empty( &in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) // the empty line before end of inquiry |
continue; |
ScrollBox_Push( MNORMAL, buffer ); |
} // end: while( !fifo_is_empty(&in_fifo) ) |
clear_key_all(); |
// if (EchoAnswerOn) ScrollBox_Push_P( MNORMAL, PSTR("2 E A on") ); else ScrollBox_Push_P( MNORMAL, PSTR("2 E A off") ); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
/* |
uint16_t bt_discover_OLD( char result[8][12] ) |
{ |
char buffer[255]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint16_t pos = 0; |
uint16_t Timeout = 10000; |
uint16_t posC = 0; |
uint8_t Result=false; |
PKT_Progress_Init( 25, 0, 0,0); // 25 Progress Steps |
if (!bt_set_mode(BLUETOOTH_MASTER)) Result = false; |
PKT_Progress_Next(); // Progressbar |
send_cmd(BT_TEST, NULL); |
if( !send_cmd(BT_FIND_DEVICES, NULL)) Result = false; |
PKT_Progress_Next(); // Progressbar |
do |
{ |
uart_receive(); |
Timeout--; |
posC++; |
_delay_ms(1); |
write_ndigit_number_u(0,5,fifo_getcount(&in_fifo),5,0,0); |
if (posC ==1000) |
{ |
PKT_Progress_Next(); // Progressbar |
posC = 0; |
} |
if (fifo_is_full(&in_fifo)) break; |
} while( (Timeout != 0) && (!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))) ); |
if (Timeout==0) |
{ |
Result = false; |
} |
else |
{ |
for (uint16_t i = 0; i < 40000; i++) |
{ |
if( (i % 2000) == 0 ) |
PKT_Progress_Next(); // Progressbar |
uart_receive(); |
write_ndigit_number_u( 0,5,fifo_getcount(&in_fifo),5,0,0); |
_delay_ms(1); |
} |
} |
while( !fifo_is_empty(&in_fifo) ) |
{ |
// Get next line |
while (!fifo_cmp_pgm(&in_fifo, PSTR("\r\n"))) |
{ |
fifo_read(&in_fifo, bufferhead); |
bufferhead++; |
} |
// terminate string |
*bufferhead = 0; |
//reset bufferhead |
bufferhead = buffer; |
if (strlen(buffer) == 0) |
continue; //the empty line before end of inquiry |
if( strstr_P(buffer, PSTR("Inquiry End"))) |
//if (searchend) |
{ |
fifo_clear(&in_fifo); |
return true; |
} |
copy_DevName(&buffer[3],device_list[pos].DevName); |
copy_mac(&buffer[3], device_list[pos].mac); |
pos++; |
} // end: while( !fifo_is_empty(&in_fifo) ) |
return Result; |
} |
*/ |
//-------------------------------------------------------------- |
// ermittelt die MAC vom BTM2222 |
//-------------------------------------------------------------- |
uint8_t bt_getID( void ) |
{ |
//char buffer[255]; //oversized, but who cares? |
char buffer[80]; //oversized, but who cares? |
char *bufferhead = buffer; |
uint8_t i; |
while_timeout( !fifo_strstr_pgm(&in_fifo, PSTR("r\n")), 2000) |
uart_receive(); |
while( !fifo_is_empty(&in_fifo) ) |
{ |
while( !fifo_cmp_pgm( &in_fifo, PSTR("\r\n")) ) // Get next line |
{ |
fifo_read( &in_fifo, bufferhead); |
bufferhead++; |
} |
*bufferhead = 0; // terminate string |
bufferhead = buffer; // reset bufferhead |
if( strlen(buffer) == 0 ) |
continue; // the empty line before end of inquiry |
copy_localID( &buffer[0], &localID[0] ); |
for( i = 0; i < 13; i++) |
{ |
//lcd_putc (i, 6, localID[i],0); |
Config.bt_Mac[i] = localID[i]; |
} |
if( fifo_strstr_pgm(&in_fifo, PSTR("OK")) ) |
{ |
fifo_clear( &in_fifo ); |
return true; |
} |
else |
{ |
return false; |
} |
} // end: if( !fifo_is_empty(&in_fifo) ) |
return false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_downlink_init(void) |
{ |
fifo_init( &in_fifo, bt_buffer, IN_FIFO_SIZE); |
_delay_ms(100); |
// debug_pgm(PSTR("bt_downlink_init")); |
uart_receive(); |
fifo_clear(&in_fifo); |
// send_cmd(BT_TEST, NULL); |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init Start")); |
#endif |
if( Config.BTIsSlave == true ) // nur Init wenn BT ist Slave |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:try to set Master")); |
#endif |
//comm_mode = BT_NOECHO; |
// |
//if (!send_cmd (BT_SET_ECHO,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Echo!")); |
// #endif |
//} |
// |
//comm_mode = BT_NOANSWER; |
//if (!send_cmd(BT_SET_ANSWER,NULL)) |
//{ |
// #ifdef DEBUG |
// debug_pgm(PSTR("downlink_init:Couldn't set Answer!")); |
// #endif |
// |
//} |
comm_mode = BT_CMD; |
//send_cmd(BT_TEST, NULL); |
bt_set_EchoAnswer(true); |
if( !bt_set_mode(BLUETOOTH_MASTER) ) |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Couldn't set master!")); |
#endif |
return; |
} |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:master is set ")); |
#endif |
//WriteBTMasterFlag(); // Master merken |
comm_mode = BT_CMD; |
} |
else |
{ |
#ifdef DEBUG |
debug_pgm(PSTR("downlink_init:Master was set")); |
#endif |
comm_mode = BT_CMD; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void bt_searchDevice( void ) //Bluetoothgeräte suchen |
{ |
bt_devicecount = bt_discover(); |
} |
// |
////-------------------------------------------------------------- |
//uint16_t bt_receiveNMEA2(void) |
//{ |
// |
// char received; |
// static uint8_t line_flag = 1; |
// static char* ptr_write = bt_rx_buffer; |
// uint16_t timeout_ms=2000; |
// |
//// while_timeout(true, timeout_ms) { |
//while(1){ |
// |
// uart_receive(); |
// if (fifo_is_empty(&in_fifo)) |
// continue; |
// fifo_read(&in_fifo, &received); |
// USART_putc(received); |
//// _delay_ms(1); |
// } |
//// |
//return true; |
//} |
#endif |
#endif |
//#endif |
#endif // end: #ifdef USE_BLUETOOTH |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/bluetooth.h |
---|
0,0 → 1,182 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY bluetooth.h |
//# |
//# 25.06.2014 OG |
//# - chg: bt_discover() Parameter |
//# |
//# 16.06.2014 OG |
//# - add: BTM222_Initalize() |
//# - del: bt_init() |
//# |
//# 08.06.2014 OG |
//# - del: BT_New_Baudrate |
//# |
//# 24.06.2013 OG |
//# - add: bt_fixname() |
//############################################################################ |
#ifndef _BLUETOOTH_H_ |
#define _BLUETOOTH_H_ |
#include <avr/io.h> |
//#include <common.h> |
#include "fifo.h" |
#define SQUIRREL |
#define NUTS_LIST 16 |
#define EXTENSIONS_LIST 16 |
#define RXD_BUFFER_SIZE 150 |
//void InitBT(void); |
extern char bt_rx_buffer[RXD_BUFFER_SIZE]; |
extern volatile uint8_t bt_rx_len; |
extern volatile uint8_t bt_rx_ready; |
//extern char data_decode[RXD_BUFFER_SIZE]; |
extern uint8_t bt_rxerror; |
extern uint8_t bt_frameerror; |
extern uint8_t bt_overrun; |
extern uint8_t bt_bufferoverflow; |
extern fifo_t in_fifo; |
typedef struct _device_info device_info; |
// device info struct, holds mac , class and extensions + values of a device |
struct _device_info |
{ |
char DevName[20]; |
char mac[14]; |
// uint8_t class; |
// uint8_t extension_types[EXTENSIONS_LIST]; |
// uint16_t values_cache[EXTENSIONS_LIST]; |
}; |
extern uint8_t bt_devicecount; |
extern device_info device_list[NUTS_LIST]; |
extern char localID[15]; |
#define valid(num) (num < NUTS_LIST && (device_list[num].mac[0] != 0 || device_list[num].mac[1] != 0 || device_list[num].mac[2] != 0 || device_list[num].mac[3] != 0 || device_list[num].mac[4] != 0 || device_list[num].mac[5] != 0 || device_list[num].mac[6] != 0 || device_list[num].mac[7] != 0 || device_list[num].mac[8] != 0 || device_list[num].mac[9] != 0 || device_list[num].mac[10] != 0 || device_list[num].mac[11] != 0)) |
extern void bt_start(void); // EchoAnswervariable setzen |
void bt_set_EchoAnswer (uint8_t onoff); |
extern uint8_t bt_getID (void); |
//extern static communication_mode_t update_comm_mode(uint16_t timeout_ms); |
// Bluetooth mode ENUM |
typedef enum |
{ |
BLUETOOTH_MASTER, // < Master Mode (to create outgoinng connections). |
BLUETOOTH_SLAVE // < Slave Mode (to wait for incoming connections). |
} bt_mode_t; |
// set baudrate bluetooth |
// @return true = ok |
// false = error |
//extern uint16_t bt_setbaud (uint8_t baudrate); |
uint16_t bt_setbaud(uint8_t baudrate); |
// init bluetooth driver |
// @return always true |
// |
//extern uint16_t bt_init (void (*upate_percentage) (uint16_t)); |
//extern uint16_t bt_init (void); |
// Set the Bluetooth mode |
// @param mode bt_mode_t Bluetooth Mode ENUM (BLUETOOTH_MASTER or BLUETOOTH_SLAVE) |
// @return true if mode change was succesful, false if not |
// |
extern uint8_t bt_set_mode (const bt_mode_t mode); |
// recieve data over bluetooth |
// @param data pointer to memory for data storage |
// @param length value of length after call holds the actual recived data length |
// @param timeout_ms timeout in ms after the recive function aborts and returns with false |
// @return false if recived length > length parameter or it timeouted, true otherwise |
// |
extern uint16_t bt_receive (void * data, uint8_t length, uint16_t timeout_ms); |
// send data over bluetooth |
// @param data pointer to the data to send |
// @param length length of the data to be send |
// @return true if sendingn was successful, false otherwise |
// |
extern uint16_t bt_send (void * data, const uint8_t length); |
// squirrelt only functions |
#ifdef SQUIRREL |
// open bluetoot connection (only one at a time possible) |
// @param address connection is opened to this device mac address |
// @return true if connection was established, false otherwise |
// |
extern uint16_t bt_connect (const char *address); |
// closes bluetooth connection |
// @return false if failed, true otherwise |
// |
extern uint16_t bt_disconnect (void); |
// discover bluetooth devices |
// @param result in a 2 dimensional array first index are devicecs (max 8) second is mac address string |
// @param update_callback to inform of progress (in % from 0 to 100) |
// @return true if successful, false if error occured |
// |
//extern uint16_t bt_discover(void); |
//extern uint16_t bt_discover (char result[8][12], void (*update_callback)(const uint16_t progress)); |
extern void bt_downlink_init(void); // Auf Master stellen für Devicesuche und GPS Empfang |
extern void bt_searchDevice(void); //Bluetoothgeräte suchen |
#endif // SQUIRREL |
void uart_receive(void); |
void bt_fixname(void); |
void BTM222_Initalize( void ); |
uint8_t bt_showsettings( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/error.c |
---|
0,0 → 1,218 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
** |
* Error handling functions |
*/ |
//############################################################################ |
//# HISTORY error.c |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//########################################################################## |
#include <stdbool.h> |
#include <avr/pgmspace.h> |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
//-------------------------------------------------------------- |
inline void _send_msg(const char *msg) |
{ |
for (uint8_t i=0; i<255 && msg[i]!='\0'; i++) |
{ |
errordriver_write_c(msg[i]); |
} |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void send_pgm(const char *msg) |
{ |
uint8_t myByte; |
myByte = pgm_read_byte(msg); |
for(int i = 1; myByte != '\0'; i++) |
{ |
errordriver_write_c(myByte); |
myByte = pgm_read_byte(msg+i); |
} |
} |
//-------------------------------------------------------------- |
void error_init(void) |
{ |
error_driver_Init(); |
} |
//-------------------------------------------------------------- |
void error_putc(const char c) |
{ |
errordriver_write_c(c); |
} |
//-------------------------------------------------------------- |
void assert (bool condition, const char *msg) |
{ |
if (!condition) |
{ |
send_pgm(PSTR("ASS:")); |
_send_msg(msg); |
} |
} |
//-------------------------------------------------------------- |
void info (const char *msg) |
{ |
send_pgm(PSTR("INF:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void warn (const char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void debug (const char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
_send_msg(msg); |
} |
//-------------------------------------------------------------- |
void Error (const char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
_send_msg(msg); |
} |
#endif |
#ifdef DEBUG |
//-------------------------------------------------------------- |
void assert_pgm(bool condition, const prog_char *msg) |
{ |
if (condition) { |
send_pgm(PSTR("ASS:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
} |
//-------------------------------------------------------------- |
void info_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("INF:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
} |
//-------------------------------------------------------------- |
void warn_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("WARN:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void error_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("ERR:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
//-------------------------------------------------------------- |
void debug_pgm(const prog_char *msg) |
{ |
send_pgm(PSTR("DBG:")); |
send_pgm(msg); |
errordriver_write_c('\n'); |
errordriver_write_c('\r'); |
} |
////-------------------------------------------------------------- |
//void print_hex(uint8_t num) |
//{ |
// if (num<10) |
// error_putc(num+48); |
// else |
// { |
// switch (num) |
// { |
// case 10: |
// error_putc('A'); break; |
// case 11: |
// error_putc('B'); break; |
// case 12: |
// error_putc('C'); break; |
// case 13: |
// error_putc('D'); break; |
// case 14: |
// error_putc('E'); break; |
// case 15: |
// error_putc('F'); break; |
// default: |
// error_putc('#'); break; |
// } |
// } |
//} |
// |
// |
////-------------------------------------------------------------- |
//void byte_to_hex(uint8_t byte) |
//{ |
// uint8_t b2 = (byte & 0x0F); |
// uint8_t b1 = ((byte & 0xF0)>>4); |
// print_hex(b1); |
// print_hex(b2); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/error.h |
---|
0,0 → 1,79 |
/* |
___ ___ ___ ___ _____ |
/ /\ /__/\ / /\ /__/\ / /::\ |
/ /::\ | |::\ / /::\ \ \:\ / /:/\:\ |
/ /:/\:\ ___ ___ | |:|:\ / /:/\:\ \ \:\ / /:/ \:\ |
/ /:/~/::\ /__/\ / /\ __|__|:|\:\ / /:/ \:\ _____\__\:\ /__/:/ \__\:| |
/__/:/ /:/\:\ \ \:\ / /:/ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ \ \:\ / /:/ |
\ \:\/:/__\/ \ \:\ /:/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ \ \:\ /:/ |
\ \::/ \ \:\/:/ \ \:\ \ \:\ /:/ \ \:\ ~~~ \ \:\/:/ |
\ \:\ \ \::/ \ \:\ \ \:\/:/ \ \:\ \ \::/ |
\ \:\ \__\/ \ \:\ \ \::/ \ \:\ \__\/ |
\__\/ \__\/ \__\/ \__\/ |
___ ___ ___ ___ ___ ___ |
/ /\ / /\ /__/\ /__/\ / /\ /__/\ |
/ /:/ / /::\ | |::\ | |::\ / /::\ \ \:\ |
/ /:/ / /:/\:\ | |:|:\ | |:|:\ / /:/\:\ \ \:\ |
/ /:/ ___ / /:/ \:\ __|__|:|\:\ __|__|:|\:\ / /:/ \:\ _____\__\:\ |
/__/:/ / /\ /__/:/ \__\:\ /__/::::| \:\ /__/::::| \:\ /__/:/ \__\:\ /__/::::::::\ |
\ \:\ / /:/ \ \:\ / /:/ \ \:\~~\__\/ \ \:\~~\__\/ \ \:\ / /:/ \ \:\~~\~~\/ |
\ \:\ /:/ \ \:\ /:/ \ \:\ \ \:\ \ \:\ /:/ \ \:\ ~~~ |
\ \:\/:/ \ \:\/:/ \ \:\ \ \:\ \ \:\/:/ \ \:\ |
\ \::/ \ \::/ \ \:\ \ \:\ \ \::/ \ \:\ |
\__\/ \__\/ \__\/ \__\/ \__\/ \__\/ |
* |
* Error handling functions. |
*/ |
#ifndef __ERROR__ |
#define __ERROR__ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include "../main.h" |
#ifdef DEBUG |
void error_init(void); |
void error_putc(const char c); |
void assert (bool condition, const char *msg); |
void info (const char *msg); |
void warn(const char *msg); |
void debug(const char *msg); |
void Error(const char *msg); |
void assert_pgm(bool condition, const prog_char *msg); |
void info_pgm (const prog_char *msg); |
void warn_pgm(const prog_char *msg); |
void debug_pgm(const prog_char *msg); |
void error_pgm(const prog_char *msg); |
void byte_to_hex(uint8_t byte); |
#else |
#define error_init() {} |
#define error_putc(c) {} |
#define assert(cond, msg) {} |
#define info(msg) {} |
#define warn(msg) {} |
#define debug(msg) {} |
#define error(msg) {} |
#define assert_pgm(cond, msg) {} |
#define info_pgm(msg) {} |
#define warn_pgm(msg) {} |
#define debug_pgm(msg) {} |
#define error_pgm(msg) {} |
#define byte_to_hex(byte) {} |
#endif |
#endif //__ERROR__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/error_driver.c |
---|
0,0 → 1,27 |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
//#include "cpu.h" |
#include "error_driver.h" |
#include "../main.h" |
#ifdef DEBUG |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
void errordriver_write_c(char c) |
{ |
USART_putc(c); |
} |
void error_driver_Init(void) |
{ |
// USART_Init(UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/error_driver.h |
---|
0,0 → 1,22 |
/* |
* Functions to write error message to FTDI or USART |
*/ |
#ifndef __ERROR_DRIVER__ |
#define __ERROR_DRIVER__ |
#include <avr/io.h> |
#include "../main.h" |
#ifdef DEBUG |
extern void errordriver_write_c(char c); |
extern void error_driver_Init(void); |
#else |
#define errordriver_write_c(c) {} |
#define error_driver_init() {} |
#endif |
#endif //__ERROR_DRIVER__ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/fifo.c |
---|
0,0 → 1,197 |
/** |
* a simple Fifo |
* @file fifo.c |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//############################################################################ |
#include <string.h> |
#include "fifo.h" |
//#if defined HWVERSION1_3W || defined HWVERSION3_9 || defined HWVERSION1_2W |
//-------------------------------------------------------------- |
void fifo_init (fifo_t * fifo, uint8_t * buffer, uint16_t size) |
{ |
fifo->size = size; |
fifo->buffer = buffer; |
fifo->head = 0; |
fifo->count = 0; |
} |
//-------------------------------------------------------------- |
uint16_t fifo_getcount (const fifo_t * fifo) |
{ |
return fifo->count; |
} |
//-------------------------------------------------------------- |
bool fifo_is_empty (const fifo_t * fifo) |
{ |
return (fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_is_full (const fifo_t * fifo) |
{ |
return (fifo->size - fifo->count == 0); |
} |
//-------------------------------------------------------------- |
bool fifo_read (fifo_t * fifo, char *data) |
{ |
if (fifo_is_empty (fifo)) |
return false; |
uint8_t *head = fifo->buffer + fifo->head; |
*data = (char) * head; |
fifo->head = ( (fifo->head + 1) % fifo->size); |
fifo->count--; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_write (fifo_t * fifo, const char data) |
{ |
if (fifo_is_full (fifo)) |
return false; |
uint8_t *end = fifo->buffer + ( (fifo->head + fifo->count) % fifo->size); |
*end = (uint8_t) data; |
fifo->count++; |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_clear (fifo_t * fifo) |
{ |
fifo->count = 0; |
fifo->head = 0; |
return true; |
} |
//-------------------------------------------------------------- |
static bool fifo_cmp_pgm_at (fifo_t * fifo, const char * pgm, const uint16_t index) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= (i + index)) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i + index) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// We found the string, lets move the pointer |
fifo->head = ( (fifo->head + i + index) % fifo->size); |
fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
bool fifo_search (fifo_t * fifo, const char * pgm) |
{ |
uint16_t i; |
uint8_t fifo_byte; |
uint8_t pgm_byte; |
for (i = 0; pgm_read_byte (pgm + i) != 0; i++) |
{ |
if (fifo->count <= i) |
return false; |
pgm_byte = pgm_read_byte (pgm + i); |
fifo_byte = * (fifo->buffer + ( (fifo->head + i) % fifo->size)); |
if (fifo_byte != pgm_byte) |
return false; |
} |
// // We found the string, lets move the pointer |
// fifo->head = ( (fifo->head + i + index) % fifo->size); |
// |
// fifo->count -= (i + index); |
return true; |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t * fifo, const char * pgm) |
{ |
return fifo_cmp_pgm_at (fifo, pgm, 0); |
} |
//-------------------------------------------------------------- |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t * fifo, const char * pgm) |
{ |
for (uint16_t i = 0; i < fifo->count; i++) |
{ |
if (fifo_cmp_pgm_at (fifo, pgm, i)) |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Idee: buffer nach str durchsuchen ohne die fifo Zeiger zu aendern |
// ungetestet |
//-------------------------------------------------------------- |
bool fifo_isstr( fifo_t * fifo, const char * str) |
{ |
/* |
void *memmem(const void *s1, |
size_t len1, |
const void *s2, |
size_t len2); |
The memmem() function finds the start of the first occurrence of the substring s2 of length len2 in the memory area s1 of length len1. |
*/ |
return memmem( fifo->buffer + fifo->head, fifo->count, str, strlen(str) ) != NULL; |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth/fifo.h |
---|
0,0 → 1,107 |
/** |
* a simple Fifo |
* @file fifo.h |
* @author Pascal Schnurr |
*/ |
//############################################################################ |
//# HISTORY fifo.c |
//# |
//# 26.06.2014 OG |
//# - add: fifo_isstr() |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//############################################################################ |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#ifndef _FIFO_H_ |
#define _FIFO_H_ |
/** |
*fifo data structure all vital fifo information |
*/ |
typedef struct |
{ |
uint16_t count; /**< current number of elements */ |
uint16_t head; /**< position of the head element */ |
uint16_t size; /**< size equals max number of entrys*/ |
uint8_t* buffer; /**< pointer to memory area where the fifo is to be saved */ |
} fifo_t; |
uint16_t fifo_getcount (const fifo_t * fifo); |
/** \brief initialize of a fifo |
* sets all the information in your given fifo structure |
* @param fifo pointer to an allocated fifo_t structure |
* @param buffer pointer to an a allocated memory space for the fifo of size = sizeof(uint8_t) * size |
* @param size max number of entrys the fifo will hold |
*/ |
void fifo_init (fifo_t *fifo, uint8_t *buffer, uint16_t size); |
/** \brief checks if fifo is empty |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if empty otherwise false |
*/ |
bool fifo_is_empty (const fifo_t *fifo); |
/** \brief checks if fifo is full |
* @param fifo pointer to your initialized fifo_t structure |
* @return true if full otherwise false |
*/ |
bool fifo_is_full (const fifo_t *fifo); |
/** \brief clears the fifo |
* resets your fifo structure to 0 elements |
* @param fifo pointer to your initialized fifo_t structure |
* @return always true (never fails) |
*/ |
bool fifo_clear (fifo_t *fifo); |
/** \brief reads head of fifo |
* reads the first element and removes it |
* @param fifo pointer to your initialized fifo_t structure |
* @return false if fifo is empty false otherwise |
*/ |
bool fifo_read (fifo_t *fifo, char *data); |
/** \brief inserts a char into the fifo |
* adds a char to the end of the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param data the char data to be inserted |
* @return false if fifo is full true otherwise |
*/ |
bool fifo_write (fifo_t *fifo, const char data); |
/** \brief compares first elements with prog_char string |
* if pgm equals the first elements of the fifo these elements are removed |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char string for comparison |
* @return true if pgm is equal to the first entrys in the fifo, false otherwise |
*/ |
bool fifo_cmp_pgm (fifo_t* fifo, const char* pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_search (fifo_t * fifo, const char * pgm); |
/** \brief searches a string in the whole fifo |
* starts at the beginning and searches for the pgm string in the fifo, |
* if they are found previous entrys and the string are removed from the fifo |
* @param fifo pointer to your initialized fifo_t structure |
* @param pgm a prog_char with the search string |
* @return true if found, false otherwise |
*/ |
bool fifo_strstr_pgm (fifo_t *fifo, const char *pgm); |
bool fifo_isstr( fifo_t * fifo, const char * str); |
#endif /* _FIFO_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/bluetooth |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+_old_source |
/Transportables_Koptertool/PKT/trunk/connect.c |
---|
0,0 → 1,436 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.c |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: wait_and_restore() redundanten Code einsparen |
//# - add: Port_BLE2Wi() (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 28.02.2014 OG |
//# - chg: alle Anzeigescreens von 'PKT Connect' auf Connect_Screen() umgestellt |
//# - add: Connect_Screen() |
//# |
//# 12.02.2014 OG |
//# - chg: Port_WiFly2Wi() Auskommentiert: "unused variable 'i'" |
//# |
//# 15.07.2013 Cebra |
//# - chg: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC, Texte geändert |
//# |
//# 02.07.2013 Cebra |
//# - add: Port_FC2CFG_WL, Konfiguration Wlan Modul mit PC |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# - add: Port_WiFly2Wi, Wlan-Wi232 Verbindung |
//# |
//# 24.6.2013 Cebra |
//# - fix: Fehler beim UART1 bei der Rückschaltung von USB2FC,USB2WI,BT2FC,BT2WI |
//# |
//# 19.06.2013 OG |
//# - chg: Screen-Layout von Port_BT2Wi(), Port_USB2Wi() |
//############################################################################ |
#include "cpu.h" |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "lcd/lcd.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "messages.h" |
#include "lipo/lipo.h" |
#include "main.h" |
#include "bluetooth/bluetooth.h" |
#include "wifly/PKT_WiFlyHQ.h" |
#include "uart/uart1.h" |
#include "utils/xutils.h" |
//#if defined HWVERSION3_9 |
//-------------------------------------------------------------- |
void Change_Output(uint8_t UartMode) // Schaltet die Rx/Tx Richtungen |
{ |
// hiermit werden die 74HTC125 (IC5) Gatter geschaltet |
clr_USB2FC(); // PC2 aus IC5 A/D |
clr_USB2Wi(); // PB0 aus IC5 B/C |
clr_Uart02FC(); // PC6 aus IC4 C/D |
clr_Uart02Wi(); // PC5 aus IC4 A/B |
switch (UartMode) |
{ |
case USB2FC: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2FC(); |
break; |
case Uart02Wi: |
set_Uart02Wi(); |
break; |
case Uart02FC: |
set_Uart02FC(); |
break; |
case USB2Wi: |
UCSR1B &= ~(1<<RXEN1); |
UCSR1B &= ~(1<<TXEN1); |
UCSR1B &= ~(1<<RXCIE1); |
DDRD &= ~(1<<DDD2); // Pins auf Eingang setzen |
DDRD &= ~(1<<DDD3); |
PORTD &= ~(1<<PD2); // Pullup aus |
PORTD &= ~(1<<PD3); |
set_USB2Wi(); |
break; |
} |
} |
//-------------------------------------------------------------- |
// gibt eine uebergebene Baud-Rate als uint32_t zurueck |
//-------------------------------------------------------------- |
uint32_t Baud_to_uint32( uint8_t baud ) |
{ |
switch( baud ) |
{ |
case Baud_2400: return( 2400 ); |
case Baud_4800: return( 4800 ); |
case Baud_9600: return( 9600 ); |
case Baud_19200: return( 19200 ); |
case Baud_38400: return( 38400 ); |
case Baud_57600: return( 57600 ); |
case Baud_115200: return( 115200 ); |
} |
return( 0 ); |
} |
//-------------------------------------------------------------- |
// Connect_Screen( |
// |
// Parameter: |
// title : PROGMEM |
// message : RAM oder PROGMEM je nach msg_progmem |
// msg_progmem: true/false |
// true = message im PROGMEM |
// false = message im RAM |
//-------------------------------------------------------------- |
void Connect_Screen( const char *title, const char *message, uint8_t msg_progmem ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, title, MINVERS); // Titel |
show_Lipo(); |
lcdx_printp_center( 2, title , MNORMAL, 0,-3); // |
lcdx_printp_center( 3, strGet(STR_ACTIVE), MNORMAL, 0,1); // "AKTIV!" |
if( (!msg_progmem && strlen(message)>0) || (msg_progmem && strlen_P(message)>0) ) |
{ |
if( msg_progmem ) |
lcdx_printp_center( 5, message, MNORMAL, 0,1); |
else |
lcdx_print_center( 5, (unsigned char *) message, MNORMAL, 0,1); |
lcd_rect_round( 0, 5*7+2, 127, 8+6, 1, R2); // Rahmen |
} |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// _port_wait_and_restore() |
// |
// Wartet bis der Benutzer beendet und restauriert Einstellungen |
// |
// gemeinsamer Code um Speicherplatz zu sparen |
//-------------------------------------------------------------- |
void wait_and_restore( void ) |
{ |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
} |
uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); |
SetBaudUart1( Config.PKT_Baudrate ); |
if( Config.U02SV2 == 1 ) |
Change_Output( Uart02FC ); |
else |
Change_Output( Uart02Wi ); |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Function: BT2FC() |
// Purpose: Connect BT direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2FC(void) |
{ |
/* |
// mit Anzeige von 'Baud' |
// verbraucht einige Bytes mehr... |
char *message; |
uint32_t baud; |
switch( Config.PKT_Baudrate ) |
{ |
case Baud_2400: baud = 2400; break; |
case Baud_4800: baud = 4800; break; |
case Baud_9600: baud = 9600; break; |
case Baud_19200: baud = 19200; break; |
case Baud_38400: baud = 38400; break; |
case Baud_57600: baud = 57600; break; |
case Baud_115200: baud = 115200; break; |
} |
message = buffered_sprintf_P( PSTR("%lu Baud"), baud); |
Connect_Screen( PSTR("BT -> SV2 Kabel"), message, false ); |
*/ |
// ohne Anzeige von 'Baud' |
Connect_Screen( PSTR("BT -> SV2 Kabel"), "", false ); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
_delay_ms(1000); |
//_delay_ms(2000); |
if( !Config.BTIsSlave==true ) |
bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2FC); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BT2Wi() |
// Purpose: Connect BT direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_BT2Wi( void ) |
{ |
char *message = "RE-ID: 0000"; |
strcpy( &message[7], Config.RE_ID); |
Connect_Screen( PSTR("BT Extender"), message, false ); |
set_Modul_On( Bluetooth ); // set_BTOn(); |
_delay_ms(1500); |
if( !Config.BTIsSlave==true ) bt_set_mode(BLUETOOTH_SLAVE); |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: WiFly2Wi() |
// Purpose: Connect WiFly direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_WiFly2Wi( void ) |
{ |
Connect_Screen( PSTR("WLAN Extender"), PSTR("169.254.1.1:2000"), true ); |
set_Modul_On( Wlan ); |
Change_Output( USB2Wi ); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: BLE2Wi() |
// Purpose: Connect BLE direct to Wi.232 |
// Hinweis: SV2 RxTx Patch erforderlich! |
// Returns: |
//-------------------------------------------------------------- |
void Port_BLE2Wi( void ) |
{ |
Connect_Screen( PSTR("BLE Extender"), "", false ); |
set_Modul_On(Wlan); // verwendet wie Wlan/WiFly SV2 |
Change_Output(USB2Wi); |
wait_and_restore(); |
} |
//-------------------------------------------------------------- |
// Function: FC2CFG_BT() |
// Purpose: Connect FC (Tx1 Pin3, Rx1 Pin4) direct to BT |
// Returns: |
//-------------------------------------------------------------- |
void Port_FC2CFG_BT(void) |
{ |
lcd_cls (); |
lcd_printp_at (0, 0, PSTR("BTM-222 Konfigurieren"), 2); |
lcd_printp_at (0, 1, PSTR("FC > MK-USB > BTM-222"), 2); |
lcd_printp_at (0, 3, PSTR("MK-USB an PC anschl. "), 0); |
lcd_printp_at (0, 4, PSTR("Zwischen MK-USB und "), 0); |
lcd_printp_at (0, 5, PSTR("PKT ein gekreuztes "), 0); |
lcd_printp_at (0, 6, PSTR("Kabel anschliessen. "), 0); |
lcd_printp_at(12, 7, strGet(ESC), 0); |
// set_BTOn(); |
set_Modul_On(Bluetooth); |
Change_Output(USB2FC); |
while(!get_key_press (1 << KEY_ESC)); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
set_Modul_On(USB); |
// set_BTOff(); |
return; |
} |
//-------------------------------------------------------------- |
// Function: USB2FC() |
// Purpose: Connect USB direct to FC-Kabel (SV2 as MKUSB) |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2FC(void) |
{ |
Connect_Screen( PSTR("USB -> SV2 Kabel"), PSTR("MK-USB"), true ); |
Change_Output(USB2FC); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} while(!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
uart1_init (UART_BAUD_SELECT(57600,F_CPU)); |
SetBaudUart1(Config.PKT_Baudrate); |
if (Config.U02SV2 == 1) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
return; |
*/ |
} |
//-------------------------------------------------------------- |
// Function: USB2Wi() |
// Purpose: Connect USB direct to Wi.232 |
// Returns: |
//-------------------------------------------------------------- |
void Port_USB2Wi(void) |
{ |
Connect_Screen( PSTR("USB -> Wi.232"), "", false ); |
Change_Output(USB2Wi); |
wait_and_restore(); |
/* |
//------ |
// 01.04.2014 OG: ersetzt durch: wait_and_restore() |
// Unterschied: zum Schluss wird bei wait_and_restore() |
// noch ein set_Modul_On(USB) gemacht - ist das relevant? |
//------ |
do |
{ |
show_Lipo(); |
} |
while( !get_key_press(1 << KEY_ESC) ); |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
get_key_press(KEY_ALL); |
return; |
*/ |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/connect.h |
---|
0,0 → 1,68 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY connect.h |
//# |
//# 10.06.2014 OG |
//# - del: Port_USB2CFG_Wi() - jetzt in Wi232.c als Wi232_ConfigPC() |
//# |
//# 13.04.2014 OG |
//# - add: Baud_to_uint32() |
//# |
//# 10.04.2014 OG |
//# - del: Port_FC2CFG_WL() |
//# |
//# 01.04.2014 OG |
//# - add: Port_BLE2Wi() |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _CONNECT_H |
#define _CONNECT_H |
void Change_Output(uint8_t UartMode); |
void Port_BT2Wi(void); |
void Port_BT2FC(void); |
void Port_WiFly2Wi( void ); |
void Port_FC2CFG_BT(void); |
void Port_USB2FC(void); |
void Port_USB2Wi(void); |
void Port_BLE2Wi( void ); |
uint32_t Baud_to_uint32( uint8_t baud ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/cpu.h |
---|
0,0 → 1,41 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _CPU_H |
#define _CPU_H |
// Quarz Frequenz in Hz |
#define F_CPU 20000000UL |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/eeprom/eeprom.c |
---|
0,0 → 1,827 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.c |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# Initialisierung der neuen Parameter hinzugefügt |
//# |
//# 17.06.2014 OG |
//# - chg: ReadParameter() - Updatecode auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: Delete_EEPROM(() Unterstuetzung Config.PKTOffTimeout (ehemals LCD_DisplayMode) |
//# |
//# 30.05.2014 OG |
//# - chg: Delete_EEPROM() Config.Lipo_UOffset auf 10000 gesetzt (vorher 8500) |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# - fix: ReadParameter() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# ab EEprom-Version 138 |
//# - fix: Delete_EEPROM() - Vorbelegung von Config.MKParam_Favs von 255 auf 0 geaendert |
//# - fix: ReadParameter() - Kommentare zu Version 136 und 137 korrigiert |
//# bzgl. der PKT-Versionsangabe und Datum ergaenzt |
//# |
//# 06.04.2014 OG |
//# - chg: ReadParameter(): umgestellt auf lcdx_printp_center() |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: ReadParameter(): upgrade Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# - add: ReadParameter(): upgrade Config.UseBLE, Config.MKParam_Favs |
//# - add: Delete_EEPROM(): init von Config.Wlan_HomeSSID, Config.Wlan_HomePassword |
//# |
//# 01.04.2014 OG |
//# - add: Delete_EEPROM(): init von Config.UseBLE |
//# - add: Delete_EEPROM(): init von Config.MKParam_Favs |
//# |
//# 27.03.2014 OG |
//# - chg: ReadParameter() Niederländisch wird auf Englisch umgeschaltet |
//# -> Vorbereitung zum entfernen der Niederländischen Sprache |
//# |
//# 11.02.2014 Cebra |
//# - add: Delete EEProm: Config.OSD_ShowCellU = false; |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU in ReadParameter (EEpromversion 0x87) |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 07.07.2013 OG |
//# - chg: Strings fuer WLan gekuerzt |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 0x85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 0x84 |
//# |
//# 03.07.2013 OG |
//# - chg: ReadParameter() - PKT-Upgrade restrukturiert bzgl. inkrementelle |
//# Updates vorheriger Versionen |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 02.07.2013 OG |
//# - chg: frei geworden: Config.OSD_HomeMKView (nicht mehr benoetigt) |
//# |
//# 26.06.2013 OG |
//# - chg: frei geworden: Config.PKT_StartInfo (nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - fix: ReadParameter(): ergaenzt um bt_fixname() um ggf. ungueltige |
//# Zeichen im Bluetooth-Namen zu eliminieren |
//# |
//# 23.06.2013 OG |
//# - chg: Delete_EEPROM(): Default von Config.Lipo_UOffset von 6000 auf 8500 gesetzt |
//# - chg: Delete_EEPROM(): Default von Config.bt_name auf "PKT" gekuerzt |
//# - chg: Delete_EEPROM(): Config.bt_name -> nicht mehr mit Leerzeichen auffuellen! |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 13.06.2013 OG |
//# - fix: Config.PKT_Accutyp hinzugefuegt in Delete_EEPROM() |
//# - chg: Default GPS-LastPosition auf 0/0 gesetzt in Delete_EEPROM() |
//# - chg: Default Config.PKT_StartInfo auf false gesetzt in Delete_EEPROM() |
//# - chg: Code Layout |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung auf Version 81, Versionsanpassung angepasst |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup EEPROM Parameter |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <avr/interrupt.h> |
#include <avr/eeprom.h> |
#include <stdbool.h> |
#include <avr/wdt.h> |
#include "../lcd/lcd.h" |
#include "../main.h" |
#include "../timer/timer.h" |
#include "eeprom.h" |
#include "../wi232/Wi232.h" |
#include "../setup/setup.h" |
#include "../bluetooth/bluetooth.h" |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../osd/osd.h" |
#include "../uart/uart1.h" |
#include "../messages.h" |
#include "../osd/osddata.h" |
#include "../mksettings/paramset.h" |
//------------------------------------------------------------------------------------------ |
ST EEMEM EEStruct; |
ST Config; |
//############################################################# |
// ANMERKUNG OG: |
// |
// es ware besser wenn die gesamte Config-struct zum PC |
// gesendet wird anstatt aufbreitete Textlines. Das spart |
// Speicher im PKT (der ist limitiert). |
// |
// Die Aufbereitung der Daten (Umrechnungen) kann dann auf |
// der PC Seite erfolgen. |
// |
// Nachteil dabei: wenn sich die Config-struct des PKT |
// ändert muss auch das PKT-Tool auf dem PC angepasst werden. |
// |
//CB: deshalb ist das auch jetzt auskommentiert |
// |
//############################################################# |
//char printbuff[100]; |
//void print_data_int8( const char *text, int8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
//// |
//// |
//void print_data_uint8( const char *text, uint8_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_uint16( const char *text, uint16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_int16( const char *text, int16_t *variable) |
//{ |
// sprintf(printbuff, "%d", *variable); |
// uart1_puts_p(text); uart1_puts(printbuff); uart1_puts("\r\n"); |
//} |
// |
// |
//void print_data_char( const char *text, char *variable, uint8_t length) |
//{ |
// uart1_puts_p(text); |
// uart1_puts(variable); |
// uart1_puts("\r\n"); |
//} |
// |
// |
//void print_time( const char *text, PKTdatetime_t variable) |
//{ |
// uart1_puts_p(text); |
// sprintf(printbuff, "%d", variable.day); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.month); |
// uart1_puts(printbuff); |
// uart1_puts("."); |
// sprintf(printbuff, "%d", variable.year); |
// uart1_puts(printbuff); |
//// uart1_puts(" "); |
//// sprintf(printbuff, "%d", variable.seconds); |
//// uart1_puts(printbuff); |
// uart1_puts("\r\n"); |
//} |
// |
// |
// |
//void print_OSD_Statistic(void) |
//{ |
// print_time (PSTR("begin_StatTime:"),Config.OSD_Statistic.begin_StatTime); |
// print_time (PSTR("end_StatTime:"),Config.OSD_Statistic.end_StatTime); |
// print_data_uint16 (PSTR("total_FlyTime:"),&Config.OSD_Statistic.total_FlyTime); |
// print_data_uint16 (PSTR("last_FlyTime:"),&Config.OSD_Statistic.last_FlyTime); |
// print_data_uint16 (PSTR("count_Errorcode:"),&Config.OSD_Statistic.count_Errorcode); |
// print_data_int16 (PSTR("max_Altimeter:"),&Config.OSD_Statistic.max_Altimeter); |
// print_data_uint16 (PSTR("max_GroundSpeed:"),&Config.OSD_Statistic.max_GroundSpeed); |
// print_data_uint16 (PSTR("max_Distance:"),&Config.OSD_Statistic.max_Distance); |
// print_data_uint16 (PSTR("max_Current:"),&Config.OSD_Statistic.max_Current); |
// print_data_uint16 (PSTR("max_Capacity:"),&Config.OSD_Statistic.max_Capacity); |
// print_data_int16 (PSTR("max_Variometer:"),&Config.OSD_Statistic.max_Variometer); |
// print_data_int8 (PSTR("max_AngleNick:"),&Config.OSD_Statistic.max_AngleNick); |
// print_data_int8 (PSTR("max_AngleRoll:"),&Config.OSD_Statistic.max_AngleRoll); |
// print_data_uint8 (PSTR("RC Quality:"),&Config.OSD_Statistic.max_RCQuality); |
// print_data_int16 (PSTR("max_TopSpeed:"),&Config.OSD_Statistic.max_TopSpeed); |
// print_data_int8 (PSTR("min_AngleNick:"),&Config.OSD_Statistic.min_AngleNick); |
// print_data_int8 (PSTR("min_AngleRoll:"),&Config.OSD_Statistic.min_AngleRoll); |
// print_data_uint8 (PSTR("min_RCQuality:"),&Config.OSD_Statistic.min_RCQuality); |
// print_data_uint8 (PSTR("min_UBat:"),&Config.OSD_Statistic.min_UBat); |
// print_data_uint8 (PSTR("LiPoCells:"),&Config.OSD_Statistic.LiPoCells); |
// print_data_uint8 (PSTR("BL_Count:"),&Config.OSD_Statistic.BL_Count); |
//} |
//void SendConfigData(void) |
//{ |
// print_data_uint8 (strGet(START_VERSIONCHECK),&Config.Version ); |
// print_data_uint8 (strGet(LOWBAT),&Config.MK_LowBat ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayTimeout ); |
// print_data_uint8 (strGet(LOWBAT),&Config.DisplayLanguage ); |
// print_data_uint8 (strGet(WITXRX),&Config.WiTXRXChannel); |
// print_data_uint8 (strGet(WINETWG),&Config.WiNetworkGroup); |
// print_data_uint8 (strGet(WINETWM),&Config.WiNetworkMode); |
// print_data_uint8 (strGet(WITIMEOUT),&Config.WiTXTO); |
// print_data_uint8 (strGet(WIUART),&Config.WiUartMTU); |
// print_data_uint8 (strGet(DISPLAY8),&Config.OSD_ShowMKSetting); |
// print_data_uint8 (strGet(DISPLAY7),&Config.LCD_DisplayMode); |
// print_data_uint8 (strGet(DISPLAY6),&Config.LCD_Kontrast); |
// print_data_uint8 (strGet(DISPLAY5),&Config.LCD_Helligkeit); |
// print_data_uint8 (PSTR("USB-Betrieb:"),&Config.USBBT); |
// print_data_uint8 (PSTR("Uart-FC/Wi:"),&Config.U02SV2); |
// print_data_uint8 (PSTR("PKT-Debug:"),&Config.Debug); |
// print_data_uint8 (PSTR("Wi232 eingebaut:"),&Config.UseWi); |
// print_data_uint8 (PSTR("BTM222 eingebaut:"),&Config.UseBT); |
// print_data_uint8 (PSTR("Wi232 ok:"),&Config.WiIsSet); |
// print_data_uint8 (PSTR("BTM222 ok:"),&Config.BTIsSet); |
// print_data_uint8 (PSTR("BTM222 Slave:"),&Config.BTIsSlave); |
// |
// print_data_char (PSTR("BTM222 Pin:"),&Config.bt_pin[0],bt_pin_length ); |
// print_data_char (PSTR("BTM222 Name:"),&Config.bt_name[0],bt_name_length); |
// print_data_char (PSTR("BTM222 REID:"),&Config.RE_ID[0],RE_ID_length ); |
// print_data_char (PSTR("BTM222 MAC:"),&Config.bt_Mac[0],bt_mac_length); |
// print_data_char (PSTR("GPS DevName:"),&Config.gps_UsedDevName[0],20); |
// print_data_char (PSTR("GPS MAC:"),&Config.gps_UsedMac[0],14); |
// |
// print_data_int32 (PSTR("LastLongitude:"),&Config.LastLongitude); |
//// print_data_int32(PSTR("LastLatitude:"),&Config.LastLatitude); |
// |
// print_data_uint8 (PSTR("PKT_IdleBeep:"),&Config.PKT_IdleBeep); |
// print_data_uint8 (strGet(DISPLAY2),&Config.PKT_StartInfo); |
// print_data_uint16 (strGet(LIPO3),&Config.Lipo_UOffset); |
// print_data_uint8 (strGet(LIPO2),&Config.PKT_Accutyp); |
// print_data_uint8 (strGet(DISPLAY9),&Config.OSD_RCErrorbeep); |
// print_data_uint8 (strGet(OSD_Invert_Out),&Config.OSD_InvertOut); |
// print_data_uint8 (strGet(OSD_LED_Form),&Config.OSD_LEDform); |
// print_data_uint8 (strGet(OSD_Send_OSD),&Config.OSD_SendOSD); |
// print_data_uint8 (strGet(FALLSPEED),&Config.OSD_Fallspeed); |
// print_data_uint8 (strGet(OSD_VARIOBEEP),&Config.OSD_VarioBeep); |
// print_data_uint8 (strGet(OSD_HOMEMKVIEW),&Config.OSD_HomeMKView); |
// print_data_uint16 (strGet(OSD_MAHWARNING),&Config.OSD_mAh_Warning); |
// print_data_uint8 (strGet(OSD_SCREENMODE),&Config.OSD_ScreenMode); |
// print_data_uint8 (strGet(OSD_LIPOBAR),&Config.OSD_LipoBar); |
// print_data_uint8 (strGet(PKT_BAUDRATE),&Config.PKT_Baudrate); |
// print_data_uint16 (strGet(FOLLOWME_1),&Config.FM_Refresh); |
// print_data_uint16 (strGet(FOLLOWME_2),&Config.FM_Speed); |
// print_data_uint16 (strGet(FOLLOWME_3),&Config.FM_Radius); |
// print_data_uint8 (strGet(HWSOUND),&Config.HWSound); |
// print_data_uint8 (strGet(HWBEEPER),&Config.HWBeeper); |
// print_data_uint8 (PSTR("sIdxSteps:"),&Config.sIdxSteps); |
// print_data_uint8 (strGet(SV_TEST3),&Config.servo_frame); |
// print_data_uint8 (strGet(SV_SINGLESTEP),&Config.single_step); |
// print_data_uint8 (strGet(SV_COUNTTEST),&Config.repeat); |
// print_data_uint8 (strGet(SV_PAUSEEND),&Config.pause); |
// print_data_uint8 (strGet(SV_PAUSEINC),&Config.pause_step); |
//// print_data_int8 (,&Config.tracking); |
//// print_data_int8 (,&Config.track_tx); |
// print_data_uint16 (PSTR("Stick1_min:"),&Config.stick_min[0]); |
// print_data_uint16 (PSTR("Stick2_min:"),&Config.stick_min[1]); |
// print_data_uint16 (PSTR("Stick3_min:"),&Config.stick_min[2]); |
// print_data_uint16 (PSTR("Stick4_min:"),&Config.stick_min[3]); |
// print_data_uint16 (PSTR("Stick5_min:"),&Config.stick_min[4]); |
// print_data_uint16 (PSTR("Stick1_max:"),&Config.stick_max[0]); |
// print_data_uint16 (PSTR("Stick2_max:"),&Config.stick_max[1]); |
// print_data_uint16 (PSTR("Stick3_max:"),&Config.stick_max[2]); |
// print_data_uint16 (PSTR("Stick4_max:"),&Config.stick_max[3]); |
// print_data_uint16 (PSTR("Stick5_max:"),&Config.stick_max[4]); |
// print_data_uint8 (PSTR("Stick1_typ:"),&Config.stick_typ[0]); |
// print_data_uint8 (PSTR("Stick2_typ:"),&Config.stick_typ[1]); |
// print_data_uint8 (PSTR("Stick3_typ:"),&Config.stick_typ[2]); |
// print_data_uint8 (PSTR("Stick4_typ:"),&Config.stick_typ[3]); |
// print_data_uint8 (PSTR("Stick5_typ:"),&Config.stick_typ[4]); |
// print_data_uint8 (PSTR("Stick1_neutral:"),&Config.stick_neutral[0]); |
// print_data_uint8 (PSTR("Stick2_neutral"),&Config.stick_neutral[1]); |
// print_data_uint8 (PSTR("Stick3_neutral:"),&Config.stick_neutral[2]); |
// print_data_uint8 (PSTR("Stick4_neutral:"),&Config.stick_neutral[3]); |
// print_data_uint8 (PSTR("Stick5_neutral:"),&Config.stick_neutral[4]); |
// print_data_uint8 (strGet(LIPO_MESSUNG),&Config.Lipomessung); |
// print_OSD_Statistic(); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWiInitFlag(void) |
{ |
Config.WiIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTInitFlag(void) |
{ |
Config.BTIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteWLInitFlag(void) |
{ |
Config.WLIsSet = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTSlaveFlag(void) |
{ |
Config.BTIsSlave = true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteBTMasterFlag(void) |
{ |
Config.BTIsSlave = false; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteLastPosition( uint32_t ELongitude, uint32_t ELatitude) |
{ |
Config.LastLongitude = ELongitude; |
Config.LastLatitude = ELatitude; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ReadParameter( void ) |
{ |
eeprom_read_block( (void*)&Config, (const void*)&EEStruct, sizeof(ST) ); |
// DEBUG !! |
//Config.Version = 0x82; |
//WriteParameter(); |
//return; |
//------------------- |
// ggf. ungueltige Zeichen aus dem |
// Bluetooth-Namen eliminieren |
//------------------- |
#ifdef USE_BLUETOOTH |
bt_fixname(); |
#endif |
//------------------- |
// 27.03.2014 OG |
// Als Vorbereitung die Niederländische Sprache |
// zu entfernen wird hier ggf. Niederländisch |
// auf Englisch umgeschaltet |
//------------------- |
if( Config.DisplayLanguage == 2 ) // 2 = ehemals Niederländisch |
Config.DisplayLanguage = 1; // -> wird zu 1 = Englisch |
//------------------- |
// 0. EEprom-Version nicht geaendert |
//------------------- |
if( Config.Version == EEpromVersion ) // nichts zu tun... |
{ |
return; // !!! EXIT !!! |
} |
//------------------- |
// 1. PKT-Upgrade NICHT Update faehig |
// ODER Downgrade |
// |
// 0x77 = PKT Version 3.6.6a |
//------------------- |
if( (Config.Version < 0x77) || (Config.Version > EEpromVersion) ) |
{ |
Delete_EEPROM(); |
return; // !!! EXIT !!! |
} |
//------------------- |
// 2. PKT-Upgrade |
//------------------- |
//++++++++++++++++++++++++++++ |
// bei Eeprom-Versionwechsel |
// IMMER loeschen |
//++++++++++++++++++++++++++++ |
#ifdef USE_WAYPOINTS |
PointList_Clear(); // Init Waypoints |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
//++++++++++++++++++++++++++++ |
// inkrementelle Updates |
//++++++++++++++++++++++++++++ |
//----------------------------- |
//--- ab PKT v3.6.9bX6 |
//----------------------------- |
if( Config.Version < 0x81 ) |
{ |
Config.timezone = 1; |
Config.summertime = 1; |
} |
//----------------------------- |
//--- ab PKT v3.7.0b |
//----------------------------- |
if( Config.Version < 0x82 ) |
{ |
Config.OSD_ShowMKSetting = true; |
} |
//----------------------------- |
//--- ab PKT v3.7.0e |
//----------------------------- |
if( Config.Version < 0x83 ) |
{ |
Config.UseWL = false; |
Config.WLIsSet = false; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
} |
//----------------------------- |
//--- ab PKT v3.7.0eX1 |
//----------------------------- |
if( Config.Version < 0x84 ) |
{ |
Config.OSD_UseScreen = 0xffffffff; // alle verfuegbaren OSD-Screens eingeschaltet |
} |
//----------------------------- |
//--- ab PKT v3.7.0f |
//----------------------------- |
if( Config.Version < 0x85 ) |
{ |
Config.Wlan_DHCP = 0; |
Config.Wlan_WPA = 0; |
Config.Wlan_Adhoc = true; |
Config.Wlan_Channel = 1; |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
} |
//----------------------------- |
//--- ab PKT v3.7.3cX5 |
//----------------------------- |
if( Config.Version < 0x87 ) |
{ |
Config.OSD_ShowCellU = false; // |
} |
//----------------------------- |
//--- 01.04.2014 OG |
//--- ab PKT v3.7.4aX7 |
//----------------------------- |
if( Config.Version < 136 ) |
{ |
Config.UseBLE = false; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
memset( Config.MKParam_Favs, param_EOF, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers |
} |
//----------------------------- |
//--- 06.04.2014 OG |
//--- ab PKT v3.7.4aX8 |
//----------------------------- |
if( Config.Version < 137 ) |
{ |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
} |
//----------------------------- |
//--- 07.05.2014 OG |
//--- ab PKT v3.80bX3 |
//----------------------------- |
if( Config.Version < 138 ) |
{ |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Array von MK-Parameter Favoriten des Benutzers (Vorbelegung jetzt 0) |
} |
//----------------------------- |
//--- 17.06.2014 OG |
//--- ab PKT v3.80cX5 |
//----------------------------- |
if( Config.Version < 139 ) |
{ |
Config.PKTOffTimeout = 0; // autom. PKT ausschalten nach n Minuten (0=immer an) |
} |
//------------------- |
// 3. Update Message |
//------------------- |
Config.Version = EEpromVersion; // Update EEPROM version number |
sei(); |
lcd_cls(); |
lcdx_printp_center( 2, PSTR("EEProm updated to"), MNORMAL, 0,0); |
lcdx_printp_center( 3, PSTR("new Version") , MNORMAL, 0,0); |
lcdx_printp_center( 5, PSTR("check settings!") , MNORMAL, 0,0); |
lcd_printp_at (18, 7, PSTR("OK") , MNORMAL); // Keyline |
while( !get_key_press(1 << KEY_ENTER) ); |
WriteParameter(); |
cli(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void WriteParameter( void ) |
{ |
copy_line(7); |
lcd_printp_at( 0, 7, PSTR(" Write EEPROM "), MNORMAL); |
eeprom_update_block( (const void*)&Config, (void*)&EEStruct, sizeof(ST) ); |
paste_line(7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delete_EEPROM( void ) |
{ |
uint8_t i; |
// EEPROM auf Default setzen |
lcd_cls(); |
lcd_printp_at( 0, 0, PSTR(" EEPROM Parameter "), MINVERS); |
lcd_printp_at( 0, 1, PSTR("werden auf") , MNORMAL); |
lcd_printp_at( 0, 2, PSTR("Standardwerte gesetzt"), MNORMAL); |
Config.MK_LowBat = 137; // 13,7V |
Config.DisplayTimeout = 0; // Display immer an (autom. LCD ausschalten nach n Minuten) |
Config.DisplayLanguage = 5; // default undefined |
Config.WiTXRXChannel = 1; // Kanal 1 MK Standard |
Config.WiNetworkGroup = 66; // Gruppe 66 MK Standard |
Config.WiNetworkMode = NetMode_Normal; // MK Standard |
Config.WiTXTO = TWaitTime16; // MK Standard |
Config.WiUartMTU = UartMTU64; // MK Standard |
Config.OSD_ShowMKSetting= true; // Anzeige MK-Setting beim OSD Start |
Config.PKTOffTimeout = 0; // PKT immer an (autom. PKT ausschalten nach n Minuten) |
Config.LCD_Kontrast = 20; // Kontrast normal |
Config.LCD_Helligkeit = 100; // Helligkeit in % |
Config.USBBT = 0; // USB Betrieb |
Config.U02SV2 = 0; // SV2 (Kabel) Standard |
Config.Debug = 0; // kein Debug |
Config.UseWi = false; // Wi.232 eingebaut? |
Config.UseBT = false; // BT-222 eingebaut? |
Config.WiIsSet = false; // Flag für die Initialisierung Wi232 |
Config.BTIsSet = false; // Flag für die Initialisierung Bluetooth |
Config.BTIsSlave = true; // Slave Flag setzen |
Config.PKT_IdleBeep = 0; // kein Piepsen bei Inaktivität |
Config.PKT_StartInfo = false; // * FREI * (ehemals: PKT Startinfo anzeigen) |
Config.OSD_RCErrorbeep = true; // OSD Receiveerrorbeep |
Config.OSD_InvertOut = false; // LED Anzeige invertiren |
Config.OSD_LEDform = 1; // Form der Anzeige ( + oder schwarz) |
Config.OSD_SendOSD = false; // OSD Daten an SV2 |
Config.OSD_Fallspeed = 40; // maximale Sinkrate |
Config.OSD_VarioBeep = 1; // Vario Beep ein |
Config.OSD_HomeMKView = true; // * FREI * (ehemals: Home Circle from MK View) |
Config.OSD_mAh_Warning = 10000; // mAh Warnschwelle |
Config.OSD_ScreenMode = 0; // Variante des OSD Screen |
Config.OSD_LipoBar = 0; // Bargraphanzeige für MK Lipo |
Config.PKT_Baudrate = Baud_57600; // Baudrate für BT und Wi232 |
Config.PKT_Accutyp = true; // verwendeter Akkutyp (true=Lipo, false=LiON) |
Config.Lipo_UOffset = 10000; // Offset für PKT-Lipomessung |
Config.FM_Azimuth = 0; // FollowMe Azimuth |
Config.FM_Distance = 0; // FollowMe Distance |
Config.FM_Speed = 30; // FollowMe Speed in m/s *0.1 |
Config.FM_Radius = 5; // Waypoint Tolerance Radius in meter |
Config.HWSound = 0; // Hardware Sounderweiterung an PD7 |
Config.HWBeeper = 1; // Hardware Beeper an PC7 |
Config.Volume = 0; // Lautstaerke |
Config.LastLongitude = 0x00000000; |
Config.LastLatitude = 0x00000000; |
strcpy_P(Config.bt_pin , PSTR("0000")); |
strcpy_P(Config.bt_name, PSTR("PKT")); // Bluetooth-Name max. 10 Zeichen! - NICHT mit Leerzeichen auffüllen (bt_name_length) |
strcpy_P(Config.bt_Mac , PSTR("0000-00-000000")); |
strcpy_P(Config.RE_ID , PSTR("0000")); |
for( i = 0; i < 20; i++) |
{ |
Config.gps_UsedDevName[i] = 0; // benutztes GPS Device Name |
} |
for( i = 0; i < 14; i++) |
{ |
Config.gps_UsedMac[i] = '0'; // benutztes GPS Device Mac Adresse |
} |
Config.gps_UseGPS = false; // ist GPS aktiv? |
Config.gps_UsedGPSMouse = GPS_Bluetoothmouse1; |
//Config.WiIsSet = false; // 15.07.2013 CB doppelt drin |
//Config.BTIsSet = false; |
Config.Version = EEpromVersion; |
Config.sIdxSteps = STEPS_255; |
Config.servo[0].rev = SERVO_REV; |
Config.servo[0].min = SERVO_I0_RIGHT; |
Config.servo[0].max = SERVO_I0_LEFT; |
Config.servo[0].mid = SERVO_I0_MIDDLE; |
Config.servo[1].rev = SERVO_REV; |
Config.servo[1].min = SERVO_I0_RIGHT; |
Config.servo[1].max = SERVO_I0_LEFT; |
Config.servo[1].mid = SERVO_I0_MIDDLE; |
Config.servo_frame = SERVO_PERIODE; |
Config.single_step = SINGLE_STEP; // nur bei Test-Servo |
Config.repeat = REPEAT; // nur bei Test-Servo |
Config.pause = PAUSE; // nur bei Test-Servo |
Config.pause_step = PAUSE_STEP; // nur bei Test-Servo |
Config.tracking = TRACKING_MIN; |
Config.track_hyst = TRACKING_HYSTERESE; |
Config.track_tx = 0; |
for( i=0; i<5; i++) |
{ |
Config.stick_min[i] = 30+i; // Joystick |
Config.stick_max[i] = 270+i; // Joystick |
Config.stick_typ[i] = 0; // Joystick |
Config.stick_dir[i] = 0; // Joystick |
Config.stick_neutral[i] = 0; // Joystick |
} |
Config.Lipomessung = true; |
Config.timezone = 1; |
Config.summertime = 1; |
strcpy_P( Config.Wlan_SSID , PSTR("PKT")); // Wlan |
strcpy_P( Config.Wlan_Password, PSTR("12345678")); // Wlan |
Config.WLIsSet = false; // Wlan |
Config.UseWL = false; |
Config.OSD_UseScreen = 0xffffffff; // alle OSD-Screens eingeschaltet |
Config.Wlan_DHCP = 0; // kein DHCP |
Config.Wlan_WPA = 0; // 0 = adhoc 1= WPA2 |
Config.Wlan_Adhoc = true; // Adhoc |
Config.Wlan_Channel = 1; // Wlan Channel |
strcpy_P( Config.Wlan_IP , PSTR("169.254.001.001")); |
strcpy_P( Config.Wlan_Netmask, PSTR("255.255.000.000")); |
strcpy_P( Config.Wlan_Gateway, PSTR("169.254.001.001")); |
Config.OSD_ShowCellU = false; |
Config.UseBLE = false; |
memset( Config.MKParam_Favs, 0, MAX_MKPARAM_FAVORITES ); // Favoriten MK-Parameter (Anzahl: MAX_MKPARAM_FAVORITES = 10) |
strcpy_P( Config.Wlan_HomeSSID , PSTR("")); // WiFly Home-Wlan: SSID (Home-WLAN) |
strcpy_P( Config.Wlan_HomePassword, PSTR("")); // WiFly Home-Wlan: Passwort (Home-WLAN) |
//------------------------------------------------- |
//------------------------------------------------- |
#ifdef USE_WAYPOINTS |
PointList_Clear(); |
#endif |
STAT_Init(); // Init OSD Statistik |
GPS_User_Init(); // Init GPS Positionen |
MKErr_Log_Init(); // Init MK Errorlog |
WriteParameter(); |
//lcd_printp_at (0, 4, PSTR("Waypoints loeschen"), 0); |
//EEWayPointList_Clear(); |
lcd_printp_at( 0, 6, PSTR("!!Check Parameter!! "), MNORMAL); |
lcd_printp_at(18, 7, PSTR("OK") , MNORMAL); |
sei(); |
set_beep( 200, 0x0080, BeepNormal); |
while( !get_key_press (1 << KEY_ENTER) ); |
//#if defined HWVERSION3_9 |
clr_V_On(); |
//#else |
// |
// wdt_enable( WDTO_250MS ); |
// while (1) |
// {;} |
//#endif |
} |
//-------------------------------------------------------------- |
// |
//void EEWayPointList_Clear(void) // löschen der Waypointliste im EEProm |
//{ |
// uint8_t i; |
// PKTWayPoint.Waypoint.Position.Latitude = 0; |
// PKTWayPoint.Waypoint.Position.Longitude = 0; |
// PKTWayPoint.Waypoint.Position.Altitude = 0; |
// PKTWayPoint.Waypoint.Heading = 361; |
// for(i = 0; i < MAX_WPLIST_LEN; i++) |
// { |
// PKTWayPointDirectory.WPList.WPDirectory[i] = 0; |
// } |
// for(i = 0; i < NumberOfWaypoints; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWayPointList[i].WPIndex, i); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Position.Status, INVALID); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Latitude, (void*)&EEWayPointList[i].Waypoint.Position.Latitude, sizeof(EEWayPointList[i].Waypoint.Position.Latitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Longitude, (void*)&EEWayPointList[i].Waypoint.Position.Longitude, sizeof(EEWayPointList[i].Waypoint.Position.Longitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Position.Altitude, (void*)&EEWayPointList[i].Waypoint.Position.Altitude, sizeof(EEWayPointList[i].Waypoint.Position.Altitude)); |
// eeprom_write_block ((const void*)&PKTWayPoint.Waypoint.Heading, (void*)&EEWayPointList[i].Waypoint.Heading, sizeof(EEWayPointList[i].Waypoint.Heading)); |
// |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.ToleranceRadius, 0); // in meters, if the MK is within that range around the target, then the next target is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.HoldTime, 0); // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Type, POINT_TYPE_INVALID); |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.Event_Flag, 0); // future implementation |
// eeprom_write_byte (&EEWayPointList[i].Waypoint.AltitudeRate, 0); // no change of setpoint |
// } |
// for(i = 0; i < NumberOfWPLists; i++) |
// { |
// lcd_printp (PSTR("."), 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListnumber, i); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.WPListAktiv, false); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.POI_CAM_NICK_CTR, 0); |
// eeprom_write_byte (&EEWPDirectory[i].WPList.UsePOI, 0); |
// eeprom_write_block ((const void*)&PKTWayPointDirectory.WPList.WPDirectory, (void*)&EEWPDirectory[i].WPList.WPDirectory, sizeof(EEWPDirectory[i].WPList.WPDirectory)); |
// |
// } |
// lcd_printp (PSTR("\r\n"), 0); |
//} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/eeprom/eeprom.h |
---|
0,0 → 1,333 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2013 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY eeprom.h |
//# |
//# 03.08.2015 CB |
//# - chg: Obsolete Parameter geändert und für FollowMe verwendet, EEprom Version bleibt gleich |
//# Parameter FM_Refresh in FM_Azimuth geändert |
//# Parameter hyst_u_min in FM_Distance geändert |
//# |
//# 25.06.2014 OG |
//# - chg: Kommentare der eeprom Struct aktualisert bzgl. FollowMe und Tracking |
//# |
//# 17.06.2014 OG |
//# - chg: EEpromversion erhoeht auf 139 |
//# |
//# 13.06.2014 OG |
//# - chg: LCD_DisplayMode (Obsolete) geaendert zu PKTOffTimeout |
//# |
//# 31.05.2014 OG |
//# - Recherche welche Config-Werte ueberahupt noch im PKT-Code verwendet werden |
//# und ggf. "OBSOLETE" Anmerkung in der Config-Struct |
//# - Config-Struct wurde um etliche Kommentare ergaenzt bzgl. Verwendungszweck |
//# |
//# 30.05.2014 OG |
//# - chg: Kommentare geaendert |
//# |
//# 26.05.2014 OG |
//# - chg: Config.LCD_DisplayMode als "OBSOLETE" markiert |
//# |
//# 07.05.2014 OG |
//# - chg: EEpromversion erhoeht auf 138 |
//# -> keine neuen Parameter, neue Version nur fuer geanderte |
//# Initialisierung von MKParam_Favs |
//# |
//# 06.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 137 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# |
//# 01.04.2014 OG |
//# - chg: EEpromversion erhoeht auf 136 |
//# - chg: ACHTUNG! EEpromversion umgestellt von HEX auf DEZIMAL: 0x87 => 135 |
//# - add: Config.UseBLE |
//# - add: Config.MKParam_Favs |
//# - add: MAX_MKPARAM_FAVORITES |
//# |
//# 03.02.2014 OG |
//# - add: Config.OSD_ShowCellU; EEpromversion erhoeht auf 0x87 |
//# |
//# 28.08.2013 Cebra |
//# - chg: LastLatitude war uint32_t, auf int32_t korrigiert |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 85 |
//# |
//# 04.07.2013 OG |
//# - add: Config.OSD_UseScreen; Epromversion erhoeht auf 84 |
//# |
//# 02.07.2013 Cebra |
//# - add: neue Parameter fuer Wlan; EEpromversion erhoeht auf 83 |
//# |
//# 20.06.2013 CB |
//# - chg: EEPROM Versionsänderung auf 82 wegen Wechsel LCD_Orientation zu OSD_ShowMKSetting |
//# |
//# 15.06.2013 OG |
//# - chg: Config.LCD_ORIENTATION zu Config.OSD_ShowMKSetting |
//# |
//# 31.05.2013 CB |
//# - chg: EEPROM Strukturänderung, Listen/Statistik ganz ans Ende gesetzt |
//# |
//# 31.05.2013 OG |
//# - chg: Eeprom Version auf 81 wegen Erweiterungen der Statistik-Daten |
//# - chg: Code-Layout |
//# |
//# 12.04.2013 CB |
//# - chg: Kommentarergänzung U02SV2 |
//# |
//# 28.03.2013 CB |
//# - add: save and upgrade OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//############################################################################ |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <stdbool.h> |
#include "../mk-data-structs.h" |
#include "../connect.h" |
#include "../tracking/ng_servo.h" |
#include "../waypoints/waypoints.h" |
#include "../osd/osd.h" |
//[General] |
//FileVersion = 2 |
//NumberOfWaypoints = 15 |
//UsePOI = 0 |
//POI_CAM_NICK_CTRL = 0 |
//[POI] |
//Altitude = 1 |
//Latitude = 46.7140763 |
//Longitude = 19.2507334 |
//[Waypoint1] |
//Latitude = 46.7145686 |
//Longitude = 19.2515702 |
//Radius = 10 |
//Altitude = 15 |
//ClimbRate = 0 |
//DelayTime = 4 |
//WP_Event_Channel_Value = 96 |
//Heading = 180 |
#define EEpromVersion 139 // wird nach jeder Parametererweiterung hochgezählt |
// Anmerkung 01.04.2014 OG: umgestellt auf DEZIMALE schreibweise (wiso war das vorher Hex?) |
#define NumberOfWaypoints 55 // Anzahl der Waypoints in der EEPromliste |
#define NumberOfWPLists 5 // Anzahl WP Listen im PKT |
#define MAX_LIST_LEN 31 // Länge Waypointlist |
#define MAX_MKPARAM_FAVORITES 10 // Anzahl der MK-Parameter Favoriten (nicht aendern!) |
#define bt_pin_length 4 |
#define RE_ID_length 4 // Länge der RE-ID |
#define bt_name_length 10 |
#define bt_mac_length 14 |
#define GPS_Bluetoothmouse1 0 // NMEA BT-Mouse |
#define GPS_Mikrokopter 1 // GPS Daten vom MK für Tracking |
#define wlan_password_length 10 // Länge Wlan-Passwort |
#define wlan_ssid_length 10 // Länge Wlan-SSID |
#define wlan_ip_length 15 |
#define wlan_netmask_length 15 |
#define wlan_gateway_length 15 |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
#define MAX_WPLIST_LEN 31 |
//typedef struct { |
// uint8_t rev; |
// uint16_t min; |
// uint16_t max; |
// uint16_t mid; |
//} servo_t; |
typedef struct |
{ |
uint8_t WPIndex; // Index in der EEpromliste |
Point_t Waypoint; // Waypoint |
} WayPoints; |
typedef struct |
{ |
uint8_t WPListnumber; // Nummer der WP Liste im PKT |
uint8_t WPListAktiv; // Liste aktiv |
uint8_t WPDirectory[31]; // Enthält die Indexe der Waypoints im EEPROM |
uint8_t UsePOI; |
uint8_t POI_CAM_NICK_CTR; |
} WPListHeader; |
typedef struct |
{ |
WPListHeader WPList; // Waypointliste im PKT |
} WPListDirectory; |
typedef struct SStructure |
{ |
uint8_t Version; // PKT-Eeprom Version! |
uint8_t MK_LowBat; // MK-LowBat Warnung u.a. fuer osd.c |
uint8_t DisplayTimeout; // autom. LCD ausschalten nach n Minuten |
uint8_t DisplayLanguage; // eingestellte Sprache |
uint8_t WiTXRXChannel; // Wi.232: |
uint8_t WiNetworkGroup; // Wi.232: |
uint8_t WiNetworkMode; // Wi.232: |
uint8_t WiTXTO; // Wi.232: |
uint8_t WiUartMTU; // Wi.232: |
uint8_t OSD_ShowMKSetting; // Anzeige MK-Setting beim OSD Start |
uint8_t PKTOffTimeout; // autom. PKT ausschalten nach n Minuten |
uint8_t LCD_Kontrast; // LCD-Kontrast |
uint8_t LCD_Helligkeit; // LCD-Helligkeit - Aktuell nicht mehr verfuegbar weil auskommentiert! (mit HW 3.9x geht das sowiso nicht mehr!) |
uint8_t USBBT; // ## OBSOLETE ## 31.05.2014 OG: wird nicht verwendet! (wofuer war da in der Vergangenheit?) |
uint8_t U02SV2; // 0=Wi232-Verbindung zum MK, 1 = Kabelverbindung zum MK |
uint8_t Debug; // ??? OBSOLETE ??? 31.05.2014 OG: wird eigentlich nicht mehr verwendet (war zum debuggen vom PKT) |
uint8_t UseWi; // Wi232 wird genutzt |
uint8_t UseBT; // BT wird genutzt |
uint8_t WiIsSet; // Wi232 ist initialisiert |
uint8_t BTIsSet; // BT ist initialisiert |
uint8_t BTIsSlave; // Slave Flag |
char bt_pin[bt_pin_length + 1]; // BT Pinnummer |
char bt_name[bt_name_length + 1]; // BT Name |
char RE_ID[RE_ID_length + 1]; // RE-ID |
char bt_Mac[bt_mac_length + 1]; // MAC-Adresse BTM222 |
char gps_UsedDevName[20]; // benutztes GPS Device Name |
char gps_UsedMac[14]; // benutztes GPS Device Mac Adresse |
uint8_t gps_UseGPS; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() - "ist GPS aktiv?" |
uint8_t gps_UsedGPSMouse; // ## GGF. OBSOLETE ## 25.06.2014 OG: siehe setup.c/Setup_GPSMouse() und tracking.c/PKT_tracking() - "GPS Maustyp" |
int32_t LastLongitude; // Letzte Position |
int32_t LastLatitude; // Letzte Position |
uint8_t PKT_IdleBeep; // ## OBSOLETE ## 31.05.2014 OG: wurde das jemals verwendet? |
uint8_t PKT_StartInfo; // ## OBSOLETE ## 26.06.2013 OG: ehemals "PKT Startinfo anzeigen" |
uint16_t Lipo_UOffset; // Offset für die Lipospannugsmessung |
uint8_t PKT_Accutyp; // verwendeter Akkutyp |
uint8_t OSD_RCErrorbeep; // Empfangsausffallwarnung im OSD Screen |
uint8_t OSD_InvertOut; // Out1/2 invertiert anzeigen |
uint8_t OSD_LEDform; // Form der Anzeige ( + oder schwarz) |
uint8_t OSD_SendOSD; // OSD Daten an SV2 senden |
uint8_t OSD_Fallspeed; // maximale Sinkrate |
uint8_t OSD_VarioBeep; // Vario Beep im OSD Screen |
uint8_t OSD_HomeMKView; // ## OBSOLETE ## 02.07.2013 OG: ehemals "Home Circle from MK-View" |
uint16_t OSD_mAh_Warning; // mAh Warnschwelle |
uint8_t OSD_ScreenMode; // Variante des OSD-Screen |
uint8_t OSD_LipoBar; // Bargraphanzeige für MK Lipo |
uint8_t PKT_Baudrate; // Baudrate für BT und Wi232 |
uint16_t FM_Azimuth; // Azimuth für FollowMe 4.8.2015 CB |
uint16_t FM_Speed; // FollowMe Speed in m/s *0.1 |
uint16_t FM_Radius; // Waypoint Tolerance Radius in meter |
uint8_t HWSound; // Hardware Sounderweiterung an PD7 |
uint8_t HWBeeper; // Hardware Beeper an PC7 |
uint8_t Volume; // Lautstärke |
servo_t servo[2]; // Tracking: |
uint8_t sIdxSteps; // Tracking: |
uint16_t FM_Distance; // Distance für FollowMe 4.8.2015 CB |
uint8_t servo_frame; // Tracking: |
uint8_t single_step; // Tracking: |
uint8_t repeat; // Tracking: |
uint8_t pause; // Tracking: |
uint8_t pause_step; // Tracking: |
uint8_t tracking; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_hyst; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint8_t track_tx; // ## OBSOLETE ## 31.05.2014 OG: das wird nirgendwo verwendet! |
uint16_t stick_min[5]; // Joystick: Stickparameter |
uint16_t stick_max[5]; // Joystick: |
uint8_t stick_typ[5]; // Joystick: |
uint8_t stick_dir[5]; // Joystick: |
uint8_t stick_neutral[5]; // Joystick: Stick neutralisierend: ja/nein |
uint8_t Lipomessung; // wenn Lipomessung deaktiviert(Lötbrücke öffnen), kann der Kanal als Stick verwendet werden |
int8_t timezone; // Einstellbereich -12 .. 0 .. 12 Defaultwert: 1 (entspricht unserer Zeitzone) |
uint8_t summertime; // Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) Defaultwert: 1 (entspricht unserer Sommerzeit in D) |
uint8_t UseWL; // WLAN (WiFly) wird an SV2 genutzt (SV2 Patch erforderlich) |
char Wlan_SSID[wlan_ssid_length+1]; // Wlan SSID |
char Wlan_Password[wlan_password_length+1]; // Wlan Passwort |
uint8_t WLIsSet; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Wlan ist initialisiert" |
uint32_t OSD_UseScreen; // welche OSD-Screens nutzen (Bit-codiert) |
uint8_t Wlan_DHCP; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-4" |
uint8_t Wlan_WPA; // ## OBSOLETE ## 31.05.2014 OG: ehemals "0-8" |
uint8_t Wlan_Adhoc; // ## OBSOLETE ## 31.05.2014 OG: ehemals "false = AP , true = Adhoc" |
uint8_t Wlan_Channel; // 0-13 |
char Wlan_IP[wlan_ip_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "IP-Adresse" (16 Bytes!) |
char Wlan_Netmask[wlan_netmask_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "SubnetMask" (16 Bytes!) |
char Wlan_Gateway[wlan_gateway_length+1]; // ## OBSOLETE ## 31.05.2014 OG: ehemals "Defaultgateway" (16 Bytes!) |
uint8_t OSD_ShowCellU; // OSD Azeige: Anzeige UBat als Einzelzellenspannung (ja/nein) |
uint8_t UseBLE; // Bluetooth 4 LowPower wird genutzt (RedBearLab BLE Mini) wird an SV2 genutzt (SV2 Patch erforderlich) |
unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
char Wlan_HomeSSID[wlan_ssid_length+1]; // Wlan SSID (Home-WLAN) |
char Wlan_HomePassword[wlan_password_length+1]; // Wlan Passwort (Home-WLAN) |
//!!!neue Parameter ab hier anfügen!!!!!! |
// ab hier werden die Parameter bei EEPROMversionsänderungen gelöscht |
Point_t PointList[MAX_LIST_LEN]; // ab EEPROM Version 78 |
osd_statistic_t OSD_Statistic; // ab EEPROM Version 79 & 81 OSD Statistiken |
pkt_gpspos_t GPS_User[MAX_GPS_USER]; // ab EEPROM Version 79 speichert Benutzer GPS-Positionen |
mkerror_t MKErr_Log[MAX_MKERR_LOG]; // ab EEPROM Version 79 speichert auftretende ErrorCode's vom MK |
} ST; |
extern ST Config; |
void ReadParameter (void); |
void WriteParameter (void); |
void ReadLastPosition(void); |
void WriteLastPosition(uint32_t ELongitude,uint32_t ELatitude); |
void WriteWiInitFlag(void); |
void WriteBTInitFlag(void); |
void WriteWLInitFlag(void); |
void WriteBTSlaveFlag(void); |
void WriteBTMasterFlag(void); |
void Delete_EEPROM(void); |
void SendConfigData(void); |
//void EEWayPointList_Clear(void); // l�schen der Waypointliste im EEProm |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/followme/followme.c |
---|
0,0 → 1,673 |
/* |
* FollowMe.c |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY followme.c |
//# |
//# 20.09.2015 Starter |
//# FollowMeStep2 erweitert auf aktuelle GPS-Daten und followme_calculate_offset(...) |
//# |
//# 03.08.2015 Cebra |
//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe |
//# |
//# 20.07.2015 CB |
//# - chg: FollowMe Datensatz an NC.211 angepasst |
//# |
//# 27.06.2014 OG |
//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX |
//# |
//# 26.06.2014 OG |
//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h) |
//# |
//# 24.06.2014 OG |
//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c) |
//# - del: Variable CheckGPS |
//# |
//# 21.06.2014 OG |
//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der |
//# Entfernung zwischen Kopter und Target |
//# - chg: MK-Timeoutout Erkennung verbessert/angepasst |
//# |
//# 19.06.2014 OG |
//# - erster Prototyp der Follow Me zum Kopter sendet |
//# - etliche Aenderungen im Codeaufbau |
//# |
//# 01.06.2014 OG |
//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# Hauptmenue erledigt wird |
//# |
//# 13.05.2014 OG |
//# - chg: FollowMe() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb |
//# auskommentiert weil man das a) vorallem nicht benoetigt |
//# und b) die load_setting() so nicht mehr existiert |
//# (der Codebereich kann meines erachtens geloescht werden) |
//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es |
//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird |
//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt) |
//# und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio) |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 05.05.2013 Cebra |
//# - chg: #ifdef USE_FOLLOWME |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdio.h> |
#include "../main.h" |
#ifdef USE_FOLLOWME |
#include "../followme/followme.h" |
#include "../osd/osd.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../gps/gps.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
//####################################################################################################################### |
//-------------------- |
// Timings |
//-------------------- |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
#define COSD_WASFLYING 4 |
// global definitions and global vars |
NaviData_t *naviData; |
unsigned char Element; |
uint16_t heading_home; |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
// Flags |
//uint8_t COSD_FLAGS2 = 0; |
// |
//GPS_Pos_t last5pos[7]; |
uint8_t FM_error = 0; |
//--------------------- |
// Waypoint Types |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
//--------------------- |
// Status |
// GPS_Pos_t |
// aus MK source |
// |
// TODO OG: verschieben nach: mk-data-structs.h |
//--------------------- |
#define INVALID 0x00 |
#define NEWDATA 0x01 |
#define PROCESSED 0x02 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void FollowMe( void ) |
{ |
//uint8_t status; |
GPS_Pos_t currpos; |
uint8_t tmp_dat; |
uint8_t redraw; |
uint8_t drawmode; |
uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
uint32_t send_followme_counter; |
int8_t ok; |
int8_t xoffs; |
int8_t yoffs; |
Point_t FollowMe; |
uint8_t mktimeout = false; |
GPS_PosDev_t targetdev; |
//--------------------- |
// 1. Daten GPS-Maus |
//--------------------- |
ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
if( ok <= 0 ) |
{ |
return; // Fehler bzgl. BT GPS-Maus -> exit |
} |
//--------------------- |
// 2. Follow Me |
//--------------------- |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
lcd_cls (); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
timer_mk_timeout = MK_TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
MKLiPoCells_Init(); |
redraw = true; |
NMEA.Counter = 0; |
NMEA_GPGGA_counter_old = 0; |
send_followme_counter = 0; |
while( (receiveNMEA) ) |
{ |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
if( redraw ) |
{ |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte |
lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance) |
lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:" |
draw_icon_mk( 1, 18); |
draw_icon_target_round( 1, 50); |
redraw = false; |
} |
//----------------------------------------- |
// neue MK Daten vorhanden |
//----------------------------------------- |
if( rxd_buffer_locked ) // neue MK Daten |
{ |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
if( mktimeout ) redraw = true; |
mktimeout = false; |
FM_error = 0; // noch benoetigt? |
currpos.Latitude = naviData->CurrentPosition.Latitude; |
currpos.Longitude = naviData->CurrentPosition.Longitude; |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln |
//################# |
//# MK |
//################# |
//----------------- |
// Oben: MK-Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: MK-Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 3; |
yoffs = -1; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// MK: Sat Anzahl |
//----------------- |
yoffs = -27; |
if( naviData->SatsInUse > 5 ) drawmode = MNORMALX; |
else drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs); |
draw_icon_satmini( 116+3, 42+yoffs); |
//----------------- |
// MK: GPS |
//----------------- |
writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude |
writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude |
// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude); |
// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude); |
//################# |
//# DISTANCE TO TARGET |
//################# |
//----------------- |
// Target: Distance to Target |
//----------------- |
xoffs = 7; |
yoffs = 4; |
//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition); |
lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) ); |
//################# |
//# TARGET (GPS Maus) |
//################# |
yoffs = 8; |
//----------------- |
// Send Counter |
//----------------- |
//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
//----------------- |
// Target: Fix |
//----------------- |
if( NMEA.SatFix == 1 || NMEA.SatFix == 2 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix |
//----------------- |
// Target: Sat Anzahl |
//----------------- |
if( NMEA.SatsInUse > 5 ) |
drawmode = MNORMALX; |
else |
drawmode = MINVERSX; |
writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs); |
draw_icon_satmini( 116+4, 40+yoffs); |
//----------------- |
// Target: Lat / Long |
//----------------- |
writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
rxd_buffer_locked = FALSE; |
timer_mk_timeout = MK_TIMEOUT; |
} |
//----------------------------------------- |
// if( !NMEA_receiveBT() ) |
// { |
// receiveNMEA = false; |
// } |
// else |
// { |
// NMEA_GetNewData(); |
// } |
// Sourcecode aus NaviCtrl/V2.10e/uart1.c |
// FOLLOW_ME |
// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
// { |
// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
// { |
// TransmitAlsoToFC = 1; |
// // update FollowMe content |
// FollowMe.Position.Longitude = GPSData.Position.Longitude; |
// FollowMe.Position.Latitude = GPSData.Position.Latitude; |
// FollowMe.Position.Status = NEWDATA; |
// FollowMe.Position.Altitude = 1; |
// // 0 -> no Orientation |
// // 1-360 -> CompassCourse Setpoint |
// // -1 -> points to WP1 -> itself |
// FollowMe.Heading = -1; |
// FollowMe.ToleranceRadius = 1; |
// FollowMe.HoldTime = 60; |
// FollowMe.Event_Flag = 1; |
// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list |
// FollowMe.Type = POINT_TYPE_WP; |
// FollowMe.WP_EventChannelValue = 100; // set servo value |
// FollowMe.AltitudeRate = 0; // do not change height |
// FollowMe.Speed = 0; // rate to change the Position (0 = max) |
// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
// FollowMe.Name[0] = 'F'; // Name of that point (ASCII) |
// FollowMe.Name[1] = 'O'; // Name of that point (ASCII) |
// FollowMe.Name[2] = 'L'; // Name of that point (ASCII) |
// FollowMe.Name[3] = 'L'; // Name of that point (ASCII) |
// FollowMe.reserve[0] = 0; // reserve |
// FollowMe.reserve[1] = 0; // reserve |
// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe)); |
// } |
// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
// } |
// |
//----------------------------------------- |
// neue NMEA Daten? |
//----------------------------------------- |
if(NMEA_isdataready() && receiveNMEA) |
{ |
if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
{ |
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
{ |
//Config.FM_Refresh |
FollowMe.Position.Status = NEWDATA; |
FollowMe.Position.Longitude = NMEA.Longitude; |
FollowMe.Position.Latitude = NMEA.Latitude; |
FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
// FollowMe.Event_Flag = 0; // no event |
FollowMe.Event_Flag = 1; // 20.7.2015 CB |
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
FollowMe.Name[1] = 'O'; |
FollowMe.Name[2] = 'L'; |
FollowMe.Name[3] = 'L'; |
// FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
send_followme_counter++; |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
} |
NMEA_GPGGA_counter_old = NMEA.Counter; |
} |
} |
//----------------------------------------- |
// TASTEN |
//----------------------------------------- |
if( get_key_press(1 << KEY_ESC) ) |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
break; |
} |
//----------------------------------------- |
//----------------------------------------- |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
//OSDScreenRefresh = OSD_SCREEN_REDRAW; |
// OSD_MK_Connect( MK_CONNECT ); |
} |
} // end: while( (receiveNMEA) ); |
//--------------------- |
// BEENDEN |
//--------------------- |
OSD_active = false; |
//--------------------- |
// GPS beenden |
//--------------------- |
GPSMouse_Disconnect(); |
} |
// |
#ifdef USE_FOLLOWME_STEP2 |
void Debug_GPS (void) |
{ |
uint8_t redraw; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
redraw = true; |
int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen Quick an Dirty ;-) |
if( retcode <= 0 ) |
{ |
return; |
} |
while( true ) |
{ |
//######## Quell Koordinaten ############################## |
NMEApos.lat = NMEA.Latitude; |
NMEApos.lon = NMEA.Longitude; |
//######################################################### |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
if( redraw ) |
{ |
lcd_cls(); |
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMeStep2") ); // Titel: oben, mitte |
lcdx_printf_center_P( 1, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
lcdx_printf_center_P( 3, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
redraw = false; |
} |
//################# |
//# DISTANCE TO TARGET |
//################# |
//lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
//################# |
//# TARGET Azimuth |
//################# |
//lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
followme_calculate_offset(&NMEApos, &NMEATarget, 10, 10); |
writex_gpspos( 1, 4, (int32_t)NMEATarget.lat , MNORMAL, 0,0 ); // Ziel Latitude |
writex_gpspos(10, 4, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude |
if( get_key_press(1 << KEY_ESC) ) |
{ |
GPSMouse_Disconnect(); |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
redraw = true; |
// break; |
} |
} |
} |
#endif |
#endif // #ifdef USE_FOLLOWME |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/followme/followme.h |
---|
0,0 → 1,14 |
/* |
* FollowMe.h |
* |
* Created on: 18.05.2012 |
* Author: cebra |
*/ |
#ifndef FOLLOWME_H_ |
#define FOLLOWME_H_ |
void FollowMe (void); |
void Debug_GPS (void); |
#endif /* FOLLOWME_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/followme |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/trunk/gps/gps.c |
---|
0,0 → 1,274 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.c |
//# |
//# 20.09.2015 Startet |
//# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
//# followme_calculate_offset(...) |
//# |
//# 03.08.2015 cebra |
//# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
//# int nmea_move_horz( |
//# const nmeaPOS *start_pos, /**< Start position in radians */ |
//# nmeaPOS *end_pos, /**< Result position in radians */ |
//# double azimuth, /**< Azimuth (degree) [0, 359] */ |
//# double distance) /**< Distance (km) */ |
//# |
//# 27.06.2014 OG - NEU |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
#include "../mk-data-structs.h" |
#include "../gps/mymath.h" |
#include "gps.h" |
/* |
// definiert in: mk_data-stucts.h |
typedef struct |
{ |
u16 Distance; // distance to target in cm |
s16 Bearing; // course to target in deg |
} __attribute__((packed)) GPS_PosDev_t; |
*/ |
/* |
// definiert in: mk_data-stucts.h |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
*/ |
//-------------------------------------------------------------- |
#define NMEA_PI (3.141592653589793) /**< PI value */ |
#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ |
#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ |
#define R (6371) |
#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ |
#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ |
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ |
#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
// Definitonen für FollowMeStep2 |
#define LONG_DIV 10000000 |
#define LAT_DIV LONG_DIV |
#define FOLLOWME_M2DEG 111111 |
#define FOLLOWME_ROUND_100 100 |
# define NMEA_POSIX(x) x |
/** |
* \fn nmea_degree2radian |
* \brief Convert degree to radian |
*/ |
double nmea_degree2radian(double val) |
{ return (val * NMEA_PI180); } |
//------------------------------------------------------------------------------------------ |
nmeaPOS NMEApos; |
nmeaPOS NMEATarget; |
/** |
* \brief Horizontal move of point position |
*/ |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
) |
{ |
nmeaPOS p1 = *start_pos; |
int RetVal = 1; |
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
azimuth = nmea_degree2radian(azimuth); |
end_pos->lat = asin( sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); |
end_pos->lon = p1.lon + atan2( sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); |
if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) |
{ |
end_pos->lat = 0; end_pos->lon = 0; |
RetVal = 0; |
} |
return RetVal; |
} |
// Berechnet die Position der Kopters für FollowMeStep2 |
// Momentan wird die gleich Position ausgegeben |
// Benutzt die c_cos_8192 der FC |
// TODO: move to followme.c |
uint8_t followme_calculate_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_long /**< Distance long(m) */ |
) |
{ |
nmeaPOS pktPos = *pPktPos; |
target_pos->lat = pktPos.lat + ( d_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ); |
target_pos->lon = pktPos.lon + ( d_long * ( LONG_DIV / FOLLOWME_M2DEG ) * 8192 ) / abs ( c_cos_8192( (int16_t)(pktPos.lat / LONG_DIV ) ) ); |
return 1; |
} |
//############################################################################################### |
//-------------------------------------------------------------- |
GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
GPS_PosDev_t PosDev; |
lon1 = pos1.Longitude; |
lat1 = pos1.Latitude; |
lon2 = pos2.Longitude; |
lat2 = pos2.Latitude; |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = (1113 * (lat1 - lat2) / 10000); |
PosDev.Bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ); // |
//PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10; // *10 um von dm auf cm zu kommen |
return PosDev; |
} |
///** |
// * \brief Calculate distance between two points |
// * \return Distance in meters |
// */ |
//int32_t nmea_distance( |
// const nmeaPOS *from_pos, /**< From position in radians */ |
// const nmeaPOS *to_pos /**< To position in radians */ |
// ) |
//{ |
// int32_t dist = ((int32_t)NMEA_EARTHRADIUS_M) * acos( |
// sin(to_pos->lat) * sin(from_pos->lat) + |
// cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) |
// ); |
// return dist; |
//} |
//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
//// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ double lat1, lon1, lat2, lon2, d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = (double)pos->Longitude / 10000000.0; |
// lat2 = (double)pos->Latitude / 10000000.0; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
// // es wird jedoch in Meter weiter gerechnet |
// d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
// dlat = 111300 * (double)(lat1 - lat2); |
// // returns a value in metres http://www.kompf.de/gps/distcalc.html |
// geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
// if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
// geo.distance = sqrt(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
/* |
geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
geo_t geo; |
lon1 = home->Home_Lon; |
lat1 = home->Home_Lat; |
lon2 = pos->Longitude; |
lat2 = pos->Latitude; |
if( !CheckGPS ) |
{ |
writex_gpspos( 0, 3, home->Home_Lat , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 3, home->Home_Lon , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
writex_gpspos( 0, 4, pos->Latitude , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 4, pos->Longitude , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
//lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
} |
// Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator |
// es wird jedoch in dm Meter weiter gerechnet |
// (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1 |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = 1113 * (lat1 - lat2) / 10000; |
geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
if( !CheckGPS ) |
{ |
lcd_printp_at (0, 5, PSTR("Bear:"), 0); |
lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing ); |
//lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance ); |
//lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (20, 5, PSTR("m"), 0); |
} |
return(geo); |
} |
*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps/gps.h |
---|
0,0 → 1,57 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gps.h |
//# |
//# 20.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPS_H_ |
#define GPS_H_ |
GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ); |
/** |
* Position data in fractional degrees or radians |
*/ |
typedef struct _nmeaPOS |
{ |
int32_t lat; /**< Latitude */ |
int32_t lon; /**< Longitude */ |
} nmeaPOS; |
extern nmeaPOS NMEApos; |
extern nmeaPOS NMEATarget; |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
int nmea_move_horz1( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
double azimuth, /**< Azimuth (degree) [0, 359] */ |
double distance /**< Distance (km) */ |
); |
uint8_t followme_calculate_offset( |
const nmeaPOS *pkt_pos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_lon /**< Distance long(m) */ |
); |
#endif // #define GPS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps/gpsmouse.c |
---|
0,0 → 1,532 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.c |
//# |
//# 30.07.2015 CB |
//# - add: Anzeige von CRC Fehlern im NMEA Datensatz in GPSMouse_ShowData |
//# |
//# 28.07.2015 CB |
//# - fix: Überprüfung von führenden 00 in der MAC-Adresse der BT Maus abgeschaltet |
//# |
//# 28.06.2014 OG |
//# - fix: GPSMouse_ShowData() - wenn BT-Datenverlust dann immer GPSMouse_Disconnect() |
//# auch wenn waitsatfix Modus aktiv ist |
//# - fix: GPSMouse_ShowData() - Rueckgabe von 0 wenn Verbindung bereits vorhanden war |
//# und dabei aber ein Verbindungsverlust zur BT GPS-Maus auftrat |
//# |
//# 27.06.2014 OG |
//# - chg: GPSMouse_Disconnect() - Anzeige von BT-Datenverlust optimiert |
//# - chg: GPSMouse_ShowData() - nach GPSMouse_Connect ein clear_key_all() eingefuegt |
//# |
//# 24.06.2014 OG |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 23.06.2014 OG |
//# - chg: GPSMouse_ShowData() - neue Parameter und Return-Codes fuer |
//# hoehere Flexibilitaet |
//# |
//# 22.06.2014 OG - NEU |
//# ehemals BT_ShowGPSData() in setup.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../setup/setup.h" |
#include "../eeprom/eeprom.h" |
#include "../lipo/lipo.h" |
#include "../timer/timer.h" |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../uart/uart1.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../messages.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
#include "../gps/gpsmouse.h" |
static const char STR_FIX0[] PROGMEM = " no"; |
static const char STR_FIX1[] PROGMEM = " GPS"; |
static const char STR_FIX2[] PROGMEM = "DGPS"; |
static const char STR_FIX6[] PROGMEM = "esti"; |
static const char STR_FIX[] PROGMEM = " ---"; |
//-------------------------------------------------------------- |
// retcode = GPSMouse_Connect() |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: verbunden |
//-------------------------------------------------------------- |
int8_t GPSMouse_Connect( void ) |
{ |
uint8_t status; |
if( !BT_CheckBTM222() ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return -2; // -2: kein BTM222 eingebaut |
} |
// deaktiviert, MAC Adressen von Handys haben keine 00 in der MAC-Adresse |
// if( (Config.gps_UsedMac[5] == '0') || (Config.gps_UsedMac[6] == '0') ) |
// { |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// PKT_Message_P( strGet(STR_NOGPSMOUSE), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "keine GPS-Maus!" |
// return -1; // -1: keine GPS Maus konfiguriert |
// } |
//-------------------- |
// verbinde BT GPS-Maus |
//-------------------- |
PKT_Title( Config.gps_UsedDevName, true, true); // PKT_Title( text, lShowLipo, clearscreen ) |
lcdx_printp_center( 2, strGet(STR_GPSMOUSECONNECT), MNORMAL, 0,1); // "suche GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
if( Config.BTIsSlave ) |
{ |
bt_downlink_init(); |
} |
status = bt_connect( Config.gps_UsedMac ); |
uart1_flush(); |
fifo_clear(&in_fifo); |
UART1_receiveNMEA = true; // Uartflag setzen zur Erkennung der NMEA Datensätze |
PKT_Gauge_End(); // Gauge: Ende |
if( !status ) // keine Verbindung zum BT-Device |
{ |
set_BTOff(); |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_GPSMOUSECONNECTION), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "GPS-Maus Verbindung" |
return 0; // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
} |
return 1; // 1 = ok, Verbindung steht |
} |
//-------------------------------------------------------------- |
// GPSMouse_Disconnect() |
//-------------------------------------------------------------- |
void GPSMouse_Disconnect( void ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
//--------------------- |
// GPS beenden |
//--------------------- |
UART1_receiveNMEA = false; |
if( !receiveNMEA ) // ggf. anders implementieren, z.B. via Parameter; aber erstmal geht's auch so |
{ |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 7, MINVERSX, 0 ); |
lcdx_printp_center( 7, strGet(STR_BT_LOSTDATA), MINVERSX, 0,0); // "BT Datenverlust" |
set_beep( 600, 0xffff, BeepNormal); |
} |
lcdx_printp_center( 2, strGet(STR_GPSMOUSEDISCONNECT), MNORMAL, 0,1); // "trenne GPS-Maus" |
PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
receiveNMEA = false; // Uartflag zurücksetzen, es kommen keine NMEA Datensätze mehr |
if (!bt_disconnect()); |
{ |
// lcdx_printp_center( 2, PSTR("Error on disconnect"), MNORMAL, 0,1); // Fehler beim Disconncet aufgetreten |
// _delay_ms(2000); // 09.08.2015 cebra, muss noch untersucht werden, ist aber jetzt kein Problem |
} |
set_BTOff(); |
PKT_Gauge_End(); // Gauge: Ende |
} |
//-------------------------------------------------------------- |
// retcode = GPSMouse_ShowData( mode, waitsatfix ) |
// |
// zeigt GPS-Daten der aktuellen BT-Maus an |
// |
// PARAMETER: |
// show: |
// was soll in der 2. Zeile angezeigt werden |
// GPSMOUSE_SHOW_TIME || GPSMOUSE_SHOW_WAITSATFIX |
// |
// waitsatifix: |
// 0 = Verbindungsaufbau; Daten anzeigen und beenden der |
// BT-Verbindung (fuer setup.c) |
// n = Zeit (via timer) die nach einem erfolgreichen Satfix gewartet wird bis |
// GPSMouse_ShowData() beendet wird - die Verbindung zur GPS-Maus bleibt erhalten |
// und kann weiterverwendet werden! (z.B. in followme.c) |
// |
// RUECKGABE: |
// -2: kein BTM222 eingebaut |
// -1: keine GPS Maus konfiguriert |
// 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
// 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
// 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// |
// zusammengefasst: |
// >0 : Ok |
// <=0: Fehler |
//-------------------------------------------------------------- |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ) |
{ |
uint8_t redraw = true; |
uint8_t state_posfix = 0; |
const char *pStr; |
int8_t yoffs; |
int8_t retcode; |
//-------------------- |
// BT GPS-Maus verbinden |
//-------------------- |
retcode = GPSMouse_Connect(); |
if( retcode <= 0 ) |
{ |
return retcode; |
} |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
clear_key_all(); |
//--------------------- |
// zeige GPS-Daten |
//--------------------- |
receiveNMEA = true; |
do |
{ |
//------------------------- |
// Screen redraw |
//------------------------- |
if( redraw ) |
{ |
PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
if( show == GPSMOUSE_SHOW_TIME ) // Zeile 2: GPS-Zeit |
{ |
lcdx_printp_at( 6, 1, PSTR("["), MNORMAL, -1,2); // Mitte: GPS Time |
lcdx_printp_at(15, 1, PSTR("]"), MNORMAL, -2,2); // .. |
lcd_line( 0, 13, 34, 13, 1); // .. |
lcd_line(92, 13, 127, 13, 1); // .. |
} |
if( show == GPSMOUSE_SHOW_WAITSATFIX ) // Zeile 2: "warte auf Satfix..." |
{ |
yoffs = 1; |
lcdx_cls_row( 1, MINVERS, yoffs); |
lcdx_printp_center( 1, strGet(STR_WAITSATFIX), MINVERS, 0,yoffs); // "warte auf Satfix..." |
} |
// yoffs = 1; |
// lcdx_cls_row( 1, MINVERS, yoffs); |
// lcdx_printp_center( 1,strGet(STR_WAITNMEA), MINVERS, 0,yoffs); // "warte auf NMEA..." |
lcdx_printp_at( 0, 2, PSTR("Fix:") , MNORMAL, 0,3); // Links; Fix |
lcdx_printp_at( 0, 3, PSTR("Sat:") , MNORMAL, 0,3); // Links; Sat |
lcdx_printp_at( 0, 4, PSTR("Alt:") , MNORMAL, 0,3); // Links; Alt |
lcdx_printp_at(11, 2, PSTR("RCnt:"), MNORMAL, 0,3); // Rechts: RCnt - Zaehler empfangener GPGGA Datenpakete |
lcdx_printp_at(11, 3, PSTR("CErr:"), MNORMAL, 0,3); // Rechts: RErr - Zaehler fehlerhafte NMEA Pakete |
lcdx_printp_at(11, 4, PSTR("HDOP:"), MNORMAL, 0,3); // Rechts: HDOP |
lcdx_cls_row( 5, MINVERS, 4); // Hintergrund invers |
lcdx_printp_at( 1, 5, strGet(START_LASTPOS1), MINVERS, 0,4); // "Breitengr. Längengr." |
lcd_rect( 0, (4*8)+11, 127, (2*8)+4, 1); // Rahmen |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT-Update geht hier nicht... |
// wegen der Hardware-Verbindung |
//------------------------- |
//if( PKT_CtrlHook() ) |
//{ |
// redraw = true; |
//} |
//------------------------- |
//------------------------- |
if(NMEA_isdataready()) // NMEA Daten vorhanden? |
{ |
if( show == GPSMOUSE_SHOW_TIME ) |
{ |
lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
} |
//-------------- |
// Sat-Fix |
//-------------- |
//writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
switch( NMEA.SatFix ) |
{ |
case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
default: pStr = 0; // Sat-Fix Code unbekannt! |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA.SatFix); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
break; |
} |
if( pStr ) |
{ |
lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
} |
if( NMEA.SatFix==1 || NMEA.SatFix==2 ) // Sat-Fix OK => einen Haken anzeigen |
lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Sat-Anzahl |
//-------------- |
writex_ndigit_number_u( 6, 3, NMEA.SatsInUse, 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
if( NMEA.SatsInUse >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
else |
lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
//-------------- |
// Altitude |
//-------------- |
lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA.Altitude); // Links: Altitude |
//-------------- |
// empfangene GPGGA Datenpakete |
//-------------- |
lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA.Counter ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
//-------------- |
// NMEA CRC Fehler |
//-------------- |
lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl CRC Fehler im NMEA GPGGA-Pakete |
// lcdx_printf_at( 15, 3, mode, 0, 3, "%3.5ld", (uint32_t)NMEA_getBearing()); |
// writex_ndigit_number_u_10th(15, 3, NMEA_getBearing(), 4,0, MNORMAL, 0,3); |
//-------------- |
// HDOP |
//-------------- |
if( NMEA.HDOP == 0 ) // Rechts:: HDOP |
lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
else |
writex_ndigit_number_u_10th(16, 4, NMEA.HDOP, 4,0, MNORMAL, 0,3); |
//-------------- |
// Lat / Lon |
//-------------- |
writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); // unten rechts: Longitude |
//-------------- |
//-------------- |
if( waitsatfix > 0 ) |
{ |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==0) ) |
{ |
state_posfix = 1; |
timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
} |
if( (NMEA.SatFix!=1 && NMEA.SatFix!=2) && (state_posfix==1) && (timer!=0) ) |
{ |
state_posfix = 0; |
} |
if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==1) && (timer==0) ) |
{ |
clear_key_all(); |
return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
} |
} |
} |
if (timer_nmea_timeout == 0) receiveNMEA = false; |
} while( !get_key_press(1 << KEY_ESC) && receiveNMEA ); |
//--------------------- |
// GPS beenden |
//--------------------- |
if( (waitsatfix == 0) || !receiveNMEA ) |
{ |
GPSMouse_Disconnect(); |
} |
clear_key_all(); |
return (receiveNMEA ? 1 : 0); // 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
} |
// save, Anzeige der Daten im Format der neuen NMEA Routinen, nach Umbau auf NMEA Structur nicht mehr notwendig |
// { |
// |
// |
// if( show == GPSMOUSE_SHOW_TIME ) |
// { |
//// lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
// |
// writex_ndigit_number_u( 7, 1, NMEA_getHour(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 9, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 10, 1, NMEA_getMinute(), 2,0, MNORMAL, -2,2); |
// lcdx_printp_at( 12, 1, PSTR(":") , MNORMAL, -2,2); |
// writex_ndigit_number_u( 13, 1, NMEA_getSecond(), 2,0, MNORMAL, -2,2); |
// |
// } |
// |
// //-------------- |
// // Sat-Fix |
// //-------------- |
// //writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
// switch( NMEA_getGPSfix() ) |
// { |
// case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
// case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
// case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
// case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
// default: pStr = 0; // Sat-Fix Code unbekannt! |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA_getGPSfix()); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
// break; |
// } |
// |
// if( pStr ) |
// { |
// lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
// } |
// |
// if( NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2 ) // Sat-Fix OK => einen Haken anzeigen |
// lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Sat-Anzahl |
// //-------------- |
// writex_ndigit_number_u( 6, 3, NMEA_getSatellites(), 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
// if( NMEA_getSatellites() >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
// lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
// else |
// lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
// |
// |
// //-------------- |
// // Altitude |
// //-------------- |
// lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA_getAltitude()); // Links: Altitude |
// |
// |
// //-------------- |
// // empfangene GPGGA Datenpakete |
// //-------------- |
// lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getNMEAcounter() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
// |
// //-------------- |
// // NMEA CRC Fehler |
// //-------------- |
// lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
// |
//// writex_ndigit_number_u(1, 5, UART1_GPGGA , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(5, 5, bt_overrun , 4,0, MNORMAL, 0,3); |
//// writex_ndigit_number_u(10, 5, bt_bufferoverflow , 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // HDOP |
// //-------------- |
// if(NMEA_getHDOP() == 0 ) // Rechts:: HDOP |
// lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
// else |
// writex_ndigit_number_u_10th(16, 4, NMEA_getHDOP(), 4,0, MNORMAL, 0,3); |
// |
// |
// //-------------- |
// // Lat / Lon |
// //-------------- |
// writex_gpspos( 1, 6, NMEA_getLatitude() , MNORMAL, 0,6 ); // unten links: Latitude |
// writex_gpspos(12, 6, NMEA_getLongitude(), MNORMAL, 0,6 ); // unten rechts: Longitude |
// |
// |
// //-------------- |
// //-------------- |
// if( waitsatfix > 0 ) |
// { |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==0) ) |
// { |
// state_posfix = 1; |
// timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
// } |
// |
// if( (NMEA_getGPSfix()!=1 && NMEA_getGPSfix()!=2) && (state_posfix==1) && (timer!=0) ) |
// { |
// state_posfix = 0; |
// } |
// |
// if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==1) && (timer==0) ) |
// { |
// clear_key_all(); |
// return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
// } |
// } |
// |
// } |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps/gpsmouse.h |
---|
0,0 → 1,46 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY gpsmouse.h |
//# |
//# 24.06.2014 OG |
//# - chg: Parameteraenderung bei GPSMouse_ShowData() |
//# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
//# |
//# 22.06.2014 OG |
//# - add: GPSMouse_ShowData() |
//# - add: #define GPSMOUSE_SHOWDATA, GPSMOUSE_WAITSATFIX |
//# |
//# 22.06.2014 OG - NEU |
//############################################################################ |
#ifndef GPSMOUSE_H_ |
#define GPSMOUSE_H_ |
//---------------------------------- |
// GPSMouse_ShowData() - show |
//---------------------------------- |
#define GPSMOUSE_SHOW_TIME 1 |
#define GPSMOUSE_SHOW_WAITSATFIX 2 |
//---------------------------------- |
// minimal akzeptierte Sat-Zahl fuer |
// Steuerungsfunktionen des MK |
//---------------------------------- |
#define GPSMOUSE_MINSAT 6 |
//---------------------------------- |
// Export |
//---------------------------------- |
int8_t GPSMouse_Connect( void ); |
void GPSMouse_Disconnect( void ); |
int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ); |
#endif // #define GPSMOUSE_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps/mymath.c |
---|
0,0 → 1,214 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY mymath.c |
//# |
//# 24.04.2013 OG |
//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM' |
//# wegen Compile-Error in AtmelStudio 6 |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <math.h> |
#include <avr/pgmspace.h> |
#include "mymath.h" |
// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29 |
unsigned int sqrt32(uint32_t q) |
{ uint16_t r, mask; |
uint16_t i = 8*sizeof (r) -1; |
r = mask = 1 << i; |
for (; i > 0; i--) { |
mask >>= 1; |
if (q < (uint32_t) r*r) |
r -= mask; |
else |
r += mask; |
} |
if (q < (uint32_t) r*r) |
r -= 1; |
return r; |
} |
// aus Orginal MK source code |
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
8161, 8172, 8181, 8187, 8191, 8192}; |
//double c_cos_8192(int16_t angle) |
int16_t c_cos_8192(int16_t angle) |
{ |
int8_t m,n; |
int16_t sinus; |
angle = 90 - angle; |
// avoid negative angles |
if (angle < 0) |
{ |
m = -1; |
angle = -angle; |
} |
else m = +1; |
// fold angle to interval 0 to 359 |
angle %= 360; |
// check quadrant |
if (angle <= 90) n = 1; // first quadrant |
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
else {angle = 360 - angle; n = -1;} //fourth quadrant |
// get lookup value |
sinus = sinlookup[angle]; |
// calculate sinus value |
return (sinus * m * n); |
} |
// ---------------------------------------------------------------------------------------------- |
const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, |
75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, |
61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, |
44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, |
18, 14, 10, 0}; |
int16_t c_arccos2(int32_t a, int32_t b) |
{ |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
} |
/* ---------------------------------------------------------------------------------------------- |
atan2.S |
Author: Reiner Patommel |
atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder. |
CORDIC can be used to calculate trigonometric, hyperbolic and linear |
functions and is until today implemented in most pocket calculators. |
The algorithm makes only use of simple integer arithmetic. |
The CORDIC equations in vectoring mode for trigonometric functions are: |
Xi+1 = Xi - Si * (Yi * 1 / 2^i) |
Yi+1 = Yi + Si * (Xi * 1 / 2^i) |
Zi+1 = Zi - Si * atan(1 / 2^i) |
where Si = +1 if Yi < 0 else Si = -1 |
The rotation angles are limited to -PI/2 and PI/2 i.e. |
-90 degrees ... +90 degrees |
For the calculation of atan2(x,y) we need a small pre-calculated table of |
angles or radians with the values Tn = atan(1/2^i). |
We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and |
accumulate the rotated angles or radians in Z. The direction of the rotation |
will be positive or negative depending on the sign of Y after the previous |
rotation and the rotation angle will decrease from step to step. (The first |
step is 45 degrees, the last step is 0.002036 degrees for n = 15). |
After n rotations the variables will have the following values: |
Yn = ideally 0 |
Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector) |
Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians) |
c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to |
approx. 1.64676 for n = 15. |
In order to represent X, Y and Z as integers we introduce a scaling factor Q |
which is chosen as follows: |
1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and |
set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0. |
2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q |
3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2 |
i.e. X and Z are always > 0 and therefore can be unsigned. |
(We do the quadrant correction afterwards based on the initial signs of x and y) |
4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140. |
i.e. |
Xnmax = 65536 --> unsigned int |
Ynmax = +/- 28140 --> signed int |
Znmax = PI/2 * 28140 = 44202 --> unsigned int |
The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad. |
Below is atan2 and atan in C: */ |
#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x]) |
const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; |
int16_t my_atan2(int32_t y, int32_t x) |
{ unsigned char i; |
uint32_t xQ; |
int32_t yQ; |
uint32_t angle = 0; |
uint32_t tmp; |
double x1, y1; |
x1 = abs(x); |
y1 = abs(y); |
if (y1 == 0) { |
if (x < 0) |
return (180); |
else |
return 0; |
} |
if (x1 < y1) { |
x1 /= y1; |
y1 = 1; |
} |
else { |
y1 /= x1; |
x1 = 1; |
} |
xQ = SCALED(x1); |
yQ = SCALED(y1); |
for (i = 0; i < T-1; i++) { |
tmp = xQ >> i; |
if (yQ < 0) { |
xQ -= yQ >> i; |
yQ += tmp; |
angle -= getAtanVal(i); |
} |
else { |
xQ += yQ >> i; |
yQ -= tmp; |
angle += getAtanVal(i); |
} |
} |
angle = RAD_TO_DEG * angle/Q; |
if (x <= 0) |
angle = 180 - angle; |
if (y <= 0) |
return(-angle); |
return(angle); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps/mymath.h |
---|
0,0 → 1,15 |
#ifndef _MYMATH_H_ |
#define _MYMATH_H_ |
#include <avr/io.h> |
#define T 16 |
#define Q 28140 |
#define SCALED(X) ((int32_t)((X) * Q)) |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
uint16_t sqrt32(uint32_t qzahl); |
int16_t c_cos_8192(int16_t angle); |
int16_t my_atan2(int32_t y, int32_t x); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/gps |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Copy of gpsmouse.h |
+ |
+_old_source |
+ |
+Copy of gpsmouse.c |
/Transportables_Koptertool/PKT/trunk/lcd/Font8x8.c |
---|
0,0 → 1,273 |
/* |
* font8x8.c |
* LCD-OSD |
* |
* Created by Peter Mack on 26.12.09. |
* Copyright 2009 SCS GmbH & Co. KG. All rights reserved. |
* |
*/ |
#include <avr/pgmspace.h> |
const uint8_t Font8x8[256][8]PROGMEM= |
{ |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x00 |
{0x7E,0x81,0x95,0xB1,0xB1,0x95,0x81,0x7E}, // 0x01 |
{0x7E,0xFF,0xEB,0xCF,0xCF,0xEB,0xFF,0x7E}, // 0x02 |
{0x0E,0x1F,0x3F,0x7E,0x3F,0x1F,0x0E,0x00}, // 0x03 |
{0x08,0x1C,0x3E,0x7F,0x3E,0x1C,0x08,0x00}, // 0x04 |
{0x38,0x3A,0x9F,0xFF,0x9F,0x3A,0x38,0x00}, // 0x05 |
{0x10,0x38,0xBC,0xFF,0xBC,0x38,0x10,0x00}, // 0x06 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x07 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x08 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x09 |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0A |
{0x70,0xF8,0x88,0x88,0xFD,0x7F,0x07,0x0F}, // 0x0B |
{0x00,0x4E,0x5F,0xF1,0xF1,0x5F,0x4E,0x00}, // 0x0C |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x0D |
{0xC0,0xFF,0x7F,0x05,0x05,0x65,0x7F,0x3F}, // 0x0E |
{0x99,0x5A,0x3C,0xE7,0xE7,0x3C,0x5A,0x99}, // 0x0F |
{0x7F,0x3E,0x3E,0x1C,0x1C,0x08,0x08,0x00}, // 0x10 |
{0x08,0x08,0x1C,0x1C,0x3E,0x3E,0x7F,0x00}, // 0x11 |
{0x00,0x24,0x66,0xFF,0xFF,0x66,0x24,0x00}, // 0x12 |
{0x00,0x5F,0x5F,0x00,0x00,0x5F,0x5F,0x00}, // 0x13 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0x14 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0x15 |
{0x00,0x70,0x70,0x70,0x70,0x70,0x70,0x00}, // 0x16 |
{0x80,0x94,0xB6,0xFF,0xFF,0xB6,0x94,0x80}, // 0x17 |
{0x00,0x04,0x06,0x7F,0x7F,0x06,0x04,0x00}, // 0x18 |
{0x00,0x10,0x30,0x7F,0x7F,0x30,0x10,0x00}, // 0x19 |
{0x08,0x08,0x08,0x2A,0x3E,0x1C,0x08,0x00}, // 0x1A |
{0x08,0x1C,0x3E,0x2A,0x08,0x08,0x08,0x00}, // 0x1B |
{0x3C,0x3C,0x20,0x20,0x20,0x20,0x20,0x00}, // 0x1C |
{0x08,0x1C,0x3E,0x08,0x08,0x3E,0x1C,0x08}, // 0x1D |
{0x30,0x38,0x3C,0x3E,0x3E,0x3C,0x38,0x30}, // 0x1E |
{0x06,0x0E,0x1E,0x3E,0x3E,0x1E,0x0E,0x06}, // 0x1F |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 0x20 |
{0x00,0x06,0x5F,0x5F,0x06,0x00,0x00,0x00}, // 0x21 |
{0x00,0x07,0x07,0x00,0x07,0x07,0x00,0x00}, // 0x22 |
{0x14,0x7F,0x7F,0x14,0x7F,0x7F,0x14,0x00}, // 0x23 |
{0x24,0x2E,0x6B,0x6B,0x3A,0x12,0x00,0x00}, // 0x24 |
{0x46,0x66,0x30,0x18,0x0C,0x66,0x62,0x00}, // 0x25 |
{0x30,0x7A,0x4F,0x5D,0x37,0x7A,0x48,0x00}, // 0x26 |
{0x04,0x07,0x03,0x00,0x00,0x00,0x00,0x00}, // 0x27 |
{0x00,0x1C,0x3E,0x63,0x41,0x00,0x00,0x00}, // 0x28 |
{0x00,0x41,0x63,0x3E,0x1C,0x00,0x00,0x00}, // 0x29 |
{0x08,0x2A,0x3E,0x1C,0x1C,0x3E,0x2A,0x08}, // 0x2A |
{0x08,0x08,0x3E,0x3E,0x08,0x08,0x00,0x00}, // 0x2B |
{0x00,0xA0,0xE0,0x60,0x00,0x00,0x00,0x00}, // 0x2C |
{0x08,0x08,0x08,0x08,0x08,0x08,0x00,0x00}, // 0x2D |
{0x00,0x00,0x60,0x60,0x00,0x00,0x00,0x00}, // 0x2E |
{0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00}, // 0x2F |
{0x3E,0x7F,0x59,0x4D,0x7F,0x3E,0x00,0x00}, // 0x30 |
{0x42,0x42,0x7F,0x7F,0x40,0x40,0x00,0x00}, // 0x31 |
{0x62,0x73,0x59,0x49,0x6F,0x66,0x00,0x00}, // 0x32 |
{0x22,0x63,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x33 |
{0x18,0x1C,0x16,0x13,0x7F,0x7F,0x10,0x00}, // 0x34 |
{0x27,0x67,0x45,0x45,0x7D,0x39,0x00,0x00}, // 0x35 |
{0x3C,0x7E,0x4B,0x49,0x79,0x30,0x00,0x00}, // 0x36 |
{0x03,0x63,0x71,0x19,0x0F,0x07,0x00,0x00}, // 0x37 |
{0x36,0x7F,0x49,0x49,0x7F,0x36,0x00,0x00}, // 0x38 |
{0x06,0x4F,0x49,0x69,0x3F,0x1E,0x00,0x00}, // 0x39 |
{0x00,0x00,0x6C,0x6C,0x00,0x00,0x00,0x00}, // 0x3A |
{0x00,0xA0,0xEC,0x6C,0x00,0x00,0x00,0x00}, // 0x3B |
{0x08,0x1C,0x36,0x63,0x41,0x00,0x00,0x00}, // 0x3C |
{0x14,0x14,0x14,0x14,0x14,0x14,0x00,0x00}, // 0x3D |
{0x00,0x41,0x63,0x36,0x1C,0x08,0x00,0x00}, // 0x3E |
{0x02,0x03,0x51,0x59,0x0F,0x06,0x00,0x00}, // 0x3F |
{0x3E,0x7F,0x41,0x5D,0x5D,0x1F,0x1E,0x00}, // 0x40 |
{0x7C,0x7E,0x13,0x13,0x7E,0x7C,0x00,0x00}, // 0x41 |
{0x41,0x7F,0x7F,0x49,0x49,0x7F,0x36,0x00}, // 0x42 |
{0x1C,0x3E,0x63,0x41,0x41,0x63,0x22,0x00}, // 0x43 |
{0x41,0x7F,0x7F,0x41,0x63,0x7F,0x1C,0x00}, // 0x44 |
{0x41,0x7F,0x7F,0x49,0x5D,0x41,0x63,0x00}, // 0x45 |
{0x41,0x7F,0x7F,0x49,0x1D,0x01,0x03,0x00}, // 0x46 |
{0x1C,0x3E,0x63,0x41,0x51,0x73,0x72,0x00}, // 0x47 |
{0x7F,0x7F,0x08,0x08,0x7F,0x7F,0x00,0x00}, // 0x48 |
{0x00,0x41,0x7F,0x7F,0x41,0x00,0x00,0x00}, // 0x49 |
{0x30,0x70,0x40,0x41,0x7F,0x3F,0x01,0x00}, // 0x4A |
{0x41,0x7F,0x7F,0x08,0x1C,0x77,0x63,0x00}, // 0x4B |
{0x41,0x7F,0x7F,0x41,0x40,0x60,0x70,0x00}, // 0x4C |
{0x7F,0x7F,0x06,0x0C,0x06,0x7F,0x7F,0x00}, // 0x4D |
{0x7F,0x7F,0x06,0x0C,0x18,0x7F,0x7F,0x00}, // 0x4E |
{0x1C,0x3E,0x63,0x41,0x63,0x3E,0x1C,0x00}, // 0x4F |
{0x41,0x7F,0x7F,0x49,0x09,0x0F,0x06,0x00}, // 0x50 |
{0x1E,0x3F,0x21,0x71,0x7F,0x5E,0x00,0x00}, // 0x51 |
{0x41,0x7F,0x7F,0x19,0x39,0x6F,0x46,0x00}, // 0x52 |
{0x26,0x67,0x4D,0x59,0x7B,0x32,0x00,0x00}, // 0x53 |
{0x03,0x41,0x7F,0x7F,0x41,0x03,0x00,0x00}, // 0x54 |
{0x7F,0x7F,0x40,0x40,0x7F,0x7F,0x00,0x00}, // 0x55 |
{0x1F,0x3F,0x60,0x60,0x3F,0x1F,0x00,0x00}, // 0x56 |
{0x7F,0x7F,0x30,0x18,0x30,0x7F,0x7F,0x00}, // 0x57 |
{0x63,0x77,0x1C,0x08,0x1C,0x77,0x63,0x00}, // 0x58 |
{0x07,0x4F,0x78,0x78,0x4F,0x07,0x00,0x00}, // 0x59 |
{0x67,0x73,0x59,0x4D,0x47,0x63,0x71,0x00}, // 0x5A |
{0x00,0x7F,0x7F,0x41,0x41,0x00,0x00,0x00}, // 0x5B |
{0x01,0x03,0x06,0x0C,0x18,0x30,0x60,0x00}, // 0x5C |
{0x00,0x41,0x41,0x7F,0x7F,0x00,0x00,0x00}, // 0x5D |
{0x08,0x0C,0x06,0x03,0x06,0x0C,0x08,0x00}, // 0x5E |
{0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80}, // 0x5F |
{0x00,0x00,0x03,0x07,0x04,0x00,0x00,0x00}, // 0x60 |
{0x20,0x74,0x54,0x54,0x3C,0x78,0x40,0x00}, // 0x61 |
{0x41,0x3F,0x7F,0x44,0x44,0x7C,0x38,0x00}, // 0x62 |
{0x38,0x7C,0x44,0x44,0x6C,0x28,0x00,0x00}, // 0x63 |
{0x30,0x78,0x48,0x49,0x3F,0x7F,0x40,0x00}, // 0x64 |
{0x38,0x7C,0x54,0x54,0x5C,0x18,0x00,0x00}, // 0x65 |
{0x48,0x7E,0x7F,0x49,0x03,0x02,0x00,0x00}, // 0x66 |
{0x98,0xBC,0xA4,0xA4,0xF8,0x7C,0x04,0x00}, // 0x67 |
{0x41,0x7F,0x7F,0x08,0x04,0x7C,0x78,0x00}, // 0x68 |
{0x00,0x44,0x7D,0x7D,0x40,0x00,0x00,0x00}, // 0x69 |
{0x40,0xC4,0x84,0xFD,0x7D,0x00,0x00,0x00}, // 0x6A |
{0x41,0x7F,0x7F,0x10,0x38,0x6C,0x44,0x00}, // 0x6B |
{0x00,0x41,0x7F,0x7F,0x40,0x00,0x00,0x00}, // 0x6C |
{0x7C,0x7C,0x0C,0x18,0x0C,0x7C,0x78,0x00}, // 0x6D |
{0x7C,0x7C,0x04,0x04,0x7C,0x78,0x00,0x00}, // 0x6E |
{0x38,0x7C,0x44,0x44,0x7C,0x38,0x00,0x00}, // 0x6F |
{0x84,0xFC,0xF8,0xA4,0x24,0x3C,0x18,0x00}, // 0x70 |
{0x18,0x3C,0x24,0xA4,0xF8,0xFC,0x84,0x00}, // 0x71 |
{0x44,0x7C,0x78,0x44,0x1C,0x18,0x00,0x00}, // 0x72 |
{0x48,0x5C,0x54,0x54,0x74,0x24,0x00,0x00}, // 0x73 |
{0x00,0x04,0x3E,0x7F,0x44,0x24,0x00,0x00}, // 0x74 |
{0x3C,0x7C,0x40,0x40,0x3C,0x7C,0x40,0x00}, // 0x75 |
{0x1C,0x3C,0x60,0x60,0x3C,0x1C,0x00,0x00}, // 0x76 |
{0x3C,0x7C,0x60,0x30,0x60,0x7C,0x3C,0x00}, // 0x77 |
{0x44,0x6C,0x38,0x10,0x38,0x6C,0x44,0x00}, // 0x78 |
{0x9C,0xBC,0xA0,0xA0,0xFC,0x7C,0x00,0x00}, // 0x79 |
{0x4C,0x64,0x74,0x5C,0x4C,0x64,0x00,0x00}, // 0x7A |
{0x08,0x08,0x3E,0x77,0x41,0x41,0x00,0x00}, // 0x7B |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0x7C |
{0x41,0x41,0x77,0x3E,0x08,0x08,0x00,0x00}, // 0x7D |
{0x02,0x03,0x01,0x03,0x02,0x03,0x01,0x00}, // 0x7E |
{0x78,0x7C,0x46,0x43,0x46,0x7C,0x78,0x00}, // 0x7F |
{0x1E,0xBF,0xE1,0x61,0x33,0x12,0x00,0x00}, // 0x80 |
{0x3A,0x7A,0x40,0x40,0x7A,0x7A,0x40,0x00}, // 0x81 |
{0x38,0x7C,0x56,0x57,0x5D,0x18,0x00,0x00}, // 0x82 |
{0x02,0x23,0x75,0x55,0x55,0x7D,0x7B,0x42}, // 0x83 |
{0x21,0x75,0x54,0x54,0x7D,0x79,0x40,0x00}, // 0x84 |
{0x20,0x75,0x57,0x56,0x7C,0x78,0x40,0x00}, // 0x85 |
{0x00,0x22,0x77,0x55,0x55,0x7F,0x7A,0x40}, // 0x86 |
{0x1C,0xBE,0xE2,0x62,0x36,0x14,0x00,0x00}, // 0x87 |
{0x02,0x3B,0x7D,0x55,0x55,0x5D,0x1B,0x02}, // 0x88 |
{0x39,0x7D,0x54,0x54,0x5D,0x19,0x00,0x00}, // 0x89 |
{0x38,0x7D,0x57,0x56,0x5C,0x18,0x00,0x00}, // 0x8A |
{0x01,0x45,0x7C,0x7C,0x41,0x01,0x00,0x00}, // 0x8B |
{0x02,0x03,0x45,0x7D,0x7D,0x43,0x02,0x00}, // 0x8C |
{0x00,0x45,0x7F,0x7E,0x40,0x00,0x00,0x00}, // 0x8D |
{0x79,0x7D,0x26,0x26,0x7D,0x79,0x00,0x00}, // 0x8E |
{0x70,0x7A,0x2D,0x2D,0x7A,0x70,0x00,0x00}, // 0x8F |
{0x44,0x7C,0x7E,0x57,0x55,0x44,0x00,0x00}, // 0x90 |
{0x20,0x74,0x54,0x54,0x7C,0x7C,0x54,0x54}, // 0x91 |
{0x7C,0x7E,0x0B,0x09,0x7F,0x7F,0x49,0x00}, // 0x92 |
{0x32,0x7B,0x49,0x49,0x7B,0x32,0x00,0x00}, // 0x93 |
{0x32,0x7A,0x48,0x48,0x7A,0x32,0x00,0x00}, // 0x94 |
{0x30,0x79,0x4B,0x4A,0x78,0x30,0x00,0x00}, // 0x95 |
{0x3A,0x7B,0x41,0x41,0x7B,0x7A,0x40,0x00}, // 0x96 |
{0x38,0x79,0x43,0x42,0x78,0x78,0x40,0x00}, // 0x97 |
{0xBA,0xBA,0xA0,0xA0,0xFA,0x7A,0x00,0x00}, // 0x98 |
{0x39,0x7D,0x44,0x44,0x44,0x7D,0x39,0x00}, // 0x99 |
{0x3D,0x7D,0x40,0x40,0x7D,0x3D,0x00,0x00}, // 0x9A |
{0x38,0x7C,0x64,0x54,0x4C,0x7C,0x38,0x00}, // 0x9B |
{0x68,0x7E,0x7F,0x49,0x43,0x66,0x20,0x00}, // 0x9C |
{0x5C,0x3E,0x73,0x49,0x67,0x3E,0x1D,0x00}, // 0x9D |
{0x44,0x6C,0x38,0x38,0x6C,0x44,0x00,0x00}, // 0x9E |
{0x40,0xC8,0x88,0xFE,0x7F,0x09,0x0B,0x02}, // 0x9F |
{0x20,0x74,0x56,0x57,0x7D,0x78,0x40,0x00}, // 0xA0 |
{0x00,0x44,0x7E,0x7F,0x41,0x00,0x00,0x00}, // 0xA1 |
{0x30,0x78,0x48,0x4A,0x7B,0x31,0x00,0x00}, // 0xA2 |
{0x38,0x78,0x40,0x42,0x7B,0x79,0x40,0x00}, // 0xA3 |
{0x7A,0x7B,0x09,0x0B,0x7A,0x73,0x01,0x00}, // 0xA4 |
{0x7A,0x7B,0x19,0x33,0x7A,0x7B,0x01,0x00}, // 0xA5 |
{0x00,0x26,0x2F,0x29,0x2F,0x2F,0x28,0x00}, // 0xA6 |
{0x00,0x26,0x2F,0x29,0x29,0x2F,0x26,0x00}, // 0xA7 |
{0x30,0x78,0x4D,0x45,0x60,0x20,0x00,0x00}, // 0xA8 |
{0x1C,0x22,0x7D,0x4B,0x5B,0x65,0x22,0x1C}, // 0xA9 |
{0x08,0x08,0x08,0x08,0x38,0x38,0x00,0x00}, // 0xAA |
{0x61,0x3F,0x1F,0xCC,0xEE,0xAB,0xB9,0x90}, // 0xAB |
{0x61,0x3F,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xAC |
{0x00,0x00,0x60,0xFA,0xFA,0x60,0x00,0x00}, // 0xAD |
{0x08,0x1C,0x36,0x22,0x08,0x1C,0x36,0x22}, // 0xAE |
{0x22,0x36,0x1C,0x08,0x22,0x36,0x1C,0x08}, // 0xAF |
{0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00}, // 0xB0 |
{0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55}, // 0xB1 |
{0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF}, // 0xB2 |
{0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00}, // 0xB3 |
{0x10,0x10,0x10,0xFF,0xFF,0x00,0x00,0x00}, // 0xB4 |
{0x70,0x78,0x2C,0x2E,0x7B,0x71,0x00,0x00}, // 0xB5 |
{0x72,0x79,0x2D,0x2D,0x79,0x72,0x00,0x00}, // 0xB6 |
{0x71,0x7B,0x2E,0x2C,0x78,0x70,0x00,0x00}, // 0xB7 |
{0x1C,0x22,0x5D,0x55,0x55,0x41,0x22,0x1C}, // 0xB8 |
{0x14,0x14,0xF7,0xF7,0x00,0xFF,0xFF,0x00}, // 0xB9 |
{0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0x00}, // 0xBA |
{0x14,0x14,0xF4,0xF4,0x04,0xFC,0xFC,0x00}, // 0xBB |
{0x14,0x14,0x17,0x17,0x10,0x1F,0x1F,0x00}, // 0xBC |
{0x18,0x3C,0x24,0xE7,0xE7,0x24,0x24,0x00}, // 0xBD |
{0x2B,0x2F,0xFC,0xFC,0x2F,0x2B,0x00,0x00}, // 0xBE |
{0x10,0x10,0x10,0xF0,0xF0,0x00,0x00,0x00}, // 0xBF |
{0x00,0x00,0x00,0x1F,0x1F,0x10,0x10,0x10}, // 0xC0 |
{0x10,0x10,0x10,0x1F,0x1F,0x10,0x10,0x10}, // 0xC1 |
{0x10,0x10,0x10,0xF0,0xF0,0x10,0x10,0x10}, // 0xC2 |
{0x00,0x00,0x00,0xFF,0xFF,0x10,0x10,0x10}, // 0xC3 |
{0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10}, // 0xC4 |
{0x10,0x10,0x10,0xFF,0xFF,0x10,0x10,0x10}, // 0xC5 |
{0x22,0x77,0x55,0x57,0x7E,0x7B,0x41,0x00}, // 0xC6 |
{0x72,0x7B,0x2D,0x2F,0x7A,0x73,0x01,0x00}, // 0xC7 |
{0x00,0x00,0x1F,0x1F,0x10,0x17,0x17,0x14}, // 0xC8 |
{0x00,0x00,0xFC,0xFC,0x04,0xF4,0xF4,0x14}, // 0xC9 |
{0x14,0x14,0x17,0x17,0x10,0x17,0x17,0x14}, // 0xCA |
{0x14,0x14,0xF4,0xF4,0x04,0xF4,0xF4,0x14}, // 0xCB |
{0x00,0x00,0xFF,0xFF,0x00,0xF7,0xF7,0x14}, // 0xCC |
{0x14,0x14,0x14,0x14,0x14,0x14,0x14,0x14}, // 0xCD |
{0x14,0x14,0xF7,0xF7,0x00,0xF7,0xF7,0x14}, // 0xCE |
{0x66,0x3C,0x3C,0x24,0x3C,0x3C,0x66,0x00}, // 0xCF |
{0x05,0x27,0x72,0x57,0x7D,0x38,0x00,0x00}, // 0xD0 |
{0x49,0x7F,0x7F,0x49,0x63,0x7F,0x1C,0x00}, // 0xD1 |
{0x46,0x7D,0x7D,0x55,0x55,0x46,0x00,0x00}, // 0xD2 |
{0x45,0x7D,0x7C,0x54,0x55,0x45,0x00,0x00}, // 0xD3 |
{0x44,0x7D,0x7F,0x56,0x54,0x44,0x00,0x00}, // 0xD4 |
{0x0A,0x0E,0x08,0x00,0x00,0x00,0x00,0x00}, // 0xD5 |
{0x00,0x44,0x7E,0x7F,0x45,0x00,0x00,0x00}, // 0xD6 |
{0x02,0x45,0x7D,0x7D,0x45,0x02,0x00,0x00}, // 0xD7 |
{0x01,0x45,0x7C,0x7C,0x45,0x01,0x00,0x00}, // 0xD8 |
{0x10,0x10,0x10,0x1F,0x1F,0x00,0x00,0x00}, // 0xD9 |
{0x00,0x00,0x00,0xF0,0xF0,0x10,0x10,0x10}, // 0xDA |
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, // 0xDB |
{0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0}, // 0xDC |
{0x00,0x00,0x00,0x77,0x77,0x00,0x00,0x00}, // 0xDD |
{0x00,0x45,0x7F,0x7E,0x44,0x00,0x00,0x00}, // 0xDE |
{0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}, // 0xDF |
{0x38,0x7C,0x46,0x47,0x45,0x7C,0x38,0x00}, // 0xE0 |
{0xFC,0xFE,0x2A,0x2A,0x3E,0x14,0x00,0x00}, // 0xE1 |
{0x3A,0x7D,0x45,0x45,0x45,0x7D,0x3A,0x00}, // 0xE2 |
{0x38,0x7C,0x45,0x47,0x46,0x7C,0x38,0x00}, // 0xE3 |
{0x32,0x7B,0x49,0x4B,0x7A,0x33,0x01,0x00}, // 0xE4 |
{0x3A,0x7F,0x45,0x47,0x46,0x7F,0x39,0x00}, // 0xE5 |
{0x80,0xFE,0x7E,0x20,0x20,0x3E,0x1E,0x00}, // 0xE6 |
{0x42,0x7E,0x7E,0x54,0x1C,0x08,0x00,0x00}, // 0xE7 |
{0x41,0x7F,0x7F,0x55,0x14,0x1C,0x08,0x00}, // 0xE8 |
{0x3C,0x7C,0x42,0x43,0x7D,0x3C,0x00,0x00}, // 0xE9 |
{0x3A,0x79,0x41,0x41,0x79,0x3A,0x00,0x00}, // 0xEA |
{0x3C,0x7D,0x43,0x42,0x7C,0x3C,0x00,0x00}, // 0xEB |
{0xB8,0xB8,0xA2,0xA3,0xF9,0x78,0x00,0x00}, // 0xEC |
{0x0C,0x5C,0x72,0x73,0x5D,0x0C,0x00,0x00}, // 0xED |
{0x02,0x02,0x02,0x02,0x02,0x02,0x00,0x00}, // 0xEE |
{0x00,0x00,0x02,0x03,0x01,0x00,0x00,0x00}, // 0xEF |
{0x10,0x10,0x10,0x10,0x10,0x10,0x00,0x00}, // 0xF0 |
{0x44,0x44,0x5F,0x5F,0x44,0x44,0x00,0x00}, // 0xF1 |
{0x28,0x28,0x28,0x28,0x28,0x28,0x00,0x00}, // 0xF2 |
{0x71,0x35,0x1F,0x4C,0x66,0x73,0xD9,0xF8}, // 0xF3 |
{0x06,0x0F,0x09,0x7F,0x7F,0x01,0x7F,0x7F}, // 0xF4 |
{0xDA,0xBF,0xA5,0xA5,0xFD,0x59,0x03,0x02}, // 0xF5 |
{0x08,0x08,0x6B,0x6B,0x08,0x08,0x00,0x00}, // 0xF6 |
{0x00,0x80,0xC0,0x40,0x00,0x00,0x00,0x00}, // 0xF7 |
{0x00,0x06,0x0F,0x09,0x0F,0x06,0x00,0x00}, // 0xF8 |
{0x02,0x02,0x00,0x00,0x02,0x02,0x00,0x00}, // 0xF9 |
{0x00,0x00,0x00,0x10,0x10,0x00,0x00,0x00}, // 0xFA |
{0x00,0x12,0x13,0x1F,0x1F,0x10,0x10,0x00}, // 0xFB |
{0x00,0x11,0x15,0x15,0x1F,0x1F,0x0A,0x00}, // 0xFC |
{0x00,0x19,0x1D,0x15,0x17,0x12,0x00,0x00}, // 0xFD |
{0x00,0x00,0x3C,0x3C,0x3C,0x3C,0x00,0x00}, // 0xFE |
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00} // 0xFF |
}; |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd/Font8x8.h |
---|
0,0 → 1,31 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X8_H |
#define _FONT8X8_H |
#include <avr/pgmspace.h> |
extern const uint8_t Font8x8[256][8]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd/font8x6.c |
---|
0,0 → 1,232 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY font8x6.c |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK (ehemals 'Antenne' Ascii 31) |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: ASC 11 (0x0B) SYMBOL_SMALLDEGREE |
//# - add: ASC 16 (0x10) SYMBOL_RCQUALITY |
//############################################################################ |
#include <avr/pgmspace.h> |
// one byte is a column |
// bit 7 is the bottom |
// |
// 123456 |
// L 1 | XXX | |
// O 2 |X X | |
// W 4 |X X | |
// 8 | XXX | |
// H 1 |X X | |
// I 2 |X X | |
// G 4 | XXX | |
// H 8 | | |
// |
// 0x36,0x49,0x49,0x49,0x36,0x00 |
// |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | X| |
// 8 | X | |
// H 1 | X | |
// I 2 |X X | |
// G 4 | X | |
// H 8 | | |
// 0x20,0x40,0x20,0x10,0x08,0x04 |
// 123456 |
// L 1 | | |
// O 2 | X| |
// W 4 | X | |
// 8 | X | |
// H 1 |X X | |
// I 2 | X | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x10,0x08,0x04,0x02 |
// 123456 |
// L 1 | | |
// O 2 | | |
// W 4 | XX| |
// 8 | XX | |
// H 1 |X XX | |
// I 2 | XX | |
// G 4 | | |
// H 8 | | |
// 0x10,0x20,0x30,0x18,0x0c,0x04 |
//---------------------------------------------------------------- |
const uint8_t font8x6[128][6] PROGMEM = |
{ |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 0 00 (not useable) |
{ 0x78,0x15,0x14,0x15,0x78,0x00 }, // ASCII - 1 01 'Ä' |
{ 0x20,0x55,0x54,0x55,0x78,0x00 }, // ASCII - 2 02 'ä' |
{ 0x38,0x45,0x44,0x45,0x38,0x00 }, // ASCII - 3 03 'Ö' |
{ 0x30,0x49,0x48,0x49,0x30,0x00 }, // ASCII - 4 04 'ö' |
{ 0x3c,0x41,0x40,0x41,0x3c,0x00 }, // ASCII - 5 05 'Ü' |
{ 0x38,0x41,0x40,0x21,0x78,0x00 }, // ASCII - 6 06 'ü' |
{ 0x7e,0x15,0x15,0x15,0x0a,0x00 }, // ASCII - 7 07 'ß' |
{ 0x22,0x17,0x0F,0x17,0x22,0x00 }, // ASCII - 8 08 SAT Symbol |
{ 0x00,0x84,0x82,0xFF,0x82,0x84 }, // ASCII - 9 09 Altitude Symbol |
{ 0x1c,0x14,0x1c,0x00,0x00,0x00 }, // ASCII - 10 0A (not useable) SYMBOL_AVG |
{ 0x00,0x07,0x05,0x07,0x00,0x00 }, // ASCII - 11 0B small degree SYMBOL_SMALLDEGREE |
{ 0x10,0x38,0x54,0x10,0x10,0x1e }, // ASCII - 12 0C Enter Symbol |
{ 0x18,0x0c,0x18,0x00,0x00,0x00 }, // ASCII - 13 0D (not useable) SYMBOL_MAX |
{ 0x10,0x10,0x10,0x10,0x10,0x10 }, // ASCII - 14 0E hor. line |
{ 0x10,0x10,0x10,0x7c,0x10,0x10 }, // ASCII - 15 0F hor. line with tick mark |
{ 0x08,0x10,0x08,0x00,0x00,0x00 }, // ASCII - 16 10 rc quality SYMBOL_MIN |
{ 0x08,0x14,0x00,0x00,0x14,0x08 }, // ASCII - 17 11 <> Change |
{ 0x10,0x08,0x04,0x04,0x08,0x10 }, // ASCII - 18 12 /\ Up |
{ 0x08,0x10,0x20,0x20,0x10,0x08 }, // ASCII - 19 13 \/ Down |
{ 0x00,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 20 14 < Left |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 21 15 > Right |
{ 0x04,0x02,0x7f,0x02,0x04,0x00 }, // ASCII - 22 16 /|\ Arrow up |
{ 0x10,0x20,0x7f,0x20,0x10,0x00 }, // ASCII - 23 17 \|/ Arrow down |
{ 0x10,0x38,0x54,0x10,0x10,0x10 }, // ASCII - 24 18 <- Arrow left |
{ 0x10,0x10,0x10,0x54,0x38,0x10 }, // ASCII - 25 19 -> Arrow right |
{ 0x10,0x18,0x1c,0x1c,0x18,0x10 }, // ASCII - 26 1A ^ Triangle up |
{ 0x08,0x18,0x38,0x38,0x18,0x08 }, // ASCII - 27 1B v Triangle down |
{ 0x00,0x08,0x1c,0x3e,0x7f,0x00 }, // ASCII - 28 1C < Triangle left |
{ 0x00,0x7f,0x3e,0x1c,0x08,0x00 }, // ASCII - 29 1D > Triangle right |
{ 0x06,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 30 1E '°' big degree SYMBOL_BIGDEGREE |
// { 0x06,0x49,0x7d,0x49,0x06,0x00 }, // ASCII - 31 1F Antenne (ALT) |
{ 0x10,0x20,0x10,0x08,0x04,0x02 }, // ASCII - 31 1F SYMBOL_CHECK |
{ 0x00,0x00,0x00,0x00,0x00,0x00 }, // ASCII - 32 20 ' ' |
{ 0x00,0x00,0x2f,0x00,0x00,0x00 }, // ASCII - 33 21 '!' |
{ 0x00,0x07,0x00,0x07,0x00,0x00 }, // ASCII - 34 22 '"' |
{ 0x14,0x7f,0x14,0x7f,0x14,0x00 }, // ASCII - 35 23 '#' |
{ 0x24,0x2a,0x6b,0x2a,0x12,0x00 }, // ASCII - 36 24 '$' |
{ 0x23,0x13,0x08,0x64,0x62,0x00 }, // ASCII - 37 25 '%' |
{ 0x36,0x49,0x55,0x22,0x50,0x00 }, // ASCII - 38 26 '&' |
{ 0x00,0x05,0x03,0x00,0x00,0x00 }, // ASCII - 39 27 ''' |
{ 0x00,0x1c,0x22,0x41,0x00,0x00 }, // ASCII - 40 28 '(' |
{ 0x00,0x41,0x22,0x1c,0x00,0x00 }, // ASCII - 41 29 ')' |
{ 0x14,0x08,0x3e,0x08,0x14,0x00 }, // ASCII - 42 2a '*' |
{ 0x08,0x08,0x3e,0x08,0x08,0x00 }, // ASCII - 43 2b '+' |
{ 0x00,0x50,0x30,0x00,0x00,0x00 }, // ASCII - 44 2c ',' |
{ 0x08,0x08,0x08,0x08,0x08,0x00 }, // ASCII - 45 2d '-' |
{ 0x00,0x60,0x60,0x00,0x00,0x00 }, // ASCII - 46 2e '.' |
{ 0x20,0x10,0x08,0x04,0x02,0x00 }, // ASCII - 47 2f '/' |
{ 0x3e,0x51,0x49,0x45,0x3e,0x00 }, // ASCII - 48 30 '0' |
{ 0x00,0x42,0x7f,0x40,0x00,0x00 }, // ASCII - 49 31 '1' |
{ 0x42,0x61,0x51,0x49,0x46,0x00 }, // ASCII - 50 32 '2' |
{ 0x21,0x41,0x45,0x4b,0x31,0x00 }, // ASCII - 51 33 '3' |
{ 0x18,0x14,0x12,0x7f,0x10,0x00 }, // ASCII - 52 34 '4' |
{ 0x27,0x45,0x45,0x45,0x39,0x00 }, // ASCII - 53 35 '5' |
{ 0x3c,0x4a,0x49,0x49,0x30,0x00 }, // ASCII - 54 36 '6' |
{ 0x03,0x01,0x71,0x09,0x07,0x00 }, // ASCII - 55 37 '7' |
{ 0x36,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 56 38 '8' |
{ 0x06,0x49,0x49,0x29,0x1e,0x00 }, // ASCII - 57 39 '9' |
{ 0x00,0x36,0x36,0x00,0x00,0x00 }, // ASCII - 58 3a ':' |
{ 0x00,0x56,0x36,0x00,0x00,0x00 }, // ASCII - 59 3b ';' |
{ 0x08,0x14,0x22,0x41,0x00,0x00 }, // ASCII - 60 3c '<' |
{ 0x14,0x14,0x14,0x14,0x14,0x00 }, // ASCII - 61 3d '=' |
{ 0x00,0x41,0x22,0x14,0x08,0x00 }, // ASCII - 62 3e '>' |
{ 0x02,0x01,0x51,0x09,0x06,0x00 }, // ASCII - 63 3f '?' |
{ 0x32,0x49,0x79,0x41,0x3e,0x00 }, // ASCII - 64 40 '@' |
{ 0x7e,0x11,0x11,0x11,0x7e,0x00 }, // ASCII - 65 41 'A' |
{ 0x7f,0x49,0x49,0x49,0x36,0x00 }, // ASCII - 66 42 'B' |
{ 0x3e,0x41,0x41,0x41,0x22,0x00 }, // ASCII - 67 43 'C' |
{ 0x7f,0x41,0x41,0x22,0x1c,0x00 }, // ASCII - 68 44 'D' |
{ 0x7f,0x49,0x49,0x49,0x41,0x00 }, // ASCII - 69 45 'E' |
{ 0x7f,0x09,0x09,0x09,0x01,0x00 }, // ASCII - 70 46 'F' |
{ 0x3e,0x41,0x49,0x49,0x7a,0x00 }, // ASCII - 71 47 'G' |
{ 0x7f,0x08,0x08,0x08,0x7f,0x00 }, // ASCII - 72 48 'H' |
{ 0x00,0x41,0x7f,0x41,0x00,0x00 }, // ASCII - 73 49 'I' |
{ 0x20,0x40,0x41,0x3f,0x01,0x00 }, // ASCII - 74 4a 'J' |
{ 0x7f,0x08,0x14,0x22,0x41,0x00 }, // ASCII - 75 4b 'K' |
{ 0x7f,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 76 4c 'L' |
{ 0x7f,0x02,0x0c,0x02,0x7f,0x00 }, // ASCII - 77 4d 'M' |
{ 0x7f,0x04,0x08,0x10,0x7f,0x00 }, // ASCII - 78 4e 'N' |
{ 0x3e,0x41,0x41,0x41,0x3e,0x00 }, // ASCII - 79 4f 'O' |
{ 0x7f,0x09,0x09,0x09,0x06,0x00 }, // ASCII - 80 50 'P' |
{ 0x3e,0x41,0x51,0x21,0x5e,0x00 }, // ASCII - 81 51 'Q' |
{ 0x7f,0x09,0x19,0x29,0x46,0x00 }, // ASCII - 82 52 'R' |
{ 0x46,0x49,0x49,0x49,0x31,0x00 }, // ASCII - 83 53 'S' |
{ 0x01,0x01,0x7f,0x01,0x01,0x00 }, // ASCII - 84 54 'T' |
{ 0x3f,0x40,0x40,0x40,0x3f,0x00 }, // ASCII - 85 55 'U' |
{ 0x1f,0x20,0x40,0x20,0x1f,0x00 }, // ASCII - 86 56 'V' |
{ 0x3f,0x40,0x38,0x40,0x3f,0x00 }, // ASCII - 87 57 'W' |
{ 0x63,0x14,0x08,0x14,0x63,0x00 }, // ASCII - 88 58 'X' |
{ 0x07,0x08,0x70,0x08,0x07,0x00 }, // ASCII - 89 59 'Y' |
{ 0x61,0x51,0x49,0x45,0x43,0x00 }, // ASCII - 90 5a 'Z' |
{ 0x7f,0x41,0x41,0x00,0x00,0x00 }, // ASCII - 91 5b '[' |
{ 0x02,0x04,0x08,0x10,0x20,0x00 }, // ASCII - 92 5c '\' |
{ 0x00,0x41,0x41,0x7f,0x00,0x00 }, // ASCII - 93 5d ']' |
{ 0x04,0x02,0x01,0x02,0x04,0x00 }, // ASCII - 94 5e '^' |
{ 0x40,0x40,0x40,0x40,0x40,0x00 }, // ASCII - 95 5f '_' |
{ 0x00,0x01,0x02,0x04,0x00,0x00 }, // ASCII - 96 60 '`' |
{ 0x20,0x54,0x54,0x54,0x78,0x00 }, // ASCII - 97 61 'a' |
{ 0x7f,0x48,0x44,0x44,0x38,0x00 }, // ASCII - 98 62 'b' |
{ 0x38,0x44,0x44,0x44,0x20,0x00 }, // ASCII - 99 63 'c' |
{ 0x38,0x44,0x44,0x48,0x7f,0x00 }, // ASCII - 100 64 'd' |
{ 0x38,0x54,0x54,0x54,0x18,0x00 }, // ASCII - 101 65 'e' |
{ 0x08,0x7e,0x09,0x01,0x02,0x00 }, // ASCII - 102 66 'f' |
{ 0x0c,0x52,0x52,0x52,0x3e,0x00 }, // ASCII - 103 67 'g' |
{ 0x7f,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 104 68 'h' |
{ 0x00,0x44,0x7d,0x40,0x00,0x00 }, // ASCII - 105 69 'i' |
{ 0x20,0x40,0x44,0x3d,0x00,0x00 }, // ASCII - 106 6a 'j' |
{ 0x7f,0x10,0x28,0x44,0x00,0x00 }, // ASCII - 107 6b 'k' |
{ 0x00,0x41,0x7f,0x40,0x00,0x00 }, // ASCII - 108 6c 'l' |
{ 0x7c,0x04,0x18,0x04,0x78,0x00 }, // ASCII - 109 6d 'm' |
{ 0x7c,0x08,0x04,0x04,0x78,0x00 }, // ASCII - 110 6e 'n' |
{ 0x38,0x44,0x44,0x44,0x38,0x00 }, // ASCII - 111 6f 'o' |
{ 0x7c,0x14,0x14,0x14,0x08,0x00 }, // ASCII - 112 70 'p' |
{ 0x08,0x14,0x14,0x18,0x7c,0x00 }, // ASCII - 113 71 'q' |
{ 0x7c,0x08,0x04,0x04,0x08,0x00 }, // ASCII - 114 72 'r' |
{ 0x48,0x54,0x54,0x54,0x20,0x00 }, // ASCII - 115 73 's' |
{ 0x04,0x3f,0x44,0x40,0x20,0x00 }, // ASCII - 116 74 't' |
{ 0x3c,0x40,0x40,0x20,0x7c,0x00 }, // ASCII - 117 75 'u' |
{ 0x1c,0x20,0x40,0x20,0x1c,0x00 }, // ASCII - 118 76 'v' |
{ 0x3c,0x40,0x38,0x40,0x3c,0x00 }, // ASCII - 119 77 'w' |
{ 0x44,0x28,0x10,0x28,0x44,0x00 }, // ASCII - 120 78 'x' |
{ 0x0c,0x50,0x50,0x50,0x3c,0x00 }, // ASCII - 121 79 'y' |
{ 0x44,0x64,0x54,0x4c,0x44,0x00 }, // ASCII - 122 7a 'z' |
{ 0x00,0x08,0x36,0x41,0x00,0x00 }, // ASCII - 123 7b '{' |
{ 0x00,0x00,0x7f,0x00,0x00,0x00 }, // ASCII - 124 7c '|' |
{ 0x00,0x41,0x36,0x08,0x00,0x00 }, // ASCII - 125 7d '}' |
{ 0x08,0x08,0x2a,0x1c,0x08,0x00 }, // ASCII - 126 7e -> |
{ 0x08,0x1c,0x2a,0x08,0x08,0x00 }, // ASCII - 127 7f <- |
}; |
/* |
{ 0x02,0x0a,0x2a,0x0a,0x02,0x00 }, // ASCII - 16 10 rc quality SYMBOL_RCQUALITY (wieder entfernt weil der Platz gebraucht wurde) |
*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd/font8x6.h |
---|
0,0 → 1,30 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - font provided by Claas Anders "CaScAdE" Rathje * |
* - umlauts and special characters by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
#ifndef _FONT8X6_H |
#define _FONT8X6_H |
#include <avr/pgmspace.h> |
//extern prog_uint8_t font8x6[128][6]; |
extern const uint8_t font8x6[128][6]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd/lcd.c |
---|
0,0 → 1,1990 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.c |
//# |
//# 10.11.2014 cebra |
//# - fix: lcd_init() weiter Helligkeitseinstellungen korrigiert (OCR2A) |
//# |
//# 28.06.2014 OG |
//# - chg: lcdx_cls_rowwidth() kompatibel zu mode MINVERSX und MNORMALX |
//# - chg: lcdx_cls_row() auf lcdx_cls_rowwidth() umgestellt |
//# - fix: lcdx_printp() umgestellt auf strlen_P() statt strlen() |
//# |
//# 27.06.2014 OG |
//# - add: die lcd-print Funktionen wurden um MINVERSX und MNORMALX erweitert |
//# um einen zusaetzlichen Rand links und oben als Linie zu zeichnen |
//# (wird z.B. in followme.c verwendet) |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# - add: include <avr/interrupt.h> |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 26.05.2014 OG |
//# - chg: LCD_Init() - LCD-Modus nur noch normal (kein Invers mehr) |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 10.04.2014 OG |
//# - add: include: lipo.h |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# - chg: PRINTF_BUFFER_SIZE von 80 auf 40 |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: lcd_frect_round() |
//# - chg: lcd_rect_round() um R2 ergaenzt |
//# |
//# 12.02.2014 OG |
//# - add: lcd_rect_round() |
//# |
//# 04.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Aufbau der Formatmaske |
//# |
//# 03.02.2014 OG |
//# - fix: writex_ndigit_number_u_100th() Berechnung Formatstring korrigiert |
//# - fix: writex_ndigit_number_u_100th() Parameter 'mode' fehlte |
//# |
//# 16.06.2013 OG |
//# - chg: LCD_ORIENTATION festgesetzt auf 0 (Normal) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - chg: writex_datetime_time(), writex_datetime_date() Lokalzeitberechnung |
//# via UTCdatetime2local() |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer GPS-Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 29.04.2013 OG |
//# - chg: lcd_cls() geaendert auf memset |
//# |
//# 28.04.2013 OG |
//# - chg: high-level Funktionen wie writex_ndigit... auf xprintf umgebaut |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# siehe dazu: Doku in utils/xstring.h fuer xprintf |
//# |
//# 27.03.2013 OG |
//# - add: writex_datetime_time() |
//# - add: writex_datetime_date() |
//# - add: Show_PKTError_NoRAM() |
//# |
//# 22.03.2013 OG |
//# - add: writex_gpspos() |
//# |
//# 11.03.2013 OG |
//# - fix: writex_time() Ausgabe Doppelpunkt ":" ergaenzt um mode,xoffs,yoffs |
//# |
//# 07.03.2013 OG |
//# - del: Kompatibilitaetsfunktion lcd_printpj_at() entfernt |
//# |
//# 06.03.2013 OG |
//# - lcdx_putc() wurde erweitert mit Unterstuetzung des 8x8 Font (alte Jeti |
//# Funktionen) und Pixel-Verschiebung (xoffs,yoffs) |
//# - etliche Char basierte Funktionen wurden erweitert um Parameter xoffs,yoffs |
//# um die Ausgabe um +/- Pixel zu verschieben. Fuer Pixelgenaue Positionierung |
//# von OSD-Screen Elementen. |
//# Die neuen Funktionen mit Pixel-Verschiebung beginnen mit lcdx_, writex_ ... |
//# - add: Kompatibilitaet gewaehrleistet durch Mapper-Funktionen |
//# - add: defines fuer Char-Drawmode's (MNORMAL, MINVERS, MBIG, MBIGINVERS) |
//# - del: Jeti-Funktionen (teilweise ersetzt durch Kompatibilitaetsfunktionen) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <stdarg.h> |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "lcd.h" |
#include "../main.h" |
#include "../HAL_HW3_9.h" |
#include "../utils/xutils.h" // xprintf, UTCdatetime2local |
#include "../lipo/lipo.h" // show_Lipo() |
#include "font8x6.h" |
#ifdef USE_FONT_BIG |
#include "Font8x8.h" |
#endif |
#define DISP_W 128 |
#define DISP_H 64 |
#define DISP_BUFFER ((DISP_H * DISP_W) / 8) |
#define LINE_BUFFER (((DISP_H/8) * DISP_W) / 8) |
#define Jeti 1 // Jeti Routinen |
volatile uint8_t display_buffer[DISP_BUFFER]; // Display-Puffer, weil nicht zurückgelesen werden kann |
volatile uint8_t line_buffer[LINE_BUFFER]; // Zeilen-Puffer, weil nicht zurückgelesen werden kann |
volatile uint16_t display_buffer_pointer; // Pointer auf das aktuell übertragene Byte |
volatile uint8_t display_buffer_counter; // Hilfszähler zur Selektierung der Page |
volatile uint8_t display_page_counter; // aktuelle Page-Nummer |
volatile uint8_t display_mode; // Modus für State-Machine |
volatile uint8_t LCD_ORIENTATION = 0; // 0=Normal / 1=reversed |
// DOG: 128 x 64 with 6x8 Font => 21 x 8 |
// MAX7456: 30 x 16 |
uint8_t lcd_xpos; |
uint8_t lcd_ypos; |
char s[7]; |
#define PRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer lcdx_printf_at() / lcdx_printf_at_P() |
char printf_buffer[PRINTF_BUFFER_SIZE]; |
char format_buffer[20]; |
//----------------------------------------------------------- |
void send_byte( uint8_t data ) |
{ |
clr_cs (); |
SPDR = data; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
// * Writes one command byte |
// * cmd - the command byte |
// |
void lcd_command( uint8_t cmd ) |
{ |
// LCD_SELECT(); |
// LCD_CMD(); |
// spi_write(cmd); |
// LCD_UNSELECT(); |
clr_cs (); |
SPDR = cmd; |
while (!(SPSR & (1<<SPIF))); |
//SPSR = SPSR; |
set_cs (); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_set_contrast( uint8_t value ) |
{ |
cli(); |
clr_A0(); |
send_byte( 0x81 ); |
send_byte( value ); // Daten zum LCD senden |
set_A0(); |
sei(); |
} |
//----------------------------------------------------------- |
void lcd_cls( void ) |
{ |
uint16_t i, j; |
memset( (void *)display_buffer, 0, 1024); |
// for (i = 0; i < DISP_BUFFER; i++) |
// display_buffer[i] = 0x00; |
for (i = 0; i < 8; i++) |
{ |
clr_A0 (); |
send_byte (0xB0 + i); //1011xxxx |
send_byte (0x10); //00010000 |
// send_byte(0x04); //00000100 gedreht plus 4 Byte |
// send_byte(0x00); //00000000 |
send_byte (LCD_ORIENTATION); //00000000 |
set_A0 (); |
for (j = 0; j < 128; j++) |
send_byte (0x00); |
} |
lcd_xpos = 0; |
lcd_ypos = 0; |
} |
//----------------------------------------------------------- |
void set_adress (uint16_t adress, uint8_t data) |
{ |
uint8_t page; |
uint8_t column; |
page = adress >> 7; |
clr_A0 (); |
send_byte (0xB0 + page); |
column = (adress & 0x7F) + LCD_ORIENTATION; |
send_byte (0x10 + (column >> 4)); |
send_byte (column & 0x0F); |
set_A0 (); |
send_byte (data); |
} |
//----------------------------------------------------------- |
void scroll (void) |
{ |
uint16_t adress; |
for (adress = 0; adress < 896; adress++) |
{ |
display_buffer[adress] = display_buffer[adress + 128]; |
set_adress (adress, display_buffer[adress]); |
} |
for (adress = 896; adress < 1024; adress++) |
{ |
display_buffer[adress] = 0; |
set_adress (adress, 0); |
} |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
// + Plot (set one Pixel) |
//----------------------------------------------------------- |
// mode: |
// 0=Clear, 1=Set, 2=XOR |
void lcd_plot (uint8_t xpos, uint8_t ypos, uint8_t mode) |
{ |
uint16_t adress; |
uint8_t mask; |
if ((xpos < DISP_W) && (ypos < DISP_H)) |
{ |
adress = (ypos / 8) * DISP_W + xpos; // adress = 0/8 * 128 + 0 = 0 |
mask = 1 << (ypos & 0x07); // mask = 1<<0 = 1 |
adress &= DISP_BUFFER - 1; |
switch (mode) |
{ |
case 0: |
display_buffer[adress] &= ~mask; |
break; |
case 1: |
display_buffer[adress] |= mask; |
break; |
case 2: |
display_buffer[adress] ^= mask; |
break; |
} |
set_adress (adress, display_buffer[adress]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t ch; |
uint16_t adress; |
uint8_t x1,y1; |
uint8_t x0,y0; |
uint8_t xw; |
uint8_t mask; |
uint8_t bit; |
uint8_t *font; |
//------------------------ |
// char translate |
//------------------------ |
switch (c) |
{ // ISO 8859-1 |
case 0xc4: c = 0x01; break; // Ä |
case 0xe4: c = 0x02; break; // ä |
case 0xd6: c = 0x03; break; // Ö |
case 0xf6: c = 0x04; break; // ö |
case 0xdc: c = 0x05; break; // Ü |
case 0xfc: c = 0x06; break; // ü |
case 0xdf: c = 0x1e; break; // ß c = 0x07; ° (used by Jeti) |
} |
c &= 0x7f; |
//------------------------ |
// Font Parameter setzen |
//------------------------ |
#ifdef USE_FONT_BIG |
if( mode <=2 ) // normaler font (8x6) |
{ |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
} |
else // grosser font (8x8) |
{ |
font = (uint8_t *) &Font8x8[0][0]; |
xw = 8; |
} |
#else |
font = (uint8_t *) &font8x6[0][0]; |
xw = 6; |
#endif |
//------------------------ |
//------------------------ |
x0 = (x*xw) + xoffs; |
y0 = (y*8) + yoffs; |
if( yoffs == 0) |
{ |
//---------------------------------------------------------- |
// orginaler Character Algo |
// |
// funktioniert auch mit x Verschiebung aber nicht |
// mit y Verschiebung. |
// |
// Da 8 Bit aufeinmal gesetzt werden ist dieser Algo |
// bzgl. Geschwindigkeit effektiver als der Y-Algo. |
//---------------------------------------------------------- |
if( mode==MINVERS || mode==MBIGINVERS ) |
lcd_frect( (x*xw)+xoffs, (y*8), xw-1, 7, 1); // invertierte Darstellung |
adress = y * 128 + x * xw + xoffs; |
adress &= 0x3FF; |
for( x1 = 0; x1 < xw; x1++) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
if( mode==MINVERS || mode==MBIGINVERS ) |
display_buffer[adress + x1] ^= ch; |
else |
display_buffer[adress + x1] = ch; |
set_adress (adress + x1, display_buffer[adress + x1]); |
} |
} |
else |
{ |
//---------------------------------------------------------- |
// Y-Algo |
// neuer Character Algo (nur wenn Pixel y-Verschiebung) |
//---------------------------------------------------------- |
for( x1 = 0; x1 < xw; x1++ ) |
{ |
ch = pgm_read_byte (font + x1 + c * xw); |
mask = 1; |
for( y1 = 0; y1 < 8; y1++ ) |
{ |
bit = (ch & mask); |
if( bit ) |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 0 : 1) ); |
else |
lcd_plot( x0+x1, y0+y1, ( (mode==MINVERS || mode==MBIGINVERS) ? 1 : 0) ); |
mask = (mask << 1); |
} |
} |
} |
} |
//----------------------------------------------------------- |
//--- Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode ) |
{ |
lcdx_putc( x, y, c, mode, 0,0 ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w) |
{ |
uint8_t lcd_width; |
uint8_t lcd_zpos; |
uint8_t i; |
uint8_t max = 21; |
lcd_width = w; |
lcd_xpos = x; |
lcd_ypos = y; |
if ((lcd_xpos + lcd_width) > max) |
lcd_width = max - lcd_xpos; |
lcd_zpos = lcd_xpos + lcd_width; |
for (i = lcd_xpos; i < lcd_zpos; i++) |
lcd_putc (i, lcd_ypos, 0x20, 0); |
} |
//----------------------------------------------------------- |
void wait_1ms (void) |
{ |
_delay_ms (1); |
} |
//----------------------------------------------------------- |
void wait_ms (uint16_t time) |
{ |
uint16_t i; |
for (i = 0; i < time; i++) |
wait_1ms (); |
} |
//----------------------------------------------------------- |
void LCD_Init( uint8_t LCD_Mode ) // LCD_Mode 0= Default Mode 1= EEPROM-Parameter) |
{ |
lcd_xpos = 0; |
lcd_ypos = 0; |
// DDRB = 0xFF; |
// SPI max. speed |
// the DOGM128 lcd controller can work at 20 MHz |
SPCR = (1 << SPE) | (1 << MSTR) | (1 << CPHA) | (1 << CPOL); |
SPSR = (1 << SPI2X); |
set_cs(); |
clr_reset(); |
wait_ms(10); |
set_reset(); |
clr_cs(); |
clr_A0(); |
send_byte( 0x40 ); //Display start line = 0 |
//------------------------------------------------------------ |
// 10.11.2014 cebra |
// Umschaltung der Displayansicht wird nicht mehr benötigt |
// if (LCD_Mode == 1) |
// { |
// if (LCD_ORIENTATION == 0) |
// { |
// send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
// else |
// { |
// send_byte( 0xA0 ); // A1 normal A0 reverse(original) |
// send_byte( 0xC8 ); // C0 normal C8 reverse(original) |
// } |
// } |
// else |
// { |
send_byte( 0xA1 ); // A1 normal A0 reverse(original) |
send_byte( 0xC0 ); // C0 normal C8 reverse(original) |
// } |
/* |
//------------------------------------------------------------ |
// 26.05.2014 OG: einstellen von Normal/Invers durch den User |
// nicht mehr unterstuetzt, da der Modus Invers beim PKT |
// auf verschiedenen Display's nicht gut aussieht. |
// Ab jetzt nur noch LC-Modus Normal. |
//------------------------------------------------------------ |
if (LCD_Mode == 1) |
{ |
if (Config.LCD_DisplayMode == 0) |
send_byte (0xA6); //Display normal, not mirrored |
else |
send_byte (0xA7); //Display reverse, not mirrored |
} |
else |
send_byte (0xA6); |
*/ |
send_byte( 0xA6 ); //Display normal, not mirrored |
send_byte( 0xA2 ); //Set bias 1/9 (Duty 1/65) |
send_byte( 0x2F ); //Booster, regulator and follower on |
send_byte( 0xF8 ); //Set internal booster to 4x |
send_byte( 0x00 ); //Set internal booster to 4x |
send_byte( 0x27 ); //resistor ratio set |
if( LCD_Mode == 1 ) |
{ |
send_byte( 0x81 ); //Electronic volume register set |
send_byte( Config.LCD_Kontrast ); //Electronic volume register set |
} |
else |
{ |
send_byte( 0x81 ); |
send_byte( 0x16 ); |
} |
send_byte( 0xAC ); //Cursor |
send_byte( 0x00 ); //No Cursor |
send_byte( 0xAF ); //No indicator |
if( Config.HWSound == 0 ) |
{ |
if( LCD_Mode == 1 ) |
{ |
//------------------------------------------------------------ |
//10.11.2014 cebra, |
//Helligkeitseinstellungen werden nicht mehr genutzt |
// Helligkeit setzen |
// OCR2A = Config.LCD_Helligkeit * 2.55; |
//OCR2A = 255; |
} |
else |
{ |
// OCR2A = 255; |
} |
} |
lcd_cls(); |
} |
//----------------------------------------------------------- |
// sicher eine Zeile für die Statusanzeige |
void copy_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
line_buffer[i] = display_buffer[adress+i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
// holt gesicherte Zeile wieder zurück |
void paste_line (uint8_t y) |
{ |
uint8_t i; |
uint16_t adress; |
adress = y * 128 + 0 * 6; |
adress &= 0x3FF; |
for (i = 0; i < 6*21; i++) |
{ |
display_buffer[adress+i] =line_buffer[i]; |
set_adress (adress + i, display_buffer[adress + i]); |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
while (*s) |
{ |
lcdx_putc(x, y, *s++, mode, xoffs,yoffs); |
x++; |
} |
}/* lcd_puts */ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_puts_at( uint8_t x, uint8_t y, const char *s, uint8_t mode ) |
{ |
lcdx_puts_at( x, y, s, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void new_line (void) |
{ |
lcd_ypos++; |
if (lcd_ypos > 7) |
{ |
scroll (); |
lcd_ypos = 7; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns (const char *text, uint8_t mode) |
{ |
while (pgm_read_byte(text)) |
{ |
switch (pgm_read_byte(text)) |
{ |
case 0x0D: |
lcd_xpos = 0; |
break; |
case 0x0A: |
new_line(); |
break; |
default: |
lcd_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode); |
lcd_xpos++; |
if (lcd_xpos > 21) |
{ |
lcd_xpos = 0; |
// new_line (); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_printpns (text, mode); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// erweitert bei mode MINVERSX und MNORMALX links und oben den |
// Text um jeweils einen Pixel -> der Invers-Modus sieht besser aus |
//-------------------------------------------------------------- |
uint8_t _lcdx_print_modeextend( uint8_t progmem, uint8_t x, uint8_t y, uint8_t textlen, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
uint8_t draw = 0; |
if( (mode == MNORMALX) || (mode == MINVERSX) ) |
{ |
if( mode == MNORMALX ) mode = MNORMAL; |
else mode = MINVERS; |
if( mode == MINVERS ) |
draw = 1; |
if( (y*8)+yoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs, (y*8)+yoffs-1, (x*6)+xoffs+(textlen*6)-1, (y*8)+yoffs-1, draw); // horizontale Linie ueber dem Text |
if( (x*6)+xoffs-1 >= 0 ) |
lcd_line( (x*6)+xoffs-1, (y*8)+yoffs-1, (x*6)+xoffs-1, (y*8)+yoffs+7, draw); // vertikale Linie links neben dem Text |
} |
return mode; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void _lcdx_print_outchar( unsigned char c, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc( lcd_xpos, lcd_ypos, c, mode, xoffs,yoffs ); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_modeextend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
_lcdx_print_outchar( *text, mode, xoffs,yoffs); |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
char c; |
mode = _lcdx_print_modeextend( true, lcd_xpos, lcd_ypos, strlen_P( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( (c = pgm_read_byte(text)) ) |
{ |
_lcdx_print_outchar( c, mode, xoffs,yoffs); |
text++; |
} |
} |
/* |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print( uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( false, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // RAM Modus |
while( *text ) |
{ |
switch( *text ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, *text, mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp( const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
mode = _lcdx_print_invers_extend( true, lcd_xpos, lcd_ypos, strlen( (const char *)text), mode, xoffs, yoffs ); // PROGMEM Modus |
while( pgm_read_byte(text) ) |
{ |
switch( pgm_read_byte(text) ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, pgm_read_byte(text), mode, xoffs,yoffs); |
lcd_xpos++; |
if( lcd_xpos > 21 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
text++; |
} |
} |
*/ |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp (const char *text, uint8_t mode) |
{ |
lcdx_printp ( text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_printp (text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode) |
{ |
lcdx_printp_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
//--- lcd_print: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print (uint8_t *text, uint8_t mode ) |
{ |
lcdx_print (text, mode, 0,0 ); |
} |
//----------------------------------------------------------- |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcdx_print (text, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen((const char *)text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_print_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
xoffs = xoffs + ((uint8_t)(64 - ( (strlen_P(text)*6) /2))); // Pixelgenau zentrieren (fuer 6x8 font) |
lcdx_printp_at( 0, y, text, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_center_P( y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Die Ausgabe wird zentriert, |
// |
// Parameter: |
// y : Position in Char y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_center( y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
//--- lcd_print_at: Kompatibilitaet |
//----------------------------------------------------------- |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode ) |
{ |
lcdx_print_at ( x, y, text, mode, 0,0); |
} |
//----------------------------------------------------------- |
void print_display (uint8_t *text) |
{ |
while (*text) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, *text, 0); |
lcd_xpos++; |
if (lcd_xpos >= 20) |
{ |
lcd_xpos = 0; |
new_line (); |
} |
text++; |
} |
} |
//----------------------------------------------------------- |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
print_display (text); |
} |
//----------------------------------------------------------- |
// + Line (draws a line from x1,y1 to x2,y2 |
// + Based on Bresenham line-Algorithm |
// + found in the internet, modified by thkais 2007 |
//----------------------------------------------------------- |
void lcd_line( unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode) |
{ |
int x, y, count, xs, ys, xm, ym; |
x = (int) x1; |
y = (int) y1; |
xs = (int) x2 - (int) x1; |
ys = (int) y2 - (int) y1; |
if (xs < 0) |
xm = -1; |
else |
if (xs > 0) |
xm = 1; |
else |
xm = 0; |
if (ys < 0) |
ym = -1; |
else |
if (ys > 0) |
ym = 1; |
else |
ym = 0; |
if (xs < 0) |
xs = -xs; |
if (ys < 0) |
ys = -ys; |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
if (xs > ys) // Flat Line <45 degrees |
{ |
count = -(xs / 2); |
while (x != x2) |
{ |
count = count + ys; |
x = x + xm; |
if (count > 0) |
{ |
y = y + ym; |
count = count - xs; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
else // Line >=45 degrees |
{ |
count =- (ys / 2); |
while (y != y2) |
{ |
count = count + xs; |
y = y + ym; |
if (count > 0) |
{ |
x = x + xm; |
count = count - ys; |
} |
lcd_plot ((unsigned char) x, (unsigned char) y, mode); |
} |
} |
} |
//----------------------------------------------------------- |
// + Filled rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_frect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
uint16_t i; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
for (i = y1; i <= y2; i++) |
{ |
lcd_line (x1, i, x2, i, mode); |
} |
} |
//----------------------------------------------------------- |
// ausgefuelltes Rechteck mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_frect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of rectangle |
// + x1, y1 = upper left corner |
//----------------------------------------------------------- |
void lcd_rect( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode) |
{ |
uint16_t x2, y2; |
if (x1 >= DISP_W) |
x1 = DISP_W - 1; |
if (y1 >= DISP_H) |
y1 = DISP_H - 1; |
x2 = x1 + widthx; |
y2 = y1 + widthy; |
if (x2 > DISP_W) |
x2 = DISP_W; |
if (y2 > DISP_H) |
y2 = DISP_H; |
lcd_line (x1, y1, x2, y1, mode); |
lcd_line (x2, y1, x2, y2, mode); |
lcd_line (x2, y2, x1, y2, mode); |
lcd_line (x1, y2, x1, y1, mode); |
} |
//----------------------------------------------------------- |
// Rechteck mit mit abgerundeten Ecken |
// |
// Hinweis: |
// r (Radius) ist aktuell 'pseudo' und unterstuetzt |
// nur R0 (=0), R1 (=1) oder R2 (=2= |
//----------------------------------------------------------- |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r) |
{ |
lcd_rect( x1, y1, widthx, widthy, mode); |
switch(r) |
{ |
case R0: break; |
case R2: |
lcd_plot( x1+1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1 , y1+1 , !mode); |
lcd_plot( x1+1 , y1+1 , mode); |
lcd_plot( x1+widthx-1, y1 , !mode); // Ecke rechts oben |
lcd_plot( x1+widthx , y1+1 , !mode); |
lcd_plot( x1+widthx-1, y1+1 , mode); |
lcd_plot( x1 , y1+widthy-1, !mode); // Ecke links unten |
lcd_plot( x1+1 , y1+widthy , !mode); |
lcd_plot( x1+1 , y1+widthy-1, mode); |
lcd_plot( x1+widthx-1, y1+widthy , !mode); // Ecke rechts unten |
lcd_plot( x1+widthx , y1+widthy-1, !mode); |
lcd_plot( x1+widthx-1, y1+widthy-1, mode); |
case R1: |
lcd_plot( x1 , y1 , !mode); // Ecke links oben |
lcd_plot( x1+widthx , y1 , !mode); // Ecke rechts oben |
lcd_plot( x1 , y1+widthy , !mode); // Ecke links unten |
lcd_plot( x1+widthx , y1+widthy , !mode); // Ecke rechts unten |
} |
} |
//----------------------------------------------------------- |
// + outline of a circle |
// + Based on Bresenham-algorithm found in wikipedia |
// + modified by thkais (2007) |
//----------------------------------------------------------- |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_plot (x0, y0 + radius, mode); |
lcd_plot (x0, y0 - radius, mode); |
lcd_plot (x0 + radius, y0, mode); |
lcd_plot (x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y --; |
ddF_y += 2; |
f += ddF_y; |
} |
x ++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
lcd_plot (x0 + y, y0 + x, mode); |
lcd_plot (x0 - y, y0 + x, mode); |
lcd_plot (x0 + y, y0 - x, mode); |
lcd_plot (x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// + filled Circle |
// + modified circle-algorithm thkais (2007) |
//----------------------------------------------------------- |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius,uint8_t mode) |
{ |
int16_t f = 1 - radius; |
int16_t ddF_x = 0; |
int16_t ddF_y = -2 * radius; |
int16_t x = 0; |
int16_t y = radius; |
lcd_line (x0, y0 + radius, x0, y0 - radius, mode); |
lcd_line (x0 + radius, y0, x0 - radius, y0, mode); |
while (x < y) |
{ |
if (f >= 0) |
{ |
y--; |
ddF_y += 2; |
f += ddF_y; |
} |
x++; |
ddF_x += 2; |
f += ddF_x + 1; |
lcd_line (x0 + x, y0 + y, x0 - x, y0 + y, mode); |
lcd_line (x0 + x, y0 - y, x0 - x, y0 - y, mode); |
lcd_line (x0 + y, y0 + x, x0 - y, y0 + x, mode); |
lcd_line (x0 + y, y0 - x, x0 - y, y0 - x, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) r); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) r); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode) |
{ |
uint8_t xc, yc; |
double deg_rad; |
deg_rad = (deg * M_PI) / 180.0; |
yc = y - (uint8_t) round (cos (deg_rad) * (double) ry); |
xc = x + (uint8_t) round (sin (deg_rad) * (double) rx); |
lcd_line (x, y, xc, yc, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode) |
{ |
const int16_t rx2 = rx * rx; |
const int16_t ry2 = ry * ry; |
int16_t F = round (ry2 - rx2 * ry + 0.25 * rx2); |
int16_t ddF_x = 0; |
int16_t ddF_y = 2 * rx2 * ry; |
int16_t x = 0; |
int16_t y = ry; |
lcd_plot (x0, y0 + ry, mode); |
lcd_plot (x0, y0 - ry, mode); |
lcd_plot (x0 + rx, y0, mode); |
lcd_plot (x0 - rx, y0, mode); |
// while ( 2*ry2*x < 2*rx2*y ) { we can use ddF_x and ddF_y |
while (ddF_x < ddF_y) |
{ |
if(F >= 0) |
{ |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F -= ddF_y; |
} |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x + ry2; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
F = round (ry2 * (x + 0.5) * (x + 0.5) + rx2 * (y - 1) * (y - 1) - rx2 * ry2); |
while(y > 0) |
{ |
if(F <= 0) |
{ |
x += 1; // east |
ddF_x += 2 * ry2; |
F += ddF_x; |
} |
y -= 1; // south |
ddF_y -= 2 * rx2; |
F += rx2 - ddF_y; |
lcd_plot (x0 + x, y0 + y, mode); |
lcd_plot (x0 + x, y0 - y, mode); |
lcd_plot (x0 - x, y0 + y, mode); |
lcd_plot (x0 - x, y0 - y, mode); |
} |
} |
//----------------------------------------------------------- |
// |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode) |
{ |
lcd_ellipse (x0, y0, radius + 3, radius, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode) |
{ |
lcd_ellipse_line(x, y, r + 3, r, deg, mode); |
} |
//----------------------------------------------------------- |
// |
void lcd_view_font (uint8_t page) |
{ |
int x; |
int y; |
lcd_cls (); |
lcd_printp (PSTR(" 0123456789ABCDEF\r\n"), 0); |
lcd_printpns_at (0, 7, PSTR(" \x1a \x1b Exit"), 0); |
lcd_ypos = 2; |
for (y = page * 4 ; y < (page * 4 + 4); y++) |
{ |
if (y < 10) |
{ |
lcd_putc (0, lcd_ypos, '0' + y, 0); |
} |
else |
{ |
lcd_putc (0, lcd_ypos, 'A' + y - 10, 0); |
} |
lcd_xpos = 2; |
for (x = 0; x < 16; x++) |
{ |
lcd_putc (lcd_xpos, lcd_ypos, y * 16 + x, 0); |
lcd_xpos++; |
} |
lcd_ypos++; |
} |
} |
//----------------------------------------------------------- |
uint8_t hdigit (uint8_t d) |
{ |
if (d < 10) |
{ |
return '0' + d; |
} |
else |
{ |
return 'A' + d - 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos, lcd_ypos, hdigit (h & 0x0f), mode); |
} |
//----------------------------------------------------------- |
void lcd_print_hex (uint8_t h, uint8_t mode) |
{ |
// lcd_xpos = x; |
// lcd_ypos = y; |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h >> 4), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, hdigit (h & 0x0f), mode); |
lcd_putc (lcd_xpos++, lcd_ypos, ' ', mode); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung |
// mit erweitertem xoffs,yoffs (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// xoffs,yoffs: Verschiebung in Pixel |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, xoffs, yoffs); |
} |
//----------------------------------------------------------- |
// lcdx_printf_at( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (RAM-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus RAM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
// _xvsnprintf( int useprogmem, char *buffer, size_t n, const char *format, va_list ap ) |
_xvsnprintf( false, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//----------------------------------------------------------- |
// lcd_printf_at_P( x,y, mode, xoffs,yoffs, format, ...) |
// |
// Ausgabe von n-Parametern via Formattierung (PROGMEN-Version) |
// |
// Parameter: |
// x,y : Position in Char x,y |
// mode : MNORMAL, MINVERS, ... |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, printf_buffer, PRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
lcdx_print_at( x, y, (unsigned char *)printf_buffer, mode, 0,0); |
} |
//################################################################################################################################## |
//################################################################################################################################## |
//----------------------------------------------------------- |
void lcd_write_number_u (uint8_t number) |
{ |
uint8_t num = 100; |
uint8_t started = 0; |
while (num > 0) |
{ |
uint8_t b = number / num; |
if (b > 0 || started || num == 1) |
{ |
lcd_putc (lcd_xpos++, lcd_ypos, '0' + b, 0); |
started = 1; |
} |
number -= b * num; |
num /= 10; |
} |
} |
//----------------------------------------------------------- |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
lcd_write_number_u (number); |
} |
//----------------------------------------------------------- |
// numtype: 'd' oder 'u' |
// |
// Ergebnis: ein String in format_buffer |
// -> "%4d" "%4.2d" "%03.1d" "%4u" usw... |
//----------------------------------------------------------- |
void make_number_format( char numtype, int16_t length, int16_t decimals, uint8_t pad ) |
{ |
register char *p; |
register char *psrc; |
p = format_buffer; |
*p = '%'; p++; // start '%' |
if(pad) { *p = '0'; p++; } // pad '0' |
itoa( length, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // vorkomma |
if( decimals > 0 ) |
{ |
*p = '.'; p++; // punkt '.' |
itoa( decimals, s, 10 ); |
psrc = s; while(*psrc) *p++ = *psrc++; // nachkomma |
} |
*p = numtype; p++; // 'd' oder 'u' |
*p = 0; |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length, 0, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s (x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a unsigned <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_10th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_u_10th( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'u', length-2, 2, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad) |
{ |
writex_ndigit_number_u_100th( x, y, number, length, pad, MNORMAL, 0,0); |
} |
//----------------------------------------------------------- |
// Write only some digits of a signed <number> at <x>/<y> to MAX7456 display memory |
// as /10th of the value |
// <num> represents the largest multiple of 10 that will still be displayable as |
// the first digit, so num = 10 will be 0-99 and so on |
// <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
// |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
make_number_format( 'd', length-1, 1, pad ); // ergebnis in: format_buffer |
lcdx_printf_at( x, y, mode, xoffs, yoffs, format_buffer, number ); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode) |
{ |
writex_ndigit_number_s_10th ( x, y, number, length, pad, mode, 0,0); |
} |
//----------------------------------------------------------- |
// write <seconds> as human readable time at <x>/<y> to MAX7456 display mem |
// |
void writex_time( uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
uint16_t min = seconds / 60; |
seconds -= min * 60; |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u", min, seconds); |
/* |
writex_ndigit_number_u (x, y, min, 2, 0,mode, xoffs,yoffs); |
lcdx_putc (x + 2, y, ':', mode, xoffs,yoffs); |
writex_ndigit_number_u (x + 3, y, seconds, 2, 1,mode, xoffs,yoffs); |
*/ |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void write_time( uint8_t x, uint8_t y, uint16_t seconds) |
{ |
writex_time( x, y, seconds, 0, 0,0); |
} |
//-------------------------------------------------------------- |
// writex_datetime_time() |
// |
// Anzeigeformat: 'hh:mm:ss' |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u:%02u:%02u", (uint8_t)(dtlocal.seconds/3600), (uint8_t)((dtlocal.seconds/60)%60), (uint8_t)(dtlocal.seconds%60)); |
} |
//-------------------------------------------------------------- |
// writex_datetime_date() |
// |
// Anzeigeformat: 'dd.mm.yyyy' (keine Unterstuetzung von anderen Formaten aus aller Welt) |
// |
// datetime : vom Typ PKTdatetime_t |
//-------------------------------------------------------------- |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
PKTdatetime_t dtlocal; |
if( datetime.year > 0 ) |
{ |
UTCdatetime2local( &dtlocal, &datetime ); |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%02u.%02u.%04u", dtlocal.day, dtlocal.month, dtlocal.year); |
} |
else |
{ |
// keine UTCZeit -> keine Datumsanzeige |
lcdx_printp_at( x, y, PSTR(" . . "), mode, xoffs,yoffs); |
} |
} |
//----------------------------------------------------------- |
// writes a single gps-pos |
//----------------------------------------------------------- |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printf_at( x, y, mode, xoffs, yoffs, "%3.5ld", GPSpos/100); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Show_PKTError_NoRAM(void) |
{ |
lcd_cls(); |
lcd_rect( 8, 8, 127-16, 6*8, 1); |
// 123456789012345678901 |
lcd_printp_at( 2,2 , PSTR("** OUT OF RAM **"), MINVERS); |
lcd_printp_at( 2,4 , PSTR("this function is"), MNORMAL); |
lcd_printp_at( 2,5 , PSTR("not available!"), MNORMAL); |
while ( !( (get_key_press (1 << KEY_ENTER)) || (get_key_press (1 << KEY_ESC)) || (get_key_press (1 << KEY_PLUS)) || (get_key_press (1 << KEY_MINUS)) ) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ShowTitle_P( const char *title, uint8_t clearscreen ) |
{ |
if( clearscreen ) |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( strlen_P(title) < 17 ) |
show_Lipo(); |
lcd_printp_at ( 1, 0, title , MINVERS); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void Popup_Draw( uint8_t heigthC ) |
{ |
uint8_t y, h; |
h = heigthC*8; |
y = 63-h; |
//lcd_frect( 0, (hC*8)-6, 127, 63-(hC*8)+6, 1); // Box white |
lcd_frect( 0, y-8, 127, 3, 0); // Box clear |
lcd_frect( 0, y-4, 127, h+4, 1); // Box fill white |
lcd_plot ( 0, y-4, 0); // Ecke links oben 1 |
lcd_plot ( 127, y-4, 0); // Ecke rechts oben 1 |
lcd_line ( 1, y-4, 0, y-3, 0); // Ecke links oben 2 |
lcd_line ( 127-1, y-4, 127, y-3, 0); // Ecke rechts oben 2 |
} |
//----------------------------------------------------------- |
// lcdx_cls_rowwidth( y, width, mode, xoffs,yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
// |
// width: in Zeichen |
//----------------------------------------------------------- |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
int8_t xadd = 0; |
int8_t yadd = 0; |
if( mode == MNORMALX || mode == MINVERSX ) |
{ |
if( xoffs > 0 ) xadd = 1; |
if( (y*8)+yoffs > 0 ) yadd = 1; |
if( mode == MNORMALX ) |
mode = MNORMAL; |
else |
mode = MINVERS; |
} |
lcd_frect( xoffs-xadd, (y*8)+yoffs-yadd, (width*6)+xadd, 7+yadd, (mode == MNORMAL ? 0 : 1) ); // Zeile loeschen |
} |
//----------------------------------------------------------- |
// lcdx_cls_row( y, mode, yoffs ) |
// |
// loescht eine Zeile auf dem Display |
// mode ist kompatibel zu MNORMALX, MINVERSX |
//----------------------------------------------------------- |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_rowwidth( y, 21, mode, 0, yoffs ); |
} |
//#################################################################################### |
//#################################################################################### |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_setpos( uint8_t x, uint8_t y ) |
{ |
lcd_xpos = x; |
lcd_ypos = y; |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void lcd_print_char( uint8_t c, uint8_t mode ) |
{ |
switch( c ) |
{ |
case 0x0D: lcd_xpos = 0; |
break; |
case 0x0A: new_line(); |
break; |
default: lcdx_putc (lcd_xpos, lcd_ypos, c, mode, 0,0); |
lcd_xpos++; |
if( lcd_xpos > 20 ) |
{ |
lcd_xpos = 0; |
new_line(); |
} |
break; |
} |
} |
//---------------------------------------------------- |
// gibt einen Linefeed aus (CR+LF) |
//---------------------------------------------------- |
void lcd_print_LF( void ) |
{ |
lcd_print_char( 0x0D, MNORMAL ); |
lcd_print_char( 0x0A, MNORMAL ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd/lcd.h |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* - original LCD control by Thomas "thkais" Kaiser * |
* - special number formating routines taken from C-OSD * |
* from Claas Anders "CaScAdE" Rathje * |
* - some extension, ellipse and circ_line by Peter "woggle" Mack * |
* Thanks to Oliver Schwaneberg for adding several functions to this library!* |
* * |
* Author: Jan Michel (jan at mueschelsoft dot de) * |
* License: GNU General Public License, version 3 * |
* Version: v0.93 September 2010 * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lcd.h |
//# |
//# 27.06.2014 OG |
//# - add: define MINVERSX, MNORMALX |
//# |
//# 11.06.2014 OG |
//# - add: lcd_set_contrast() |
//# |
//# 04.06.2014 OG |
//# - add: lcdx_cls_rowwidth() |
//# |
//# 02.05.2014 OG |
//# - add: Popup_Draw() (ehemals in osd.c) |
//# |
//# 13.04.2014 OG |
//# - add: lcd_print_LF() |
//# |
//# 11.04.2014 OG |
//# - add: lcdx_cls_row() |
//# |
//# 08.04.2014 OG |
//# - add: lcdx_printf_center(), lcdx_printf_center_P() |
//# |
//# 07.04.2014 OG |
//# - add: lcd_setpos(), lcd_print_char() |
//# |
//# 04.04.2014 OG |
//# - add: ShowTitle_P() |
//# |
//# 28.02.2014 OG |
//# - del: show_baudrate() |
//# |
//# 16.02.2014 OG |
//# - add: lcdx_printp_center(), lcdx_print_center() |
//# |
//# 13.02.2014 OG |
//# - add: R# define's fuer lcd_rect_round() |
//# |
//# 12.02.2014 OG |
//# - add: lcd_frect_round() |
//# - add: lcd_rect_round() |
//# |
//# 03.02.2014 OG |
//# - fix: bei writex_ndigit_number_u_100th() fehlte der Parameter 'mode' |
//# |
//# 07.07.2013 OG |
//# - add: SYMBOL_CHECK |
//# |
//# 11.06.2013 OG |
//# - add: SYMBOL_AVG, SYMBOL_MIN, SYMBOL_MAX fuer OSDDATA Anzeige |
//# - del: Antennen-Symbol von OSD_General (wird wieder gezeichnet) |
//# |
//# 15.05.2013 OG |
//# - add: define SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY |
//# |
//# 03.05.2013 OG |
//# - fix: writex_gpspos() - Anzeige negativer Koordinaten |
//# - fix: Anzeigefehler writex_datetime_time() |
//# - chg: writex_datetime_date() & writex_datetime_time() Parameter |
//# 'timeoffset' entfernt da das durch PKT-Config geregelt werden soll |
//# |
//# 28.04.2013 OG |
//# - add: lcdx_printf_at(), lcdx_printf_at_P() |
//# lcd_printf_at(), lcd_printf_at_P() |
//# - del: write_gps_pos() |
//# |
//# 22.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 11.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 07.03.2013 OG |
//# - siehe lcd.c |
//# |
//# 06.03.2013 OG |
//# - add: extended Funktionen lcdx_... / writex... |
//############################################################################ |
#ifndef _LCD_H |
#define _LCD_H |
#include <stdarg.h> // Notwendig! (OG) |
#include "../timer/timer.h" |
#define MNORMAL 0 // Zeichendarstellung: Normal |
#define MINVERS 2 // Zeichendarstellung: Invers |
#define MBIG 3 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Normal |
#define MBIGINVERS 4 // OBSOLETE! - Zeichendarstellung: grosser Zeichensatz (8x8) Invers |
#define MNORMALX 5 // Zeichendarstellung: Normal - oben und Links wird eine 1 Pixel Linie gezogen |
#define MINVERSX 6 // Zeichendarstellung: Invers - oben und Links wird eine 1 Pixel Linie gezogen |
// fuer lcd_rect_round() |
#define R0 0 // Radius 0 |
#define R1 1 // Radius 1 |
#define R2 2 // Radius 2 |
#define SYMBOL_AVG 10 |
#define SYMBOL_MAX 13 |
#define SYMBOL_MIN 16 |
#define SYMBOL_SMALLDEGREE 11 |
#define SYMBOL_BIGDEGREE 30 |
#define SYMBOL_CHECK 31 |
//------------------------------------------------------------------------------------ |
void lcdx_printf_at( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcd_printf_at( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcd_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, const char *format, ... ); |
void lcdx_printf_center( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
void lcdx_printf_center_P( uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ); |
//------------------------------------------------------------------------------------ |
// X-tended |
void lcdx_putc( uint8_t x, uint8_t y, uint8_t c, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcdx_print (uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp (const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_ndigit_number_u_100th( uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_puts_at(uint8_t x, uint8_t y, const char *s, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_time (uint8_t x, uint8_t y, uint16_t seconds, uint8_t mode, int8_t xoffs, int8_t yoffs); |
//void writex_gpspos( uint8_t x, uint8_t y, uint32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_gpspos( uint8_t x, uint8_t y, int32_t GPSpos, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void writex_datetime_time( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void writex_datetime_date( uint8_t x, uint8_t y, PKTdatetime_t datetime, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void Show_PKTError_NoRAM(void); |
void ShowTitle_P( const char *title, uint8_t clearscreen ); |
//------------------------------------------------------------------------------------ |
void lcd_rect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcd_frect_round( uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode, uint8_t r); |
void lcdx_print_center( uint8_t y, uint8_t *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_printp_center( uint8_t y, const char *text, uint8_t mode, int8_t xoffs, int8_t yoffs); |
void lcdx_cls_row( uint8_t y, uint8_t mode, int8_t yoffs ); |
void lcdx_cls_rowwidth( uint8_t y, uint8_t width, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void lcd_setpos( uint8_t x, uint8_t y ); |
void lcd_print_char( uint8_t c, uint8_t mode ); |
void lcd_print_LF( void ); |
void Popup_Draw( uint8_t heigthC ); |
/* |
//----------------------------------------------------------------------------- |
// Command Codes |
//----------------------------------------------------------------------------- |
//1: Display on/off |
#define LCD_DISPLAY_ON 0xAF //switch display on |
#define LCD_DISPLAY_OFF 0xAE //switch display off |
//2: display start line set (lower 6 bits select first line on lcd from 64 lines in memory) |
#define LCD_START_LINE 0x40 |
//3: Page address set (lower 4 bits select one of 8 pages) |
#define LCD_PAGE_ADDRESS 0xB0 |
//4: column address (lower 4 bits are upper / lower nibble of column address) |
#define LCD_COL_ADDRESS_MSB 0x10 |
#define LCD_COL_ADDRESS_LSB 0x00 //second part of column address |
//8: select orientation (black side of the display should be further away from viewer) |
#define LCD_BOTTOMVIEW 0xA1 //6 o'clock view |
#define LCD_TOPVIEW 0xA0 //12 o'clock view |
//9: select normal (white background, black pixels) or reverse (black background, white pixels) mode |
#define LCD_DISPLAY_POSITIVE 0xA6 //not inverted mode |
#define LCD_DISPLAY_INVERTED 0xA7 //inverted display |
//10: show memory content or switch all pixels on |
#define LCD_SHOW_NORMAL 0xA4 //show dram content |
#define LCD_SHOW_ALL_POINTS 0xA5 //show all points |
//11: lcd bias set |
#define LCD_BIAS_1_9 0xA2 |
#define LCD_BIAS_1_7 0xA3 |
//14: Reset Controller |
#define LCD_RESET_CMD 0xE2 |
//15: output mode select (turns display upside-down) |
#define LCD_SCAN_DIR_NORMAL 0xC0 //normal scan direction |
#define LCD_SCAN_DIR_REVERSE 0xC8 //reversed scan direction |
//16: power control set (lower 3 bits select operating mode) |
//Bit 0: Voltage follower on/off - Bit 1: Voltage regulator on/off - Bit 2: Booster circuit on/off |
#define LCD_POWER_CONTROL 0x28 //base command |
#define LCD_POWER_LOW_POWER 0x2F |
#define LCD_POWER_WIDE_RANGE 0x2F |
#define LCD_POWER_LOW_VOLTAGE 0x2B |
//17: voltage regulator resistor ratio set (lower 3 bits select ratio) |
//selects lcd voltage - 000 is low (~ -2V), 111 is high (~ - 10V), also depending on volume mode. Datasheet suggests 011 |
#define LCD_VOLTAGE 0x20 |
//18: Volume mode set (2-byte command, lower 6 bits in second word select value, datasheet suggests 0x1F) |
#define LCD_VOLUME_MODE_1 0x81 |
#define LCD_VOLUME_MODE_2 0x00 |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
//19: static indicator (2-byte command), first on/off, then blinking mode |
#define LCD_INDICATOR_ON 0xAD //static indicator on |
#define LCD_INDICATOR_OFF 0xAC //static indicator off |
#define LCD_INDICATOR_MODE_OFF 0x00 |
#define LCD_INDICATOR_MODE_1HZ 0x01 |
#define LCD_INDICATOR_MODE_2HZ 0x10 |
#define LCD_INDICATOR_MODE_ON 0x11 |
//20: booster ratio set (2-byte command) |
#define LCD_BOOSTER_SET 0xF8 //set booster ratio |
#define LCD_BOOSTER_234 0x00 //2x-4x |
#define LCD_BOOSTER_5 0x01 //5x |
#define LCD_BOOSTER_6 0x03 //6x |
//#endif |
//22: NOP command |
#define LCD_NOP 0xE3 |
//#if DISPLAY_TYPE == 102 |
////25: advanced program control |
//#define LCD_ADV_PROG_CTRL 0xFA |
//#define LCD_ADV_PROG_CTRL2 0x10 |
//#endif |
//----------------------------------------------------------------------------- |
// Makros to execute commands |
//----------------------------------------------------------------------------- |
#define LCD_SWITCH_ON() lcd_command(LCD_DISPLAY_ON) |
#define LCD_SWITCH_OFF() lcd_command(LCD_DISPLAY_OFF) |
#define LCD_SET_FIRST_LINE(i) lcd_command(LCD_START_LINE | ((i) & 0x3F)) |
#define LCD_SET_PAGE_ADDR(i) lcd_command(LCD_PAGE_ADDRESS | ((i) & 0x0F)) |
#define LCD_SET_COLUMN_ADDR(i) lcd_command(LCD_COL_ADDRESS_MSB | ((i>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((i) & 0x0F)) |
#define LCD_GOTO_ADDRESS(page,col); lcd_command(LCD_PAGE_ADDRESS | ((page) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_MSB | ((col>>4) & 0x0F)); \ |
lcd_command(LCD_COL_ADDRESS_LSB | ((col) & 0x0F)); |
#define LCD_SET_BOTTOM_VIEW() lcd_command(LCD_BOTTOMVIEW) |
#define LCD_SET_TOP_VIEW() lcd_command(LCD_TOPVIEW) |
#define LCD_SET_MODE_POSITIVE() lcd_command(LCD_DISPLAY_POSITIVE) |
#define LCD_SET_MODE_INVERTED() lcd_command(LCD_DISPLAY_INVERTED) |
#define LCD_SHOW_ALL_PIXELS_ON() lcd_command(LCD_SHOW_ALL_POINTS) |
#define LCD_SHOW_ALL_PIXELS_OFF() lcd_command(LCD_SHOW_NORMAL) |
#define LCD_SET_BIAS_RATIO_1_7() lcd_command(LCD_BIAS_1_7) |
#define LCD_SET_BIAS_RATIO_1_9() lcd_command(LCD_BIAS_1_9) |
#define LCD_SEND_RESET() lcd_command(LCD_RESET_CMD) |
#define LCD_ORIENTATION_NORMAL() lcd_command(LCD_SCAN_DIR_NORMAL) |
#define LCD_ORIENTATION_UPSIDEDOWN() lcd_command(LCD_SCAN_DIR_REVERSE) |
#define LCD_SET_POWER_CONTROL(i) lcd_command(LCD_POWER_CONTROL | ((i) & 0x07)) |
#define LCD_SET_LOW_POWER() lcd_command(LCD_POWER_LOW_POWER) |
#define LCD_SET_WIDE_RANGE() lcd_command(LCD_POWER_WIDE_RANGE) |
#define LCD_SET_LOW_VOLTAGE() lcd_command(LCD_POWER_LOW_VOLTAGE) |
#define LCD_SET_BIAS_VOLTAGE(i) lcd_command(LCD_VOLTAGE | ((i) & 0x07)) |
#define LCD_SET_VOLUME_MODE(i) lcd_command(LCD_VOLUME_MODE_1); \ |
lcd_command(LCD_VOLUME_MODE_2 | ((i) & 0x3F)) |
//#if DISPLAY_TYPE == 128 || DISPLAY_TYPE == 132 |
#define LCD_SET_INDICATOR_OFF() lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_INDICATOR_MODE_OFF) |
#define LCD_SET_INDICATOR_STATIC() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_ON) |
#define LCD_SET_INDICATOR_1HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_1HZ) |
#define LCD_SET_INDICATOR_2HZ() lcd_command(LCD_INDICATOR_ON); \ |
lcd_command(LCD_INDICATOR_MODE_2HZ) |
#define LCD_SET_INDICATOR(i,j) lcd_command(LCD_INDICATOR_OFF | ((i) & 1)); \ |
lcd_command(((j) & 2)) |
#define LCD_SLEEP_MODE lcd_command(LCD_INDICATOR_OFF); \ |
lcd_command(LCD_DISPLAY_OFF); \ |
lcd_command(LCD_SHOW_ALL_POINTS) |
//#endif |
//#if DISPLAY_TYPE == 102 |
//#define LCD_TEMPCOMP_HIGH 0x80 |
//#define LCD_COLWRAP 0x02 |
//#define LCD_PAGEWRAP 0x01 |
//#define LCD_SET_ADV_PROG_CTRL(i) lcd_command(LCD_ADV_PROG_CTRL); |
// lcd_command(LCD_ADV_PROG_CTRL2 & i) |
//#endif |
*/ |
extern volatile uint8_t LCD_ORIENTATION; |
//#define LCD_LINES 8 |
//#define LCD_COLS 21 |
extern uint8_t lcd_xpos; |
extern uint8_t lcd_ypos; |
void lcd_set_contrast( uint8_t value ); |
void lcd_command(uint8_t cmd); |
void send_byte (uint8_t data); |
void LCD_Init (uint8_t LCD_Mode); |
void new_line (void); |
void lcd_puts_at(uint8_t x, uint8_t y,const char *s, uint8_t mode ); |
void lcd_putc (uint8_t x, uint8_t y, uint8_t c, uint8_t mode); |
void send_byte (uint8_t data); |
void lcd_print (uint8_t *text, uint8_t mode); |
void lcd_print_at (uint8_t x, uint8_t y, uint8_t *text, uint8_t mode); |
void lcd_printp (const char *text, uint8_t mode); |
void lcd_printp_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_printpns (const char *text, uint8_t mode); |
void lcd_printpns_at (uint8_t x, uint8_t y, const char *text, uint8_t mode); |
void lcd_cls (void); |
void lcd_cls_line (uint8_t x, uint8_t y, uint8_t w); |
void print_display (uint8_t *text); |
void print_display_at (uint8_t x, uint8_t y, uint8_t *text); |
void copy_line (uint8_t y); |
void paste_line (uint8_t y); |
void lcd_plot (uint8_t x, uint8_t y, uint8_t mode); |
void lcd_line (unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, uint8_t mode); |
void lcd_rect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_frect (uint8_t x1, uint8_t y1, uint8_t widthx, uint8_t widthy, uint8_t mode); |
void lcd_circle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_fcircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_circ_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_ellipse (int16_t x0, int16_t y0, int16_t rx, int16_t ry, uint8_t mode); |
void lcd_ellipse_line (uint8_t x, uint8_t y, uint8_t rx, uint8_t ry, uint16_t deg, uint8_t mode); |
void lcd_ecircle (int16_t x0, int16_t y0, int16_t radius, uint8_t mode); |
void lcd_ecirc_line (uint8_t x, uint8_t y, uint8_t r, uint16_t deg, uint8_t mode); |
void lcd_view_font (uint8_t page); |
void lcd_print_hex_at (uint8_t x, uint8_t y, uint8_t h, uint8_t mode); |
void lcd_write_number_u (uint8_t number); |
void lcd_write_number_u_at (uint8_t x, uint8_t y, uint8_t number); |
void lcd_print_hex (uint8_t h, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_u (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad,uint8_t mode); |
/** |
* Write only some digits of a signed <number> at <x>/<y> |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 007 instead of 7 |
*/ |
void write_ndigit_number_s (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_10th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* Write only some digits of a unsigned <number> at <x>/<y> as /100th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_u_100th (uint8_t x, uint8_t y, uint16_t number, int16_t length, uint8_t pad); |
/** |
* Write only some digits of a signed <number> at <x>/<y> as /10th of the value |
* <length> represents the length to rightbound the number |
* <pad> = 1 will cause blank spaced to be filled up with zeros e.g. 00.7 instead of .7 |
*/ |
void write_ndigit_number_s_10th (uint8_t x, uint8_t y, int16_t number, int16_t length, uint8_t pad, uint8_t mode); |
/** |
* write <seconds> as human readable time at <x>/<y> |
*/ |
void write_time (uint8_t x, uint8_t y, uint16_t seconds); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lcd |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/trunk/lipo/lipo.c |
---|
0,0 → 1,178 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY lipo.c |
//# |
//# 15.06.2014 OG |
//# - fix; show_Lipo() - wenn die Spannung unter 3.20 Volt ging, wurde wieder |
//# ein voller Akkubalken angezeigt aufgrund einer Subtraktion - fixed |
//# |
//# 21.02.2014 OG |
//# - chg: show_Lipo() neu geschrieben - etwas smarter, ein paar Bytes kleiner |
//# |
//# 20.02.2014 OG |
//# - chg: Codeformattierungen |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "lipo.h" |
#include "../eeprom/eeprom.h" |
// Global variables |
double accumulator = 0; //!< Accumulated 10-bit samples |
double Vin = 0; //!< 16-bit float number result |
short temp = 0; //!< Temporary variable |
short samples = 0; //!< Number of conversions |
uint16_t volt_avg = 0; |
//-------------------------------------------------------------- |
//! ADC interrupt routine |
//-------------------------------------------------------------- |
ISR (ADC_vect) |
{ |
accumulator += ADCW; |
samples++; |
if( samples>4095 ) |
{ |
oversampled(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ADC_Init (void) |
{ |
DDRA &= ~(1<<DDA1); // MartinR |
PORTA &= ~(1<<PORTA1); // MartinR |
ADMUX = (0<<REFS1) | (1<<REFS0); // externe 5V Referenzspannung nutzen |
ADMUX = (ADMUX & ~(0x1F)) | (1 & 0x1F); // ADC1 verwenden |
ADCSRA = (1<<ADEN)|(1<<ADIE)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // Prescaler 128, Freilaufend, Interrupte frei |
} |
//-------------------------------------------------------------- |
// Error compensation, Scaling 16-bit result, Rounding up, |
// Calculate 16-bit result, Resets variables |
// |
// Quelle AVR121: Enhancing ADC resolution by versampling |
//-------------------------------------------------------------- |
void oversampled(void) |
{ |
cli(); |
accumulator += Config.Lipo_UOffset; //5150 Offset error compensation |
// accumulator *= 0.9993; // Gain error compensation |
accumulator *= 0.9600; //0.9800 Gain error compensation |
temp=(int)accumulator%64; |
accumulator/=64; // Scaling the answer |
if(temp>=32) |
{ |
accumulator += 1; // Round up |
} |
// Vin = (accumulator/65536)*4.910; // Calculating 16-bit result |
Vin =accumulator/7.5; |
volt_avg = Vin; |
// write_ndigit_number_u(0, 3, Vin, 5, 0,0); |
// write_ndigit_number_u(0, 4, volt_avg, 5, 0,0); |
samples = 0; |
accumulator = 0; |
sei(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void show_Lipo( void ) |
{ |
int16_t w = 0; |
//-------------------------------------------- |
// Batterie Symbol zeichnen |
//-------------------------------------------- |
lcd_rect(104, 0, 23, 7, 1); // Rahmen aussen |
lcd_rect(105, 1, 21, 5, 0); // Rahmen innen |
lcd_frect(102, 0, 1, 7, 0); // ganz links etwas loeschen |
lcd_rect(103,2,1,3,1); // Batterie "Kappe" |
//-------------------------------------------- |
// Batterie Balken berechnen / zeichnen |
//-------------------------------------------- |
// |
// Config.PKT_Accutyp |
// true = LiPO Akku mit max. 4.2 Volt (420) |
// false = LiON Akku mit max. 4.1 Volt (410) |
if( volt_avg > 320 ) |
{ |
w = (volt_avg-320)/(Config.PKT_Accutyp ? 4.8 : 4.5); // die Faktoren 4.8 (kann bis 5.0) und 4.5 fuer LiPo / LiON wurden per Test ermittelt |
//lcdx_printf_at_P( 12, 1, MNORMAL, 0,0, PSTR("v=%3d"), w ); // Debug Anzeige um Faktoren zu ermitteln |
} |
if(w<0) w = 0; |
if(w>20) w = 20; // nicht mehr als 20 Pixel Breite |
if( w>0 ) |
lcd_frect( 106+(20-w), 2, w-1, 3, 1); // Batterie Balken: fill |
if( w<20 ) |
lcd_frect( 106, 2, 19-w, 3, 0); // Batterie Balken: clear |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/lipo/lipo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _LIPO_H |
#define _LIPO_H |
short samples; //!< Number of conversions |
double Vin; |
double accumulator; |
uint16_t volt_avg; |
void ADC_Init (void); |
void oversampled(void); |
void show_Lipo(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/main.c |
---|
0,0 → 1,202 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mk/mksettings.h" geaendert auf "mksettings/mksettings.h" |
//# |
//# 29.03.2014 OG |
//# - chg: MK_Show_LastGPS_Position() ersetzt durch OSDDATA_ShowLastGPSPosition() |
//# |
//# 28.03.2014 OG |
//# - chg: mk_last_position() ersetzt durch MK_Show_LastGPS_Position() (mkbase.c) |
//# - del: mk_last_position() |
//# |
//# 21.02.2014 OG |
//# - chg: mk_show_lastposition() auf PKT_TitlePKTVersion() angepasst |
//# |
//# 19.02.2014 OG |
//# - chg: main() hinzugefuegt: MKSettings_TMP_Init0() |
//# - add: #include "mk/mksettings.h" |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 11.02.2014 OG |
//# - chg: main() Aufruf von searchMK() bzgl. Parametern angepasst |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# |
//# 02.02.2014 OG |
//# - chg: Screen-Layout von der Anzeige der letzten MK-Position und |
//# Ausgliederung davon in mk_show_lastposition() |
//# |
//# 29.01.2014 OG |
//# - del: #include "display.h" |
//# - chg: verschoben: MK_load_setting() und searchMK() nach mk/mkbase.c |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: searchMK(): kuerzerer Fehlerbeep bei falscher FC-Revision und |
//# automatisches beenden des Fehlerscreens nach 4 Sekunden |
//# |
//# 24.01.2014 OG |
//# - fix: searchMK(): wenn MK_load_setting() keine Daten liefert wird keine |
//# falsche RC-Settings-Revisionsnummer mehr angezeigt sondern "ERR!" |
//# - chg: searchMK(): Bestaetigung Fehleranzeige bei falscher FC-Revision |
//# geaendert von KEY_ESC nach KEY_ENTER - Grund: das PKT behandelt |
//# dieses Problem nun flexibler und z.B. OSD (und anderes) geht noch |
//# - chg: searchMK(): beschleunigte Erkennung inkompatibler FC-Settings-Revisionen |
//# - chg: timings in MK_load_setting() |
//# |
//# 07.1.2016 Cebra |
//# - chg: setzen globale Variable für Abfrage WrongFCVersion etwas verändert |
//# |
//# 26.06.2013 OG |
//# - del: searchMK() Layout und Code-Struktur |
//# - fix: searchMK() laden der MK-Settings |
//# - add: MK_load_setting() - wird ggf. spaeter zu einer bessern Stelle verschoben |
//# - del: Code zu USE_PKT_STARTINFO |
//# |
//# 20.05.2013 OG |
//# - chg: searchMK() Code fuer KEY_ESC geaendert |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Aufruf von main_menu() geaendert zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 16.05.2013 Cebra |
//# - chg: Startinfo wird nur noch komplett angezeigt wenn im Setup auf JA |
//# |
//# 15.05.2013 OG |
//# - chg: define USE_PKT_STARTINFO ergaenzt (siehe main.h) |
//# |
//# 07.05.2013 Cebra |
//# - chg: wenn NC vorhanden wird die Version der NC in NCversion gespeichert |
//# sonst die FC-Version in FCversion |
//# |
//# 27.04.2013 OG |
//# - chg: GPS-Positionsausgabe umgestellt auf writex_gpspos() |
//# - chg: einige clear-lines auf lcd_frect() umgestellt um Speicher zu sparen |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
//************************************************************************************ |
// Watchdog integrieren und abschalten, wird für Bootloader benötigt |
// !!muss hier stehen bleiben!! |
//-------------------------------------------------------------- |
void wdt_init(void) __attribute__((naked)) __attribute__((section(".init1"))); |
//-------------------------------------------------------------- |
void wdt_init(void) |
{ |
MCUSR = 0; |
wdt_disable(); |
return; |
} |
//************************************************************************************ |
// erst ab hier weitere #includes |
#include "main.h" |
#include "lcd/lcd.h" |
#include "uart/usart.h" |
#include "mk-data-structs.h" |
#include "menu.h" |
#include "timer/timer.h" |
#include "eeprom/eeprom.h" |
#include "wi232/Wi232.h" |
#include "motortest/twimaster.h" |
#include "messages.h" |
#include "utils/scrollbox.h" |
#include "pkt/pkt.h" |
#include "lipo/lipo.h" |
#include "mk/mkbase.h" |
#include "mksettings/mksettings.h" |
#include "osd/osddata.h" |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
volatile uint8_t mode = 0; |
uint8_t hardware = 0; |
uint8_t current_hardware = 0; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int main(void) |
{ |
InitHWPorts(); // Hardwareanhängige Ports konfigurieren |
// dafür wird je nach Hardware die HAL_HWxxx verwendet |
// Define dazu in der main.h |
hardware = NO; |
current_hardware = 0; |
MKSettings_TMP_Init0(); // TMP-Setting initialisieren |
if( Config.LastLongitude>0x00000000 && Config.LastLatitude>0x00000000 ) |
{ |
OSDDATA_ShowLastGPSPosition(); // letzte MK GPS-Position zeigen |
} |
searchMK(true); // MK suchen (true = MK_Info() anzeigen) |
while(true) |
{ |
Menu_Main(); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/main.h |
---|
0,0 → 1,444 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY main.h |
//# |
//# 20.09.2015 Starter |
//# - FollowMeStep2 wird jetzt nurnoch von USE_FOLLOWME_STEP2 aktiviert |
//# |
//# 19.09.2015 Cebra |
//# - add: Ordner 10DOF hinzugefügt für die Bearbeitung des GY-87 Sensorboard |
//# Kompass, Gyro, ACC, Luftdruck |
//# |
//# 03.08.2015 Cebra |
//# - add: Define für GPS Debug hinzugefügt für GPS Berechnung FollowMe |
//# - add: #define USE_WAYPOINTS , Code sparen |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85e wegen einigen Änderungen in GPS-Maus Bereich, CRC Erkennung und Weitergabe |
//# |
//# |
//# 30.07.2015 Cebra |
//# - chg: neue Version 3.85d wegen Änderungen in gpsmouse.c |
//# |
//# |
//# 16.07.2015 Cebra |
//# - chg: neue Version 3.85a wegen FC2.11a |
//# |
//# 09.04.2015 Cebra |
//# - chg: neue Version 3.84a wegen FC2.09j |
//# |
//# 05.04.2015 Cebra (PKT383b) |
//# - chg: Änderungen wegen Fehlfunktion mit NC 2.09h |
//# SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) |
//# ergänzt um 2. Parameter |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - chg: Anpassung an FC209d |
//# |
//# 27.01.2015 Cebra (PKT382a) |
//# - chg: Anpassung an FC209a |
//# |
//# 26.09.2014 Cebra (PKT381b) |
//# |
//# |
//# 26.09.2014 Cebra (PKT381a) |
//# |
//# 01.07.2014 OG (PKT380e) |
//# |
//# 28.06.2014 OG (PKT380dX4) |
//# |
//# 27.06.2014 OG (PKT380dX3) |
//# - chg: USE_FOLLOWME eingeschaltet |
//# - chg: USE_TRACKING abgeschaltet |
//# |
//# 22.06.2014 OG (PKT380dX3) |
//# |
//# 20.06.2014 OG (PKT380dX2) |
//# |
//# 18.06.2014 OG (PKT380dX1) |
//# |
//# 18.06.2014 OG (PKT380d) |
//# |
//# 05.06.2014 OG (PKT380cX5) |
//# |
//# 02.06.2014 OG (PKT380cX4) |
//# Update auf Windows Atmel AVR Studio 6.2.1153 (Release) |
//# (von ehemals Atmel AVR Studio 6.2 Beta) |
//# |
//# 27.05.2014 OG (PKT380cX3) |
//# |
//# 20.05.2014 OG (PKT380cX2) |
//# |
//# 19.05.2014 OG (PKT380cX1) |
//# - add: USE_OSD_SCREEN_WAYPOINTS (keine Freigabe! In Entwicklung!) |
//# |
//# 18.05.2014 OG (PKT380cX1) |
//# |
//# 15.05.2014 OG (PKT380c) |
//# |
//# 15.05.2014 OG (PKT380bX7) |
//# - chg: USE_OSD_SCREEN_3DLAGE eingeschaltet (war ungewollt abgeschaltet) |
//# |
//# 14.05.2014 OG (PKT380bX7) |
//# |
//# 13.05.2014 OG (PKT380bX6) |
//# - add: USE_MAINMENU_SEPARATOR (aus menu.h hierhin verschoben) |
//# - del: MKINFO_AUTO_REFRESH |
//# |
//# 12.05.2014 OG (PKT380bX5b) |
//# - chg: keine Code-Changes - dafuer Compiler jetzt Atmel Studio 6.2 Beta |
//# (statt bisher Atmel Studio 5.1) |
//# |
//# 07.05.2014 OG (PKT380bX4) |
//# |
//# 07.05.2014 OG (PKT380bX3) |
//# - fix: EEpromversion erhoeht auf 138 (siehe eeprom.c) |
//# |
//# 02.05.2014 OG (PKT380bX2) |
//# |
//# 28.04.2014 OG |
//# - chg: USE_OSD_SCREEN_OLD abgeschaltet |
//# |
//# 20.04.2014 OG (PKT380bX1) |
//# |
//# 18.04.2014 OG (PKT380b) |
//# |
//# 17.04.2014 OG (PKT380aX1) |
//# - chg: PKT Versionsanzeige geaendert auf "3.80a" statt wie frueher "3.8.0a" |
//# |
//# 16.04.2014 OG (PKT380a) |
//# |
//# 14.04.2014 OG (PKT374aX8) |
//# |
//# 02.04.2014 OG |
//# - chg: define fuer Baud_2400 von 0 auf 7 geaendert |
//# (0/false wird benoetigt um zu erkennen ob keine Baud-Rate erkannt wurde) |
//# |
//# 01.04.2014 OG |
//# - add: USE_SV2MODULE_BLE (Bluetooth 4 Low Energy Modul an SV2) |
//# |
//# 31.03.2014 OG (PKT374aX7) |
//# |
//# 30.03.2014 OG (PKT374aX6) |
//# Sprachunterstuetzung fuer Hollaendisch vollstaendig entfernt |
//# |
//# 30.03.2014 OG (PKT374aX5) |
//# Source-Schnipp PKT374aX5 |
//# |
//# 01.03.2014 OG (PKT374aX5) |
//# |
//# 26.02.2014 OG (PKT374aX4) |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 22.02.2014 OG (PKT374aX4) |
//# |
//# 17.02.2014 OG (PKT374aX3) |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 14.02.2014 OG (PKT374aX2) |
//# - add: MKINFO_AUTO_REFRESH |
//# - del: veraltete PKT defines HWVERSION... entfernt |
//# |
//# 12.02.2014 OG (PKT374aX2) |
//# - del: defines zu MKVERSIONnnn entfernt |
//# |
//# 10.02.2014 OG (PKT374aX1) |
//# - add: ... |
//# |
//# 05.02.2014 OG (PKT374aX1) |
//# - add: DEBUG_NEW_PARAMETERS - nur fuer Entwicklung! |
//# |
//# 30.01.2014 OG (PKT373cX4) |
//# - add: USE_BLUETOOTH |
//# |
//# 29.01.2014 OG (3.7.3cX3) |
//# - del: extern uint8_t searchMK(void) ist jetzt in mk/mkbase.c |
//# |
//# 24.01.2014 OG (3.7.3cX1) |
//# - add: DEBUG_FC_COMMUNICATION (Auswirkung in usart.c) |
//# |
//# 06.01.2014 CB |
//# - add: Abschaltung MK Parameter aendern bei nicht passender FC Software |
//# |
//# 02.12.2013 CB |
//# - add: Anpassung Parameter an FC 2.01f |
//# |
//# 21.10.2013 CB |
//# - add: Anpassung Parameter an FC 2.01a |
//# |
//# 16.07.2013 CB |
//# - chg: neuen Versionsnummer wegen Wiflypatch |
//# |
//# 07.07.2013 OG |
//# - add: ABO_TIMEOUT und von 3 sec auf 2 sec gesetzt (war vorher in timer.h) |
//# - del: define USE_OSD_SCREEN_ELECTRIC_N |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# - add: DEBUG_SV2_EXTENSION um Debug-Ausgaben fuer den SV2-Patch an-/auszuschalten |
//# |
//# 05.07.2013 CB |
//# - add: USE_WLAN |
//# |
//# 30.06.2013 OG |
//# - add: USE_OSD_PKTHOOK |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO (wird nicht mehr benoetigt) |
//# |
//# 24.06.2013 OG |
//# - add: USE_PKTTOOLS_SV2 - zeigt SV2-Verbindungsmenuepunkte in PKT-Tools an |
//# bzw. nicht an. Am PKT-SV2 liegen +5 Volt für die FC/NC an. |
//# Anmerkung OG: bei meinen Kompilaten wird das erstmal ausgeschaltet |
//# sein bis 100% klar ist ob das keine Probleme macht |
//# wenn der Kopter mit FC/NC nicht bereits anderweitig |
//# (Netzteil/Lipo) mit Strom versorgt wird. |
//# |
//# 22.06.2013 Cebra |
//# - chg: Fehler bei der Lipooffset Einstellung beseitigt, neue Version 3.7.0c |
//# |
//# 20.06.2013 Cebra |
//# - chg: falsche Versionsnummer korrigiert, neu jetzt 3.7.0b |
//# |
//# 13.06.2013 OG |
//# - del: USE_PCFASTCONNECT (nicht mehr benoetigt) |
//# |
//# 12.06.2013 Cebra |
//# - chg: Versionswechsel auf 3.7.0a, Zielrelease für FC-Software 0.91 (bzw.1.00??) |
//# |
//# 10.06.2013 Cebra |
//# - add: #define MKVERSION091f für FC-Version 0.91f |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a für FC-Version 0.91a |
//# |
//# 19.05.2013 OG |
//# - del: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# -> verschoben nach pkt/pkt.c |
//# - chg: Kommentare zum Module-Support |
//# |
//# 19.05.2013 OG |
//# - add: USE_MODULEINFO |
//# - add: PKT_Update(), PKT_CheckUpdate(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: USE_MKGPSINFO |
//# |
//# 15.05.2013 OG |
//# - add: define USE_PKT_STARTINFO |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 13.05.2013 Cebra |
//# - add: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//# 05.05.2013 Cebra |
//# - chg: #define USE_FOLLOWME |
//# |
//# 03.05.2013 OG |
//# - del: USE_XPRINTF_LONG - spart keinen Platz mehr und ist nun dauerhaft notwendig |
//# |
//# 28.04.2013 OG |
//# - add: define USE_PCFASTCONNECT |
//# - add: define USE_FONT_BIG |
//# - add: define USE_XPRINTF_LONG |
//# - chg: Anordnung der Feature- & Debug-defines um einfacher via Copy&Paste |
//# die Einstellungen notieren bzw. posten zu koennen |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: USE_OSD_... defines |
//############################################################################ |
#ifndef _MAIN_H |
#define _MAIN_H |
// Softwareversion des PKT |
#define PKTSWVersion "3.85_f" // PKT Version |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
#define USE_MAINMENU_SEPARATOR // Menuetrennlinien im PKT-Hauptmenues anzeigen |
//######################################################################### |
//# MODULE SUPPORT |
//# ein-/ausgeschalten von Modulen des PKT um ggf. Speicherplatz zu sparen |
//# |
//# Hinweis: |
//# wenn neue USE_* hinzugefuegt werden bitte die in PKT_Info()(pkt.c) |
//# ergaenzen fuer eine aktualisierte Modul-Info |
//######################################################################### |
//--------------------------------------------- |
//- Module: Hauptfunktionen und allgemeine |
//--------------------------------------------- |
#define USE_OSDDATA // zeigt die OSD-Statistikdaten an (ca. 3.5 KByte) |
#define USE_MKSETTINGS // lesen, aktivieren und bearbeiten der MK-Settings 1-5 (ca. ?? KByte) |
#define USE_MKDEBUGDATA // Anzeige MK-Debug-Data (ca. 1.7 KByte) |
#define USE_MKDISPLAY // Anzeige MK-Display (ca. 1 KByte) |
#define USE_MKGPSINFO // Anzeige MK-GPS-Daten (ca. 2 KByte) |
//#define USE_SOUND // PKT Sounderweiterung, benoetigt entsprechende Hardware (ca. 2.5 Kbyte) |
#define USE_MODULEINFO // Anzeige installierter Module in PKT_Info() (ca. 1.8 KByte) |
#define USE_BLUETOOTH // Bluetooth Unterstuetzung des PKT (ca. 13 KByte) |
// Achtung! Das schaltet USE_TRACKING, USE_FOLLOWME aus! |
//#define USE_WAYPOINTS // Waypoint mit dem PKT |
//Entwicklung: |
//#define USE_ACCCALIBRATION // schaltet ACC Calibration im Menü ein/aus |
//#define USE_KOMPASS // Tilt kompensierter Kompass für FollowMe Funktionen |
//--------------------------------------------- |
//- Module: OSD-Screens |
//--------------------------------------------- |
//#define USE_OSD_SCREEN_OLD // OSD Screens OSD0, OSD1, OSD2 (ca. 6.6 KByte) |
#define USE_OSD_SCREEN_NAVIGATION // OSD-Screen: Navigation |
#define USE_OSD_SCREEN_WAYPOINTS // OSD-Screen: Waypoints |
#define USE_OSD_SCREEN_ELECTRIC // OSD-Screen: Electric |
#define USE_OSD_SCREEN_MKSTATUS // OSD-Screen: MK-Status |
#define USE_OSD_SCREEN_USERGPS // OSD-Screen: UserGPS |
#define USE_OSD_SCREEN_3DLAGE // OSD-Screen: 3D-Lage (ca. 900 Bytes) |
#define USE_OSD_SCREEN_STATISTIC // OSD-Screen: Statistic |
//--------------------------------------------- |
//- Module fuer spezielle Benutzergruppen |
//--------------------------------------------- |
#define USE_FOLLOWME // FollowMe Funktionen (ca. 3 Kbyte) |
#define USE_FOLLOWME_STEP2 // FollowMe Funktionen Abstand und Winkel einstellbar |
//#define USE_JOYSTICK // Joystick Support, benoetigt spezielle Hardware (ca. 4.1 KByte) |
#define USE_WLAN // WLAN WiFly Modul an SV2 (ca. 6 KByte)) |
#define USE_SV2MODULE_BLE // Bluetooth 4 Low Energy Modul - externes Modul an SV2 (RedBearLab BLE Mini) (ca. 200 Bytes) |
//--------------------------------------------- |
//- Module mit geringer Relevanz |
//--------------------------------------------- |
//#define USE_I2CMOTORTEST // I2C Funktionen für Motortest (ca. 3.8 KByte) |
//#define USE_FONT_BIG // grosser 8x8 Font (ca. 2.2 KByte, bei Verwendung von USE_OSD_SCREEN_OLD ca. 2.4 KByte) |
//--------------------------------------------- |
//- Module unfertig bzw. noch in der Entwicklung |
//--------------------------------------------- |
//#define USE_TRACKING // Antennentracking, benoetigt spezielle Hardware (ca. 6 KByte) |
//--------------------------------------------- |
//- zusaetzliche Optionen |
//--------------------------------------------- |
//#define USE_OSD_PKTHOOK |
#define USE_PKTTOOLS_SV2 // im Menue von PKT-Tools erscheinen Punkte zur Verbindung zum MK via |
// 10-Pol-Kabel am PKT-SV2. Der PKT-SV2-Anschluss liefert zur FC/NC +5 Volt! |
//######################################################################### |
//# Debug Module & Einstellungen |
//# Nur fuer die Entwicklung - fuer Veroeffentlichung alles abschalten! |
//######################################################################### |
//#define IgnoreFCVersion // keine FC-Revisions Pruefung |
//#define DEBUG_SV2_EXTENSION // ... |
//#define USE_OSD_DEMO // zeigt Demo-Daten in den OSD-Screens (sofern vom Screen unterstuetzt) (fuer Fotos) (nicht fuer die Oeffentlichkeit) |
//#define USE_OSD_SCREEN_DEBUG // zusaetzliche Debug-Screen's aktivieren (ca. 1 KByte) (nicht fuer die Oeffentlichkeit) |
//#define DEBUG // ?? Funktion unbekannt! |
//#define DEBUG_FC_COMMUNICATION // in usart.c Debugausgaben auf dem PKT bzgl. Datenempfang vom Kopter |
//#define DEBUG_PARAMSET // nur fuer Entwicklung! Fuer Release ABSCHALTEN! |
//#define DEBUG_GPS // Entwicklung der GPS Berechnung für FollowMe (wird im Moment nicth genutzt) |
//######################################################################### |
//# Einstellungen |
//######################################################################### |
//#define ABO_TIMEOUT 300 // 3 sec |
#define ABO_TIMEOUT 200 // 2 sec |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ ggf. abhaengige USE-defines deaktivieren |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef USE_BLUETOOTH |
#undef USE_TRACKING |
#undef USE_FOLLOWME |
#endif |
// Fusebits fü½r Hardware 1.2 D7 DC FC |
// Fusebits für Hardware 1.3 D7 DC FC |
// Fusebits für Hardware 3.x D7 DC FC |
// avrdude -pm1284p -cavr109 -P/dev/ttyUSB1 -b115200 -V -Uflash:w:Dateiname.hex:a |
// hier die entsprechende Hardwareversion der Leiterplatte einstellen |
#define HWVERSION3_9 // Hardware Cebra Oktober 2011 ATmega1284P |
#ifdef HWVERSION3_9 |
#include "HAL_HW3_9.h" |
#endif |
#define NO 0 |
#define NC 1 |
#define FC 2 |
#define MK3MAG 3 |
#define MKGPS 4 |
#define Wi232 5 |
// CB #define ENABLE_PWM |
// Baud Rate |
#define Baud_9600 1 |
#define Baud_19200 2 |
#define Baud_38400 3 |
#define Baud_57600 4 |
#define Baud_115200 5 |
#define Baud_4800 6 |
#define Baud_2400 7 |
//---------------------------------------------------------------------------- |
// Anmerkung: 29.01.2014 OG |
// - muss spaeter an geeignete Stelle verschoben werden (ggf. mkbase.c/usart.c) |
//---------------------------------------------------------------------------- |
extern volatile uint8_t mode; |
extern uint8_t hardware; |
extern uint8_t current_hardware; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/menu.c |
---|
0,0 → 1,848 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.c |
//# |
//# 03.08.2015 Cebra |
//# - add: Menü _DefMenu_Main_NO erweitert für GPS Test , mit #define schaltbar |
//# |
//# 09.04.2015 Cebra |
//# - add: Vorbereitung für ACC Kalibrartion aus dem Menü |
//# #ifdef USE_ACCCALIBRATION |
//# |
//# 27.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Tracking etwas nach oben verschoben |
//# |
//# 26.06.2014 OG |
//# - chg: Menu_PKTSpezial() - ergaenzt um Setup_FollowMe() |
//# |
//# 18.06.2014 OG |
//# - chg: Menu_PKTSpezial() umorganisiert |
//# |
//# 14.06.2014 OG |
//# - chg: _DefMenu_Main_NO() - "PKT Spezial" wird nicht mehr angezeigt wenn |
//# kein Kopter gefunden wurde; aktuell sind dort nur Funktionen |
//# enthalten die einen Kopter benoetigen - wenn sich das mal irgendwann |
//# aendert muss man das anpassen |
//# |
//# 01.06.2014 OG |
//# - chg: Menu_PKTSpezial() - Aufruf von Tracking und Followme ergaenzt |
//# um Abfrage MKVersion.isNC |
//# - chg: _ConfigMenu_Main() umgestellt auf MKVersion.isNC/isFC |
//# |
//# 14.05.2014 OG |
//# - chg: Aufruf von gps() geaendert zu MK_Gps_Info() |
//# - chg: include "gps/gps.h" geaendert zu "mk/mkgpsinfo.h" |
//# |
//# 13.05.2014 OG |
//# - del: USE_MAINMENU_SEPARATOR (verschoben nach main.h) |
//# - chg: Menu_Main() - del unused variable 'event' |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_PKTSpezial() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 08.04.2014 OG |
//# - chg: die Menuetrennlinien werden jetzt via define USE_MAINMENU_SEPARATOR |
//# angezeigt (das define ist hier im Code) und sind erstmal wieder |
//# abgeschaltet |
//# |
//# 08.04.2014 OG |
//# - add: _DefMenu_Main_NC() - Menuetrennlinien hinzugefuegt |
//# - add: _DefMenu_Main_FC(), _DefMenu_Main_NO() - Menuetrennlinien hinzugefuegt |
//# |
//# 01.04.2014 OG |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# - add: PKT-Connect(): "BLE Extender" (ext. Bluetooth 4 Low Energy Modul an SV2) |
//# - fix: Menu_PKTConnect(): "WLAN Extender" nur sichtar wenn Config.UseWL gesetzt ist |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 24.03.2014 OG |
//# - chg: "MK Info" vor "MK Settings" verschoben |
//# |
//# 28.02.2014 OG |
//# - chg: ID_MKSETTINGS in den Menues fuer NC/FC weiter nach oben geschoben |
//# |
//# 27.02.2014 OG |
//# - chg: Menuetexte fuer Menu_PKTConnect() |
//# |
//# 26.02.2014 OG |
//# - chg: DEBUG_NEW_PARAMETERS umbenannt zu DEBUG_PARAMSET |
//# |
//# 18.02.2014 OG |
//# - chg: Aufruf von MK_Parameters() ist jetzt MK_Settings() und geht auf |
//# den Source mk/mksettings.c |
//# |
//# 17.02.2014 OG |
//# - chg: aus "MK Parameters" ist "MK Settings" geworden! Entsprechende |
//# defines zu ID's, USE und Sprachen haben sich geaendert |
//# - chg: umbenannt: USE_MKPARAMETER -> USE_MKSETTINGS |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# - chg: auf mk/mkparameters.c/h umgestellt -> MK_Parameters() |
//# - chg: auf 'MKVersion.paramsetOK' umgestellt |
//# |
//# 05.02.2014 OG |
//# - add: DEBUG_NEW_PARAMETERS in Hauptmenues fuer Entwicklung |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.01.2014 OG |
//# - chg: Umstrukturierung vom Hauptmenue |
//# chg: "PKT Tools" -> "PKT Connect" (nur noch Connect-Funktionen drin) |
//# add: "PKT Spezial" -> MK-Motortest und Spezial-Funktionen |
//# wie Joystick, Tracking, FollowMe |
//# - chg: call: display_debug() -> MK_DebugData() |
//# - chg: call: display_data() -> MK_Display() |
//# - chg: #include "debug.h" -> "mk/mkdebugdata.h" |
//# - chg: #include "display.h" -> #include "mk/mkdisplay.h" |
//# - add: #include "mk/mkbase.h" |
//# |
//# 25.01.2014 OG |
//# - chg: MK-Display und MK-DebugData auch bei falscher FC-Revision wieder |
//# aktiviert |
//# |
//# 07.01.2014 Cebra |
//# - chg: Abfrage WrongFCVersion etwas verändert |
//# |
//# 06.01.2014 Cebra |
//# - add: Alle Menüpunkte die FC versionabhängig werden bei falscher FC Version ausgeblendet |
//# |
//# 04.10.2013 Cebra |
//# - add: Motortest ohne FC = I2C-Anschluss PKT, mit FC/NC Motortest über FC |
//# |
//# 26.06.2013 Cebra |
//# - add: Menüpunkt Wlan Rangeextender |
//# |
//# 24.06 2013 OG |
//# - chg: Menu_PKTTools(): Menuepunkte fuer PKT-SV2-Verbindungen werden |
//# durch define USE_PKTTOOLS_SV2 (main.h) an-/ausgeschaltet |
//# - chg: Menu_PKTTools(): Menuepunkte werden je nach aktiviertem Wi232 und |
//# Bluetooth ein-/ausgeblendet |
//# - chg: Menuetexte fuer ID_USB2SV2 und ID_BT2SV2 geaendert |
//# - chg: defines fuer Menuetexte von PKT-Tools geaendert |
//# - del: verschiedene Exec_* Funktionen |
//# |
//# 24.06 2013 Cebra |
//# - add: Menuepunkt MKUSB in PKT-Tools |
//# |
//# 13.06.2013 OG |
//# - chg: Fastconnect auf Menu_PKTTools() umgelegt |
//# - chg: Menueeintraege von Menu_PKTTools() modifiziert und reduziert |
//# - chg: in allen Hauptmenues ist PKT-Setup und PKT-Info drin |
//# |
//# 11.06.2013 OG |
//# - chg: Code zu Menu_OSDData() ausgegliedert nach osddata.c |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 21.05.2013 OG |
//# - chg: Menu_Main() umgestellt auf MENUCTRL_EVENT |
//# - fix: Menu_Main() nach Menu-Redraw searchMK() |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# - del: include utils/menuold.h |
//# |
//# 20.05.2013 OG |
//# - chg: Layout Menutitel |
//# |
//# 19.05.2013 OG |
//# - chg: Menuepunkte Tracking und FollowMe werden nur angezeigt wenn BTM222 |
//# installiert ist (Config.UseBT == true) |
//# - add: Menu_PKTTools() (ehemals in tools.c) |
//# - del: PC_Fast_Connect() verschoben nach pkt.c |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.c verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - chg: PKT_Info() erweitert um optionale Module-Info (via USE_MODULEINFO) |
//# und Credits chronoligisch umgedreht |
//# |
//# 18.05.2013 OG |
//# - add: USE_JOYSTICK, USE_TRACKING, USE_OSDDATA, USE_MKPARAMETER, USE_MKDISPLAY |
//# - chg: redundante Menue-Strings sind untereinander verpointert um Platz zu |
//# sparen |
//# - chg: Umstellung auf neue menuctrl.c und damit starke Strukturaenderung |
//# Hinweis: autom. PKT-Update via Hauptmenue geht noch nicht - wird |
//# wieder eingebaut (wenn m�glich direkt in menuctrl.c dann geht |
//# das in allen Menues) |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//# |
//# 16.05.2013 OG |
//# - add: define USE_MKGPSINFO fuer gps() |
//# |
//# 05.05.2013 Cebra |
//# - chg: Fehler im Menü bei nicht verfügbaren Funktionen |
//# |
//# 02.05.2013 OG |
//# - chg: Menuetext: osddata_menuitems: "MK Fehlerliste" -> "MK Fehler" |
//# - chg: Menuetext: "OSD Anzeige" wieder zurueck zu "OSD" |
//# |
//# 28.04.2013 OG |
//# - chg: Show_Version() auf ScrollBox umgestellt |
//# - chg: Menuetext "Debug Data" zu "MK Debug Data" (kam frueher zu Verwirrungen bei mir) |
//# - chg: main_menu() bzgl. Menu_OSDDATA() |
//# - add: Menu_OSDDATA() (aus ehemaligem osd/osd_tools.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: OSD-Tools im Menue integriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 14.04.2013 OG |
//# - WIP: Anzeige "OSD-Daten" in param_menuitems_nc, param_menuitems_no |
//# |
//# 04.04.2013 Cebra |
//# - chg:Texttuning |
//# not_possible, für Menüpunkte nicht nicht wählbar sind |
//# |
//# 03.04.2013 OG |
//# - fix: Anzeigefehler wenn (hardware == NC) und nicht gesetzt define 'USE_MKDEBUGDATA' (bzw. 'analognames') |
//# - chg: Layout von Anzeige wenn USE_MKDEBUGDATA nicht verfuegbar (jetzt Invers) |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# |
//# 30.03.2013 CB |
//# - add: automatischer Start der PKT-Updatefunktion im Mainmenüe für das Updatetool |
//# |
//# 28.03.2013 CB |
//# - chg: Hinweis wenn Debug Data nicht möglich ist |
//# |
//# 10.03.2013 Cebra |
//# - add: menu_select, gemeinsame Routine für alle Setupmenüs |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> |
#include "main.h" |
#include "lcd/lcd.h" |
#include "menu.h" |
#include "mk/mkdisplay.h" |
#include "mk/mkdebugdata.h" |
#include "timer/timer.h" |
#include "osd/osd.h" |
#include "osd/osddata.h" |
#include "motortest/motortest.h" |
#include "mk/mkgpsinfo.h" |
#include "eeprom/eeprom.h" |
#include "setup/setup.h" |
#include "uart/uart1.h" |
#include "mk-data-structs.h" |
#include "wi232/Wi232.h" |
#include "connect.h" |
#include "lipo/lipo.h" |
#include "messages.h" |
#include "bluetooth/bluetooth.h" |
#include "followme/followme.h" |
#include "tracking/ng_servo.h" |
#include "tracking/tracking.h" |
#include "stick/stick.h" |
#include "utils/menuctrl.h" |
#include "pkt/pkt.h" |
#include "mk/mkbase.h" |
#include "mksettings/paramset.h" |
#include "mksettings/mksettings.h" |
//############################################################################ |
char titlemain[] = "PKT v "; // buffer fuer PKT-Versionsinfo |
//123456789012345678901 |
// nur fuer Entwicklung... |
static const char DEBUG_PARAMSET_Text[] PROGMEM = "DEBUG PARAMSET"; |
//############################################################################ |
//----------------------- |
// main_menu |
//----------------------- |
#define ID_OSD 1 |
#define ID_OSDDATA 2 |
#define ID_MKDISPLAY 3 |
#define ID_MKSETTINGS 4 |
#define ID_MKDEBUGDATA 5 |
#define ID_MKGPSINFO 6 |
#define ID_SEARCHMK 7 |
#define ID_PKTCONNECT 8 |
#define ID_PKTSETUP 9 |
#define ID_PKTINFO 10 |
#define ID_PKTSPEZIAL 11 |
#define ID_MKINFO 12 |
#define ID_MKACCCAL 13 |
#define ID_DEBUG_GPS 14 |
static const char SEARCHMK_de[] PROGMEM = "Suche Mikrokopter"; |
static const char SEARCHMK_en[] PROGMEM = "search Kopter"; |
static const char OSD_de[] PROGMEM = "OSD"; |
#define OSD_en OSD_de |
static const char OSDDATA_de[] PROGMEM = "OSD Daten"; |
static const char OSDDATA_en[] PROGMEM = "OSD Data"; |
static const char MKINFO_de[] PROGMEM = "MK Info"; |
#define MKINFO_en MKINFO_de |
static const char MKDISPLAY_de[] PROGMEM = "MK Display"; |
#define MKDISPLAY_en MKDISPLAY_de |
static const char MKACCCAL_de[] PROGMEM = "MK ACC Kalibr."; |
static const char MKACCCAL_en[] PROGMEM = "MK ACC Calibr."; |
static const char MKSETTINGS_de[] PROGMEM = "MK Settings"; |
#define MKSETTINGS_en MKSETTINGS_de |
static const char MKDEBUGDATA_de[] PROGMEM = "MK Daten"; |
static const char MKDEBUGDATA_en[] PROGMEM = "MK Data"; |
static const char MKGPSINFO_de[] PROGMEM = "MK GPS Info"; |
#define MKGPSINFO_en MKGPSINFO_de |
static const char PKTCONNECT_de[] PROGMEM = "PKT Connect"; |
#define PKTCONNECT_en PKTCONNECT_de |
static const char PKTSPEZIAL_de[] PROGMEM = "PKT Spezial"; |
static const char PKTSPEZIAL_en[] PROGMEM = "PKT Special"; |
static const char PKTSETUP_de[] PROGMEM = "PKT Setup"; |
#define PKTSETUP_en PKTSETUP_de |
static const char PKTINFO_de[] PROGMEM = "PKT Info"; |
#define PKTINFO_en PKTINFO_de |
#ifdef DEBUG_GPS |
static const char PKTGPS_de[] PROGMEM = "PKT GPS Debug"; |
#define PKTGPS_en PKTGPS_de |
#endif |
//############################################################################ |
//----------------------------- |
// Menu_PKTSpezial() |
//----------------------------- |
#define ID_BLTESTER 60 |
#define ID_TRACKING 61 |
#define ID_JOYSTICK 62 |
#define ID_FOLLOWME 63 |
#define ID_FOLLOWMESETUP 64 |
static const char BLTESTER_de[] PROGMEM = "MK Motortest"; |
#define BLTESTER_en BLTESTER_de |
static const char TRACKING_de[] PROGMEM = "Tracking"; |
#define TRACKING_en TRACKING_de |
static const char JOYSTICK_de[] PROGMEM = "Joystick"; |
#define JOYSTICK_en JOYSTICK_de |
static const char FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define FOLLOWME_en FOLLOWME_de |
static const char FOLLOWMESETUP_de[] PROGMEM = "Follow Me Setup"; |
#define FOLLOWMESETUP_en FOLLOWMESETUP_de |
//############################################################################ |
//----------------------------- |
// Menu_PKTConnect() |
//----------------------------- |
#define ID_BT2WI232 40 |
#define ID_USB2WI232 41 |
#define ID_USB2SV2 42 |
#define ID_BT2SV2 43 |
#define ID_WIFLY2WI232 44 |
#define ID_BLE2WI232 45 |
static const char BT2WI232_de[] PROGMEM = "BT Extender"; |
#define BT2WI232_en BT2WI232_de |
static const char BLE2WI232_de[] PROGMEM = "BLE Extender"; |
#define BLE2WI232_en BLE2WI232_de |
static const char WIFLY2WI232_de[] PROGMEM = "WLAN Extender"; |
#define WIFLY2WI232_en WIFLY2WI232_de |
static const char USB2WI232_de[] PROGMEM = "USB -> Wi232"; |
#define USB2WI232_en USB2WI232_de |
#ifdef USE_PKTTOOLS_SV2 |
static const char USB2SV2_de[] PROGMEM = "USB -> SV2"; |
#define USB2SV2_en USB2SV2_de |
#endif |
#ifdef USE_PKTTOOLS_SV2 |
static const char BT2SV2_de[] PROGMEM = "BT -> SV2"; |
#define BT2SV2_en BT2SV2_de |
#endif |
//############################################################################################# |
//# Hilfsfunktionen & Verschiedenes |
//############################################################################################# |
//-------------------------------------------------------------- |
// wird von Menu_PKTTools() verwendet |
//-------------------------------------------------------------- |
void Exec_BLTESTER(void) |
{ |
if(hardware == NO) motor_test( I2C_Mode ); |
if(hardware == NC) motor_test( FC_Mode ); |
if(hardware == FC) motor_test( FC_Mode ); |
} |
//############################################################################################# |
//# Menu: Spezial |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer 'PKTSpezial' |
//-------------------------------------------------------------- |
void Menu_PKTSpezial( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Spezial" |
//MenuCtrl_SetTitle_P( PSTR("PKT Spezial") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems: PKTSpezial |
//--------------- |
#ifdef USE_FOLLOWME |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_FOLLOWME , MENU_ITEM, &FollowMe , FOLLOWME_de , FOLLOWME_en ); |
MenuCtrl_PushML2_P( ID_FOLLOWMESETUP, MENU_ITEM, &Setup_FollowMe, FOLLOWMESETUP_de, FOLLOWMESETUP_en ); |
} |
#endif |
#ifdef USE_TRACKING |
if( Config.UseBT && MKVersion.isNC ) |
{ |
MenuCtrl_PushML2_P( ID_TRACKING , MENU_ITEM, &PKT_tracking , TRACKING_de , TRACKING_en ); |
} |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( ID_JOYSTICK , MENU_ITEM, &joystick , JOYSTICK_de , JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_BLTESTER , MENU_ITEM, &Exec_BLTESTER , BLTESTER_de , BLTESTER_en ); |
// {"Servo Tester ","servo test ","Servo Tester "}, |
// if((val+offset) == 2 ) servo_test(); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: PKT-Connect |
//############################################################################################# |
//-------------------------------------------------------------- |
// Menue fuer; PKT-Connect |
// |
// verbindet das PKT mit anderen Devices wie Tablet usw. |
//-------------------------------------------------------------- |
void Menu_PKTConnect( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle_P( PSTR("PKT Connect") ); // kann nicht auf MenuCtrl_SetTitleFromParentItem() umgestellt |
// werden da PKT-Connect auch durch einen Hot-Key aufgerufen |
// werden kann |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_BLUETOOTH |
if( Config.UseBT && Config.UseWi ) |
MenuCtrl_PushML2_P( ID_BT2WI232 , MENU_ITEM, &Port_BT2Wi , BT2WI232_de , BT2WI232_en ); |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch && Config.UseBLE ) |
MenuCtrl_PushML2_P( ID_BLE2WI232 , MENU_ITEM, &Port_BLE2Wi , BLE2WI232_de , BLE2WI232_en ); |
#endif |
if( PCB_SV2RxTxPatch && Config.UseWL ) |
MenuCtrl_PushML2_P( ID_WIFLY2WI232 , MENU_ITEM, &Port_WiFly2Wi , WIFLY2WI232_de , WIFLY2WI232_en ); |
if( Config.UseWi ) |
MenuCtrl_PushML2_P( ID_USB2WI232 , MENU_ITEM, &Port_USB2Wi , USB2WI232_de , USB2WI232_en ); |
#ifdef USE_PKTTOOLS_SV2 // am PKT-SV2 liegen +5 Volt an... |
MenuCtrl_PushML2_P( ID_USB2SV2 , MENU_ITEM, &Port_USB2FC , USB2SV2_de , USB2SV2_en ); |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
MenuCtrl_PushML2_P( ID_BT2SV2 , MENU_ITEM, &Port_BT2FC , BT2SV2_de , BT2SV2_en ); |
#endif |
#endif // end: #ifdef USE_PKTTOOLS_SV2 |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# Menu: Menu_Main |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NC( void ) |
{ |
MenuCtrl_PushML2_P( ID_OSD , MENU_ITEM, &osd , OSD_de , OSD_en ); |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MKGPSINFO |
MenuCtrl_PushML2_P( ID_MKGPSINFO , MENU_ITEM, &MK_Gps_Info , MKGPSINFO_de , MKGPSINFO_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_FC( void ) |
{ |
MenuCtrl_PushML2_P( ID_MKINFO , MENU_ITEM, NOFUNC , MKINFO_de , MKINFO_en ); |
#ifdef USE_MKSETTINGS |
if( MKVersion.paramsetOK ) |
{ |
MenuCtrl_PushML2_P( ID_MKSETTINGS, MENU_SUB , &MK_Settings , MKSETTINGS_de, MKSETTINGS_en ); |
} |
#endif |
#ifdef USE_MKDISPLAY |
// ACC_Display = false; |
MenuCtrl_PushML2_P( ID_MKDISPLAY , MENU_ITEM, &MK_Display , MKDISPLAY_de , MKDISPLAY_en ); |
#endif |
#ifdef USE_ACCCALIBRATION |
if ((MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_EQUAL) || (MKVersion_Cmp( MKVersion.FCVer, 2,9,'i') == VERSION_GREATER)) |
{ |
ACC_Display = true; |
MenuCtrl_PushML2_P( ID_MKACCCAL , MENU_ITEM, &MK_Display , MKACCCAL_de , MKACCCAL_en ); |
} |
#endif |
#ifdef USE_MKDEBUGDATA |
MenuCtrl_PushML2_P( ID_MKDEBUGDATA, MENU_ITEM, &MK_DebugData , MKDEBUGDATA_de, MKDEBUGDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
MenuCtrl_PushML2_P( ID_PKTSPEZIAL , MENU_SUB , &Menu_PKTSpezial , PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT , MENU_SUB , &Menu_PKTConnect , PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM ,&PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _DefMenu_Main_NO( void ) |
{ |
#ifdef USE_FOLLOWME |
#ifdef DEBUG_GPS |
MenuCtrl_PushML2_P( ID_DEBUG_GPS , MENU_ITEM, &Debug_GPS , PKTGPS_de , PKTGPS_en ); |
#endif |
#endif |
MenuCtrl_PushML2_P( ID_SEARCHMK , MENU_ITEM, NOFUNC , SEARCHMK_de , SEARCHMK_en ); |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_OSDDATA |
MenuCtrl_PushML2_P( ID_OSDDATA , MENU_SUB , &Menu_OSDData , OSDDATA_de , OSDDATA_en ); |
#endif |
#ifdef USE_MAINMENU_SEPARATOR |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
//MenuCtrl_PushML2_P( ID_PKTSPEZIAL, MENU_SUB , &Menu_PKTSpezial, PKTSPEZIAL_de , PKTSPEZIAL_en ); |
MenuCtrl_PushML2_P( ID_PKTCONNECT, MENU_SUB , &Menu_PKTConnect, PKTCONNECT_de , PKTCONNECT_en ); |
MenuCtrl_PushML2_P( ID_PKTSETUP , MENU_SUB , NOFUNC , PKTSETUP_de , PKTSETUP_en ); |
MenuCtrl_PushML2_P( ID_PKTINFO , MENU_ITEM, &PKT_Info , PKTINFO_de , PKTINFO_en ); |
#ifdef DEBUG_PARAMSET |
MenuCtrl_Push_P( 0 , MENU_ITEM, ¶msetDEBUG , DEBUG_PARAMSET_Text ); |
#endif |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _ConfigMenu_Main( void ) |
{ |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( titlemain ); |
//MenuCtrl_SetShowBatt( true ); |
//----------- |
// Tasten |
//----------- |
MenuCtrl_SetKey( KEY_ESC , strGet(OFF), &PKT_SwitchOff ); |
MenuCtrl_SetKey( KEY_ENTER_LONG , NOTEXT, &Menu_PKTConnect ); |
if ( MKVersion.isNC ) _DefMenu_Main_NC(); |
else if( MKVersion.isFC ) _DefMenu_Main_FC(); |
else _DefMenu_Main_NO(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Main( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT = Config.UseBT; // merkt sich Bluetooth Einstellung falls durch Setup geaendert |
uart1_flush(); |
ADC_Init(); // ADC für Lipomessung // MartinR hierher verschoben |
get_key_press(KEY_ALL); |
strncpy( &titlemain[5], PKTSWVersion, 12); // baue Titel mit PKT-Versionsnummer zusammen |
MenuCtrl_ShowLevel( true ); // zeige Menuelevel in der Titelzeile aller Menues |
_ConfigMenu_Main(); // initialisiert das Menu je nach Hardware (NO,FC,NC) verschieden |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); // wartet nicht auf Event, springt direkt zurueck (wegen evtl. BT-Aenderung) |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------- |
// ID_SEARCHMK |
//-------------- |
if( itemid == ID_SEARCHMK ) // neuer Hardware Modus? |
{ |
if( searchMK(true) ) // true = zeige MK_Info() an |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_MKINFO |
//-------------- |
if( itemid == ID_MKINFO ) |
{ |
if( MK_Info( 0, true) ) // neuer Hardware Modus? (in MK_Info() kann man eine Neusuche des MK starten) (true = mit Settings-Refresh) |
{ |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
} |
} |
//-------------- |
// ID_PKTSETUP |
//-------------- |
if( itemid == ID_PKTSETUP ) |
{ |
Setup_MAIN(); |
} |
//-------------- |
// Bluetooth geaendert? |
//-------------- |
if( UseBT != Config.UseBT ) // falls sich im PKT-Setup die Einstellung bzgl. installiertem Bluetooth Modul |
{ // geaendert hat werden ggf. andere Menuepunkte angezeigt (Tracking, FollowMe) |
MenuCtrl_Destroy(); // Menu verwerfen... |
_ConfigMenu_Main(); // ...und neues Menu initialisieren (je nach Hardware: NO,FC,NC) |
UseBT = Config.UseBT; |
} |
} |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/menu.h |
---|
0,0 → 1,57 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menu.h |
//# |
//# 19.05.2013 OG |
//# - chg: main_menu() umbenannt zu Menu_Main() |
//# |
//# 19.05.2013 OG |
//# - chg: Funktionen nach main.h verschoben |
//# CheckUpdate(), Update_PKT(), PKT_Info(), PKT_SwitchOff() |
//# |
//# 18.05.2013 OG |
//# - del: Ausgliederung der alten Menue-Funktionen nach utils/menuold.c |
//# inkl. not_possible() |
//############################################################################ |
#ifndef MENU_H |
#define MENU_H |
void Menu_Main(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/messages.c |
---|
0,0 → 1,829 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.c |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 28.06.2014 OG |
//# - chg: Text von ERROR_NODATA statt "Datenverlust!" jetzt "MK Datenverlust!" |
//# - chg: Text von OSD_ERROR statt "FEHLER: Datenverlust!" jetzt "MK Datenverlust!" |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 11.05.2014 OG |
//# - chg: STR_SAVE_de - jetzt "Speichern" statt "speichern" |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# - chg: OSD_ERROR_de,OSD_ERROR_en Space am Ende entfernt / Ausrufungszeichen hinzugefuegt |
//# |
//# 09.04.2014 OG |
//# - chg: WL1 von "WiFly vorh.:" zu "WiFly Modus:" |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 02.04.2014 OG |
//# - chg: Text WL1_de gekuerzt |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: Text zu WL1 geaendert von "Wlan eingebaut" zu "WiFly vorhanden" |
//# -> das ist ein Aufsteckmodul und wird nicht eingebaut |
//# - chg: strGet() angepasst auf 2 Sprachen (Deutsch, Englisch) |
//# - del: Hollaendisch vollstaendig geloescht |
//# |
//# 29.03.2014 OG |
//# - chg: Test geaendert von START_LASTPOS2 zu "löschen Nein" |
//# - chg: einige Texte bzgl. OSD-Data gekuerzt wegen Umstellung auf lcdx_printp_center() |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - chg: Texte von BOOT1, BOOT2, TESTSTRING |
//# - add: STR_VON, STR_NACH, STR_PKT |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: STARTMSG2 |
//# - add: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - chg: START_LASTPOS |
//# - chg: START_LASTPOS1 |
//# - chg: START_LASTPOS3 |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - del: etliche ":" aus Bezeichnungen geloescht (bei verschiedenen Einstellungen) |
//# -> woher kamen die ueberhaupt? In der Historie sehe ich dazu nichts! |
//# - add: MSG_WARNUNG, UPDATE3 |
//# - chg: UPDATE1, UPDATE2 (PKT Update) |
//# - chg: SHUTDOWN (Space's entfernt) |
//# - chg: START_VERSIONCHECK3: angepasst auf neue flexibilitaet des PKT wenn |
//# FC-Revision nicht unterstuetzt wird |
//# - chg: OK_de von "Ok" nach "OK" |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: Texte geaendert von START_VERSIONCHECK... |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting (fuer Setup: ein-/ausschalten der MK-Settingsanzeige) |
//# - del: DISPLAY8 (ehemals LCD-Orientierung, nicht mehr benoetigt) |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 OG |
//# - chg: kleine Textaenderung an STARTMSG2 |
//# - chg: Code-Layout |
//# |
//# 13.06.2013 CB |
//# - add: Texte für OSD Statistik erweitert |
//# |
//# 25.05.2013 OG |
//# - chg: redundante Strings durch define ersetzt um Speicherplatz zu sparen |
//# |
//# 05.05.2013 Cebra |
//# - add: Erweiterung PKT Zeitsetup |
//# |
//# 25.04.2013 Cebra |
//# - chg: LIPO entfernt, Text doppelt |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "eeprom/eeprom.h" |
#include "messages.h" |
typedef enum |
{ |
GERMAN, |
ENGLISH, |
NETHERLAND, |
LAST_LANGUAGE, |
} LANGUAGE; |
static const char KEYLINE1_de[] PROGMEM = " \x1a \x1b Ende OK"; |
static const char KEYLINE1_en[] PROGMEM = " \x1a \x1b end OK"; |
static const char KEYLINE2_de[] PROGMEM = " \x18 \x19 Ende OK"; |
static const char KEYLINE2_en[] PROGMEM = " \x18 \x19 end OK"; |
static const char KEYLINE3_de[] PROGMEM = " \x18 \x19 Ende "; |
static const char KEYLINE3_en[] PROGMEM = " \x18 \x19 end "; |
static const char KEYLINE4_de[] PROGMEM = "Ende OK"; |
static const char KEYLINE4_en[] PROGMEM = "end OK"; |
static const char KEYLINE5_de[] PROGMEM = " \x18 \x19 Abbr. OK"; |
static const char KEYLINE5_en[] PROGMEM = " \x18 \x19 cancel OK"; |
static const char KEYCANCEL_de[] PROGMEM = " Abbr."; |
static const char KEYCANCEL_en[] PROGMEM = "cancel"; |
static const char BOOT1_de[] PROGMEM = "Taste 1 Sekunde"; |
static const char BOOT1_en[] PROGMEM = "press button"; |
static const char BOOT2_de[] PROGMEM = "festhalten!"; |
static const char BOOT2_en[] PROGMEM = "for 1 second!"; |
static const char START_LASTPOS_de[] PROGMEM = "Letzte Position"; |
static const char START_LASTPOS_en[] PROGMEM = "last position"; |
static const char START_LASTPOS1_de[] PROGMEM = "Breitengr. Längengr."; |
static const char START_LASTPOS1_en[] PROGMEM = "latitude longitude"; |
static const char START_LASTPOS2_de[] PROGMEM = "löschen Nein"; |
static const char START_LASTPOS2_en[] PROGMEM = "delete No"; |
static const char START_LASTPOS3_de[] PROGMEM = "Google Eingabe:"; |
static const char START_LASTPOS3_en[] PROGMEM = "Google Input:"; |
static const char START_SEARCHFC_de[] PROGMEM = "suche Mikrokopter..."; |
static const char START_SEARCHFC_en[] PROGMEM = "searching Kopter..."; |
static const char ENDE_de[] PROGMEM = "Ende"; |
static const char ENDE_en[] PROGMEM = "end"; |
static const char OK_de[] PROGMEM = "OK"; |
static const char OK_en[] PROGMEM = "ok"; |
static const char FEHLER_de[] PROGMEM = "Fehler"; |
static const char FEHLER_en[] PROGMEM = "error"; |
static const char AKTIV_de[] PROGMEM = "aktiv"; |
static const char AKTIV_en[] PROGMEM = "activ"; |
static const char STR_SAVE_de[] PROGMEM = "Speichern"; |
static const char STR_SAVE_en[] PROGMEM = "save"; |
static const char STR_SAVING_de[] PROGMEM = "wird gespeichert..."; |
static const char STR_SAVING_en[] PROGMEM = "saving..."; |
static const char STR_DISCARD_de[] PROGMEM = "Verwerfen"; |
static const char STR_DISCARD_en[] PROGMEM = "discard"; |
static const char STR_COPY_de[] PROGMEM = "Kopieren"; |
static const char STR_COPY_en[] PROGMEM = "copy"; |
static const char STR_SEARCH_de[] PROGMEM = "suche"; |
static const char STR_SEARCH_en[] PROGMEM = "search"; |
static const char STR_FOUND_de[] PROGMEM = "gefunden"; |
static const char STR_FOUND_en[] PROGMEM = "found"; |
static const char STR_SET_de[] PROGMEM = "setze"; |
static const char STR_SET_en[] PROGMEM = "set"; |
static const char STR_WITH_de[] PROGMEM = "mit"; |
static const char STR_WITH_en[] PROGMEM = "with"; |
static const char STR_WITHOUT_de[] PROGMEM = "ohne"; |
static const char STR_WITHOUT_en[] PROGMEM = "without"; |
static const char STR_SWITCHMOTOROFF_de[] PROGMEM = "Motoren ausschalten!"; |
static const char STR_SWITCHMOTOROFF_en[] PROGMEM = "switch motors off!"; |
static const char ON_de[] PROGMEM = "Ein "; |
static const char ON_en[] PROGMEM = "On "; |
static const char OFF_de[] PROGMEM = "Aus "; |
static const char OFF_en[] PROGMEM = "off "; |
static const char ESC_de[] PROGMEM = "ESC"; |
#define ESC_en ESC_de |
static const char SHUTDOWN_de[] PROGMEM = "PKT ausschalten?"; |
static const char SHUTDOWN_en[] PROGMEM = "shutdown PKT?"; |
static const char YESNO_de[] PROGMEM = "Ja Nein"; |
static const char YESNO_en[] PROGMEM = "yes no"; |
static const char NOYES_de[] PROGMEM = "Nein Ja"; |
static const char NOYES_en[] PROGMEM = "no yes"; |
static const char DELETEDATA_de[] PROGMEM = "Daten löschen?"; |
static const char DELETEDATA_en[] PROGMEM = "delete data?"; |
static const char UPDATE1_de[] PROGMEM = "Verbinde PKT mit PC!"; |
static const char UPDATE1_en[] PROGMEM = "Connect PKT to PC"; |
static const char UPDATE2_de[] PROGMEM = "Drücke 'Start' für"; |
static const char UPDATE2_en[] PROGMEM = "Press 'Start' for"; |
static const char UPDATE3_de[] PROGMEM = "min. 2 Sekunden"; |
static const char UPDATE3_en[] PROGMEM = "min. 2 seconds"; |
static const char ENDSTART_de[] PROGMEM = "Ende Start"; |
static const char ENDSTART_en[] PROGMEM = "End Start"; |
static const char CONNECT13_de[] PROGMEM = "Modul eingebaut"; |
static const char CONNECT13_en[] PROGMEM = "Modul built in"; |
static const char KABEL_de[] PROGMEM = "Kabel"; |
static const char KABEL_en[] PROGMEM = "cable"; |
static const char SLAVE_de[] PROGMEM = "Slave"; |
#define SLAVE_en SLAVE_de |
static const char NORMAL_de[] PROGMEM = "Normal"; |
#define NORMAL_en NORMAL_de |
static const char REVERSE_de[] PROGMEM = "Reverse"; |
static const char REVERSE_en[] PROGMEM = "inverse"; |
static const char ENDOK_de[] PROGMEM = "Ende OK"; |
static const char ENDOK_en[] PROGMEM = "End OK"; |
static const char EEPROM1_de[] PROGMEM = "PKT-EEProm"; |
static const char EEPROM1_en[] PROGMEM = "Realy delete"; |
static const char EEPROM2_de[] PROGMEM = "wirklich löschen?"; |
static const char EEPROM2_en[] PROGMEM = "PKT-EEProm?"; |
static const char DEUTSCH_de[] PROGMEM = "deutsch"; |
static const char DEUTSCH_en[] PROGMEM = "german"; |
static const char ENGLISCH_de[] PROGMEM = "englisch"; |
static const char ENGLISCH_en[] PROGMEM = "english"; |
static const char YES_de[] PROGMEM = "Ja "; |
static const char YES_en[] PROGMEM = "yes "; |
static const char NOO_de[] PROGMEM = "Nein"; |
static const char NOO_en[] PROGMEM = "no "; |
static const char OSD_3D_V_de[] PROGMEM = "V"; |
static const char OSD_3D_V_en[] PROGMEM = "F"; |
static const char OSD_3D_H_de[] PROGMEM = "H"; |
static const char OSD_3D_H_en[] PROGMEM = "B"; |
static const char OSD_3D_L_de[] PROGMEM = "L"; |
#define OSD_3D_L_en OSD_3D_L_de |
static const char OSD_3D_R_de[] PROGMEM = "R"; |
#define OSD_3D_R_en OSD_3D_R_de |
static const char OSD_3D_NICK_de[] PROGMEM = "Ni"; |
#define OSD_3D_NICK_en OSD_3D_NICK_de |
static const char OSD_3D_ROLL_de[] PROGMEM = "Ro"; |
#define OSD_3D_ROLL_en OSD_3D_ROLL_de |
static const char OSD_ERROR_de[] PROGMEM = "MK Datenverlust!"; |
static const char OSD_ERROR_en[] PROGMEM = "MK Data lost!"; |
static const char PARA_AKTIVI_de[] PROGMEM = "Aktivieren"; |
static const char PARA_AKTIVI_en[] PROGMEM = "activate"; |
static const char PARA_COPY_de[] PROGMEM = "Kopiere Setting"; |
static const char PARA_COPY_en[] PROGMEM = "copy settings"; |
static const char PARA_COPYQ_de[] PROGMEM = "Wirklich kopieren?"; |
static const char PARA_COPYQ_en[] PROGMEM = "really copy?"; |
static const char GPS2_de[] PROGMEM = "gewähltes GPS Gerät "; |
static const char GPS2_en[] PROGMEM = "selected GPS device "; |
static const char STATS_ITEM_0_de[] PROGMEM = "max Höhe :"; |
static const char STATS_ITEM_0_en[] PROGMEM = "max altitude:"; |
static const char STATS_ITEM_1_de[] PROGMEM = "max Geschw. :"; |
static const char STATS_ITEM_1_en[] PROGMEM = "max speed :"; |
static const char STATS_ITEM_2_de[] PROGMEM = "max Entfern.:"; |
static const char STATS_ITEM_2_en[] PROGMEM = "max distance:"; |
static const char STATS_ITEM_3_de[] PROGMEM = "min Spannung:"; |
static const char STATS_ITEM_3_en[] PROGMEM = "min voltage :"; |
static const char STATS_ITEM_4_de[] PROGMEM = "max Zeit :"; |
static const char STATS_ITEM_4_en[] PROGMEM = "max time :"; |
static const char STATS_ITEM_5_de[] PROGMEM = "max Strom :"; |
static const char STATS_ITEM_5_en[] PROGMEM = "max current :"; |
static const char STATS_ITEM_6_de[] PROGMEM = "Ent.Kapazit.:"; |
static const char STATS_ITEM_6_en[] PROGMEM = "UsedCapacity:"; |
static const char POTI_de[] PROGMEM = "Poti "; |
static const char POTI_en[] PROGMEM = "poti "; |
static const char TASTER_de[] PROGMEM = "Taster"; |
static const char TASTER_en[] PROGMEM = "switch"; |
static const char STAT_OSDBL_de[] PROGMEM = "OSD- und BL-Daten"; |
static const char STAT_OSDBL_en[] PROGMEM = "OSD- and BL-Data"; |
static const char STAT_ERROR_de[] PROGMEM = "MK Fehlerliste"; |
static const char STAT_ERROR_en[] PROGMEM = "MK errorlist"; |
static const char STAT_GPS_de[] PROGMEM = "User GPS-Daten"; |
static const char STAT_GPS_en[] PROGMEM = "User GPS-data"; |
static const char STAT_POS_de[] PROGMEM = "letzte Position"; |
static const char STAT_POS_en[] PROGMEM = "last position"; |
static const char STAT_ALL_de[] PROGMEM = "ALLE Daten"; |
static const char STAT_ALL_en[] PROGMEM = "ALL data"; |
static const char DELETE_de[] PROGMEM = "löschen?"; |
static const char DELETE_en[] PROGMEM = "delete?"; |
static const char STR_OSDSCREEN_GENERAL_de[] PROGMEM = "General"; |
#define STR_OSDSCREEN_GENERAL_en STR_OSDSCREEN_GENERAL_de |
static const char STR_OSDSCREEN_NAVIGATION_de[] PROGMEM = "Navigation"; |
#define STR_OSDSCREEN_NAVIGATION_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_WAYPOINTS_de[] PROGMEM = "Waypoints"; |
#define STR_OSDSCREEN_WAYPOINTS_en STR_OSDSCREEN_NAVIGATION_de |
static const char STR_OSDSCREEN_ELECTRIC_de[] PROGMEM = "Electric"; |
#define STR_OSDSCREEN_ELECTRIC_en STR_OSDSCREEN_ELECTRIC_de |
static const char STR_OSDSCREEN_MKSTATUS_de[] PROGMEM = "MK-Status"; |
#define STR_OSDSCREEN_MKSTATUS_en STR_OSDSCREEN_MKSTATUS_de |
static const char STR_OSDSCREEN_USERGPS_de[] PROGMEM = "User GPS"; |
#define STR_OSDSCREEN_USERGPS_en STR_OSDSCREEN_USERGPS_de |
static const char STR_OSDSCREEN_3DLAGE_de[] PROGMEM = "3D Lage"; |
#define STR_OSDSCREEN_3DLAGE_en STR_OSDSCREEN_3DLAGE_de |
static const char STR_OSDSCREEN_STATISTIC_de[] PROGMEM = "Statistics"; |
#define STR_OSDSCREEN_STATISTIC_en STR_OSDSCREEN_STATISTIC_de |
static const char STR_OSDSCREEN_OSD0_de[] PROGMEM = "OSD0"; |
#define STR_OSDSCREEN_OSD0_en STR_OSDSCREEN_OSD0_de |
static const char STR_OSDSCREEN_OSD1_de[] PROGMEM = "OSD1"; |
#define STR_OSDSCREEN_OSD1_en STR_OSDSCREEN_OSD1_de |
static const char STR_OSDSCREEN_OSD2_de[] PROGMEM = "OSD2"; |
#define STR_OSDSCREEN_OSD2_en STR_OSDSCREEN_OSD2_de |
static const char NOMKFOUND_de[] PROGMEM = "keinen MK gefunden!"; |
static const char NOMKFOUND_en[] PROGMEM = "no MK found!"; |
static const char STR_ERROR_de[] PROGMEM = "FEHLER"; |
static const char STR_ERROR_en[] PROGMEM = "ERROR"; |
static const char ERROR_NODATA_de[] PROGMEM = "MK Datenverlust!"; |
static const char ERROR_NODATA_en[] PROGMEM = "MK Data lost!"; |
static const char MSG_LOADSETTINGS_de[] PROGMEM = "lade Settings..."; |
static const char MSG_LOADSETTINGS_en[] PROGMEM = "loading settings..."; |
static const char MSG_ACTIVATESETTING_de[] PROGMEM = "aktiviere Setting..."; |
static const char MSG_ACTIVATESETTING_en[] PROGMEM = "activate setting..."; |
static const char EDIT_SETTING_de[] PROGMEM = "Bearbeiten"; |
static const char EDIT_SETTING_en[] PROGMEM = "edit"; |
static const char STR_VON_de[] PROGMEM = "von"; |
static const char STR_VON_en[] PROGMEM = "from"; |
static const char STR_NACH_de[] PROGMEM = "nach"; |
static const char STR_NACH_en[] PROGMEM = "to"; |
static const char STR_PKT_de[] PROGMEM = "PKT"; |
#define STR_PKT_en STR_PKT_de |
static const char STR_ACTIVE_de[] PROGMEM = "AKTIV!"; |
static const char STR_ACTIVE_en[] PROGMEM = "ACTIVE!"; |
static const char MODULE_EXIST_de[] PROGMEM = "Modul vorhanden"; |
static const char MODULE_EXIST_en[] PROGMEM = "Module exist"; |
static const char STR_INITIALIZE_de[] PROGMEM = "Initialisieren"; |
static const char STR_INITIALIZE_en[] PROGMEM = "Initialize"; |
static const char STR_LONGPRESS_de[] PROGMEM = "langer Tastendruck:"; |
static const char STR_LONGPRESS_en[] PROGMEM = "long press:"; |
static const char STR_FAV_ADD_de[] PROGMEM = "Fav hinzugefügt!"; |
static const char STR_FAV_ADD_en[] PROGMEM = "fav added!"; |
static const char STR_FAV_FULL_de[] PROGMEM = "* Fav ist voll *"; |
static const char STR_FAV_FULL_en[] PROGMEM = "* fav is full *"; |
static const char STR_FAV_EXIST_de[] PROGMEM = "* Fav vorhanden *"; |
static const char STR_FAV_EXIST_en[] PROGMEM = "* fav exists*"; |
static const char STR_FAV_NOTEXIST_de[] PROGMEM = "kein Fav vorhanden!"; |
static const char STR_FAV_NOTEXIST_en[] PROGMEM = "no fav exist!"; |
static const char STR_MENUCTRL_DELITEM_de[] PROGMEM = "Eintrag entfernt!"; |
static const char STR_MENUCTRL_DELITEM_en[] PROGMEM = "item deleted!"; |
static const char STR_MENUCTRL_DELASK_de[] PROGMEM = "Eintrag entfernen?"; |
static const char STR_MENUCTRL_DELASK_en[] PROGMEM = "delete item?"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_de[] PROGMEM = "nicht möglich!"; |
static const char STR_MENUCTRL_NOTPOSSIBLE_en[] PROGMEM = "not possible"; |
static const char STR_HELP_PKTOFFTIME1_de[] PROGMEM = "Minuten (0=Aus)"; |
static const char STR_HELP_PKTOFFTIME1_en[] PROGMEM = "minutes (0=off)"; |
static const char STR_HELP_LOWBAT1_de[] PROGMEM = "<4.20 Einzelzelle "; |
static const char STR_HELP_LOWBAT1_en[] PROGMEM = "<4.20 single cell "; |
static const char STR_HELP_LOWBAT2_de[] PROGMEM = ">4.20 Gesamtspannu."; |
static const char STR_HELP_LOWBAT2_en[] PROGMEM = ">4.20 total voltage"; |
static const char STR_HELP_LIPOOFFSET1_de[] PROGMEM = "verstellen, bis die"; |
static const char STR_HELP_LIPOOFFSET1_en[] PROGMEM = "adjust offset until"; |
static const char STR_HELP_LIPOOFFSET2_de[] PROGMEM = "Spannung passt!"; |
static const char STR_HELP_LIPOOFFSET2_en[] PROGMEM = "voltage fits!"; |
static const char STR_DATA_de[] PROGMEM = "Daten"; |
static const char STR_DATA_en[] PROGMEM = "data"; |
static const char STR_READING_de[] PROGMEM = "lese"; |
static const char STR_READING_en[] PROGMEM = "reading"; |
static const char STR_LABELS_de[] PROGMEM = "Labels"; |
static const char STR_LABELS_en[] PROGMEM = "labels"; |
static const char STR_EXTSV2MODULE_de[] PROGMEM = "externes SV2-Modul"; |
static const char STR_EXTSV2MODULE_en[] PROGMEM = "external SV2 module"; |
static const char STR_ATTENTION_de[] PROGMEM = "* ACHTUNG *"; |
static const char STR_ATTENTION_en[] PROGMEM = "* ATTENTION *"; |
static const char STR_SWITCHOFFMK_de[] PROGMEM = "Kopter ausschalten!"; |
static const char STR_SWITCHOFFMK_en[] PROGMEM = "switch off kopter!"; |
static const char STR_WI232CONFIG_de[] PROGMEM = "Wi.232 Konfig."; |
static const char STR_WI232CONFIG_en[] PROGMEM = "Wi.232 Config"; |
static const char STR_USBCONNECTED_de[] PROGMEM = "mit USB verbunden"; |
static const char STR_USBCONNECTED_en[] PROGMEM = "connected with USB"; |
static const char STR_SEEMKWIKI_de[] PROGMEM = "siehe MK-Wiki:"; |
static const char STR_SEEMKWIKI_en[] PROGMEM = "see MK-Wiki:"; |
static const char STR_PKT_SWITCHOFF_NOW_de[] PROGMEM = "jetzt ausschalten?"; |
static const char STR_PKT_SWITCHOFF_NOW_en[] PROGMEM = "switch off now?"; |
static const char STR_PKT_RESTART_NOW_de[] PROGMEM = "PKT neu starten!"; |
static const char STR_PKT_RESTART_NOW_en[] PROGMEM = "restart PKT!"; |
static const char STR_WI232_ACTIVATE_de[] PROGMEM = "aktiviere Wi.232!"; |
static const char STR_WI232_ACTIVATE_en[] PROGMEM = "activate Wi.232!"; |
static const char STR_WAITSATFIX_de[] PROGMEM = "* warte auf Satfix *"; |
static const char STR_WAITSATFIX_en[] PROGMEM = "* waiting satfix *"; |
static const char STR_GPSMOUSECONNECT_de[] PROGMEM = "verbinde GPS-Maus"; |
static const char STR_GPSMOUSECONNECT_en[] PROGMEM = "search gps-mouse"; |
static const char STR_GPSMOUSEDISCONNECT_de[] PROGMEM = "trenne GPS-Maus"; |
static const char STR_GPSMOUSEDISCONNECT_en[] PROGMEM = "disjoin gps-mouse"; |
static const char STR_NOBTM222_de[] PROGMEM = "kein BTM222 vorh."; |
static const char STR_NOBTM222_en[] PROGMEM = "no BTM222 installed"; |
static const char STR_NOGPSMOUSE_de[] PROGMEM = "keine GPS-Maus!"; |
static const char STR_NOGPSMOUSE_en[] PROGMEM = "no gps-mouse!"; |
static const char STR_GPSMOUSECONNECTION_de[] PROGMEM = "GPS-Maus Verbindung"; |
static const char STR_GPSMOUSECONNECTION_en[] PROGMEM = "gps-mouse connection"; |
static const char STR_SEARCH_BT_ASK_de[] PROGMEM = "BT Geräte suchen?"; |
static const char STR_SEARCH_BT_ASK_en[] PROGMEM = "search BT devices?"; |
static const char STR_SEARCH_BT_de[] PROGMEM = "suche BT Geräte"; |
static const char STR_SEARCH_BT_en[] PROGMEM = "search BT devices"; |
static const char STR_NO_BT_FOUND_de[] PROGMEM = "kein Gerät gefunden"; |
static const char STR_NO_BT_FOUND_en[] PROGMEM = "no device found"; |
static const char STR_BT_DEVICES_de[] PROGMEM = "BT Geräte"; |
static const char STR_BT_DEVICES_en[] PROGMEM = "BT devices"; |
static const char STR_BT_SEARCHTIME_de[] PROGMEM = "(ca. 2 Minuten)"; |
static const char STR_BT_SEARCHTIME_en[] PROGMEM = "(ca. 2 minutes)"; |
static const char STR_BT_LOSTDATA_de[] PROGMEM = "BT Datenverlust"; |
static const char STR_BT_LOSTDATA_en[] PROGMEM = "BT data loss"; |
static const char STR_METERS_de[] PROGMEM = "Meter"; |
static const char STR_METERS_en[] PROGMEM = "meters"; |
static const char STR_WAITNMEA_de[] PROGMEM = "Warte auf GPS Daten"; |
static const char STR_WAITNMEA_en[] PROGMEM = "waiting for GPS Data"; |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
static const char LAST_STR_de[] PROGMEM = "Lastring"; |
#define LAST_STR_en LAST_STR_de |
//****************************************************************** |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * const strings[] PROGMEM = |
{ |
KEYLINE1_de, KEYLINE1_en, |
KEYLINE2_de, KEYLINE2_en, |
KEYLINE3_de, KEYLINE3_en, |
KEYLINE4_de, KEYLINE4_en, |
KEYLINE5_de, KEYLINE5_en, |
KEYCANCEL_de, KEYCANCEL_en, |
BOOT1_de, BOOT1_en, |
BOOT2_de, BOOT2_en, |
START_LASTPOS_de, START_LASTPOS_en, |
START_LASTPOS1_de, START_LASTPOS1_en, |
START_LASTPOS2_de, START_LASTPOS2_en, |
START_LASTPOS3_de, START_LASTPOS3_en, |
START_SEARCHFC_de, START_SEARCHFC_en, |
ENDE_de, ENDE_en, |
OK_de, OK_en, |
FEHLER_de, FEHLER_en, |
AKTIV_de, AKTIV_en, |
STR_SAVE_de, STR_SAVE_en, |
STR_SAVING_de, STR_SAVING_en, |
STR_DISCARD_de, STR_DISCARD_en, |
STR_COPY_de, STR_COPY_en, |
STR_SEARCH_de, STR_SEARCH_en, |
STR_FOUND_de, STR_FOUND_en, |
STR_SET_de, STR_SET_en, |
STR_WITH_de, STR_WITH_en, |
STR_WITHOUT_de, STR_WITHOUT_en, |
STR_SWITCHMOTOROFF_de, STR_SWITCHMOTOROFF_en, |
ON_de, ON_en, |
OFF_de, OFF_en, |
ESC_de, ESC_en, |
SHUTDOWN_de, SHUTDOWN_en, |
YESNO_de, YESNO_en, |
NOYES_de, NOYES_en, |
DELETEDATA_de, DELETEDATA_en, |
UPDATE1_de, UPDATE1_en, |
UPDATE2_de, UPDATE2_en, |
UPDATE3_de, UPDATE3_en, |
ENDSTART_de, ENDSTART_en, |
CONNECT13_de, CONNECT13_en, |
KABEL_de, KABEL_en, |
SLAVE_de, SLAVE_en, |
NORMAL_de, NORMAL_en, |
REVERSE_de, REVERSE_en, |
ENDOK_de, ENDOK_en, |
EEPROM1_de, EEPROM1_en, |
EEPROM2_de, EEPROM2_en, |
DEUTSCH_de, DEUTSCH_en, |
ENGLISCH_de, ENGLISCH_en, |
YES_de, YES_en, |
NOO_de, NOO_en, |
OSD_3D_V_de, OSD_3D_V_en, |
OSD_3D_H_de, OSD_3D_H_en, |
OSD_3D_L_de, OSD_3D_L_en, |
OSD_3D_R_de, OSD_3D_R_en, |
OSD_3D_NICK_de, OSD_3D_NICK_en, |
OSD_3D_ROLL_de, OSD_3D_ROLL_en, |
OSD_ERROR_de, OSD_ERROR_en, |
PARA_AKTIVI_de, PARA_AKTIVI_en, |
PARA_COPY_de, PARA_COPY_en, |
PARA_COPYQ_de, PARA_COPYQ_en, |
GPS2_de, GPS2_en, |
STATS_ITEM_0_de, STATS_ITEM_0_en, |
STATS_ITEM_1_de, STATS_ITEM_1_en, |
STATS_ITEM_2_de, STATS_ITEM_2_en, |
STATS_ITEM_3_de, STATS_ITEM_3_en, |
STATS_ITEM_4_de, STATS_ITEM_4_en, |
STATS_ITEM_5_de, STATS_ITEM_5_en, |
STATS_ITEM_6_de, STATS_ITEM_6_en, |
POTI_de, POTI_en, |
TASTER_de, TASTER_en, |
STAT_OSDBL_de, STAT_OSDBL_en, |
STAT_ERROR_de, STAT_ERROR_en, |
STAT_GPS_de, STAT_GPS_en, |
STAT_POS_de, STAT_POS_en, |
STAT_ALL_de, STAT_ALL_en, |
DELETE_de, DELETE_en, |
STR_OSDSCREEN_GENERAL_de, STR_OSDSCREEN_GENERAL_en, |
STR_OSDSCREEN_NAVIGATION_de, STR_OSDSCREEN_NAVIGATION_en, |
STR_OSDSCREEN_WAYPOINTS_de, STR_OSDSCREEN_WAYPOINTS_en, |
STR_OSDSCREEN_ELECTRIC_de, STR_OSDSCREEN_ELECTRIC_en, |
STR_OSDSCREEN_MKSTATUS_de, STR_OSDSCREEN_MKSTATUS_en, |
STR_OSDSCREEN_USERGPS_de, STR_OSDSCREEN_USERGPS_en, |
STR_OSDSCREEN_3DLAGE_de, STR_OSDSCREEN_3DLAGE_en, |
STR_OSDSCREEN_STATISTIC_de, STR_OSDSCREEN_STATISTIC_en, |
STR_OSDSCREEN_OSD0_de, STR_OSDSCREEN_OSD0_en, |
STR_OSDSCREEN_OSD1_de, STR_OSDSCREEN_OSD1_en, |
STR_OSDSCREEN_OSD2_de, STR_OSDSCREEN_OSD2_en, |
NOMKFOUND_de, NOMKFOUND_en, |
STR_ERROR_de, STR_ERROR_en, |
ERROR_NODATA_de, ERROR_NODATA_en, |
MSG_LOADSETTINGS_de, MSG_LOADSETTINGS_en, |
MSG_ACTIVATESETTING_de, MSG_ACTIVATESETTING_en, |
EDIT_SETTING_de, EDIT_SETTING_en, |
STR_VON_de, STR_VON_en, |
STR_NACH_de, STR_NACH_en, |
STR_PKT_de, STR_PKT_en, |
STR_ACTIVE_de, STR_ACTIVE_en, |
MODULE_EXIST_de, MODULE_EXIST_en, |
STR_INITIALIZE_de, STR_INITIALIZE_en, |
STR_LONGPRESS_de, STR_LONGPRESS_en, |
STR_FAV_ADD_de, STR_FAV_ADD_en, |
STR_FAV_FULL_de, STR_FAV_FULL_en, |
STR_FAV_EXIST_de, STR_FAV_EXIST_en, |
STR_FAV_NOTEXIST_de, STR_FAV_NOTEXIST_en, |
STR_MENUCTRL_DELITEM_de, STR_MENUCTRL_DELITEM_en, |
STR_MENUCTRL_DELASK_de, STR_MENUCTRL_DELASK_en, |
STR_MENUCTRL_NOTPOSSIBLE_de, STR_MENUCTRL_NOTPOSSIBLE_en, |
STR_HELP_PKTOFFTIME1_de, STR_HELP_PKTOFFTIME1_en, |
STR_HELP_LOWBAT1_de, STR_HELP_LOWBAT1_en, |
STR_HELP_LOWBAT2_de, STR_HELP_LOWBAT2_en, |
STR_HELP_LIPOOFFSET1_de, STR_HELP_LIPOOFFSET1_en, |
STR_HELP_LIPOOFFSET2_de, STR_HELP_LIPOOFFSET2_en, |
STR_DATA_de, STR_DATA_en, |
STR_READING_de, STR_READING_en, |
STR_LABELS_de, STR_LABELS_en, |
STR_EXTSV2MODULE_de, STR_EXTSV2MODULE_en, |
STR_ATTENTION_de, STR_ATTENTION_en, |
STR_SWITCHOFFMK_de, STR_SWITCHOFFMK_en, |
STR_WI232CONFIG_de, STR_WI232CONFIG_en, |
STR_USBCONNECTED_de, STR_USBCONNECTED_en, |
STR_SEEMKWIKI_de, STR_SEEMKWIKI_en, |
STR_PKT_SWITCHOFF_NOW_de, STR_PKT_SWITCHOFF_NOW_en, |
STR_PKT_RESTART_NOW_de, STR_PKT_RESTART_NOW_en, |
STR_WI232_ACTIVATE_de, STR_WI232_ACTIVATE_en, |
STR_WAITSATFIX_de, STR_WAITSATFIX_en, |
STR_GPSMOUSECONNECT_de, STR_GPSMOUSECONNECT_en, |
STR_GPSMOUSEDISCONNECT_de, STR_GPSMOUSEDISCONNECT_en, |
STR_NOBTM222_de, STR_NOBTM222_en, |
STR_NOGPSMOUSE_de, STR_NOGPSMOUSE_en, |
STR_GPSMOUSECONNECTION_de, STR_GPSMOUSECONNECTION_en, |
STR_SEARCH_BT_ASK_de, STR_SEARCH_BT_ASK_en, |
STR_SEARCH_BT_de, STR_SEARCH_BT_en, |
STR_NO_BT_FOUND_de, STR_NO_BT_FOUND_de, |
STR_BT_DEVICES_de, STR_BT_DEVICES_en, |
STR_BT_SEARCHTIME_de, STR_BT_SEARCHTIME_en, |
STR_BT_LOSTDATA_de, STR_BT_LOSTDATA_en, |
STR_METERS_de, STR_METERS_en, |
STR_WAITNMEA_de, STR_WAITNMEA_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
LAST_STR_de, LAST_STR_en, |
}; // end: strings |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache den entsprechenden String |
// aus der obigen Stringtabelle zureuck |
//-------------------------------------------------------------- |
const char * strGet( int str_no) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings[(str_no*2)+1] ); // Englisch (=1) |
return (const char*) pgm_read_word( &strings[str_no*2] ); // Deutsch (=0) |
} |
//-------------------------------------------------------------- |
// gibt je nach konfigurierter Sprache Parameter str_de oder |
// Parameter str_en zurueck |
// |
// wird z.B. in Verbindung mit de/en-Menuetexten verwendet |
//-------------------------------------------------------------- |
const char * strGetLanguage( char const *str_de, char const *str_en) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 1 ) return str_en; // Englisch (=1) |
return str_de; // Deutsch (=0) |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/messages.h |
---|
0,0 → 1,205 |
/**************************************************************************************** |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Languagesupport: * |
* http://www.netrino.com/Embedded-Systems/How-To/Firmware-Internationalization * |
* Nigel Jones * |
****************************************************************************************/ |
//############################################################################ |
//# HISTORY messages.h |
//# |
//# 08.08.2015 cebra |
//# - add: STR_WAITNMEA |
//# |
//# 27.06.2014 OG |
//# - add: STR_BT_SEARCHTIME, STR_METERS, STR_BT_LOSTDATA |
//# |
//# 25.06.2014 OG |
//# - add: STR_SEARCH_BT_ASK, STR_SEARCH_BT, STR_NO_BT_FOUND |
//# |
//# 24.06.2014 OG |
//# - add: STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION |
//# - add: STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT |
//# |
//# 23.06.2014 OG |
//# - add: STR_WAITSATFIX |
//# |
//# 13.06.2014 OG |
//# - add: STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE |
//# - del: mehrere CONNECT.. Strings |
//# - del: DISPLAY3 |
//# |
//# 11.06.2014 OG |
//# - add: strGetLanguage() |
//# - add: KEYLINE5 |
//# - del: TESTSTRING |
//# |
//# 10.06.2014 OG |
//# - add: STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI |
//# - add: STR_ATTENTION, STR_SWITCHOFFMK |
//# - del: CONNECT21, CONNECT22, CONNECT24, CONNECT25 |
//# |
//# 08.06.2014 OG |
//# - add: STR_EXTSV2MODULE |
//# |
//# 04.06.2014 OG |
//# - add: STR_DATA |
//# - add: STR_READING, STR_LABELS |
//# |
//# 01.06.2014 OG |
//# - del: weitere unbenoetigte Strings geloescht |
//# |
//# 31.05.2014 OG |
//# - add: STR_WITH, STR_WITHOUT |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 30.05.2014 OG |
//# - add: STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2 |
//# - add: STR_HELP_LOWBAT1, STR_HELP_LOWBAT2 |
//# |
//# 29.05.2014 OG |
//# - del: etliche Strings die nicht mehr benoetigt werden geloescht |
//# |
//# 28.05.2014 OG |
//# - add: STR_HELP_PKTLIGHTOFF1 |
//# |
//# 26.05.2014 OG |
//# - add: STR_OSDSCREEN_WAYPOINTS |
//# - del: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 06.05.2014 OG |
//# - add: STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE |
//# - del: STR_FAV_DELETE |
//# |
//# 03.05.2014 OG |
//# - add: STR_FAV_ADD, STR_FAV_DELETE, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST |
//# |
//# 28.04.2014 OG |
//# - add: STR_LONGPRESS |
//# |
//# 04.04.2014 OG |
//# - add: STR_SEARCH, STR_FOUND, STR_SET |
//# |
//# 03.04.2014 OG |
//# - add: STR_INITIALIZE |
//# |
//# 01.04.2014 OG |
//# - add: BLE_EXIST, MODULE_EXIST |
//# |
//# 30.03.2014 OG |
//# - chg: NUM_LANG von 3 auf 2 reduziert (Hollaendisch vollstaendig geloescht) |
//# |
//# 27.03.2014 OG |
//# - add: STR_SAVE, STR_DISCARD, STR_COPY, STR_SWITCHMOTOROFF, STR_SAVING |
//# |
//# 19.03.2014 OG |
//# - add: KEYCANCEL |
//# |
//# 27.02.2014 OG |
//# - add: STR_ACTIVE |
//# |
//# 20.02.2014 OG |
//# - add: STR_VON, STR_NACH |
//# |
//# 17.02.2014 OG |
//# - add: EDIT_SETTING |
//# - add: STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING |
//# |
//# 12.02.2014 OG |
//# - del: START_MSG2 |
//# - add:: NOMKFOUND |
//# |
//# 04.02.2014 OG |
//# - add: CHANGENORMREV1, CHANGENORMREV2 |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU |
//# |
//# 02.02.2014 OG |
//# - del: START_LASTPOSDEL |
//# |
//# 24.01.2014 OG |
//# - add: MSG_WARNUNG, UPDATE3 |
//# |
//# 15.07.2013 Cebra |
//# - add: Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screen Namen |
//# |
//# 02.07.2013 Cebra |
//# - add: Menuetexte fuer Wlan |
//# |
//# 27.06.2013 OG |
//# - del: OSD_HOMEMKVIEW |
//# |
//# 26.06.2013 OG |
//# - del: einige nicht mehr benoetigte Texte |
//# - chg: START_VERSIONCHECK5 zu MSG_FEHLER2 |
//# |
//# 15.06.2013 OG |
//# - add: OSD_MKSetting |
//# |
//# 13.06.2013 CB |
//# - add: ENUM Texte für OSD Statistik erweitert |
//# |
//############################################################################ |
#ifndef MESSAGES_H |
#define MESSAGES_H |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings, LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
KEYLINE1, KEYLINE2, KEYLINE3, KEYLINE4, KEYLINE5, KEYCANCEL, BOOT1, BOOT2, |
START_LASTPOS, START_LASTPOS1, START_LASTPOS2, START_LASTPOS3, |
START_SEARCHFC, ENDE, OK, FEHLER, AKTIV, STR_SAVE, STR_SAVING, STR_DISCARD, STR_COPY, STR_SEARCH, |
STR_FOUND, STR_SET, STR_WITH, STR_WITHOUT, STR_SWITCHMOTOROFF, |
ON, OFF, ESC, SHUTDOWN, YESNO, NOYES,DELETEDATA, UPDATE1, UPDATE2, UPDATE3, ENDSTART, CONNECT13, |
KABEL, SLAVE, NORMAL, REVERSE, ENDOK, EEPROM1, EEPROM2, DEUTSCH, ENGLISCH, |
YES, NOO, OSD_3D_V, OSD_3D_H, OSD_3D_L, |
OSD_3D_R, OSD_3D_NICK, OSD_3D_ROLL, OSD_ERROR, PARA_AKTIVI, PARA_COPY, PARA_COPYQ, |
GPS2, STATS_ITEM_0, STATS_ITEM_1, STATS_ITEM_2, STATS_ITEM_3, STATS_ITEM_4, STATS_ITEM_5, STATS_ITEM_6, |
POTI, TASTER, STAT_OSDBL, STAT_ERROR, STAT_GPS, STAT_POS, STAT_ALL, DELETE, |
STR_OSDSCREEN_GENERAL, STR_OSDSCREEN_NAVIGATION, STR_OSDSCREEN_WAYPOINTS, STR_OSDSCREEN_ELECTRIC, STR_OSDSCREEN_MKSTATUS, STR_OSDSCREEN_USERGPS, STR_OSDSCREEN_3DLAGE, |
STR_OSDSCREEN_STATISTIC, STR_OSDSCREEN_OSD0, STR_OSDSCREEN_OSD1, STR_OSDSCREEN_OSD2, NOMKFOUND, |
STR_ERROR, ERROR_NODATA, MSG_LOADSETTINGS, MSG_ACTIVATESETTING, EDIT_SETTING, |
STR_VON, STR_NACH, STR_PKT, STR_ACTIVE, MODULE_EXIST, STR_INITIALIZE, STR_LONGPRESS, |
STR_FAV_ADD, STR_FAV_FULL, STR_FAV_EXIST, STR_FAV_NOTEXIST, STR_MENUCTRL_DELITEM, STR_MENUCTRL_DELASK, STR_MENUCTRL_NOTPOSSIBLE, |
STR_HELP_PKTOFFTIME1, STR_HELP_LOWBAT1, STR_HELP_LOWBAT2, STR_HELP_LIPOOFFSET1, STR_HELP_LIPOOFFSET2, |
STR_DATA, STR_READING, STR_LABELS, STR_EXTSV2MODULE, STR_ATTENTION, STR_SWITCHOFFMK, STR_WI232CONFIG, STR_USBCONNECTED, STR_SEEMKWIKI, |
STR_PKT_SWITCHOFF_NOW, STR_PKT_RESTART_NOW, STR_WI232_ACTIVATE, |
STR_WAITSATFIX, STR_GPSMOUSECONNECT, STR_GPSMOUSEDISCONNECT, STR_NOBTM222, STR_NOGPSMOUSE, STR_GPSMOUSECONNECTION, STR_SEARCH_BT_ASK, |
STR_SEARCH_BT, STR_NO_BT_FOUND, STR_BT_DEVICES, STR_BT_SEARCHTIME, STR_BT_LOSTDATA, STR_METERS,STR_WAITNMEA, |
LAST_STR |
} STR; |
#define NUM_LANG 2 // German, English |
const char * strGet( int str_no); |
const char * strGetLanguage( char const *str_de, char const *str_en); |
void Test_Language (void); // bleibt für Tests |
#endif /* _MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkbase.c |
---|
0,0 → 1,825 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.c |
//# |
//# 21.06.2014 OG |
//# - chg: searchMK() schaltet am Ende wieder auf die NC um sofern diese |
//# vorhanden ist |
//# |
//# 19.05.2014 OG |
//# - chg: MKVersion_Setting_print() umgestellt auf strGet(STR_ERROR) fuer |
//# Multi-Sprachenunterstuetzung |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 13.05.2014 OG |
//# - chg: MK_Info() - del: MKINFO_AUTO_REFRESH (nicht mehr unterstuetzt) |
//# - chg: MK_Setting_write() - del: unused variable 'lsetting' |
//# |
//# 17.04.2014 OG |
//# - chg: MK_Setting_load - Ansprechverhalten verbessert wenn Windows |
//# Mikrokoptertool aktiv ist |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 24.03.2014 OG |
//# - chg: searchMK(): doppeltes MK_SwitchToNC() um evtl. vorhandene NC |
//# zuverlaessiger zu erkennen |
//# - fix: searchMK(): RetVal initialisiert |
//# - chg: Default-Anzeigezeit von MK_Info() in searchMK() auf 5 Sek. reduziert |
//# |
//# 21.02.2014 OG |
//# - fix: MKVersion_Setting_print() Anzeige der Settings-Nummer gekuerzt |
//# |
//# 20.02.2014 OG |
//#- chg: searchMK() auf aktuelle Version von PKT_TitlePKTVersion() angepasst |
//# |
//# 20.02.2014 OG |
//# - chg: MKVersion_Setting_print() um ein paar Bytes zu sparen |
//# - chg: MK_Info() um ein paar Bytes zu sparen |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() (Anleitung bei der Funktion beachten!) |
//# - del: WrongFCVersion |
//# |
//# 12.02.2014 OG |
//# - chg: verschiedene Verbesserungen an MK_Info(), MK_load_settings(), searckMK() |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# |
//# 08.02.2014 OG |
//# - chg: searckMK() Anpassung an MKVersion |
//# - chg: MK_load_setting() Anpassung an MKVersion und Parameter geaendert |
//# - add: MKVersion_Init(), MKVersion_print_at() |
//# |
//# 03.02.2014 OG |
//# - chg: Titelanzeige in searchMK() umgestellt auf PKT_ShowTitlePKTVersion() |
//# - chg: kleine Aenderung in der Anzeige "für FC..." in searchMK() |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c: MK_load_setting(), searchMK() |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include <avr/wdt.h> |
#include <avr/eeprom.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "mkbase.h" |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
MKVersion_t MKVersion; |
uint8_t cmd; |
//############################################################################################# |
//# 1. MKVersion |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKVersion_Init() |
// |
// initialisiert MKVersion auf 0 |
//-------------------------------------------------------------- |
void MKVersion_Init( void ) |
{ |
memset( &MKVersion, 0, sizeof(MKVersion_t) ); |
} |
//-------------------------------------------------------------- |
// value = MKVersion_Cmp( FC/NCver, SWMajor,SWMinor,SWPatch ) |
// |
// Vergleicht eine uebergebene FC/NC-Version mit der vom MK |
// ermittelten Version |
// |
// Parameter: |
// |
// FC/NCver: MKVersion.FCVer oder MKVersion.NCVer |
// |
// SWMajor,SWMinor,SWPatch: siehe nachfolgendes... |
// |
//--- |
// Beispiel aus osd.c/OSD_MK_GetData(): |
// |
// v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version "0.30g" |
// |
// if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
// { ... |
// |
// Sowas geht auch: |
// |
// if( v && (v == GIVEN_VERSION) ) // nur wenn aktuelle NC-Version == "0.30g"... |
// |
// oder: |
// |
// if( v && (v < GIVEN_VERSION) ) // nur wenn aktuelle NC-Version kleiner als "0.30g"... |
// |
//--- |
// Warum "if( v && ..." ? |
// |
// Wenn das PKT keine FC/NC-Version ermitteln konnte, und somit kein |
// Vergleich moeglich ist, gibt MKVersion_Cmp() false (=0) zurueck! |
//-------------------------------------------------------------- |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ) |
{ |
SWPatch = SWPatch -'a'; |
if( ver.SWMajor > 0 || ver.SWMinor > 0 ) // NC/FC Version erkannt? |
{ |
if( (ver.SWMajor == SWMajor) && (ver.SWMinor == SWMinor) && (ver.SWPatch == SWPatch) ) |
return VERSION_EQUAL; // ==2 |
if( ver.SWMajor != SWMajor ) |
{ |
if( ver.SWMajor < SWMajor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWMinor != SWMinor ) |
{ |
if( ver.SWMinor < SWMinor ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
if( ver.SWPatch < SWPatch ) |
return VERSION_LOWER; // ==1 |
else |
return VERSION_GREATER; // ==3 |
} |
return VERSION_NO; // ==0 |
} |
//-------------------------------------------------------------- |
// MKVersion_print_at() |
// |
// zeigt die aktuelle FC, NC Version auf dem LCD an |
// |
// Parameter: |
// what: NC, FC, |
//-------------------------------------------------------------- |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ) |
{ |
const char *mask = PSTR("%1u.%02u%c"); |
const char *nostr = PSTR("-----"); |
switch(what) |
{ |
case FC: if( MKVersion.FCVer.SWMajor > 0 || MKVersion.FCVer.SWMinor > 0 ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, mask, MKVersion.FCVer.SWMajor, MKVersion.FCVer.SWMinor, MKVersion.FCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
case NC: if( (MKVersion.NCVer.SWMajor != 0) || (MKVersion.NCVer.SWMinor != 0) ) |
lcdx_printf_at_P( x, y, drawmode, xoffs, yoffs, PSTR("%1u.%02u%c"), MKVersion.NCVer.SWMajor, MKVersion.NCVer.SWMinor, MKVersion.NCVer.SWPatch+'a' ); |
else |
lcdx_printp_at( x,y, nostr, drawmode, xoffs, yoffs); |
break; |
} |
} |
//-------------------------------------------------------------- |
// MKVersion_Setting_print( y, drawmode, xoffs,yoffs) |
// |
// Zeigt das aktuelle MK-Setting aus MKVersion zentriert in der |
// Zeile y auf dem PKT-Screen an |
//-------------------------------------------------------------- |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ) |
{ |
char buffer[18]; // String Buffer fuer eine Zeile |
if( MKVersion.mksetting == 0 ) |
xsnprintf_P( buffer, 18, PSTR("* %S *"), strGet(STR_ERROR) ); // FC Setting konnte NICHT gelesen werden |
else if( MKVersion.mksetting != 0 && MKVersion.paramsetOK ) // FC-Setting wurde gelesen... |
xsnprintf_P( buffer, 18, PSTR("%u %s"), MKVersion.mksetting, MKVersion.mksettingName ); // ... passende FC-Reversion -> Nummer und Name anzeigen |
else |
xsnprintf_P( buffer, 18, PSTR("%u"), MKVersion.mksetting ); // ... aber FALSCHE FC-Version -> nicht den Namen anzeigen (nur die Nummer) |
lcdx_print_center( y, (unsigned char *)buffer, drawmode, xoffs,yoffs); // Ausgabe Setting-Name (zentriert) |
} |
//############################################################################################# |
//# 2. Sonstiges - MK-Suche, Settings laden, MK-Info |
//############################################################################################# |
#define SWITCH_DELAY 25 // alt 50 |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToNC( void ) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToFC( void ) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToMAG( void ) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_SwitchToGPS( void ) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms( SWITCH_DELAY ); |
} |
//-------------------------------------------------------------- |
// ret = MK_Setting_load( lsetting, &mk_param_struct, showerror) |
// |
// holt MK-Settings - es wird auf die FC umgeschaltet! |
//-------------------------------------------------------------- |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ) |
{ |
// timeout = 15 bis 20 ist meist ein guter Wert |
//MK_SwitchToFC(); |
mode = 'Q'; // Settings Request |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout ) |
{ |
MK_SwitchToFC(); |
SendOutData( 'q', ADDRESS_FC, 1, &lsetting, 1); // lsetting == 0xff -> aktuelles MK-Setting laden |
timer2 = 25; // timer ist ggf. belegt von MK_Info() -> versuchen wir es mit timer2.... |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
} |
mode = 0; |
// Initialisierungen |
paramsetInit(0); |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
paramsetInit( (unsigned char *) pRxData ); // fuellt u.a. auch MKVersion.paramsetRevision |
} |
return MKVersion.mksetting; // wird von paramsetInit() gesetzt/initialisert |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout) |
{ |
//uint8_t timeout = 50; |
uint8_t setting; |
if( !MKVersion.paramsetOK ) |
{ |
return 0; // Error |
} |
//MK_SwitchToFC(); // ? |
setting = 0; |
mode = 'S'; // Settings |
rxd_buffer_locked = FALSE; |
while( !rxd_buffer_locked && timeout && !setting) |
{ |
SendOutData( 's', ADDRESS_FC, 2, &wsetting, 1, (pRxData+1), paramsetSize() ); // pRxData+1 = die mk_datastruct beginnt bei +1; bei 0 ist die Settingsnummer |
timer2 = 70; |
while( timer2 > 0 && !rxd_buffer_locked ); |
timeout--; |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
setting = *pRxData; |
if( !setting ) |
rxd_buffer_locked = FALSE; |
} |
} |
setting = MK_Setting_load( 0xff, 15); // damit pRxData & MKVersion initialisiert sind |
return setting; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MK_Setting_change( uint8_t setting ) |
{ |
uint8_t lsetting = 0; |
uint8_t wsetting = 0; |
lsetting = MK_Setting_load( setting, 20); |
if( lsetting == setting ) |
{ |
wsetting = MK_Setting_write( setting, 50); |
} |
/* |
//-------------------- |
// DEBUG... |
//-------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("load : %1u (req:%1u)"), lsetting, setting ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("write: %1u "), wsetting ); |
if( (setting != lsetting) || (setting != wsetting) ) |
set_beep( 1000, 0x0f0f, BeepNormal ); // Error |
timer2 = 500; while( timer2 > 0 ); |
*/ |
return wsetting; // 0 = Fehler; oder 1..5 = geschriebenes Setting |
} |
//-------------------------------------------------------------- |
// ok = searchMK( showMKInfo ) |
// |
// Parameter: |
// showMKInfo: true/false |
//-------------------------------------------------------------- |
uint8_t searchMK( uint8_t showMKInfo ) |
{ |
uint8_t timeout; |
//uint8_t setting; // aktuelles FC-Setting (1..5) |
uint8_t RetVal = false; |
uint8_t redraw = true; |
uint8_t searchbar_y; |
uint8_t searchbar_w = 0; |
MKVersion_Init(); |
//---------------- |
// Switch to NC |
//---------------- |
MK_SwitchToNC(); |
_delay_ms(50); // 24.03.2014 OG: manchmal wurde nicht auf eine vorhandene NC umgeschaltet |
MK_SwitchToNC(); // evtl. wird das durch doppeltes Senden verbessert |
//--------------------------------------- |
// MK-Suchscreen - Versionsabfrage NC |
//--------------------------------------- |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 50; |
//searchbar_y = 5*8-1; |
searchbar_y = 5*8+2; |
while( !rxd_buffer_locked ) |
{ |
//------------------------ |
// MK Suchscreen anzeigen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (mit Lipo-Anzeige) |
lcdx_printp_center( 2, strGet(STR_PKT), MNORMAL, 0,2); // "PKT" |
lcdx_printp_at( 0, 3, strGet(START_SEARCHFC), MNORMAL, 0,8); // "suche Mikrokopter..." |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
lcd_rect ( 0, searchbar_y , 126, 8, 1); // Rahmen fuer Bar-Anzeige |
searchbar_w = 2; |
redraw = false; |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
//------------------------ |
// MK Datenanfrage |
//------------------------ |
SendOutData( 'v', ADDRESS_ANY, 0); // Versions-Anfrage an MK senden |
timer = 16; // kurze Verögerung fuer Datenanfrage und Fortschrittsbalken |
while( timer > 0 ); |
//------------------------ |
// Fortschrittsbalken |
//------------------------ |
lcd_frect( 2, searchbar_y+2, searchbar_w, 4, 1); // Fortschrittsbalken zeichnen... |
searchbar_w += 2; |
if( searchbar_w >= 125 ) |
{ |
searchbar_w = 0; |
lcd_frect( 2, searchbar_y+2, 123, 4, 0); |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
hardware = NO; |
return false; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
} |
//-------------------------- |
// MK gefunden? |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
//--- |
// TODO OG: testen mit entsprechendem Hardware Setup des MK's! |
//--- |
if( (rxd_buffer[1] - 'a') == ADDRESS_FC ) |
{ |
hardware = FC; |
current_hardware = hardware; |
MKVersion.isFC = true; |
RetVal = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
if( (rxd_buffer[1] - 'a') == ADDRESS_NC ) |
{ |
hardware = NC; |
current_hardware = hardware; |
MKVersion.isNC = true; |
RetVal = true; |
memcpy( &MKVersion.NCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
//-------------------------- |
// jetzt: Version der FC abfragen |
//-------------------------- |
if( hardware == NC ) |
{ |
MK_SwitchToFC(); |
mode = 'V'; |
rxd_buffer_locked = FALSE; |
timeout = 40; |
while( !rxd_buffer_locked && timeout) |
{ |
SendOutData( 'v', ADDRESS_FC, 0); |
timer = 20; |
while( timer > 0 ); |
timeout--; |
} |
//-------------------------- |
// FC gefunden! |
// -> Version kopieren |
//-------------------------- |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
MKVersion.isFC = true; |
memcpy( &MKVersion.FCVer, (Version_t *) (pRxData), sizeof(Version_t) ); |
} |
} |
} // end: if( rxd_buffer_locked ) |
//--------------------------------------- |
// FC EEprom Version / Struktur pruefen |
//--------------------------------------- |
MK_Setting_load( 0xff, 15 ); // 0xff == aktuelles Parameterset holen; 15 == timeout; Ergebnisse in MKVersion |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( MKVersion.isNC ) |
{ |
MK_SwitchToNC(); // wieder auf NC umschalten als default |
} |
if( MKVersion.paramsetOK ) // FC Revision OK? |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep (FC-Revision ok) |
if( showMKInfo ) MK_Info( 500, true ); // Anzeige von MK_Info() max. 5 Sekunden (mit Settings-Refresh) |
} |
else // FC Revision nicht unterstuetzt... |
{ |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep (FC-Revision nicht unterstuetzt) |
if( showMKInfo ) MK_Info( 1600, true ); // Anzeige von MK_Info() max. 16 Sekunden (mit Settings-Refresh) |
} |
//timer = 50; |
//while (timer > 0); |
get_key_press(KEY_ALL); |
return RetVal; |
} // searchMK() |
//-------------------------------------------------------------- |
// chg = MK_Info( displaytimeout, refreshSettings ) |
// |
// Parameter: |
// displaytimeout : 0 = kein autom. Timeout |
// n = Anzeige verschwindet automatisch nach n Zeit |
// (bei z.B. n=600 nach 6 Sekunden) |
// |
// refreshSettings: true/false |
// true = in bestimmten Zeitintervallen (RELOAD_SETTING_TIME) |
// wird MK_Setting_load() durchgeführt um ggf. einen Kopterwechsel |
// automatisch zu erkennen. |
// Dazu muss MKINFO_AUTO_REFRESH in main.h eingeschaltet sein! |
// |
// Rueckgabe: |
// hardwarechange: true/false |
//-------------------------------------------------------------- |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ) |
{ |
uint8_t redraw; |
uint8_t old_isFC; |
uint8_t old_isNC; |
old_isFC = MKVersion.isFC; |
old_isNC = MKVersion.isNC; |
//displaytimeout = 0; |
if( displaytimeout ) |
timer = displaytimeout; |
#define RELOAD_SETTING_TIME 350 // alle 0.35 sec neue Settings vom MK laden |
timer_get_tidata = RELOAD_SETTING_TIME; // ich brauche weitere timer... und bediene mich jetzt mal bei einem osd-timer |
// (timer2 ist belegt von MK_Setting_load) |
//get_key_press(KEY_ALL); |
redraw = true; |
while( true ) |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw != 2 ) |
lcd_cls(); |
show_Lipo(); // LiPo direkt anzeigen damit es nicht flackert |
lcd_frect( 0, 0, 101, 8, 1); // Headline: Box fill Black mit Platz fuer PKT-LiPo rechts |
lcdx_printp_at( 1, 0, PSTR("MK Info"), MINVERS, 0,0); // Titel |
if( MKVersion.isFC || MKVersion.isNC ) // MK gefunden... |
{ |
lcd_frect_round( 0, 30, 127, 22, 1, R2); // Fill: Anzeigebereich der Settings in Schwarz |
//-------------------- |
// FC-Version |
//-------------------- |
lcdx_printp_at( 0, 2, PSTR("FC:") , MNORMAL, 4,-5); |
MKVersion_print_at( 4, 2, FC , MNORMAL, 2,-5); |
//-------------------- |
// FC-Revision |
//-------------------- |
lcdx_printp_at( 12, 2, PSTR("Rev:"), MNORMAL, 5,-5); |
lcdx_printf_at_P( 17, 2, MNORMAL, 2,-5, PSTR("%03u"), MKVersion.paramsetRevision ); |
if( !MKVersion.paramsetOK ) |
lcdx_printp_at( 11, 3, PSTR(" Rev-ERR!"), MINVERS, 5,-4); // Fehler FC-Revision (nicht vom PKT unterstuetzt) |
else |
lcd_frect( 11*6+5, 3*8-4, 9*6, 8, 0); // ggf. Fehleranzeige loeschen (nach refreshSettings) |
//-------------------- |
// NC-Version |
//-------------------- |
lcdx_printp_at( 0, 3, PSTR("NC:") , MNORMAL, 4,-3); |
MKVersion_print_at( 4, 3, NC , MNORMAL, 2,-3); |
//-------------------- |
// aktuelles Setting |
//-------------------- |
lcdx_printp_center( 4, PSTR("MK Setting"), MINVERS, 0,1); // "MK Setting" (zentriert) |
//lcdx_printp_at( 5, 4, PSTR("MK Setting"), MINVERS, 4,1); |
MKVersion_Setting_print( 5, MINVERS, 0,3); |
} |
else // else: if( MKVersion.isFC || MKVersion.isNC ) // KEINEN MK gefunden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(NOMKFOUND), false, 0, true, false ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
} // end: if( MKVersion.isFC || MKVersion.isNC ) |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
if( displaytimeout == 0 ) |
lcd_printp_at( 0, 7, PSTR("Refresh"), MNORMAL); // Keyline: Refresh |
redraw = false; |
} // end: if( redraw ) |
//------------------------------------------ |
// PKT-LiPo Anzeige |
//------------------------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_MINUS) && (displaytimeout == 0) ) |
{ |
searchMK( false ); // false = zeige nicht MK_Info() an |
redraw = true; |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
break; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
//------------------------------------------ |
// displaytimeout? |
//------------------------------------------ |
if( displaytimeout && !timer ) |
{ |
break; |
} |
} // end: while( true ) |
get_key_press(KEY_ALL); |
return( (old_isFC != MKVersion.isFC) || (old_isNC != MKVersion.isNC) ); |
} // MK_Info() |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkbase.h |
---|
0,0 → 1,143 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkbase.h |
//# |
//# 14.05.2014 OG |
//# - chg: include "paramset.h" geaendert auf "../mksettings/paramset.h" |
//# |
//# 29.03.2014 OG |
//# - del: MK_Show_LastGPS_Position() -> jetzt: OSDDATA_ShowLastGPSPosition()/osddata.c |
//# |
//# 28.03.2014 OG |
//# - add: MK_Show_LastGPS_Position() - ehemals in main.c |
//# |
//# 16.02.2014 OG |
//# - add: MK_SwitchToNC(), MK_SwitchToFC(), MK_SwitchToMAG(), MK_SwitchToGPS() |
//# - add: MK_Setting_write(), MK_Setting_change() |
//# - chg: umbenannt: MK_load_setting() zu MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - add: MKVersion_Cmp() |
//# - add: defines zu VERSION... fuer MKVersion_Cmp() |
//# - del: WrongFCVersion |
//# |
//# 10.02.2014 OG |
//# - add: MKVersion_Setting_print() |
//# |
//# 09.02.2014 OG |
//# - add: MK_Info() |
//# - add: MKVersion_print_at |
//# |
//# 08.02.2014 OG |
//# - chg: MK_load_setting() Parameter geaendert |
//# - add: extern MKVersion_t MKVersion |
//# |
//# 29.01.2014 OG |
//# - Ausgliederungen aus main.c |
//############################################################################ |
#ifndef _MKBASE_H |
#define _MKBASE_H |
//#include "../mksettings/paramset.h" |
#include "../mk-data-structs.h" |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char isFC; // true / false - FC vorhanden? -> wird gesetzt durch searchMK() |
unsigned char isNC; // true / false - NC vorhanden? -> wird gesetzt durch searchMK() |
Version_t FCVer; // -> wird gesetzt durch searchMK() |
Version_t NCVer; // -> wird gesetzt durch searchMK() |
unsigned char paramsetOK; // true wenn Revision in paramset.c vorhanden und initialisiert -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char paramsetRevision; // Revision FC-Parameterset -> wird gesetzt druch paramsetInit()/paramset.c |
uint8_t mksetting; // -> wird gesetzt druch paramsetInit()/paramset.c |
unsigned char mksettingName[13]; // -> wird gesetzt druch paramsetInit()/paramset.c |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
//---------------------------- |
// MK-Versionsinformationen |
// global verfuegbar |
//---------------------------- |
extern MKVersion_t MKVersion; |
//------------------------------------------------------ |
// defines fuer den Versionsvergleich von FC/NC |
// siehe: MKVersion_Cmp()... (mkbase.c) |
//------------------------------------------------------ |
#define GIVEN_VERSION 2 // das macht die Sache leserlicher fuer resultierenden Ergebnisse! => siehe Anmerkungen: MKVersion_Cmp() ! |
#define VERSION_NO 0 |
#define VERSION_LOWER 1 |
#define VERSION_EQUAL 2 |
#define VERSION_GREATER 3 |
void MKVersion_Init( void ); |
void MKVersion_print_at( uint8_t x, uint8_t y, uint8_t what, uint8_t drawmode, int8_t xoffs, int8_t yoffs ); |
void MKVersion_Setting_print( uint8_t y, uint8_t drawmode, uint8_t xoffs, uint8_t yoffs ); |
uint8_t MKVersion_Cmp( Version_t ver, unsigned char SWMajor, unsigned char SWMinor, unsigned char SWPatch ); |
void MK_SwitchToNC( void ); |
void MK_SwitchToFC( void ); |
void MK_SwitchToMAG( void ); |
void MK_SwitchToGPS( void ); |
uint8_t MK_Setting_load( uint8_t lsetting, uint8_t timeout ); |
uint8_t MK_Setting_write( uint8_t wsetting, uint8_t timeout); |
uint8_t MK_Setting_change( uint8_t setting ); |
uint8_t MK_Info( uint16_t displaytimeout, uint8_t refreshSettings ); |
uint8_t searchMK( uint8_t showMKInfo ); |
#endif // end: #ifndef _MKBASE_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkdebugdata.c |
---|
0,0 → 1,416 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.c |
//# |
//# 04.06.2014 OG |
//# komplett neues Layout |
//# - chg: MK_DebugData(), GetAnalogNames() umgestellt auf PKT_Message_Datenverlust() |
//# - chg: GetAnalogNames() Timeout reduziert |
//# - add: #include "../lipo/lipo.h" |
//# - add: #include "../pkt/pkt.h" |
//# - add: #include <stdbool.h> |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.c' jetzt 'mk/mkdebugdata.c' |
//# |
//# 03.04.2013 OG |
//# - chg: define 'analognames' zu define 'USE_MKDEBUGDATA' |
//# - add: Benutzer kann GetAnalogNames() via KEY_ESC abbrechen |
//# - chg: Screen-Layout GetAnalogNames() |
//# - chg: SwitchToNC/FC Reihenfolge |
//# |
//# 27.03.2013 OG |
//# - chg: auf malloc umgestellt |
//# - fix: diverse Anzeigefehler |
//# - chg: teilweise redunten Code entfernt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include <string.h> |
#include "../main.h" |
#ifdef USE_MKDEBUGDATA |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../uart/usart.h" |
#include "mkdebugdata.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
//-------------------------------------------------------------- |
#define PAGELINES 5 // Anzahl der Zeilen pro Anzeigescreens |
#define XOFFS 4 |
#define YOFFS 12 |
#define TIMEOUT 200 // 2 sec |
#define ANALOGTIME 20 // 200 ms |
#define MKDD_ANALOGNAME_SIZE 2*32*17 // MALLOC: 32 names, 16 characters + 1 0x00 |
// WARNING: this work for NC & FC only |
// if current_hardware == MK3MAG or MKGPS the access is outside of the array... |
//uint8_t AnalogNames[2][32][16 + 1]; // 32 names, 16 characters + 1 0x00 |
uint8_t *AnalogNames; // MALLOC: MKDD_ANALOGNAME_SIZE - 32 names, 16 characters + 1 0x00 |
uint8_t AnalogNamesRead[2] = {0,0}; |
static const char strFC[] PROGMEM = "FC"; |
static const char strNC[] PROGMEM = "NC"; |
//-------------------------------------------------------------- |
// Speicher wieder freigeben |
//-------------------------------------------------------------- |
void MKDD_MemFree( void ) |
{ |
free( AnalogNames ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showTitle( void ) |
{ |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0, PSTR(" %2S %16S"), (current_hardware==FC ? strFC : strNC), strGet(STR_DATA) ); // Titelzeile |
show_Lipo(); |
} |
//-------------------------------------------------------------- |
// lOk = GetAnalogNames() |
// |
// Return: |
// true = OK |
// false = User-Abort (KEY_ESC) |
//-------------------------------------------------------------- |
uint8_t GetAnalogNames( void ) |
{ |
uint8_t i = AnalogNamesRead[current_hardware - 1]; |
uint8_t t = 0; |
uint8_t lOk = TRUE; |
int8_t yoffs = -2; // yoffs der Progress-Bar |
lcd_cls (); |
showTitle(); // Titel |
lcdx_printf_center_P( 3, MNORMAL, 0,0, PSTR("%S %S %S"), strGet(STR_READING), (current_hardware==FC ? strFC : strNC), strGet(STR_LABELS) ); |
lcd_rect_round( 13, 40+yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
mode = 'A'; // read Names |
_delay_ms(200); |
rxd_buffer_locked = FALSE; |
timer = ANALOGTIME; |
while( (i < 32) && lOk ) |
{ |
SendOutData ('a', ADDRESS_ANY, 1, &i, 1); |
while( !rxd_buffer_locked && timer && lOk) |
lOk = !get_key_press (1 << KEY_ESC); |
if( timer ) |
{ |
Decode64 (); |
if( i == *pRxData ) |
{ |
lcd_frect( 16, 42+yoffs, (i+1)*3, 2, 1); // Progress |
memcpy( (uint8_t *) (AnalogNames+((current_hardware-1)*32*17)+(i*17)), (uint8_t *) pRxData + 1, 16); |
*(AnalogNames+((current_hardware-1)*32*17)+(i*17)+16) = 0; |
i++; |
t = 0; |
} |
else |
{ |
_delay_ms(100); |
} |
timer = ANALOGTIME; |
rxd_buffer_locked = FALSE; |
} |
else if( lOk ) |
{ // timeout occured |
t++; |
timer = ANALOGTIME; |
if( t >= 15 ) // timeout? |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
break; |
} |
} |
else |
{ |
lOk = !get_key_press( 1 << KEY_ESC ); |
} |
} |
if(lOk) AnalogNamesRead[current_hardware - 1] = i; |
//_delay_ms(4000); // DEBUG |
return lOk; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void showLabels( uint8_t page) |
{ |
uint8_t i = 0; |
for( i = 0; i < PAGELINES; i++ ) |
{ |
if( (i + (page * PAGELINES)) >= 32 ) |
{ |
//lcdx_cls_rowwidth( y, width, mode, xoffs, yoffs ) |
lcdx_cls_rowwidth( i, 20, MNORMAL, XOFFS,YOFFS ); |
} |
else |
{ |
lcdx_print_at( 0, i, AnalogNames+((current_hardware-1)*32*17)+(i + page * PAGELINES)*17, MNORMAL, XOFFS,YOFFS); |
} |
} |
} |
//-------------------------------------------------------------- |
// |
//-------------------------------------------------------------- |
void MK_DebugData(void) |
{ |
uint8_t tmp_dat; |
uint8_t i = 0; |
uint8_t page = 0; |
uint8_t pagemax = (32/PAGELINES); |
uint8_t redraw = 2; |
DebugData_t *DebugData; |
// alloc ram |
AnalogNames = malloc( MKDD_ANALOGNAME_SIZE ); |
if( !AnalogNames ) |
{ |
Show_PKTError_NoRAM(); |
return; |
} |
memset( AnalogNames, 0, MKDD_ANALOGNAME_SIZE ); // init: AnalogNames |
AnalogNamesRead[0] = 0; |
AnalogNamesRead[1] = 0; |
SwitchToFC(); |
timer = TIMEOUT; |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
if( !timer ) |
{ |
MKDD_MemFree(); |
return; |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
timer1 = 0; |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//------------------------------------------ |
//------------------------------------------ |
if( redraw ) |
{ |
if( redraw==2 ) lcd_cls(); |
showTitle(); // Titelzeile |
lcd_rect_round( 0, (1*8)+1, 127, (6*8)-4, 1, R2); // Rahmen fuer 5 Zeilen Display |
showLabels( page ); |
lcd_printp_at( 0, 7, strGet(KEYLINE3), 0); |
lcd_write_number_u_at (5, 7, page + 1); |
lcd_printp_at( (current_hardware==FC ? 3 : 19), 7, strFC, 0); |
lcd_printp_at( (current_hardware==FC ? 19 : 3), 7, strNC, 0); |
redraw = false; |
} |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
DebugData = (DebugData_t *) pRxData; |
if( !timer1 ) |
{ |
for( i = 0; i < PAGELINES && (i + (page * PAGELINES)) < 32; i++) |
{ |
writex_ndigit_number_s( 21-6, i, DebugData->Analog[i + page * PAGELINES], 5, 0, MNORMAL, XOFFS,YOFFS); |
} |
timer1 = 25; // Anzeigeverzoegerung damit es nicht flackert |
} |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} |
if( !abo_timer ) |
{ // renew abo every ... sec |
tmp_dat = 10; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
//------------------------------------------ |
// PKT LiPo anzeigen |
//------------------------------------------ |
show_Lipo(); |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = 2; |
timer = TIMEOUT; |
} |
//------------------------------------------ |
// Tasten |
//------------------------------------------ |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
page--; |
if( page > pagemax ) page = pagemax; |
redraw = true; |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
page++; |
if( page > pagemax ) page = 0; |
redraw = true; |
} |
if( (hardware == NC) && get_key_press (1 << KEY_ENTER) ) |
{ |
if( current_hardware == NC ) SwitchToFC(); |
else SwitchToNC(); |
if( AnalogNamesRead[current_hardware - 1] < 32 ) |
{ |
if( !GetAnalogNames() ) |
{ |
MKDD_MemFree(); |
return; |
} |
} |
mode = 'D'; // Debug Data |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
tmp_dat = 10; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//page = 0; |
redraw = true; |
} |
} |
while (!get_key_press(1 << KEY_ESC) && timer); // Taste: Ende oder Timeout |
clear_key_all(); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); // Request Debugdata abschalten |
mode = 0; |
rxd_buffer_locked = FALSE; |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
//SwitchToNC(); |
// free ram |
MKDD_MemFree(); |
} |
#endif // end: #ifdef USE_MKDEBUGDATA |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkdebugdata.h |
---|
0,0 → 1,51 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdebugdata.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_debug() jetzt MK_DebugData() |
//# - chg: ehemals 'debug.h' jetzt 'mk/mkdebugdata.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _MKDEBUGDATA_H |
#define _MKDEBUGDATA_H |
extern uint8_t AnalogNamesRead[2]; |
void MK_DebugData(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkdisplay.c |
---|
0,0 → 1,267 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.c |
//# |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Display() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - chg: MK_Display() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 28.04.2014 OG |
//# - chg: MK_Display() - leichte Optimierung der Display-Ausgabe um ein paar |
//# Bytes Code zu sparen |
//# |
//# 09.04.2014 OG - vervollstaendigte Steuerung des MK-Display's |
//# - chg: die Tasten fuer 'Ende' und NC/FC-Umschaltung muessen jetzt lange |
//# gedruekckt werden (get_key_long) - keine Beschriftung mehr dafuer |
//# vorhanden da die Beschriftung jetzt hoch/runter zeigt! |
//# (aber 'Ende' ist da wo es immer im PKT ist) |
//# - add: Unterstuetzung der hoch/runter Tasten |
//# - chg: diverse Aenderungen bei der Kommunkation und Einbindung aktueller |
//# Funktionen von mkbase.c |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.c' jetzt 'mk/mkdisplay.c' |
//# |
//# 25.01.2014 OG |
//# - optisches Facelift |
//# - PKT_CtrlHook() eingeklinkt |
//# - Versionshistorie hinzugefuegt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk-data-structs.h" |
#include "mkbase.h" |
#include "mkdisplay.h" |
#define TIMEOUT 500 // 5 sec |
uint8_t ACC_Display=false; |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Display(void) |
{ |
uint8_t cmd; |
uint8_t redraw = true; |
uint8_t exit = false; |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
// cmd = 0xfc; // Home = first page |
cmd = 0xff; // aktuelle Seite |
redraw = true; |
MK_SwitchToFC(); // add 31.3.2014 Cebra, definierter Zustand |
if (ACC_Display==true) |
{ |
cmd = 0x06; |
SendOutData( 'l', ADDRESS_ANY, 1, &cmd,1); |
cmd = 0xff; |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
// _delay_ms(250); |
} |
do |
{ |
//------------------------------------------ |
// Screen neu zeichnen |
//------------------------------------------ |
if( redraw ) |
{ |
ShowTitle_P( ( current_hardware == NC ? PSTR("NC") : PSTR("FC") ), true ); |
lcd_printp_at( 4, 0, PSTR("Display"), MINVERS); |
lcd_rect_round( 0, 2*7-3, 127, 5*7+3+3, 1, R2); // Rahmen |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
redraw = false; |
} |
show_Lipo(); |
SendOutData( 'h', ADDRESS_ANY, 2, &cmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
cmd = 0xff; |
_delay_ms(250); |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
// (Anzeige von unten nach oben) |
//------------------------------------------ |
rxd_buffer[83] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &rxd_buffer[3+60], MNORMAL, 5,2); |
rxd_buffer[63] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &rxd_buffer[3+40], MNORMAL, 5,1); |
rxd_buffer[43] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &rxd_buffer[3+20], MNORMAL, 5,0); |
rxd_buffer[23] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &rxd_buffer[3+ 0], MNORMAL, 5,-1); |
//------------------------------------------ |
// RX-Buffer freigeben |
//------------------------------------------ |
rxd_buffer_locked = FALSE; |
timer = TIMEOUT; |
} // end: if( rxd_buffer_locked ) |
//------------------------------------------ |
// Pruefe PKT Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) |
{ |
cmd = 0xfe; // key: rechts (next page) |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) |
{ |
cmd = 0xfd; // key: links (previous page) |
} |
if( get_key_short(1 << KEY_ESC) ) |
{ |
cmd = 0xfb; // key: runter |
} |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
cmd = 0xf7; // key: hoch |
} |
if( (hardware == NC) && get_key_long(1 << KEY_ENTER) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
if( current_hardware == NC ) |
{ |
MK_SwitchToFC(); |
redraw = true; |
} |
else |
{ |
MK_SwitchToNC(); |
MK_SwitchToNC(); |
redraw = true; |
} |
// cmd = 0xff; |
timer2 = 50; |
while( timer2 > 0 ); |
} |
if( get_key_long(1 << KEY_ESC) ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
exit = true; |
} |
} |
while( (!exit) && timer ); |
mode = 0; |
rxd_buffer_locked = FALSE; |
//------------------------------------------ |
// Timeout? |
//------------------------------------------ |
if( !timer ) |
{ // timeout occured |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkdisplay.h |
---|
0,0 → 1,53 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkdisplay.h |
//# |
//# 29.01.2014 OG |
//# - chg: ehemals display_data() jetzt MK_display() |
//# - chg: ehemals 'display.h' jetzt 'mk/mkdisplay.h' |
//# - add: Source-History ergaenzt |
//############################################################################ |
#ifndef _DISPLAY_H |
#define _DISPLAY_H |
extern uint8_t ACC_Display; |
void MK_Display(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkgpsinfo.c |
---|
0,0 → 1,425 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.c |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Gps_Info() umgestellt auf PKT_Message_Datenverlust() |
//# |
//# 14.05.2014 OG |
//# - chg: Code-Redesign; neues Screen-Layout; Code-Reduzierung; |
//# Anpassungen an neue PKT-Funktionen |
//# - chg: umbenannt von 'gps.c' zu 'mkgpsinfo.c' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "../messages.h" |
//############################################################## |
//############################################################## |
//#define DEBUG_MKGPS // erweiterte Anzeigen zum debuggen |
#define GPSTIMEOUT 300 // 3 Sekunden |
#define UBX_BUFFER_SIZE 100 // 100 Byte Buffer fuer GPS-Daten |
uint8_t ck_a = 0; |
uint8_t ck_b = 0; |
union long_union |
{ |
uint32_t dword; |
uint8_t byte[4]; |
} longUnion; |
union int_union |
{ |
uint16_t dword; |
uint8_t byte[2]; |
} intUnion; |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint32_t join_4_bytes(uint8_t Buffer[]) |
{ |
longUnion.byte[0] = *Buffer; |
longUnion.byte[1] = *(Buffer+1); |
longUnion.byte[2] = *(Buffer+2); |
longUnion.byte[3] = *(Buffer+3); |
return (longUnion.dword); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void checksum(uint8_t data) |
{ |
ck_a += data; |
ck_b += ck_a; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Gps_Info( void ) |
{ |
uint8_t UBX_buffer[UBX_BUFFER_SIZE]; |
uint8_t UBX_payload_counter = 0; |
uint8_t UBX_class = 0; |
uint8_t UBX_id = 0; |
uint8_t UBX_ck_a = 0; |
uint8_t data = 0; |
uint8_t length = 0; |
uint8_t state = 0; |
uint8_t redraw = true; |
uint8_t refresh = false; |
uint8_t yoffs = 1; // Anzeige-Verschiebung: der obere Bereich |
uint8_t yoffs2 = 4; // Anzeige-Verschiebung: GPS-Koordinaten |
int16_t v_16; // tmp-Variable |
int32_t v_32; // tmp-Variable |
#ifdef DEBUG_MKGPS |
uint8_t maxlen = 0; |
#endif |
MK_SwitchToNC(); // Anmerkung OG: warum auch immer es besser ist erst auf die NC |
MK_SwitchToGPS(); // umzuschalten... es laeuft so scheinbar zuverlaessiger |
MK_SwitchToGPS(); |
timer_mk_timeout = GPSTIMEOUT; |
do |
{ |
//------------------------ |
// REDRAW |
//------------------------ |
if( redraw ) |
{ |
ShowTitle_P( PSTR("MK GPS Info"), true); |
lcdx_printp_at( 0,1, PSTR("Fix:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,2, PSTR("Sat:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 0,3, PSTR("Alt:"), MNORMAL, 0,yoffs ); |
lcdx_printp_at( 9,1, PSTR("PDOP:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,2, PSTR("Accu:"), MNORMAL, 3,yoffs ); |
lcdx_printp_at( 9,3, PSTR("Sped:"), MNORMAL, 3,yoffs ); |
lcd_frect( 0, (4*7)+4+yoffs2, 127, 7, 1); // GPS: Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8+yoffs2); // GPS: "Breitengr Längengr" |
lcd_rect( 0, (3*8)+7+yoffs2, 127, (2*8)+4, 1); // GPS: Rahmen |
lcdx_printp_at(12,7, strGet(ENDE), MNORMAL, 0,1); // KEYLINE |
redraw = false; |
} |
//------------------------ |
// REFRESH |
//------------------------ |
if( refresh ) |
{ |
if((UBX_class == 1) && (UBX_id == 6)) // GPS: SVINFO |
{ |
//-------------- |
// GPS Status |
//-------------- |
switch( UBX_buffer[10] ) |
{ |
case 4: |
case 3: lcdx_printp_at( 5, 1, PSTR("3D"), MNORMAL, 1,yoffs); break; |
case 2: lcdx_printp_at( 5, 1, PSTR("2D"), MNORMAL, 1,yoffs); break; |
default: lcdx_printp_at( 5, 1, PSTR("no"), MNORMAL, 1,yoffs); |
} |
// GPS ok => ein Haken wird angezeigt |
if( (UBX_buffer[11] & 1) == 1 ) lcdx_putc( 7, 1, SYMBOL_CHECK, MNORMAL, 3,yoffs ); |
else lcdx_putc( 7, 1, ' ', MNORMAL, 3,yoffs ); |
//--- |
// Anzeige von "D" - evtl. DGPS (Differential GPS)? |
// aktuell nicht mehr unterstuetzt da kein Platz auf dem Screen |
//--- |
//if( (UBX_buffer[11] & 3) == 3 ) lcd_printp_at (10,0, PSTR("D"), 0); |
//else lcd_printp_at (10,0, PSTR(" "), 0); |
//-------------- |
// Sat |
//-------------- |
lcdx_printf_at_P( 5, 2, MNORMAL, 0,yoffs, PSTR("%2u"), UBX_buffer[47] ); |
//-------------- |
// PDOP |
//-------------- |
v_16 = UBX_buffer[44]+UBX_buffer[45]*255; |
lcdx_printf_at_P( 15, 1, MNORMAL, 0,yoffs, PSTR("%2.2d"), v_16 ); |
//-------------- |
// Accuracy |
//-------------- |
v_32 = (int32_t)join_4_bytes(&UBX_buffer[24]); |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,yoffs, PSTR("%2.2ldm"), v_32 ); |
} |
if((UBX_class == 1) && (UBX_id == 18)) // GPS: VELNED |
{ |
//-------------- |
// Speed |
//-------------- |
v_16 = (int16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
lcdx_printf_at_P( 15, 3, MNORMAL, 0,yoffs, PSTR("%3dkmH"), v_16 ); |
//uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
//write_ndigit_number_u (11, 4, speed, 3, 0,0); |
//lcd_printp_at (15,4, PSTR("km/h"), 0); |
} |
if((UBX_class == 1) && (UBX_id == 2)) // GPS: POSLLH |
{ |
//-------------- |
// Altitude |
//-------------- |
v_16 = (int16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//v_16 = v_16 + 3000; |
lcdx_printf_at_P( 4, 3, MNORMAL, 1,yoffs, PSTR("%4d"), v_16 ); |
//uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
//write_ndigit_number_u (11, 7, height, 4, 0,0); |
//lcd_printp_at(16,7, PSTR("m"), 0); |
//-------------- |
// Breitengrad - Lat (51.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[8]); |
writex_gpspos( 1, 4, v_32 , MNORMAL, 0,10+yoffs2); // Anzeige: Breitengrad |
//write_ndigit_number_u ( 0, 7, (uint16_t)(lat/10000000), 2, 0,0); |
//lcd_printp_at ( 2, 7, PSTR("."), 0); |
//write_ndigit_number_u ( 3, 7, (uint16_t)((lat/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u ( 7, 7, (uint16_t)((lat/10) % 100), 2, 1,0); |
//-------------- |
// Laengengrad - Long (7.) |
//-------------- |
v_32 = join_4_bytes(&UBX_buffer[4]); |
writex_gpspos( 12, 4, v_32, MNORMAL, -1,10+yoffs2); // Anzeige: Laengengrad |
//write_ndigit_number_u (10, 7, (uint16_t)(lon/10000000), 2, 0,0); |
//lcd_printp_at (12, 7, PSTR("."), 0); |
//write_ndigit_number_u (13, 7, (uint16_t)((lon/1000) % 10000), 4, 1,0); |
//write_ndigit_number_u (17, 7, (uint16_t)((lon/10) % 100), 2, 1,0); |
} |
//------------------------ |
// PKT-LiPo Anzeige |
//------------------------ |
show_Lipo(); |
refresh = false; |
} // end: if( refresh ) |
//-------------------------- |
// PROCESS DATA |
//-------------------------- |
if( uart_getc_nb( &data ) ) |
{ |
switch( state ) |
{ |
case 0: if( data == 0xB5 ) // GPS: init 1 |
{ |
UBX_payload_counter = 0; |
UBX_id = 0; |
UBX_class = 0; |
ck_a = 0; |
ck_b = 0; |
state++; |
} |
break; |
case 1: if( data == 0x62 ) // GPS: init 2 |
state++; |
else |
state = 0; |
break; |
case 2: if( data != 1 ) // GPS: class |
state = 0; |
else |
{ |
checksum(data); |
UBX_class = data; |
state++; |
} |
break; |
case 3: if( (data != 48) // GPS: id |
&&(data != 6 ) |
&&(data != 18) |
&&(data != 2 )) |
{ |
state = 0; |
} |
else |
{ |
UBX_id = data; |
checksum(data); |
state++; |
} |
break; |
case 4: if( data >= UBX_BUFFER_SIZE ) // GPS: length lo |
state = 0; |
else |
{ |
checksum(data); |
length = data; |
state++; |
} |
#ifdef DEBUG_MKGPS |
// DEBUG - Anzeige der Paketgroesse |
if( data > maxlen ) |
{ |
// groesstes gemessenes Paket lag bei 181 |
// ob man das Paket braucht ist eine andere Frage |
maxlen = data; |
lcdx_printf_at_P( 1, 7, MNORMAL, 0,1, PSTR("%3u"), maxlen ); |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
} |
#endif |
break; |
case 5: if( data != 0 ) // GPS: length hi |
state = 0; |
else |
{ |
checksum(data); |
state++; |
} |
break; |
case 6: UBX_buffer[UBX_payload_counter] = data; // GPS: data |
checksum(data); |
UBX_payload_counter++; |
length--; |
if( length == 0 ) |
{ |
state++; |
}; |
break; |
case 7: state++; // GPS: check lo |
UBX_ck_a = data; |
break; |
case 8: state = 0; // GPS: check hi |
if((UBX_ck_a == ck_a) && (data == ck_b)) |
{ |
timer_mk_timeout = GPSTIMEOUT; |
refresh = true; |
} |
} // end: switch( state ) |
} // end: if( uart_getc_nb(&data) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
/* |
// funktioniert hier nicht - warum auch immer |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
redraw = true; |
} |
*/ |
} while( !get_key_press(1 << KEY_ESC) && timer_mk_timeout ); |
//-------------------------- |
// in den Timeout gelaufen? |
//-------------------------- |
if( !timer_mk_timeout ) |
{ |
//PKT_Message_Datenverlust( timeout, beep) |
PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden |
} |
clear_key_all(); |
SwitchToNC(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk/mkgpsinfo.h |
---|
0,0 → 1,50 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkgpsinfo.h |
//# |
//# 14.05.2014 OG |
//# - chg: umbenannt von 'gps.h' zu 'mkgpsinfo.h' |
//# - chg: gps() ist jetzt MK_Gps_Info() |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef _GPS_H |
#define _GPS_H |
void MK_Gps_Info( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mk |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/trunk/mk-data-structs.h |
---|
0,0 → 1,501 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mk_data-stucts.h |
//# |
//# 16.07.2015 Cebra |
//# - chg: Receiver Selection |
//# RECEIVER_USER 8 -> RECEIVER_MLINK 8 |
//# - add: Receiver Selection |
//# RECEIVER_USER 9 |
//# |
//# 26.01.2015 Cebra |
//# - chg: #defines für ServoCompInvert an aktuelle Syntax angepasst |
//# SVNick -> SERVO_NICK_INV |
//# SVRoll -> SERVO_ROLL_INV |
//# SVRelMov -> SERVO_RELATIVE |
//# - add: #define CH_DIRECTION_1 0x08 für ComingHome Ausrichtung |
//# #define CH_DIRECTION_2 0x10 |
//# |
//# 19.06.2014 OG |
//# - chg: Aktualisiert: struct Point_t (aus Quelle: NaviCtrl V2.06f/waypoints.h - Revision 546) |
//# |
//# 12.02.2014 OG |
//# - del: die alten defines bzgl. MKVERSIONnnn und mk_param_struct_t entfernt |
//# |
//# 02.12.2013 Cebra |
//# - add: #define MKVERSION201f, Erweiterung für FC201f |
//# |
//# 21.10.2013 Cebra |
//# - add: #define MKVERSION201a, Erweiterung für FC201a |
//# |
//# 26.06.2013 OG |
//# - chg: "0.91f" -> "0.91f/l" |
//# |
//# 13.06.2013 OG |
//# - chg: "0.90j" -> "0.90h/j" |
//# - chg: Code-Layout |
//# |
//# 05.06.2013 Cebra |
//# - chg: Anpassungen an FC091c |
//# |
//# 03.06.2013 Cebra |
//# - chg: #define Fehler beseitigt |
//# |
//# 30.05.2013 Cebra |
//# - add: #define MKVERSION091a, Erweiterung für FC091a |
//# |
//# 13.05.2013 Cebra |
//# - add: #define MKVERSION090h // wegen GPS-Zeitausgabe NC |
//# |
//# 11.05.2013 OG |
//# - add: DateTime_t |
//# |
//# 20.03.2013 OG |
//# - add: BLData_t (http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct) |
//############################################################################ |
#ifndef _MK_DATA_STRUCTS_H |
#define _MK_DATA_STRUCTS_H |
#include "main.h" |
//################################################################################## |
//################################################################################## |
#define u8 uint8_t |
#define s8 int8_t |
#define u16 uint16_t |
#define s16 int16_t |
#define u32 uint32_t |
#define s32 int32_t |
#define NUMBER_OF_DEBUG_DATAS 32 |
#define ANALOG_NAME_LENGTH 16 |
// Version of supported serial protocol |
#define MIN_VERSION 7 |
#define MAX_VERSION 10 |
// Setting index |
#define SETTING_1 1 |
#define SETTING_2 2 |
#define SETTING_3 3 |
#define SETTING_4 4 |
#define SETTING_5 5 |
#define SETTING_CURRENT 0xff |
// MikroKopter defines |
// taken from |
// FC Software eeprom.h |
// |
//GlobalConfig3 aus FC/eeprom.h |
#define CFG3_NO_SDCARD_NO_START 0x01 |
#define CFG3_DPH_MAX_RADIUS 0x02 |
#define CFG3_VARIO_FAILSAFE 0x04 |
#define CFG3_MOTOR_SWITCH_MODE 0x08 //FC0.88L 7.5.12 |
#define CFG3_NO_GPSFIX_NO_START 0x10 //FC0.88L 7.5.12 |
#define CFG3_USE_NC_FOR_OUT1 0x20 |
#define CFG3_SPEAK_ALL 0x40 |
#define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//GlobalConfig |
#define CFG_HOEHENREGELUNG 0x01 |
#define CFG_HOEHEN_SCHALTER 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_KOMPASS_AKTIV 0x08 |
#define CFG_KOMPASS_FIX 0x10 |
#define CFG_GPS_AKTIV 0x20 |
#define CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define CFG_DREHRATEN_BEGRENZER 0x80 |
//Bitconfig MAsk |
#define CFG_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
#define CFG_MOTOR_BLINK1 0x10 |
#define CFG_MOTOR_OFF_LED1 0x20 |
#define CFG_MOTOR_OFF_LED2 0x40 |
#define CFG_MOTOR_BLINK2 0x80 |
// ServoCompInvert, FC0.89 entfällt und geändert s.u. |
//#define SVNick 0x01 |
//#define SVRoll 0x02 |
//#define SVRelMov 0x04 |
//ab FC 209a (26.1.2015 cebra) |
// bitcoding for EE_Parameter.ServoCompInvert |
#define SERVO_NICK_INV 0x01 |
#define SERVO_ROLL_INV 0x02 |
#define SERVO_RELATIVE 0x04 // direct poti control or relative moving of the servo value |
#define CH_DIRECTION_1 0x08 |
#define CH_DIRECTION_2 0x10 |
//CH Orientation ServoBits 0x10 0x08 |
// --> no change 0 0 |
// --> front to starting point 0 1 |
// --> rear to to starting point 1 0 |
//-> start orientation 1 1 |
// ExtraConfig |
#define CFG2_HEIGHT_LIMIT 0x01 |
#define CFG2_VARIO_BEEP 0x02 |
#define CFG_SENSITIVE_RC 0x04 // 26.1.2015 entfällt ab FC 209a, cebra |
#define CFG_3_3V_REFERENCE 0x08 |
#define CFG_NO_RCOFF_BEEPING 0x10 |
#define CFG_GPS_AID 0x20 |
#define CFG_LEARNABLE_CAREFREE 0x40 |
#define CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
// bit mask for ParamSet.Config0 |
#define CFG0_AIRPRESS_SENSOR 0x01 |
#define CFG0_HEIGHT_SWITCH 0x02 |
#define CFG0_HEADING_HOLD 0x04 |
#define CFG0_COMPASS_ACTIVE 0x08 |
#define CFG0_COMPASS_FIX 0x10 |
#define CFG0_GPS_ACTIVE 0x20 |
#define CFG0_AXIS_COUPLING_ACTIVE 0x40 |
#define CFG0_ROTARY_RATE_LIMITER 0x80 |
// defines for the receiver selection |
#define RECEIVER_PPM 0 |
#define RECEIVER_SPEKTRUM 1 |
#define RECEIVER_SPEKTRUM_HI_RES 2 |
#define RECEIVER_SPEKTRUM_LOW_RES 3 |
#define RECEIVER_JETI 4 |
#define RECEIVER_ACT_DSL 5 |
#define RECEIVER_HOTT 6 |
#define RECEIVER_SBUS 7 |
#define RECEIVER_MLINK 8 //FC2.11 16.7.2015 CB |
#define RECEIVER_USER 9 //FC2.11 16.7.2015 CB |
#define RECEIVER_UNKNOWN 0xFF |
// MikroKopter Flags |
// taken from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.73d%2Ffc.h |
//alt 0.86 |
//#define FCFLAG_MOTOR_RUN 0x01 |
//#define FCFLAG_FLY 0x02 |
//#define FCFLAG_CALIBRATE 0x04 |
//#define FCFLAG_START 0x08 |
//#define FCFLAG_NOTLANDUNG 0x10 |
//#define FCFLAG_LOWBAT 0x20 |
//#define FCFLAG_SPI_RX_ERR 0x40 |
//#define FCFLAG_I2CERR 0x80 |
// FC_StatusFlags 0.88 |
#define FC_STATUS_MOTOR_RUN 0x01 |
#define FC_STATUS_FLY 0x02 |
#define FC_STATUS_CALIBRATE 0x04 |
#define FC_STATUS_START 0x08 |
#define FC_STATUS_EMERGENCY_LANDING 0x10 |
#define FC_STATUS_LOWBAT 0x20 |
#define FC_STATUS_VARIO_TRIM_UP 0x40 |
#define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
// FC_StatusFlags2 |
#define FC_STATUS2_CAREFREE 0x01 |
#define FC_STATUS2_ALTITUDE_CONTROL 0x02 |
#define FC_STATUS2_RC_FAILSAVE_ACTIVE 0x04 |
#define FC_STATUS2_OUT1_ACTIVE 0x08 |
#define FC_STATUS2_OUT2_ACTIVE 0x10 |
// NaviCtrl Flags |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
#define NC_FLAG_FREE 0x01 |
#define NC_FLAG_PH 0x02 |
#define NC_FLAG_CH 0x04 |
#define NC_FLAG_RANGE_LIMIT 0x08 |
#define NC_FLAG_NOSERIALLINK 0x10 |
#define NC_FLAG_TARGET_REACHED 0x20 |
#define NC_FLAG_MANUAL_CONTROL 0x40 |
#define NC_FLAG_GPS_OK 0x80 |
typedef struct |
{ |
unsigned char SWMajor; |
unsigned char SWMinor; |
unsigned char ProtoMajor; |
unsigned char ProtoMinor; |
unsigned char SWPatch; |
unsigned char HardwareError[5]; |
} __attribute__((packed)) Version_t; |
// FC Debug Struct |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.h |
typedef struct // 0.86 |
{ |
uint8_t Digital[2]; |
// NC: unsigned; FC: signed !!!! |
int16_t Analog[32]; // Debugvalues |
} __attribute__((packed)) DebugData_t; |
//****************************************************************** |
// uart1.h NC 0.87, zur Zeit hier nicht verwendet 28.01.2012 CB |
#define AMPEL_FC 0x01 |
#define AMPEL_BL 0x02 |
#define AMPEL_NC 0x04 |
#define AMPEL_COMPASS 0x08 |
typedef struct //0.87 |
{ |
u8 StatusGreen; |
u8 StatusRed; |
u16 Analog[32]; // Debugwerte |
} __attribute__((packed)) DebugOut_t; |
//****************************************************************** |
// NaviCtrl OSD Structs |
// portions taken and adapted from |
// http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
typedef struct //NC uart1.h |
{ |
s16 AngleNick; // in 0.1 deg |
s16 AngleRoll; // in 0.1 deg |
s16 Heading; // in 0.1 deg |
u8 StickNick; |
u8 StickRoll; |
u8 StickYaw; |
u8 StickGas; |
u8 reserve[4]; |
} __attribute__((packed)) Data3D_t; |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
typedef struct |
{ |
u16 Distance; // distance to target in cm |
s16 Bearing; // course to target in deg |
} __attribute__((packed)) GPS_PosDev_t; |
//---------------------------- |
// aus NC waypoint.h |
// |
// AKTUALISIERT: 18.06.2014 OG |
// |
// Anmerkung 18.06.2014 OG |
// "Name" hinzugefuegt - das gibt es |
// seit dem 23.03.2012! |
//---------------------------- |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // Will be transferred to the FC and can be used as Poti value there |
u8 AltitudeRate; // rate to change the setpoint in steps of 0.1m/s |
u8 Speed; // rate to change the Position(0 = max) in steps of 0.1m/s |
u8 CamAngle; // Camera servo angle in degree (255 -> POI-Automatic) |
u8 Name[4]; // Name of that point (ASCII) |
u8 reserve[2]; // reserve |
} __attribute__((packed)) Point_t; |
//---------------------------- |
// aus NC waypoint.h |
// VERALTET! 18.06.2014 OG |
//---------------------------- |
/* |
typedef struct |
{ |
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
u8 Event_Flag; // future implementation |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // |
u8 AltitudeRate; // rate to change the setpoint |
u8 Speed; // rate to change the Position |
u8 CamAngle; // Camera servo angle |
u8 reserve[6]; // reserve |
} __attribute__((packed)) Point_t; |
*/ |
// NaviCtrl struct |
// taken from |
// http://mikrocontroller.cco-ev.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.15c%2Fuart1.h |
// |
#define NAVIDATA_VERSION 5 |
typedef struct |
{ |
u8 Version; // version of the data structure |
GPS_Pos_t CurrentPosition; // see ubx.h for details |
GPS_Pos_t TargetPosition; |
GPS_PosDev_t TargetPositionDeviation; |
GPS_Pos_t HomePosition; |
GPS_PosDev_t HomePositionDeviation; |
u8 WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
u8 WaypointNumber; // number of stored waypoints |
u8 SatsInUse; // number of satellites used for position solution |
s16 Altimeter; // hight according to air pressure |
s16 Variometer; // climb(+) and sink(-) rate |
u16 FlyingTime; // in seconds |
u8 UBat; // Battery Voltage in 0.1 Volts |
u16 GroundSpeed; // speed over ground in cm/s (2D) |
s16 Heading; // current flight direction in ° as angle to north |
s16 CompassHeading; // current compass value in |
s8 AngleNick; // current Nick angle in 1 |
s8 AngleRoll; // current Roll angle in 1 |
u8 RC_Quality; // RC_Quality |
u8 FCStatusFlags; // Flags from FC |
u8 NCFlags; // Flags from NC |
u8 Errorcode; // 0 --> okay |
u8 OperatingRadius; // current operation radius around the Home Position in m |
s16 TopSpeed; // velocity in vertical direction in cm/s |
u8 TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
u8 FCStatusFlags2; // StatusFlags2 (since version 5 added) |
s16 SetpointAltitude; // setpoint for altitude |
u8 Gas; // for future use |
u16 Current; // actual current in 0.1A steps |
u16 UsedCapacity; // used capacity in mAh |
} __attribute__((packed)) NaviData_t; |
typedef struct |
{ |
uint8_t Version; // the version of the BL (0 = old) |
uint8_t SetPoint; // written by attitude controller |
uint8_t SetPointLowerBits; // for higher Resolution of new BLs |
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present |
uint8_t ReadMode; // select data to read |
// the following bytes must be exactly in that order! |
uint8_t Current; // in 0.1 A steps, read back from BL |
uint8_t MaxPWM; // read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
int8_t Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in �C |
} __attribute__((packed)) MotorData_t; |
typedef struct |
{ |
uint8_t Revision; // must be BL_REVISION |
uint8_t SetMask; // settings mask |
uint8_t PwmScaling; // maximum value of control pwm, acts like a thrust limit |
uint8_t CurrentLimit; // current limit in A |
uint8_t TempLimit; // in �C |
uint8_t CurrentScaling; // scaling factor for current measurement |
uint8_t BitConfig; // see defines above |
uint8_t crc; // checksum |
} __attribute__((packed)) BLConfig_t; |
//############################################################################## |
//# Datenstruktur von ehemals mk_param_struct_t jetzt via mk/paramset.c ! |
//############################################################################## |
//------------------------------------- |
// 20.03.2013 OG: BLData_t |
// aus: http://www.mikrokopter.de/ucwiki/en/SerialCommands/BLDataStruct |
//------------------------------------- |
typedef struct |
{ |
unsigned char Index; // address of BL-Ctrl |
unsigned char Current; // in 0.1 A steps, read back from BL |
unsigned char Temperature; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
unsigned char MaxPWM; // EVENTUELL: read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40) |
unsigned char Status; // EVENTUELL: 7 bit for I2C error counter, highest bit indicates if motor is present |
} BLData_t; |
//------------------------------------- |
// 11.05.2013 OG: DateTime_t |
// aus: NC v0.30g timer1.h |
//------------------------------------- |
typedef struct{ |
u16 Year; |
u8 Month; |
u8 Day; |
u8 Hour; |
u8 Min; |
u8 Sec; |
u16 mSec; |
u8 Valid; |
} DateTime_t; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/mkparameters.c |
---|
0,0 → 1,2692 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.c |
//# |
//# 16.07.2015 Cebra |
//# - add: Erweiterung SingleWpControlChannel; (FC2.11a) |
//# MenuKeyChannel; (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: Erweiterung paramEditItemTable und ID_MENU_KAMERA_Items[] um neue Parameter (FC2.09j) |
//# param_ServoNickFailsave, param_ServoRollFailsave, param_Servo3Failsave, param_Servo4Failsave, param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: Comming Home Ausrichtung hinzugefügt, |
//# neue Einstellungen ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: im Menü Höhe -> param_Hoehe_TiltCompensation, FC207d |
//# |
//# 04.06.2014 OG |
//# - chg: MK_Parameters_MenuMain() eine Menue-Trennlinie hinter Favoriten eingefuegt |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: Menu_Favoriten() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# |
//# 10.05.2014 OG |
//# - del: editDisableDeclCalc() - wurde ersetzt durch eine Transform-Funktion |
//# in paramset.c und wird jetzt von editGeneric() bearbeitet |
//# |
//# 07.05.2014 OG |
//# - chg: Menu_Favoriten() - uebernimmt den gegebenen Menuetitel vom |
//# uebergeordneten Eintrag aus mkparameters_messages.h (also Multilanguage) |
//# - chg: fav_add() - erweitert um Menue-Separatoren (Trennlinien) aufzunehmen |
//# - chg: Menu_EditCategory() umgestellt auf MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: Favoriten-Verwaltung implementiert (Aenderungen an verschiedenen Funktionen) |
//# |
//# 18.04.2014 OG |
//# - fix: im ID_MENU_NAVICTRL fehlten param_NaviStickThreshold ("GPS Stick-Schwelle") |
//# und param_NaviGpsMinSat ("Min. Sat") |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Menu_EditCategory() blendet ggf. doppelt aufeinanderfolgenden |
//# Menue-Separatoren aus |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktioionen del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - add: Unterstuetzung fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 27.03.2014 OG |
//# kompletter neuer Code fuer ein erstes Release |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 20.02.2014 OG |
//# - chg: MK_Parameters_Menu() meldet "nicht verfügbar" |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mkparameters_messages.h" |
//############################################################################################# |
//# Strukturen; Forward-Deklarationen |
//############################################################################################# |
#define DEBUG_PARAMEDIT // schaltet zusaetzliche Debug-Ausgaben ein |
#define EOF 255 // End Of File (bzw. end of Table...) |
#define SEPARATOR 254 // ID fuer einen Separator in einem Menue (Trennlinie) |
#define SCREEN_REFRESH 1 // neuzeichnen/refresh der Anzeige |
#define SCREEN_REDRAW 2 // neuzeichnen/refresh der Anzeige |
//-------------------------------------------- |
// editGenericCode_t |
// deklariert ein einzelnes Code-Zeichen fuer Edit-Generic |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char code; // z.B. '0', '1', 'v', 'P', 'C' ... (einzelnes Zeichen aus paramEditItem_t.format ) |
unsigned char min; |
unsigned char max; |
const char *shortText_de; // |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
//--------------------------------------------- |
//----- globale Modulvariablen |
//--------------------------------------------- |
editGenericCode_t genericCode; // Daten eines einzelnen Code-Zeichen (im RAM) |
paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_SHORT_VALUE_de[] PROGMEM = "%3u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
#define GENERIC_SHORT_VALUE_en GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_de GENERIC_SHORT_VALUE_de |
#define GENERIC_LONG_VALUE_en GENERIC_SHORT_VALUE_de |
// |
//static const char GENERIC_SHORT_VALUE_ACCZ_de[] PROGMEM = "%4u"; // nur Wert anzeigen - min/max wird uebergeordnet definiert |
//#define GENERIC_SHORT_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_de GENERIC_SHORT_VALUE_ACCZ_de |
//#define GENERIC_LONG_VALUE_ACCZ_en GENERIC_SHORT_VALUE_ACCZ_de |
static const char GENERIC_SHORT_NO_de[] PROGMEM = " N"; |
#define GENERIC_SHORT_NO_en GENERIC_SHORT_NO_de |
static const char GENERIC_LONG_NO_de[] PROGMEM = "Nein"; |
static const char GENERIC_LONG_NO_en[] PROGMEM = "No"; |
static const char GENERIC_SHORT_YES_de[] PROGMEM = " J"; |
static const char GENERIC_SHORT_YES_en[] PROGMEM = " Y"; |
static const char GENERIC_LONG_YES_de[] PROGMEM = "Ja"; |
static const char GENERIC_LONG_YES_en[] PROGMEM = "Yes"; |
static const char GENERIC_SHORT_POTI_de[] PROGMEM = " P%u"; |
#define GENERIC_SHORT_POTI_en GENERIC_SHORT_POTI_de |
static const char GENERIC_LONG_POTI_de[] PROGMEM = "Poti %u"; |
#define GENERIC_LONG_POTI_en GENERIC_LONG_POTI_de |
static const char GENERIC_SHORT_CHANNEL_de[] PROGMEM = "C%2u"; |
#define GENERIC_SHORT_CHANNEL_en GENERIC_SHORT_CHANNEL_de |
static const char GENERIC_LONG_CHANNEL_de[] PROGMEM = "Kanal %u"; |
static const char GENERIC_LONG_CHANNEL_en[] PROGMEM = "Channel %u"; |
static const char GENERIC_SHORT_SERCHANNEL_de[] PROGMEM = "S%2u"; |
#define GENERIC_SHORT_SERCHANNEL_en GENERIC_SHORT_SERCHANNEL_de |
static const char GENERIC_LONG_SERCHANNEL_de[] PROGMEM = "Ser. Kanal %u"; |
static const char GENERIC_LONG_SERCHANNEL_en[] PROGMEM = "Ser. Channel %u"; |
static const char GENERIC_SHORT_AUS_de[] PROGMEM = "Aus"; |
static const char GENERIC_SHORT_AUS_en[] PROGMEM = "Off"; |
#define GENERIC_LONG_AUS_de GENERIC_SHORT_AUS_de |
#define GENERIC_LONG_AUS_en GENERIC_SHORT_AUS_en |
static const char GENERIC_SHORT_INACTIV_de[] PROGMEM = "Ina"; |
#define GENERIC_SHORT_INACTIV_en GENERIC_SHORT_INACTIV_de |
static const char GENERIC_LONG_INACTIV_de[] PROGMEM = "Inaktiv"; |
#define GENERIC_LONG_INACTIV_en GENERIC_LONG_INACTIV_de |
static const char GENERIC_SHORT_WPEVENT_de[] PROGMEM = "WPE"; |
#define GENERIC_SHORT_WPEVENT_en GENERIC_SHORT_WPEVENT_de |
static const char GENERIC_LONG_WPEVENT_de[] PROGMEM = "WP-Event"; |
#define GENERIC_LONG_WPEVENT_en GENERIC_LONG_WPEVENT_de |
static const char GENERIC_SHORT_MINIMUM_de[] PROGMEM = "Min"; |
#define GENERIC_SHORT_MINIMUM_en GENERIC_SHORT_MINIMUM_de |
static const char GENERIC_LONG_MINIMUM_de[] PROGMEM = "Minimum"; |
#define GENERIC_LONG_MINIMUM_en GENERIC_LONG_MINIMUM_de |
static const char GENERIC_SHORT_MIDDLE_de[] PROGMEM = "Mid"; |
#define GENERIC_SHORT_MIDDLE_en GENERIC_SHORT_MIDDLE_de |
static const char GENERIC_LONG_MIDDLE_de[] PROGMEM = "Mitte"; |
static const char GENERIC_LONG_MIDDLE_en[] PROGMEM = "Middle"; |
static const char GENERIC_SHORT_MAXIMUM_de[] PROGMEM = "Max"; |
#define GENERIC_SHORT_MAXIMUM_en GENERIC_SHORT_MAXIMUM_de |
static const char GENERIC_LONG_MAXIMUM_de[] PROGMEM = "Maximum"; |
#define GENERIC_LONG_MAXIMUM_en GENERIC_LONG_MAXIMUM_de |
static const char GENERIC_SHORT_AN_de[] PROGMEM = " An"; |
static const char GENERIC_SHORT_AN_en[] PROGMEM = " On"; |
#define GENERIC_LONG_AN_de GENERIC_SHORT_AN_de |
#define GENERIC_LONG_AN_en GENERIC_SHORT_AN_en |
static const char GENERIC_SHORT_FREE_de[] PROGMEM = "Fre"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_FREE_en GENERIC_SHORT_FREE_de |
static const char GENERIC_LONG_FREE_de[] PROGMEM = "Free"; |
#define GENERIC_LONG_FREE_en GENERIC_LONG_FREE_de |
static const char GENERIC_SHORT_CH_de[] PROGMEM = " CH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_CH_en GENERIC_SHORT_CH_de |
static const char GENERIC_LONG_CH_de[] PROGMEM = "Coming Home (CH)"; |
#define GENERIC_LONG_CH_en GENERIC_LONG_CH_de |
static const char GENERIC_SHORT_PH_de[] PROGMEM = " PH"; // z.b. "GPS Modus Steuerung" |
#define GENERIC_SHORT_PH_en GENERIC_SHORT_PH_de |
static const char GENERIC_LONG_PH_de[] PROGMEM = "Position Hold (PH)"; |
#define GENERIC_LONG_PH_en GENERIC_LONG_PH_de |
static const char GENERIC_SHORT_DISABLED_de[] PROGMEM = "Dis"; // z.b. "Auto Start / Land" |
#define GENERIC_SHORT_DISABLED_en GENERIC_SHORT_DISABLED_de |
static const char GENERIC_LONG_DISABLED_de[] PROGMEM = "Disabled"; |
#define GENERIC_LONG_DISABLED_en GENERIC_LONG_DISABLED_de |
static const char GENERIC_SHORT_OUT_de[] PROGMEM = "Ou%1u"; // z.B. Servo 3 und Servo 4 |
#define GENERIC_SHORT_OUT_en GENERIC_SHORT_OUT_de |
static const char GENERIC_LONG_OUT_de[] PROGMEM = "Out %1u"; |
#define GENERIC_LONG_OUT_en GENERIC_LONG_OUT_de |
static const char GENERIC_SHORT_CH_OR_NC_de[] PROGMEM = "kÄn"; |
static const char GENERIC_SHORT_CH_OR_NC_en[] PROGMEM = "nCh"; |
static const char GENERIC_LONG_CH_OR_NC_de[] PROGMEM = "Keine Änderung"; |
static const char GENERIC_LONG_CH_OR_NC_en[] PROGMEM = "no change"; |
static const char GENERIC_SHORT_CH_OR_FH_de[] PROGMEM = "FzH"; |
static const char GENERIC_SHORT_CH_OR_FH_en[] PROGMEM = "FtH"; |
static const char GENERIC_LONG_CH_OR_FH_de[] PROGMEM = "Front zu Home"; |
static const char GENERIC_LONG_CH_OR_FH_en[] PROGMEM = "front to home"; |
static const char GENERIC_SHORT_CH_OR_RH_de[] PROGMEM = "HzH"; |
static const char GENERIC_SHORT_CH_OR_RH_en[] PROGMEM = "RtH"; |
static const char GENERIC_LONG_CH_OR_RH_de[] PROGMEM = "Heck zu Home"; |
static const char GENERIC_LONG_CH_OR_RH_en[] PROGMEM = "rear to home"; |
static const char GENERIC_SHORT_CH_OR_SO_de[] PROGMEM = "wSt"; |
static const char GENERIC_SHORT_CH_OR_SO_en[] PROGMEM = "sOr"; |
static const char GENERIC_LONG_CH_OR_SO_de[] PROGMEM = "wie beim Start"; |
static const char GENERIC_LONG_CH_OR_SO_en[] PROGMEM = "same as start"; |
//------------------------------------------------ |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char code; // '0', '1', 'v', 'P', 'C' ... |
unsigned char min; |
unsigned char max; |
const char *shortText_de; |
const char *shortText_en; |
const char *longText_de; |
const char *longText_en; |
} editGenericCode_t; |
*************************************************/ |
// Anmerkung: kann bei Bedarf evtl. zu PROGMEM umschreiben |
//editGenericCode_t const editGenericCode[] PROGMEM = |
editGenericCode_t editGenericCode[] = |
{ |
{ 'v', 0, 0, GENERIC_SHORT_VALUE_de, GENERIC_SHORT_VALUE_en, GENERIC_LONG_VALUE_de, GENERIC_LONG_VALUE_en }, // nur Wert anzeigen - min/max wird uebergeordnet definiert |
// { 'z', 0, 0, GENERIC_SHORT_VALUE_ACCZ_de, GENERIC_SHORT_VALUE_ACCZ_en, GENERIC_LONG_VALUE_ACCZ_de, GENERIC_LONG_VALUE_ACCZ_en }, // nur Wert anzeigen x 4- min/max wird uebergeordnet definiert |
{ '0', 0, 0, GENERIC_SHORT_NO_de, GENERIC_SHORT_NO_en, GENERIC_LONG_NO_de, GENERIC_LONG_NO_en }, |
{ '1', 1, 1, GENERIC_SHORT_YES_de, GENERIC_SHORT_YES_en, GENERIC_LONG_YES_de, GENERIC_LONG_YES_en }, |
{ 'P', 255,248, GENERIC_SHORT_POTI_de, GENERIC_SHORT_POTI_en, GENERIC_LONG_POTI_de, GENERIC_LONG_POTI_en }, |
{ 'C', 1, 16, GENERIC_SHORT_CHANNEL_de, GENERIC_SHORT_CHANNEL_en, GENERIC_LONG_CHANNEL_de, GENERIC_LONG_CHANNEL_en }, |
{ 'S', 17, 28, GENERIC_SHORT_SERCHANNEL_de, GENERIC_SHORT_SERCHANNEL_en, GENERIC_LONG_SERCHANNEL_de, GENERIC_LONG_SERCHANNEL_en }, |
{ 'A', 0, 0, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, |
{ 'I', 0, 0, GENERIC_SHORT_INACTIV_de, GENERIC_SHORT_INACTIV_en, GENERIC_LONG_INACTIV_de, GENERIC_LONG_INACTIV_en }, |
{ 'W', 29, 29, GENERIC_SHORT_WPEVENT_de, GENERIC_SHORT_WPEVENT_en, GENERIC_LONG_WPEVENT_de, GENERIC_LONG_WPEVENT_en }, |
{ 'U', 30, 30, GENERIC_SHORT_MINIMUM_de, GENERIC_SHORT_MINIMUM_en, GENERIC_LONG_MINIMUM_de, GENERIC_LONG_MINIMUM_en }, |
{ 'M', 31, 31, GENERIC_SHORT_MIDDLE_de, GENERIC_SHORT_MIDDLE_en, GENERIC_LONG_MIDDLE_de, GENERIC_LONG_MIDDLE_en }, |
{ 'X', 32, 32, GENERIC_SHORT_MAXIMUM_de, GENERIC_SHORT_MAXIMUM_en, GENERIC_LONG_MAXIMUM_de, GENERIC_LONG_MAXIMUM_en }, |
{ 'O', 30, 30, GENERIC_SHORT_AUS_de, GENERIC_SHORT_AUS_en, GENERIC_LONG_AUS_de, GENERIC_LONG_AUS_en }, // ein weiteres "Aus"; z.b. "Motors-Sicherheitsschalter" |
{ 'N', 31, 31, GENERIC_SHORT_AN_de, GENERIC_SHORT_AN_en, GENERIC_LONG_AN_de, GENERIC_LONG_AN_en }, // z.b. "Carefree" |
{ 'F', 30, 30, GENERIC_SHORT_FREE_de, GENERIC_SHORT_FREE_en, GENERIC_LONG_FREE_de, GENERIC_LONG_FREE_en }, // Free - z.b. "Carefree" |
{ 'H', 31, 31, GENERIC_SHORT_CH_de, GENERIC_SHORT_CH_en, GENERIC_LONG_CH_de, GENERIC_LONG_CH_en }, // Coming Home - z.b. "Carefree" |
{ 'Q', 32, 32, GENERIC_SHORT_PH_de, GENERIC_SHORT_PH_en, GENERIC_LONG_PH_de, GENERIC_LONG_PH_en }, // Positiosn Hold - z.b. "Carefree" |
{ 'D', 0, 0, GENERIC_SHORT_DISABLED_de, GENERIC_SHORT_DISABLED_en, GENERIC_LONG_DISABLED_de, GENERIC_LONG_DISABLED_en }, // Inaktiv - z.b. "Auto Start / Land" |
{ 'T', 247,246, GENERIC_SHORT_OUT_de, GENERIC_SHORT_OUT_en, GENERIC_LONG_OUT_de, GENERIC_LONG_OUT_en }, |
{ 'K', 0, 0, GENERIC_SHORT_CH_OR_NC_de, GENERIC_SHORT_CH_OR_NC_en, GENERIC_LONG_CH_OR_NC_de, GENERIC_LONG_CH_OR_NC_en }, // CommingHome Orientation, |
{ 'V', 1, 1, GENERIC_SHORT_CH_OR_FH_de, GENERIC_SHORT_CH_OR_FH_en, GENERIC_LONG_CH_OR_FH_de, GENERIC_LONG_CH_OR_FH_en }, // CommingHome Orientation, |
{ 'R', 2, 2, GENERIC_SHORT_CH_OR_RH_de, GENERIC_SHORT_CH_OR_RH_en, GENERIC_LONG_CH_OR_RH_de, GENERIC_LONG_CH_OR_RH_en }, // CommingHome Orientation, |
{ 'G', 3, 3, GENERIC_SHORT_CH_OR_SO_de, GENERIC_SHORT_CH_OR_SO_en, GENERIC_LONG_CH_OR_SO_de, GENERIC_LONG_CH_OR_SO_en }, // CommingHome Orientation, |
{ EOF, 0, 0, 0,0,0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# |
//############################################################################################# |
static const char GENERIC_NoYes[] PROGMEM = "01"; // Nein, Ja (z.B. (fast) alle Checkboxen) |
static const char GENERIC_Value[] PROGMEM = "v"; // value mit min,max aus paramEditItem (in paramEditItemTable) (z.B. Nick/Roll P (0..20) / Min. Gas (0..247) ) |
static const char GENERIC_ValueACCZ[] PROGMEM = "D"; // value mit min,max aus paramEditItem (in paramEditItemTable) x 4 (ACC Z Landing pulse ) |
static const char GENERIC_ValuePoti[] PROGMEM = "vP"; // value, Poti (z.B. Gyro Gier P) |
static const char GENERIC_ValueOutPoti[] PROGMEM = "vTP"; // value, Out, Poti (z.B. Servo 3 & 4) |
static const char GENERIC_Cannel[] PROGMEM = "C"; // Channel (z.B. Gas / Gier / Nick / Roll) |
static const char GENERIC_AusChSerWpeMMM[] PROGMEM = "ACSWUMX"; // Aus, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum (z.B. Poti 1..8) |
static const char GENERIC_DisChSerWpeOff[] PROGMEM = "DCSWO"; // Disabled, Channel, Ser. Channel, WP Event, Off (z.B. Motors-Sicherheitsschalter) |
static const char GENERIC_DisChSerWpeOffOn[] PROGMEM = "DCSWON"; // Disabled, Channel, Ser. Channel, WP Event, Off, On (z.B. Carefree) |
static const char GENERIC_DisChSerWpeFreeCHPH[] PROGMEM = "DCSWFHQ"; // Disabled, Channel, Ser. Channel, WP Event, Free, CH, PH (z.B. GPS Modus Steuerung) |
static const char GENERIC_DisChSer[] PROGMEM = "DCS"; // Disabled, Channel, Ser. Channel (z.B. Auto Start / Land) |
static const char GENERIC_CH_Orientation[] PROGMEM = "KVRG"; // Keine Änderung, vorne Home, Heck Home, gleiche Richtung (Comming Home Orientation |
static const char GENERIC_InaChSerWpeMMM[] PROGMEM = "ICSWUMX"; // Inactiv, Channel, Ser. Channel, WP Event, Minimum, Mitte, Maximum(z.B. Menu Key Channel |
/************************************************* |
//------------- |
// ZUR INFO |
//------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion - z.B. editGeneric(); cmd = MKPARAM_EDIT oder MKPARAM_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
*************************************************/ |
//---------------------- |
// HINWEIS! |
// Die unten stehende Aufgliederung in die verschiedenen Menue-Bereiche ist nur zur ORIENTIERUNG! |
// Jede paramID wird nur EINMAL deklariert - die Zuordnung in die Menues erfolgt in den |
// nachfolgenden ID_MENU_xyz_Items Strukturen! |
//---------------------- |
paramEditItem_t const paramEditItemTable[] PROGMEM = |
{ |
//----------------------------------- |
// Menue: ID_MENU_KANAELE (Kanäle) |
//----------------------------------- |
{ param_Kanalbelegung_Gas , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gas_de, param_Kanalbelegung_Gas_en }, |
{ param_Kanalbelegung_Gear , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Gear_de, param_Kanalbelegung_Gear_en }, |
{ param_Kanalbelegung_Nick , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Nick_de, param_Kanalbelegung_Nick_en }, |
{ param_Kanalbelegung_Roll , &editGeneric, GENERIC_Cannel , 0, 0, param_Kanalbelegung_Roll_de, param_Kanalbelegung_Roll_en }, |
{ param_Kanalbelegung_Poti1 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti1_de, param_Kanalbelegung_Poti1_en }, |
{ param_Kanalbelegung_Poti2 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti2_de, param_Kanalbelegung_Poti2_en }, |
{ param_Kanalbelegung_Poti3 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti3_de, param_Kanalbelegung_Poti3_en }, |
{ param_Kanalbelegung_Poti4 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti4_de, param_Kanalbelegung_Poti4_en }, |
{ param_Kanalbelegung_Poti5 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti5_de, param_Kanalbelegung_Poti5_en }, |
{ param_Kanalbelegung_Poti6 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti6_de, param_Kanalbelegung_Poti6_en }, |
{ param_Kanalbelegung_Poti7 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti7_de, param_Kanalbelegung_Poti7_en }, |
{ param_Kanalbelegung_Poti8 , &editGeneric, GENERIC_AusChSerWpeMMM , 0, 0, param_Kanalbelegung_Poti8_de, param_Kanalbelegung_Poti8_en }, |
{ param_MotorSafetySwitch , &editGeneric, GENERIC_DisChSerWpeOff , 0, 0, param_MotorSafetySwitch_de, param_MotorSafetySwitch_en }, |
{ param_GlobalConfig3_MotorSwitchMode , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_MotorSwitchMode_de, param_GlobalConfig3_MotorSwitchMode_en }, |
{ param_ExtraConfig_SensitiveRc , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_SensitiveRc_de, param_ExtraConfig_SensitiveRc_en }, |
{ param_GlobalConfig3_SpeakAll , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_SpeakAll_de, param_GlobalConfig3_SpeakAll_en }, |
//----------------------------------- |
// Menue: ID_MENU_KONFIGURATION (Konfiguration) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en }, |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_GlobalConfig_KompassAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassAktiv_de, param_GlobalConfig_KompassAktiv_en }, |
{ param_GlobalConfig_KompassFix , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_KompassFix_de, param_GlobalConfig_KompassFix_en }, |
// param_ExtraConfig_SensitiveRc |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GlobalConfig_HeadingHold , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HeadingHold_de, param_GlobalConfig_HeadingHold_en }, |
//----------------------------------- |
// Menue: ID_MENU_STICK (Stick) |
//----------------------------------- |
{ param_Stick_P , &editGeneric, GENERIC_Value , 0, 20, param_Stick_P_de, param_Stick_P_en }, |
{ param_Stick_D , &editGeneric, GENERIC_Value , 0, 20, param_Stick_D_de, param_Stick_D_en }, |
{ param_StickGier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_ExternalControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ExternalControl_de, param_ExternalControl_en }, |
//----------------------------------- |
// Menue: ID_MENU_LOOPING (Looping) |
//----------------------------------- |
{ param_BitConfig_LoopOben , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopOben_de, param_BitConfig_LoopOben_en }, |
{ param_BitConfig_LoopUnten , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopUnten_de, param_BitConfig_LoopUnten_en }, |
{ param_BitConfig_LoopLinks , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopLinks_de, param_BitConfig_LoopLinks_en }, |
{ param_BitConfig_LoopRechts , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_LoopRechts_de, param_BitConfig_LoopRechts_en }, |
{ param_LoopGasLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_LoopGasLimit_de, param_LoopGasLimit_en }, |
{ param_LoopThreshold , &editGeneric, GENERIC_Value , 0, 247, param_LoopThreshold_de, param_LoopThreshold_en }, |
{ param_LoopHysterese , &editGeneric, GENERIC_Value , 0, 247, param_LoopHysterese_de, param_LoopHysterese_en }, |
{ param_WinkelUmschlagNick , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagNick_de, param_WinkelUmschlagNick_en }, |
{ param_WinkelUmschlagRoll , &editGeneric, GENERIC_Value , 0, 247, param_WinkelUmschlagRoll_de, param_WinkelUmschlagRoll_en }, |
//----------------------------------- |
// Menue: ID_MENU_HOEHE (Höhe) |
//----------------------------------- |
{ param_GlobalConfig_HoehenRegelung , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenRegelung_de, param_GlobalConfig_HoehenRegelung_en}, |
{ param_ExtraConfig_HeightLimit , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightLimit_de, param_ExtraConfig_HeightLimit_en }, |
{ param_ExtraConfig_HeightVario , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_HeightVario_de, param_ExtraConfig_HeightVario_en }, // negiertes param_ExtraConfig_HeightLimit |
{ param_GlobalConfig_HoehenSchalter , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_HoehenSchalter_de, param_GlobalConfig_HoehenSchalter_en}, |
{ param_ExtraConfig_VarioBeep , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_VarioBeep_de, param_ExtraConfig_VarioBeep_en }, |
{ param_HoeheChannel , &editGeneric, GENERIC_DisChSerWpeOffOn, 0, 0, param_HoeheChannel_de, param_HoeheChannel_en }, |
{ param_Hoehe_MinGas , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_MinGas_de, param_Hoehe_MinGas_en }, |
{ param_Hoehe_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_P_de, param_Hoehe_P_en }, |
{ param_Luftdruck_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Luftdruck_D_de, param_Luftdruck_D_en }, |
{ param_Hoehe_ACC_Wirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_ACC_Wirkung_de, param_Hoehe_ACC_Wirkung_en }, |
{ param_MaxAltitude , &editGeneric, GENERIC_ValuePoti , 0, 247, param_MaxAltitude_de, param_MaxAltitude_en }, |
{ param_Hoehe_Verstaerkung , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_Verstaerkung_de, param_Hoehe_Verstaerkung_en }, |
{ param_Hoehe_HoverBand , &editGeneric, GENERIC_Value , 0, 247, param_Hoehe_HoverBand_de, param_Hoehe_HoverBand_en }, |
{ param_Hoehe_GPS_Z , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_GPS_Z_de, param_Hoehe_GPS_Z_en }, |
{ param_Hoehe_TiltCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Hoehe_Tilt_Comp_de, param_Hoehe_Tilt_Comp_en }, |
{ param_Hoehe_StickNeutralPoint , &editGeneric, GENERIC_Value , 0, 160, param_Hoehe_StickNeutralPoint_de, param_Hoehe_StickNeutralPoint_en }, |
{ param_StartLandChannel , &editGeneric, GENERIC_DisChSer , 0, 0, param_StartLandChannel_de, param_StartLandChannel_en }, |
{ param_LandingSpeed , &editGeneric, GENERIC_Value , 0, 247, param_LandingSpeed_de, param_LandingSpeed_en }, |
{ param_LandingPulse , &editACCZLandingPulse, GENERIC_ValueACCZ , 0, 0, param_LandingPulse_de, param_LandingPulse_en }, |
//----------------------------------- |
// Menue: ID_MENU_KAMERA (Kamera) |
//----------------------------------- |
{ param_ServoNickControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickControl_de, param_ServoNickControl_en }, |
{ param_ServoNickComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoNickComp_de, param_ServoNickComp_en }, |
{ param_ServoNickFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickFails_de, param_ServoNickFails_en }, |
{ param_GlobalConfig3_ServoNickCompOff , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_ServoNickCompOff_de, param_GlobalConfig3_ServoNickCompOff_en }, // TODO: pruefen: ab welcher Version ist das drin ??? |
{ param_ServoCompInvert_SERVO_NICK_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVNick_de, param_ServoCompInvert_SVNick_en }, |
{ param_ServoCompInvert_SERVO_RELATIVE , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRelMov_de, param_ServoCompInvert_SVRelMov_en }, |
{ param_ServoNickMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMin_de, param_ServoNickMin_en }, |
{ param_ServoNickMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoNickMax_de, param_ServoNickMax_en }, |
{ param_ServoFilterNick , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterNick_de, param_ServoFilterNick_en }, |
{ param_ServoRollControl , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollControl_de, param_ServoRollControl_en }, |
{ param_ServoRollComp , &editGeneric, GENERIC_ValuePoti , 0, 247, param_ServoRollComp_de, param_ServoRollComp_en }, |
{ param_ServoRollFailsave , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollFails_de, param_ServoRollFails_en }, |
{ param_ServoCompInvert_SERVO_ROLL_INV , &editGeneric, GENERIC_NoYes , 0, 0, param_ServoCompInvert_SVRoll_de, param_ServoCompInvert_SVRoll_en }, |
{ param_ServoRollMin , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMin_de, param_ServoRollMin_en }, |
{ param_ServoRollMax , &editGeneric, GENERIC_Value , 0, 247, param_ServoRollMax_de, param_ServoRollMax_en }, |
{ param_ServoFilterRoll , &editGeneric, GENERIC_Value , 0, 25, param_ServoFilterRoll_de, param_ServoFilterRoll_en }, |
{ param_ServoNickRefresh , &editGeneric, GENERIC_Value , 2, 8, param_ServoNickRefresh_de, param_ServoNickRefresh_en }, |
{ param_ServoManualControlSpeed , &editGeneric, GENERIC_Value , 0, 247, param_ServoManualControlSpeed_de, param_ServoManualControlSpeed_en }, |
{ param_Servo3 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo3_de, param_Servo3_en }, |
{ param_Servo3Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo3Fails_de, param_Servo3Fails_en }, |
{ param_Servo4 , &editGeneric, GENERIC_ValueOutPoti , 0, 245, param_Servo4_de, param_Servo4_en }, |
{ param_Servo4Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo4Fails_de, param_Servo4Fails_en }, |
{ param_Servo5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Servo5_de, param_Servo5_en }, |
{ param_Servo5Failsave , &editGeneric, GENERIC_Value , 0, 247, param_Servo5Fails_de, param_Servo5Fails_en }, |
{ param_Servo3OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OnValue_de, param_Servo3OnValue_en }, |
{ param_Servo3OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo3OffValue_de, param_Servo3OffValue_en }, |
{ param_Servo4OnValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OnValue_de, param_Servo4OnValue_en }, |
{ param_Servo4OffValue , &editGeneric, GENERIC_Value , 0, 247, param_Servo4OffValue_de, param_Servo4OffValue_en }, |
//{ param_CamOrientation , &editNA , 0 , 0, 0, param_CamOrientation_de, param_CamOrientation_en }, // wird evtl. nicht mehr unterstuetzt |
//----------------------------------- |
// Menue: ID_MENU_NAVICTRL (NaviCtrl) |
//----------------------------------- |
{ param_GlobalConfig_GpsAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_GpsAktiv_de, param_GlobalConfig_GpsAktiv_en }, |
{ param_NaviGpsModeChannel , &editGeneric, GENERIC_DisChSerWpeFreeCHPH , 0, 0, param_NaviGpsModeChannel_de, param_NaviGpsModeChannel_en }, |
{ param_NaviGpsGain , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsGain_de, param_NaviGpsGain_en }, |
{ param_NaviStickThreshold , &editGeneric, GENERIC_Value , 0, 247, param_NaviStickThreshold_de, param_NaviStickThreshold_en }, |
{ param_NaviGpsMinSat , &editGeneric, GENERIC_Value , 0, 247, param_NaviGpsMinSat_de, param_NaviGpsMinSat_en }, |
{ param_NaviGpsP , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsP_de, param_NaviGpsP_en }, |
{ param_NaviGpsI , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsI_de, param_NaviGpsI_en }, |
{ param_NaviGpsD , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsD_de, param_NaviGpsD_en }, |
{ param_NaviGpsPLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsPLimit_de, param_NaviGpsPLimit_en }, |
{ param_NaviGpsILimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsILimit_de, param_NaviGpsILimit_en }, |
{ param_NaviGpsDLimit , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsDLimit_de, param_NaviGpsDLimit_en }, |
{ param_NaviGpsA , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviGpsA_de, param_NaviGpsA_en }, |
{ param_NaviWindCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviWindCorrection_de, param_NaviWindCorrection_en }, |
{ param_NaviAccCompensation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAccCompensation_de, param_NaviAccCompensation_en }, |
{ param_NaviMaxFlyingRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviMaxFlyingRange_de, param_NaviMaxFlyingRange_en }, |
{ param_NaviOperatingRadius , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOperatingRadius_de, param_NaviOperatingRadius_en }, |
{ param_NaviAngleLimitation , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviAngleLimitation_de, param_NaviAngleLimitation_en }, |
{ param_NaviPH_LoginTime , &editGeneric, GENERIC_Value , 0, 247, param_NaviPH_LoginTime_de, param_NaviPH_LoginTime_en }, |
{ param_ExtraConfig_GpsAid , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_GpsAid_de, param_ExtraConfig_GpsAid_en }, |
{ param_GlobalConfig3_DphMaxRadius , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_DphMaxRadius_de, param_GlobalConfig3_DphMaxRadius_en }, |
{ param_ComingHomeAltitude , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeAltitude_de, param_ComingHomeAltitude_en }, |
{ param_ComingHomeOrientation , &editGeneric, GENERIC_CH_Orientation , 0, 3, param_ComingHomeOrientation_de, param_ComingHomeOrientation_en }, |
{ param_SingleWpControlChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_SingleWpControlChannel_de, param_SingleWpControlChannel_en }, //ab Rev.106 |
{ param_MenuKeyChannel , &editGeneric, GENERIC_InaChSerWpeMMM , 0, 0, param_MenuKeyChannel_de, param_MenuKeyChannel_en }, |
{ param_SingleWpSpeed , &editGeneric, GENERIC_ValuePoti , 0, 247, param_SingleWpSpeed_de, param_SingleWpSpeed_en }, // ab Rev. 100 |
{ param_NaviDescendRange , &editGeneric, GENERIC_Value , 0, 250, param_NaviDescendRange_de, param_NaviDescendRange_en }, |
//----------------------------------- |
// Menue: ID_MENU_AUSGAENGE (Ausgänge) |
//----------------------------------- |
{ param_J16Bitmask , &editBitmask, 0 , 0, 0, param_J16Bitmask_de, param_J16Bitmask_en }, |
{ param_J16Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J16Timing_de, param_J16Timing_en }, |
{ param_BitConfig_MotorBlink1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink1_de, param_BitConfig_MotorBlink1_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed1 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed1_de, param_BitConfig_MotorOffLed1_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_GlobalConfig3_UseNcForOut1 , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_UseNcForOut1_de, param_GlobalConfig3_UseNcForOut1_en }, // "mit WP-Event verknüpfen" |
{ param_NaviOut1Parameter , &editGeneric, GENERIC_ValuePoti , 0, 247, param_NaviOut1Parameter_de, param_NaviOut1Parameter_en }, // "AutoTrigger alle...[meter]" (bis Rev.98) |
{ param_AutoPhotoDistance , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoDistance_de, param_AutoPhotoDistance_en }, // "AutoTrigger alle [meter] in Distance" (ab Rev.100) |
{ param_AutoPhotoAtitudes , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AutoPhotoAtitudes_de, param_AutoPhotoAtitudes_en }, // "AutoTrigger alle [meter] in Altitude" (ab Rev.100) |
{ param_J17Bitmask , &editBitmask, 0 , 0, 0, param_J17Bitmask_de, param_J17Bitmask_en }, |
{ param_J17Timing , &editGeneric, GENERIC_ValuePoti , 0, 247, param_J17Timing_de, param_J17Timing_en }, |
{ param_BitConfig_MotorBlink2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorBlink2_de, param_BitConfig_MotorBlink2_en }, // "nur nach Start der Motoren aktiv" |
{ param_BitConfig_MotorOffLed2 , &editGeneric, GENERIC_NoYes , 0, 0, param_BitConfig_MotorOffLed2_de, param_BitConfig_MotorOffLed2_en }, // nur wenn "nur nach Start der Motoren aktiv" = JA ist -> bestimmt ob LED's an oder aus sind wenn die Motoren aus sind |
{ param_WARN_J16_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J16_Bitmask_de, param_WARN_J16_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
{ param_WARN_J17_Bitmask , &editBitmask, 0 , 0, 0, param_WARN_J17_Bitmask_de, param_WARN_J17_Bitmask_en }, // Bitmaske fuer Unterspannungswarnung |
//----------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES (Verschiedenes) |
//----------------------------------- |
{ param_Gas_Min , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Min_de, param_Gas_Min_en }, |
{ param_Gas_Max , &editGeneric, GENERIC_Value , 0, 247, param_Gas_Max_de, param_Gas_Max_en }, |
{ param_KompassWirkung , &editGeneric, GENERIC_ValuePoti , 0, 247, param_KompassWirkung_de, param_KompassWirkung_en }, |
//{ param_CompassOffset , &editGeneric, GENERIC_Value , 0, 255, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset , &editCompassOffset, 0 , 0, 0, param_CompassOffset_de, param_CompassOffset_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CompassOffset_DisableDeclCalc , &editGeneric, GENERIC_NoYes , 0, 0, param_CompassOffset_DisableDeclCalc_de, param_CompassOffset_DisableDeclCalc_en }, // TODO: +/- Werte sind dort kodiert |
{ param_CareFreeChannel , &editGeneric, GENERIC_DisChSerWpeOffOn , 0, 0, param_CareFreeChannel_de, param_CareFreeChannel_en }, |
{ param_ExtraConfig_LearnableCarefree , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_LearnableCarefree_de, param_ExtraConfig_LearnableCarefree_en }, |
{ param_UnterspannungsWarnung , &editGeneric, GENERIC_Value , 0, 247, param_UnterspannungsWarnung_de, param_UnterspannungsWarnung_en }, |
{ param_ComingHomeVoltage , &editGeneric, GENERIC_Value , 0, 247, param_ComingHomeVoltage_de, param_ComingHomeVoltage_en }, |
{ param_AutoLandingVoltage , &editGeneric, GENERIC_Value , 0, 247, param_AutoLandingVoltage_de, param_AutoLandingVoltage_en }, |
//{ param_ExtraConfig_33vReference , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_33vReference_de, param_ExtraConfig_33vReference_en }, // nicht mehr unterstuetzt! |
{ param_NotGasZeit , &editGeneric, GENERIC_Value , 0, 247, param_NotGasZeit_de, param_NotGasZeit_en }, |
{ param_GlobalConfig3_VarioFailsafe , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_VarioFailsafe_de, param_GlobalConfig3_VarioFailsafe_en }, |
{ param_NotGas , &editGeneric, GENERIC_Value , 0, 247, param_NotGas_de, param_NotGas_en }, |
{ param_FailSafeTime , &editGeneric, GENERIC_Value , 0, 247, param_FailSafeTime_de, param_FailSafeTime_en }, |
{ param_FailsafeChannel , &editGeneric, GENERIC_Value , 0, 16, param_FailsafeChannel_de, param_FailsafeChannel_en }, |
{ param_ExtraConfig_NoRcOffBeeping , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_NoRcOffBeeping_de, param_ExtraConfig_NoRcOffBeeping_en }, |
{ param_ExtraConfig_IgnoreMagErrAtStartup , &editGeneric, GENERIC_NoYes , 0, 0, param_ExtraConfig_IgnoreMagErrAtStartup_de, param_ExtraConfig_IgnoreMagErrAtStartup_en }, |
{ param_GlobalConfig3_NoSdCardNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoSdCardNoStart_de, param_GlobalConfig3_NoSdCardNoStart_en }, |
{ param_GlobalConfig3_NoGpsFixNoStart , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig3_NoGpsFixNoStart_de, param_GlobalConfig3_NoGpsFixNoStart_en }, |
//----------------------------------- |
// Menue: ID_MENU_GYRO (Gyro) |
//----------------------------------- |
{ param_Gyro_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_P_de, param_Gyro_P_en }, |
{ param_Gyro_Gier_P , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_P_de, param_Gyro_Gier_P_en }, |
{ param_Gyro_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_I_de, param_Gyro_I_en }, |
{ param_Gyro_Gier_I , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_Gier_I_de, param_Gyro_Gier_I_en }, |
{ param_Gyro_D , &editGeneric, GENERIC_ValuePoti , 0, 247, param_Gyro_D_de, param_Gyro_D_en }, |
{ param_DynamicStability , &editGeneric, GENERIC_ValuePoti , 0, 247, param_DynamicStability_de, param_DynamicStability_en }, |
{ param_GlobalConfig_DrehratenBegrenzer , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_DrehratenBegrenzer_de, param_GlobalConfig_DrehratenBegrenzer_en }, |
{ param_GyroAccFaktor , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccFaktor_de, param_GyroAccFaktor_en }, |
{ param_GyroAccAbgleich , &editGeneric, GENERIC_Value , 0, 247, param_GyroAccAbgleich_de, param_GyroAccAbgleich_en }, |
{ param_I_Faktor , &editGeneric, GENERIC_ValuePoti , 0, 247, param_I_Faktor_de, param_I_Faktor_en }, |
{ param_Driftkomp , &editGeneric, GENERIC_Value , 0, 247, param_Driftkomp_de, param_Driftkomp_en }, |
{ param_Gyro_Stability , &editGeneric, GENERIC_Value , 0, 247, param_Gyro_Stability_de, param_Gyro_Stability_en }, |
{ param_MotorSmooth , &editGeneric, GENERIC_Value , 0, 247, param_MotorSmooth_de, param_MotorSmooth_en }, |
//----------------------------------- |
// Menue: ID_MENU_BENUTZER (Benutzer) |
//----------------------------------- |
{ param_UserParam1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam1_de, param_UserParam1_en }, |
{ param_UserParam2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam2_de, param_UserParam2_en }, |
{ param_UserParam3 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam3_de, param_UserParam3_en }, |
{ param_UserParam4 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam4_de, param_UserParam4_en }, |
{ param_UserParam5 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam5_de, param_UserParam5_en }, |
{ param_UserParam6 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam6_de, param_UserParam6_en }, |
{ param_UserParam7 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam7_de, param_UserParam7_en }, |
{ param_UserParam8 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_UserParam8_de, param_UserParam8_en }, |
//----------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG (Achskopplung) |
//----------------------------------- |
{ param_GlobalConfig_AchsenkopplungAktiv , &editGeneric, GENERIC_NoYes , 0, 0, param_GlobalConfig_AchsenkopplungAktiv_de, param_GlobalConfig_AchsenkopplungAktiv_en }, |
{ param_AchsKopplung1 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung1_de, param_AchsKopplung1_en }, |
{ param_AchsKopplung2 , &editGeneric, GENERIC_ValuePoti , 0, 247, param_AchsKopplung2_de, param_AchsKopplung2_en }, |
{ param_CouplingYawCorrection , &editGeneric, GENERIC_ValuePoti , 0, 247, param_CouplingYawCorrection_de, param_CouplingYawCorrection_en }, |
//----------------------------------- |
// ENDE |
//----------------------------------- |
{ param_EOF, NOFUNC, 0,0,0, 0,0 } // END OF LIST - MUST BE THE LAST!! |
}; |
/************************************************************ |
NICHT MEHR UNTERSTUEZT: |
param_Receiver (Empfänger Typ: Hott, Jeti, Spektrum,...) |
************************************************************/ |
//############################################################################################# |
//# Menue's und Menuezuordnung |
//############################################################################################# |
//----------------------------------- |
// Menue ID's: Parameters Hauptmenue |
//----------------------------------- |
#define ID_MENU_FAVORITEN 1 // fuer spaeter reserviert.... |
#define ID_MENU_KANAELE 2 |
#define ID_MENU_KONFIGURATION 3 |
#define ID_MENU_STICK 4 |
#define ID_MENU_LOOPING 5 |
#define ID_MENU_HOEHE 6 |
#define ID_MENU_KAMERA 7 |
#define ID_MENU_NAVICTRL 8 |
#define ID_MENU_AUSGAENGE 9 |
#define ID_MENU_VERSCHIEDENES 10 |
#define ID_MENU_GYRO 11 |
#define ID_MENU_BENUTZER 12 |
#define ID_MENU_ACHSKOPPLUNG 13 |
#define ID_MENU_MIXERSETUP 14 // nicht verwendet / unterstuetzt |
#define ID_MENU_EASYSETUP 15 |
#define ID_MENU_TEST 66 // TEST / DEBUG |
//----------------------------------- |
// Zuordnungen von paramID's zu den jeweiligen Menue's |
// |
// Eintrag "SEPARATOR" - damit wird eine Trennlinie |
// im Menue dargestellt |
//----------------------------------- |
//------------------------------- |
// Menue: ID_MENU_KANAELE |
//------------------------------- |
unsigned char const ID_MENU_KANAELE_Items[] = |
{ |
param_Kanalbelegung_Gas, |
param_Kanalbelegung_Gear, |
param_Kanalbelegung_Nick, |
param_Kanalbelegung_Roll, |
param_Kanalbelegung_Poti1, |
param_Kanalbelegung_Poti2, |
param_Kanalbelegung_Poti3, |
param_Kanalbelegung_Poti4, |
param_Kanalbelegung_Poti5, |
param_Kanalbelegung_Poti6, |
param_Kanalbelegung_Poti7, |
param_Kanalbelegung_Poti8, |
SEPARATOR, |
param_MotorSafetySwitch, |
param_GlobalConfig3_MotorSwitchMode, |
SEPARATOR, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig3_SpeakAll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KONFIGURATION |
//------------------------------- |
unsigned char const ID_MENU_KONFIGURATION_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_GlobalConfig_GpsAktiv, |
param_GlobalConfig_KompassAktiv, |
param_GlobalConfig_KompassFix, |
param_ExtraConfig_SensitiveRc, |
param_GlobalConfig_AchsenkopplungAktiv, |
param_GlobalConfig_DrehratenBegrenzer, |
param_GlobalConfig_HeadingHold, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_STICK |
//------------------------------- |
unsigned char const ID_MENU_STICK_Items[] = |
{ |
param_Stick_P, |
param_Stick_D, |
param_StickGier_P, |
SEPARATOR, |
param_ExternalControl, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_LOOPING |
//------------------------------- |
unsigned char const ID_MENU_LOOPING_Items[] = |
{ |
param_BitConfig_LoopOben, |
param_BitConfig_LoopUnten, |
param_BitConfig_LoopLinks, |
param_BitConfig_LoopRechts, |
SEPARATOR, |
param_LoopGasLimit, |
param_LoopThreshold, |
param_LoopHysterese, |
param_WinkelUmschlagNick, |
param_WinkelUmschlagRoll, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_HOEHE |
//------------------------------- |
unsigned char const ID_MENU_HOEHE_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_ExtraConfig_HeightLimit, |
param_ExtraConfig_HeightVario, |
param_GlobalConfig_HoehenSchalter, |
param_ExtraConfig_VarioBeep, |
SEPARATOR, |
param_HoeheChannel, |
param_Hoehe_Verstaerkung, |
param_Hoehe_MinGas, |
param_Hoehe_HoverBand, |
param_Hoehe_P, |
param_Hoehe_GPS_Z, |
param_Hoehe_TiltCompensation, |
param_Luftdruck_D, |
param_Hoehe_StickNeutralPoint, |
param_Hoehe_ACC_Wirkung, |
param_MaxAltitude, |
SEPARATOR, |
param_StartLandChannel, |
param_LandingSpeed, |
param_LandingPulse, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_KAMERA |
//------------------------------- |
unsigned char const ID_MENU_KAMERA_Items[] = |
{ |
param_ServoNickControl, |
param_ServoNickComp, |
param_ServoNickFailsave, |
param_GlobalConfig3_ServoNickCompOff, // erst ab FC-Rev 96 |
param_ServoCompInvert_SERVO_NICK_INV, |
param_ServoCompInvert_SERVO_RELATIVE, |
param_ServoNickMin, |
param_ServoNickMax, |
param_ServoFilterNick, |
SEPARATOR, |
param_ServoRollControl, |
param_ServoRollComp, |
param_ServoRollFailsave, |
param_ServoCompInvert_SERVO_ROLL_INV, |
param_ServoRollMin, |
param_ServoRollMax, |
param_ServoFilterRoll, |
SEPARATOR, |
param_ServoNickRefresh, |
param_ServoManualControlSpeed, |
param_Servo3, |
param_Servo3Failsave, |
param_Servo4, |
param_Servo4Failsave, |
param_Servo5, |
param_Servo5Failsave, |
SEPARATOR, |
param_Servo3OnValue, |
param_Servo3OffValue, |
param_Servo4OnValue, |
param_Servo4OffValue, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_NAVICTRL |
//------------------------------- |
unsigned char const ID_MENU_NAVICTRL_Items[] = |
{ |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_NaviGpsGain, |
param_NaviStickThreshold, |
param_NaviGpsMinSat, |
param_NaviGpsP, |
param_NaviGpsPLimit, |
param_NaviGpsI, |
param_NaviGpsILimit, |
param_NaviGpsD, |
param_NaviGpsDLimit, |
param_NaviGpsA, |
SEPARATOR, |
param_NaviWindCorrection, |
param_NaviAccCompensation, |
param_NaviMaxFlyingRange, |
param_NaviOperatingRadius, |
param_NaviAngleLimitation, |
param_NaviPH_LoginTime, |
param_ExtraConfig_GpsAid, |
param_GlobalConfig3_DphMaxRadius, |
SEPARATOR, |
param_ComingHomeAltitude, |
param_ComingHomeOrientation, |
SEPARATOR, |
param_SingleWpControlChannel, |
param_MenuKeyChannel, |
param_SingleWpSpeed, |
SEPARATOR, |
param_NaviDescendRange, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_AUSGAENGE |
//------------------------------- |
unsigned char const ID_MENU_AUSGAENGE_Items[] = |
{ |
param_J16Bitmask, // LED 1 Bitmaske |
param_J16Timing, |
param_BitConfig_MotorBlink1, |
param_BitConfig_MotorOffLed1, |
param_GlobalConfig3_UseNcForOut1, |
param_NaviOut1Parameter, |
param_AutoPhotoDistance, |
param_AutoPhotoAtitudes, |
SEPARATOR, |
param_J17Bitmask, // LED 2 Bitmaske |
param_J17Timing, |
param_BitConfig_MotorBlink2, |
param_BitConfig_MotorOffLed2, |
SEPARATOR, |
param_WARN_J16_Bitmask, |
param_WARN_J17_Bitmask, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_VERSCHIEDENES |
//------------------------------- |
unsigned char const ID_MENU_VERSCHIEDENES_Items[] = |
{ |
param_Gas_Min, |
param_Gas_Max, |
param_KompassWirkung, |
param_CompassOffset, |
param_CompassOffset_DisableDeclCalc, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
SEPARATOR, |
param_UnterspannungsWarnung, |
param_ComingHomeVoltage, |
param_AutoLandingVoltage, |
SEPARATOR, |
param_NotGasZeit, |
param_GlobalConfig3_VarioFailsafe, // Nutze "Vario-Höhe" für "Not-Gas" -> beeinflusst 'param_NotGas' bzgl. "%" |
param_NotGas, |
param_FailSafeTime, |
param_FailsafeChannel, |
param_ExtraConfig_NoRcOffBeeping, |
SEPARATOR, |
param_ExtraConfig_IgnoreMagErrAtStartup, |
param_GlobalConfig3_NoSdCardNoStart, |
param_GlobalConfig3_NoGpsFixNoStart, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_GYRO |
//------------------------------- |
unsigned char const ID_MENU_GYRO_Items[] = |
{ |
param_Gyro_P, |
param_Gyro_I, |
param_Gyro_D, |
param_Gyro_Gier_P, |
param_Gyro_Gier_I, |
SEPARATOR, |
param_DynamicStability, |
param_GlobalConfig_DrehratenBegrenzer, |
SEPARATOR, |
param_GyroAccFaktor, |
param_GyroAccAbgleich, |
SEPARATOR, |
param_I_Faktor, |
param_Driftkomp, |
param_Gyro_Stability, |
param_MotorSmooth, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_BENUTZER |
//------------------------------- |
unsigned char const ID_MENU_BENUTZER_Items[] = |
{ |
param_UserParam1, |
param_UserParam2, |
param_UserParam3, |
param_UserParam4, |
param_UserParam5, |
param_UserParam6, |
param_UserParam7, |
param_UserParam8, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_ACHSKOPPLUNG |
//------------------------------- |
unsigned char const ID_MENU_ACHSKOPPLUNG_Items[] = |
{ |
param_GlobalConfig_AchsenkopplungAktiv, |
param_AchsKopplung1, |
param_AchsKopplung2, |
param_CouplingYawCorrection, |
EOF // *** MUST BE THE LAST! *** |
}; |
//------------------------------- |
// Menue: ID_MENU_EASYSETUP |
//------------------------------- |
unsigned char const ID_MENU_EASYSETUP_Items[] = |
{ |
param_GlobalConfig_HoehenRegelung, |
param_HoeheChannel, |
param_Hoehe_StickNeutralPoint, |
param_StartLandChannel, |
SEPARATOR, |
param_GlobalConfig_GpsAktiv, |
param_NaviGpsModeChannel, |
param_ExtraConfig_GpsAid, |
param_ComingHomeAltitude, |
SEPARATOR, |
param_CareFreeChannel, |
param_ExtraConfig_LearnableCarefree, |
EOF // *** MUST BE THE LAST! *** |
}; |
//############################################################################################# |
//# Error-Handling |
//############################################################################################# |
//# mittels des auf dem PKT angezeigten Errocodes (z.B. "E32") kann man hier schauen was bzw. |
//# wo (in welcher Funktion) der Fehler aufgetreten ist. |
//# |
//# Im Menue wird der Fehlercode rechts bei den Values dargestellt (dabei ertönen immer wieder |
//# Fehler-Beep's). |
//# In der Vollbildanzeige wird zusaetzlich ein uebergebener Wert (value) angezeigt - im Code |
//# kann man ermitteln was der Wert darstellt. |
//############################################################################################# |
#define E01_PARAMID_MENUEDITCATEGORY 1 // Menu_EditCategory() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E11_PARAMID_EDITGENERIC 11 // editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E12_PARAMID_EDITBITMASK 12 // editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E30_CODECHAR_NOT_FOUND 30 // find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E31_PARAMEDIT_ITEM_NOT_FOUND 31 // find_genericCodeByValue() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
#define E32_CODECHAR_NOT_FOUND 32 // find_genericCodeByValue() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
#define E33_VALUE_NOT_IN_FORMAT 33 // find_genericCodeByValue() - FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
#define E41_VALUE_MIN_MAX_RANGE 41 // strValueGeneric() - FEHLER: fuer die Range von 'v' (Value) wurde max < min angegeben bei Generic -> nicht unterstuetzt |
//-------------------------------------------------------------- |
// _error( errorcode, value, showscreen) |
// |
// zeigt Fehlermeldungen an |
// |
// PARAMETER: |
// errorcode : siehe Error-defines |
// value : zusaetzlicher Anzeigeparameter (nur in Screen-Anzeige) |
// showscreen: true = Fehlermeldung wird vollstaendig angezeigt. |
// - er Screen wird dabei geloescht |
// - Ende mit Taste 'Ende' |
// - langer Fehler-Beep |
// |
// false = es wird nur der Buffer mkparam_strValueBuffer |
// mit dem Fehlercode befuellt |
// - Anzeige im Menue ganz recht als "Enn" |
// - kurzer Fehler-Beep der jedoch bei jeder |
// Aktion im Menue zu hoeren ist |
// => dann auf Anzeige "Enn" achten! |
//-------------------------------------------------------------- |
void _error( uint8_t errorcode, uint8_t value, uint8_t showscreen) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("E%02u"), errorcode); // in mkparam_strValueBuffer ausgeben fuer das Menue |
if( showscreen ) |
{ |
lcd_cls(); |
lcdx_printp_center( 1, PSTR(" ERROR "), MINVERS, 0,-3); |
lcdx_printp_center( 2, PSTR("mkparameters.c"), MNORMAL, 0,0); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("Code : E%02u"), errorcode ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Value: %3u"), value ); |
lcd_printp_at(11, 7, strGet(ENDE), MNORMAL ); |
set_beep( 500, 0x000f, BeepNormal); // Beep Error |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
//-------------------------------- |
// Anmerkung: OG 25.03.2014 |
// |
// Ohne diese Verzoegerung funkioniert die Update-Erkennung via PKT-UpdateTool |
// meist nicht - ist das PKT zu schnell? |
// Kann man evtl. das PKT-UpdateTool diesbezueglich mal ueberpruefen ob man |
// Timings/Timeout's anpassen kann? |
// |
// Hinweis dazu: PKT_CtrlHook() bzw. die dort enthaltene Update-Anforderungspruefung |
// ist in den letzten Wochen beschleunigt geworden da unnoetiger Code fuer nicht |
// benoetigte Bildschirmausgaben entfernt wurde - das ist grundsaetzlich fuer das PKT |
// besser. |
// |
// Um das nachzustellen: |
// Unten das Delay auskommentieren und einen Fehler in Menu_EditCategory() provozieren |
// indem "paramID = 0;" einkommentiert wird (ist dort beschrieben) |
// Dann den Fehlerscreen im PKT anzeigen lassen und ein Update via PKT-UpdateTool initiieren, |
//-------------------------------- |
_delay_ms(150); |
PKT_CtrlHook(); |
} |
} |
else |
{ |
// kurzer Fehler-Beep - der kann wiederholt im Menue auftreten bei jedem Refresh des Menues |
set_beep( 160, 0x000f, BeepNormal); // Beep Error |
} |
} |
//############################################################################################# |
//# Edit-Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = find_paramEditItem( paramID ) |
// |
// sucht in paramEditItemTable (PROGMEM) eine paramID und |
// kopiert den Eintrag in das RAM |
// |
// PARAMETER: |
// paramID |
// |
// RUECKGABE: |
// true = paramID gefunden |
// false = nicht gefunden (Fehler) |
// |
// -- globale Modulvariablen -- |
// Ergebnisse in: |
// |
// paramEditItem : der gefundene Eintrag vom Typ paramEditItem_t (im RAM) |
// paramEditFormat: der Format-String paramEditItem.format im RAM |
//-------------------------------------------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ) |
{ |
unsigned char id; |
unsigned char *p; |
p = (unsigned char *) paramEditItemTable; |
while( true ) |
{ |
id = pgm_read_byte(p); // die aktuelle paramID aus paramEditDef |
if( id == paramID || id == param_EOF ) |
break; |
p += sizeof( paramEditItem_t ); |
} |
//----- |
// wenn die gesuchte paramID nicht gefunden wurde steht |
// im RAM-Buffer paramEdit.paramID == param_EOF !! |
//----- |
memcpy_P ( ¶mEditItem , p, sizeof(paramEditItem_t) ); // die Struktur -> in den RAM-Buffer kopieren (Zugriff: paramEditItem) |
strncpy_P( paramEditFormat, paramEditItem.format, MKPARAM_STRBUFFER_LEN); // format String -> in den RAM-Buffer kopieren (Zugriff: paramEditFormat) |
return( id!=param_EOF ); |
} |
//-------------------------------------------------------------- |
// ptrP = paramEditItemTitle() |
// |
// gibt einen Zeiger in PROGMEM auf den Titel/Bezeichnung eines |
// paramID's in der richtigen Sprache zurueck |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
/* |
const char * paramEditItemTitle( void ) |
{ |
if( Config.DisplayLanguage == 0 ) |
return( paramEditItem.title_de ); |
else |
return( paramEditItem.title_en ); |
} |
*/ |
//-------------------------------------------------------------- |
// print_paramEditItemTitle() |
// |
// gibt Titel/Bezeichnung auf den Bildschirm bei x=0, y=2 aus |
// |
// ACHTUNG! paramEditItem muss vorher initialisert worden sein! |
//-------------------------------------------------------------- |
void print_paramEditItemTitle( void ) |
{ |
lcd_printp_at( 0, 2, (Config.DisplayLanguage == 0 ? paramEditItem.title_de : paramEditItem.title_en ), MNORMAL); // Bezeichnung des paramID's |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( code ) |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCode( unsigned char code ) |
{ |
unsigned char codeIdx; |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != code ) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//----------------------------- |
// find_genericCode() - FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//----------------------------- |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E30_CODECHAR_NOT_FOUND, code, false); |
return false; |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
return true; |
} |
//-------------------------------------------------------------- |
// ok = find_genericCode( paramID, &v, &min, &max, pFormat) |
// |
// PARAMETER: |
// paramID |
// *v |
// *min |
// *max |
// *pFormat |
// |
// Ergebnis steht in: (globale Modulvariablen) |
// paramEditItem |
// paramEditFormat: der zugehoerige Format-String |
// genericCode |
//-------------------------------------------------------------- |
unsigned char find_genericCodeByValue( unsigned char paramID, unsigned char *v, unsigned char *min, unsigned char *max ) |
{ |
unsigned char codeIdx; |
char *pFormat; |
//------------------------------------------------- |
// 1. finde paramID in der paramEditItem-Tabelle |
// |
// sollte der RAM-Buffer paramEditItem bereits das |
// gesuchte enthalten dann ok |
//------------------------------------------------- |
if( paramEditItem.paramID != paramID ) |
find_paramEditItem( paramID ); |
//--- |
// FEHLER: paramEditItem nicht in Tabelle paramEditItemTable gefunden |
//--- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E31_PARAMEDIT_ITEM_NOT_FOUND, paramID, false); |
return false; |
} |
pFormat = paramEditFormat; // Zeiger auf das erste Zeichen vom Format-String (z.B. "DCSWFHQ") |
*v = paramGet( paramID ); // der aktuelle Wert des paraID |
while( true ) |
{ |
//------ |
// suche editGenericCode mittels aktuelles Zeichens *pFormat |
//------ |
codeIdx = 0; |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
//------ |
// FEHLER: Code-Zeichen nicht in Tabelle editGenericCode gefunden |
//------ |
if( editGenericCode[codeIdx].code == EOF ) |
{ |
_error( E32_CODECHAR_NOT_FOUND, *pFormat, false); |
return false; |
} |
//------ |
// suche den Value-Bereich (min/max) fuer das Code-Zeichen |
//------ |
if( editGenericCode[codeIdx].code == 'v' ) |
{ |
// bei code 'v' kommt min/max aus paramEditItem |
*min = paramEditItem.min; |
*max = paramEditItem.max; |
} |
else |
{ |
// ansonsten kommt min/max aus dem Code selber |
*min = editGenericCode[codeIdx].min; |
*max = editGenericCode[codeIdx].max; |
} |
// value gefunden (innerhalb von min/max)? |
// beruecksichtigt 'gedrehte' min/max (z.B. Poti1..8) |
if( (*min <= *max && *v >= *min && *v <= *max) || (*min > *max && *v >= *max && *v <= *min) ) |
{ |
// gefunden - Schleife verlassen |
break; |
} |
//------ |
// naechstes Code-Zeichen aus der Format-Maske |
//------ |
pFormat++; |
//------ |
// FEHLER: Ende des Format-Strings erreicht - fuer das gegebene value konnte kein passender Code im Format-String gefunden werden |
//------ |
if( *pFormat == 0 ) // Ende des Format-Strings erreicht - Fehler... |
{ |
_error( E33_VALUE_NOT_IN_FORMAT, *v, false); |
return false; |
} |
} |
memcpy( &genericCode, &editGenericCode[codeIdx], sizeof(editGenericCode_t) ); // die gefundene Struktur in den RAM-Buffer kopieren |
genericCode.min = *min; // ggf. min/max ueberschreiben durch min/max von paramEditItem |
genericCode.max = *max; |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
const char * get_genericCodeText( uint8_t cmd ) |
{ |
if( cmd == MKPARAM_LONGVALUE ) |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.longText_de; |
else return genericCode.longText_en; |
} |
else |
{ |
if( Config.DisplayLanguage == 0 ) return genericCode.shortText_de; |
else return genericCode.shortText_en; |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// ok = strValueGeneric( paramID, cmd ) |
// |
// erstellt die String Representation des Wertes von paramID |
// im Buffer mkparam_strValueBuffer. |
// |
// Mit 'cmd' wird gesteuert ob es der Lang-Text oder der |
// Kurztext (3 Zeichen) ist. |
// |
// PARAMETER: |
// paramID: |
// cmd: MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
uint8_t strValueGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
unsigned char min; |
unsigned char max; |
if( find_genericCodeByValue( paramID, &v, &min, &max) ) |
{ |
//------------------- |
// value gefunden! |
//------------------- |
if( genericCode.code == 'v' ) // Code 'v' (Value) - keine Umrechnung von min/max |
{ |
if( max < min ) |
{ |
// max < min bei 'v' (Value) wird aktuell nicht unterstuezt da |
// kein Beispiel vorhanden ist was damit ggf. gemeint ist. |
// Wenn das irgendwann benoetigt wird muss das hier gecoded werden. |
// Bis dahin wird das als Fehler angesehen (ggf. vertippt beim coden). |
_error( E41_VALUE_MIN_MAX_RANGE, v, false); |
return false; |
} |
} |
else // alles andere ausser 'v' - der Wert wird in die Range von min/max umgerechnet |
{ |
if( min <= max ) |
v = v-min; |
else |
v = min-v; // OG Notiz: v = 248 = 8 # 248 = 8, 255 = 1 # min 255 max = 248 |
v++; // fuer Anzeige +1: statt '0' eine '1' anzeigen - Beispiel: statt "Poti0" zeige "Poti1" |
} |
//------------------- |
// ertstelle String |
//------------------- |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, get_genericCodeText(cmd), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// ok = getGenericNextValue( paramID, vInc ) |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t getGenericNextValue( unsigned char paramID, int8_t vInc ) |
{ |
int v; // value von paramID - signed int: damit -1 und >255 ausgewertet werden kann |
unsigned char value; // zum holen von v via find_genericCodeByValue() |
unsigned char min; |
unsigned char max; |
unsigned char codeIdx; |
char *pFormat; |
if( find_genericCodeByValue( paramID, &value, &min, &max) ) |
{ |
v = (int)value; // value vom paramID wird gecastet auf int um -1 und >255 fuer edit zu erkennen |
//------------------------------------------------------ |
// Pointer auf das Zeichen im Format-String ermitteln |
//------------------------------------------------------ |
pFormat = paramEditFormat; |
while( *pFormat != genericCode.code ) pFormat++; |
if( min > max ) // 'umgedrehte' min/max Werte wie bei Poti1..8 |
vInc = vInc * -1; |
v = v + vInc; |
//------------------------------------------------------ |
// min/max Grenzen des Code-Zeichens ueberschritten? |
//------------------------------------------------------ |
if( (min <= max && v < min) || (min > max && v > min) ) |
{ |
if( pFormat == paramEditFormat ) // Anfang vom Code-String? -> dann gehe zum Ende (letztes Zeichen) |
{ |
while( *(pFormat+1) ) pFormat++; |
} |
else |
{ |
pFormat--; // vorheriges Format-Zeichen = vorheriger Code |
} |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.max; |
else |
v = editGenericCode[codeIdx].max; |
} |
else if( (min <= max && v > max) || (min > max && v < max) ) |
{ |
pFormat++; // naechstes Format-Zeichen = naechster Code |
if( *pFormat == 0 ) // Ende des Format-Strings? |
pFormat = paramEditFormat; |
codeIdx = 0; // code-Zeichen suchen in Tabelle editGenericCode |
while( (editGenericCode[codeIdx].code != *pFormat) && (editGenericCode[codeIdx].code != EOF) ) |
codeIdx++; |
if( editGenericCode[codeIdx].code == EOF ) // Fehler? -> exit |
{ |
return false; |
} |
if( editGenericCode[codeIdx].code == 'v' ) |
v = paramEditItem.min; |
else |
v = editGenericCode[codeIdx].min; |
} |
//------------------------------------------------------ |
// neuen Wert setzen |
//------------------------------------------------------ |
paramSet( paramID, (unsigned char)v ); |
return true; |
} // end: if( find_genericCode( paramID, &v, &min, &max) ) |
return false; |
} |
//-------------------------------------------------------------- |
// editGeneric( paramID, cmd ) |
// |
// PARAMETER: |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editGeneric( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v_org; |
uint8_t redraw; |
int8_t vInc; |
if( cmd != MKPARAM_EDIT ) |
{ |
// wenn cmd = MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
strValueGeneric( paramID, cmd); // Ergebnis in: mkparam_strValueBuffer |
return; |
} |
find_paramEditItem( paramID ); |
//------------------------ |
// editGeneric() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//------------------------ |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E11_PARAMID_EDITGENERIC, paramID, true); |
return; |
} |
lcd_cls(); |
v_org = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
vInc = 0; // 0, +1, -1 (inc/dec) |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
strValueGeneric( paramID, MKPARAM_LONGVALUE); // Lang-Text des Values anzeigen |
// nach strValueGeneric() ist auch genericCode initialisiert! |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %18s"), mkparam_strValueBuffer ); |
// DEBUG... |
#ifdef DEBUG_PARAMEDIT |
//lcd_printf_at_P( 16, 1, MNORMAL, PSTR("%c %3u"), genericCode.code, paramGet(paramID) ); |
#endif |
//lcd_printf_at_P( 16, 4, MNORMAL, PSTR("= %03u"), v ); |
if( abs(genericCode.max-genericCode.min) <= 20 ) |
_delay_ms( 200 ); // Verzoegerung |
vInc = 0; |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_press(1 << KEY_ESC) ) // Key: Cancel |
{ |
paramSet( paramID, v_org ); // org. Wert wieder herstellen |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // Key: OK |
{ |
break; |
} |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) // Key: rechts / +1 |
{ |
vInc = +1; |
} |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) // Key: links / -1 |
{ |
vInc = -1; |
} |
if( vInc != 0 ) |
{ |
getGenericNextValue( paramID, vInc ); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
// editBitmask( paramID, cmd ) |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editBitmask( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char v; |
uint8_t redraw; |
uint8_t i; |
int8_t pos; |
find_paramEditItem( paramID ); |
//----------------------- |
// editBitmask() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
v = paramGet( paramID ); |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%03u"), v); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
lcd_cls(); |
v = paramGet( paramID ); |
redraw = SCREEN_REDRAW; |
pos = 0; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern"), MINVERS, 0,0); // Titel |
lcd_printp_at( 1, 7, PSTR("0/1 \x19 OK"), MNORMAL); // Keyline: -> 0/1 OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
lcdx_printp_at( 7, 6, PSTR("\x18"), MNORMAL, -2,2); // Keyline langer Tastendruck: -> 0/1 OK |
if( (paramID == param_J16Bitmask) || (paramID == param_J17Bitmask) ) |
{ |
lcdx_printp_at( 10, 5, PSTR("(Bit1=Idle)"), MNORMAL, 0,2); // |
} |
print_paramEditItemTitle(); // Bezeichnung des paramID's (Stelle x=0, y=2) |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: nur Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
for(i = 0; i < 8; i++) |
{ |
lcd_putc( 8-i, 4, ( (v & (1 << i)) ? '1' : '0'), MNORMAL); |
} |
lcd_frect( 0, (8*5), 125-(11*6), 7, 0); // clear: Eingabemarkierung |
lcd_printp_at (pos+1, 5,PSTR("\x12"), MNORMAL); // Eingabemarkierung (Pfeil nach oben) |
lcd_printf_at_P( 15, 4, MNORMAL, PSTR("= %03u"), v ); // Anzeige des aktuellen Wertes Dezimal |
// _delay_ms( 200 ); // Verzoegerung |
redraw = 0; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if (get_key_short(1 << KEY_ENTER)) // Key: OK |
{ |
paramSet( paramID, v ); // Wert speichern |
break; // und verlassen |
} |
if( get_key_short(1 << KEY_PLUS) ) |
{ |
if(pos == 7) pos = 0; |
else pos++; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if(pos == 0) pos = 7; |
else pos--; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short (1 << KEY_MINUS) ) |
{ |
v ^= (1<<(7-pos)); |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editCompassOffset( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
int v; // signed |
uint8_t redraw; |
uint8_t bit7; |
uint8_t bit8; |
uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editCompassOffset() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
//----------------------- |
// Wert ermitteln (-60 bis 60) |
//----------------------- |
vu = paramGet( paramID ); |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
bit8 = ((vu & 0x80) ? true : false); // Bit 8 in Kombination mit Bit 7 = "Disable dec. Calc" |
// Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// Bit 8 != Bit 7: "Disable dec. Calc" AN |
lDisDeclCalc = ((bit8 == bit7) ? 0 : 1); // merken um das spaeter wieder in den Wert einzubauen |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
if( bit7 ) // Wert negativ? |
{ |
vu = vu - 1; // Umrechnung: 7-Bit Zweierkomplement |
vu = (vu ^ 0xff); // invertieren |
vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
v = (int)vu; |
v = v * -1; |
} |
else // Wert ist positiv |
{ |
v = (int)vu; |
} |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%3d"), v ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %3d"), v ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
//------------------------------------- |
// signed int in unsigned byte wandeln |
//------------------------------------- |
if( v < 0 ) // Wert negativ? |
{ |
// Umwandeln in 7-Bit Zweierkomplement |
v = v * -1; // Wert positiv machen |
vu = (unsigned char)v; // in unsigned Byte speichern |
vu = (vu ^ 0xff); // invertieren |
vu = vu + 1; // +1 |
} |
else // Wert ist positiv |
{ |
vu = (unsigned char)v; // in unsigned Byte speichern |
} |
//------------------------------------- |
// "Disable declination calc" wieder einrechnen |
//------------------------------------- |
bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
if( v <= -60 ) v = 60; |
else v = v - 1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
if( v >= 60 ) v = -60; |
else v = v + 1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ) |
{ |
unsigned char vu; // unsigned byte |
// int v; // signed |
uint8_t redraw; |
// uint8_t bit7; |
// uint8_t bit8; |
// uint8_t lDisDeclCalc; |
find_paramEditItem( paramID ); |
//----------------------- |
// editACCZLandingPulse() - FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden |
//----------------------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
// TODO: Fehlercode anpassen! |
_error( E12_PARAMID_EDITBITMASK, paramID, (cmd==MKPARAM_EDIT) ); |
return; |
} |
vu = paramGet( paramID ); |
//----------------------- |
// nur Rueckgabe des Wertes in mkparam_strValueBuffer ? |
//----------------------- |
if( cmd != MKPARAM_EDIT ) |
{ |
xsnprintf_P( mkparam_strValueBuffer, MKPARAM_STRBUFFER_LEN, PSTR("%4d"), vu*4 ); // erzeuge den Ausgabestring in mkparam_strValueBuffer |
return; |
} |
//----------------------- |
// Wert Editieren |
//----------------------- |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
while( true ) |
{ |
//----------------------- |
// Anzeige: Titel, usw.. |
//----------------------- |
if( redraw == SCREEN_REDRAW ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill |
lcdx_printp_at( 1, 0, PSTR("Ändern") , MINVERS, 0,0); // Titel |
lcd_printp_at( 0, 7, strGet(KEYLINE2) , MNORMAL); // Keyline: <- -> Ende OK |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." statt "Ende" einsetzen |
print_paramEditItemTitle(); // Bezeichnung des paramID's anzeigen |
redraw = SCREEN_REFRESH; |
} |
//--------------- |
// LiPo Anzeigen |
//--------------- |
show_Lipo(); // LiPo anzeigen |
//----------------------- |
// Anzeige: Wert |
//----------------------- |
if( redraw == SCREEN_REFRESH ) |
{ |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("> %4d"), vu *4 ); |
redraw = false; |
} |
//----------------- |
// TASTEN |
//----------------- |
if( get_key_short(1 << KEY_ESC) ) // Key: Cancel |
{ |
break; // verlassen |
} |
if( get_key_short(1 << KEY_ENTER) ) // Key: OK |
{ |
// //------------------------------------- |
// // signed int in unsigned byte wandeln |
// //------------------------------------- |
// if( v < 0 ) // Wert negativ? |
// { |
// // Umwandeln in 7-Bit Zweierkomplement |
// v = v * -1; // Wert positiv machen |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// vu = (vu ^ 0xff); // invertieren |
// vu = vu + 1; // +1 |
// } |
// else // Wert ist positiv |
// { |
// vu = (unsigned char)v; // in unsigned Byte speichern |
// } |
// |
// //------------------------------------- |
// // "Disable declination calc" wieder einrechnen |
// //------------------------------------- |
// bit7 = ((vu & 0x40) ? true : false); // Bit 7: 1 = negativ; 0 = positiv |
// |
// if( lDisDeclCalc ) bit8 = !bit7; // Bit 8 != Bit 7: "Disable dec. Calc" AN |
// else bit8 = bit7; // Bit 8 == Bit 7: "Disable dec. Calc" AUS |
// |
// if( bit8 ) vu = (vu | 0x80); // Bit 8 setzen |
// else vu = (vu & (0x80 ^ 0xff)); // Bit 8 loeschen |
//------------------------------------- |
// Wert speichern |
//------------------------------------- |
paramSet( paramID, vu ); // Wert speichern |
break; // und verlassen |
} // end: KEY_ENTER |
if( get_key_short(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2) ) |
{ |
// if( v <= -60 ) v = 60; |
// else v = v - 1; |
// |
if( vu < 191 ) vu = 190; |
else vu = vu -1; |
redraw = SCREEN_REFRESH; |
} |
if( get_key_short(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2) ) |
{ |
// if( v >= 60 ) v = -60; |
// else v = v + 1; |
if( vu >= 255 ) vu = 255; |
// if( vu == 1 ) vu = 191; |
else vu = vu +1; |
redraw = SCREEN_REFRESH; |
} |
//------------------------------------------ |
// evtl. weitere lange Tasten abfangen, da es |
// ansonsten ggf. Nebeneffekte bzgl. dem Menue |
// beim verlassen der Funktion gibt |
//------------------------------------------ |
get_key_long_rpt_sp( KEY_ALL,2); |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = SCREEN_REDRAW; |
} |
} // end: while( true ) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// editNA( paramID, cmd ) |
// |
// Hilfsfunktion die nur "nicht verfügbar" anzeigt wenn die |
// paramID noch nicht implementiert ist |
// |
// PARAMETER: |
// paramID: die paramID |
// cmd: MKPARAM_EDIT oder MKPARAM_SHORTVALUE oder MKPARAM_LONGVALUE |
//-------------------------------------------------------------- |
void editNA( unsigned char paramID, uint8_t cmd ) |
{ |
if( cmd != MKPARAM_EDIT ) |
{ |
strncpy_P( mkparam_strValueBuffer, PSTR("NA!"), MKPARAM_STRBUFFER_LEN); // "NA!" bzw, "not available" |
return; |
} |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 2000, true, true ); // "nicht verfügbar" |
} |
//############################################################################################# |
//# TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void editGenericTEST( unsigned char paramID ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
find_paramEditItem( paramID ); // Ergebnis in paramEditItem (RAM) |
if( paramEditItem.paramID == paramID ) |
{ |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("%S"), paramEditItem.title_de ); |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("format: %s"), paramEditFormat ); |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("value:%u"), paramGet(paramID) ); |
strValueGeneric( paramID, MKPARAM_LONGVALUE); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
strValueGeneric( paramID, MKPARAM_SHORTVALUE); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("vstr: %s"), mkparam_strValueBuffer ); |
} |
if( paramEditItem.paramID == param_EOF ) |
{ |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("! ERROR !") ); |
} |
//----------------------------------------- |
// Tasten... |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void TEST(void) |
{ |
//editGeneric( param_Gyro_Gier_P, MKPARAM_EDIT ); |
editGeneric( param_Kanalbelegung_Gas, MKPARAM_EDIT ); |
} |
//############################################################################################# |
//# Menues & Favoriten |
//############################################################################################# |
//-------------------------------------------------------------- |
// fav_add() |
// |
// einen Favoriten hinzufuegen |
// |
// Aufruf durch: wird durch den Menu-Controller aufgerufen |
//-------------------------------------------------------------- |
void fav_add( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//------------------------------ |
// welche paramID wurde gewaehlt? |
//------------------------------ |
paramID = MenuCtrl_GetItemId(); // gewaehlter Menuepunkt bzw. paramID (0 = keine itemID) |
//------------------------------ |
// ungueltige paramID? |
//------------------------------ |
if( paramID==0 || paramID==EOF ) |
{ |
return; // keine gueltige paramID gewaehlt -> ZURUECK |
} |
//------------------------------ |
// Duplikatsuche bzgl. paramID |
// (gilt nicht fuer SEPARATOR) |
//------------------------------ |
if( paramID!=SEPARATOR ) |
{ |
//------------------------------ |
// suche Duplikate fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=paramID); i++); |
//------------------------------ |
// Favoriten bereits vorhanden? |
// -> exit |
//------------------------------ |
if( Config.MKParam_Favs[i] == paramID ) |
{ |
set_beep( 300, 0x000f, BeepNormal); // Error Beep kurz |
PKT_Popup_P( 400, strGet(STR_FAV_EXIST),0,0,0); // "* Fav vorhanden *" (ca. 4 sec max.) |
return; |
} |
} // end: if( paramID!=SEPARATOR ) |
//------------------------------ |
// suche freien Speicherplatz fuer paramID |
//------------------------------ |
for( i=0; (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0); i++); |
//------------------------------ |
// Favoriten voll? |
// -> exit |
//------------------------------ |
if( i >= MAX_MKPARAM_FAVORITES ) |
{ |
set_beep( 500, 0x000f, BeepNormal); // Error Beep lang |
PKT_Popup_P( 400, strGet(STR_FAV_FULL),0,0,0); // "* Fav ist voll *" (ca. 4 sec max.) |
return; |
} |
//------------------------------ |
// neuen Fav merken |
//------------------------------ |
Config.MKParam_Favs[i] = paramID; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
PKT_Popup_P( 400, strGet(STR_FAV_ADD),0,0,0); // "Fav hinzugefügt!" (ca. 4 sec max.) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Favoriten( void ) |
{ |
uint8_t paramID; |
uint8_t i; |
//unsigned char MKParam_Favs[MAX_MKPARAM_FAVORITES]; // Array von MK-Parameter Favoriten des Benutzers |
if( Config.MKParam_Favs[0] == 0 ) // Favoriten vorhanden |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_FAV_NOTEXIST), false, 500, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
MenuCtrl_SetDelete( true ); // Menueeintraege loeschen (langer Druck auf "OK") |
MenuCtrl_SetMove( true ); // Menueeintraege verschieben (langer Druck auf hoch/runter) |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menu-Items |
//--------------- |
i = 0; |
while( (i<MAX_MKPARAM_FAVORITES) && (Config.MKParam_Favs[i]!=0) ) |
{ |
paramID = Config.MKParam_Favs[i]; |
if( paramID == SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
} |
else |
{ |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
MenuCtrl_PushParamEdit( paramID ); |
// existiert die paramID in der Firmware des Kopters? |
if( !paramExist(paramID) ) |
{ |
MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} |
// DEBUG! (Simulation eines deaktivierten Fav's) |
// if( i==2 ) MenuCtrl_ItemActive( paramID, false ); // paramID deaktivieren |
} // end: else: if( paramID == SEPARATOR ) |
i++; |
} // end: while(..) |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // KEY_ESC = Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// ggf. neue Menuereihenfolge |
// in der Config speichern |
//--------------- |
for( i=0; (i<MAX_MKPARAM_FAVORITES); i++) |
{ |
Config.MKParam_Favs[i] = MenuCtrl_GetItemIdByIndex( i ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_EditCategory( const unsigned char *menuitems ) |
{ |
unsigned char paramID; |
unsigned char lastmenuitem = 0; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // uebernimmt den Titel vom uebergeordneten Menuepunkt |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
MenuCtrl_SetKey( KEY_ENTER_LONG, 0, &fav_add ); |
//--------------- |
// Menuitems |
//--------------- |
while( *menuitems != EOF ) |
{ |
paramID = *menuitems; |
if( paramID == SEPARATOR ) |
{ |
// wenn der vorherige Menueeintrag bereits ein SEPARATOR war |
// dann nicht erneut einen Separator anzeigen! |
// |
// Das kann ggf. passieren wenn z.B. aufgrund der FC-Revision |
// alle Zwischeneintraege ausgeblendet wurden |
if( lastmenuitem != SEPARATOR ) |
{ |
MenuCtrl_PushSeparatorID( SEPARATOR ); // Trennlinie im Menue hinzufuegen |
lastmenuitem = paramID; |
} |
} |
else |
{ |
//------------------------------------------------------ |
// TEST / DEBUG fuer beschriebenes Problem in _error() |
// |
// Wenn man das dortige Problem nachstellen will kann |
// man hier einen Fehlerscreen provozieren! |
//------------------------------------------------------ |
//paramID = 0; |
find_paramEditItem( paramID ); |
//-------- |
// FEHLER: paramEditItem (=paramID) nicht in Tabelle paramEditItemTable gefunden - Menu_EditCategory() |
//-------- |
if( paramEditItem.paramID == param_EOF ) |
{ |
_error( E01_PARAMID_MENUEDITCATEGORY, paramID, true); |
MenuCtrl_Destroy(); |
return; |
} |
// existiert die paramID in der Firmware des Kopters? |
if( paramExist(paramEditItem.paramID) ) |
{ |
MenuCtrl_PushParamEdit( paramEditItem.paramID ); |
lastmenuitem = paramID; |
} |
} |
menuitems++; |
} |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // Menue beenden |
paramID = MenuCtrl_GetItemId(); // welcher Menu-Punkt (= paramID) |
find_paramEditItem( paramID ); // Edit-Definitionen heraussuchen (initialisiert: paramEditItem) |
paramEditItem.editfunc( paramID, MKPARAM_EDIT ); // zugeordnete Edit-Funktion aufrufen |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Parameters_MenuMain( uint8_t setting, char *settingname ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitle( settingname ); |
MenuCtrl_ShowLevel( false ); |
//MenuCtrl_SetShowBatt( true ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
//MenuCtrl_PushML2_P( ID_MENU_TEST , MENU_ITEM, &TEST, ID_MENU_TEST_de , ID_MENU_TEST_en ); |
MenuCtrl_PushML2_P( ID_MENU_FAVORITEN , MENU_ITEM, NOFUNC, ID_MENU_FAVORITEN_de , ID_MENU_FAVORITEN_en ); // Favoriten: noch nicht implementiert... |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( ID_MENU_KANAELE , MENU_ITEM, NOFUNC, ID_MENU_KANAELE_de , ID_MENU_KANAELE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KONFIGURATION, MENU_ITEM, NOFUNC, ID_MENU_KONFIGURATION_de, ID_MENU_KONFIGURATION_en ); |
MenuCtrl_PushML2_P( ID_MENU_STICK , MENU_ITEM, NOFUNC, ID_MENU_STICK_de , ID_MENU_STICK_en ); |
MenuCtrl_PushML2_P( ID_MENU_LOOPING , MENU_ITEM, NOFUNC, ID_MENU_LOOPING_de , ID_MENU_LOOPING_en ); |
MenuCtrl_PushML2_P( ID_MENU_HOEHE , MENU_ITEM, NOFUNC, ID_MENU_HOEHE_de , ID_MENU_HOEHE_en ); |
MenuCtrl_PushML2_P( ID_MENU_KAMERA , MENU_ITEM, NOFUNC, ID_MENU_KAMERA_de , ID_MENU_KAMERA_en ); |
MenuCtrl_PushML2_P( ID_MENU_NAVICTRL , MENU_ITEM, NOFUNC, ID_MENU_NAVICTRL_de , ID_MENU_NAVICTRL_en ); |
MenuCtrl_PushML2_P( ID_MENU_AUSGAENGE , MENU_ITEM, NOFUNC, ID_MENU_AUSGAENGE_de , ID_MENU_AUSGAENGE_en ); |
MenuCtrl_PushML2_P( ID_MENU_VERSCHIEDENES, MENU_ITEM, NOFUNC, ID_MENU_VERSCHIEDENES_de, ID_MENU_VERSCHIEDENES_en ); |
MenuCtrl_PushML2_P( ID_MENU_GYRO , MENU_ITEM, NOFUNC, ID_MENU_GYRO_de , ID_MENU_GYRO_en ); |
MenuCtrl_PushML2_P( ID_MENU_BENUTZER , MENU_ITEM, NOFUNC, ID_MENU_BENUTZER_de , ID_MENU_BENUTZER_en ); |
MenuCtrl_PushML2_P( ID_MENU_ACHSKOPPLUNG , MENU_ITEM, NOFUNC, ID_MENU_ACHSKOPPLUNG_de , ID_MENU_ACHSKOPPLUNG_en ); |
//MenuCtrl_PushML2_P( ID_MENU_MIXERSETUP , MENU_ITEM, NOFUNC, ID_MENU_MIXERSETUP_de , ID_MENU_MIXERSETUP_en ); // nicht mehr unterstuetzt! |
MenuCtrl_PushML2_P( ID_MENU_EASYSETUP , MENU_ITEM, NOFUNC, ID_MENU_EASYSETUP_de , ID_MENU_EASYSETUP_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
switch( itemid ) |
{ |
case ID_MENU_FAVORITEN: Menu_Favoriten(); break; |
case ID_MENU_KANAELE: Menu_EditCategory( ID_MENU_KANAELE_Items ); break; |
case ID_MENU_KONFIGURATION: Menu_EditCategory( ID_MENU_KONFIGURATION_Items ); break; |
case ID_MENU_STICK: Menu_EditCategory( ID_MENU_STICK_Items ); break; |
case ID_MENU_LOOPING: Menu_EditCategory( ID_MENU_LOOPING_Items ); break; |
case ID_MENU_HOEHE: Menu_EditCategory( ID_MENU_HOEHE_Items ); break; |
case ID_MENU_KAMERA: Menu_EditCategory( ID_MENU_KAMERA_Items ); break; |
case ID_MENU_NAVICTRL: Menu_EditCategory( ID_MENU_NAVICTRL_Items ); break; |
case ID_MENU_AUSGAENGE: Menu_EditCategory( ID_MENU_AUSGAENGE_Items ); break; |
case ID_MENU_VERSCHIEDENES: Menu_EditCategory( ID_MENU_VERSCHIEDENES_Items ); break; |
case ID_MENU_GYRO: Menu_EditCategory( ID_MENU_GYRO_Items ); break; |
case ID_MENU_BENUTZER: Menu_EditCategory( ID_MENU_BENUTZER_Items ); break; |
case ID_MENU_ACHSKOPPLUNG: Menu_EditCategory( ID_MENU_ACHSKOPPLUNG_Items ); break; |
case ID_MENU_EASYSETUP: Menu_EditCategory( ID_MENU_EASYSETUP_Items ); break; |
default: //PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("nicht verfügbar"), false, 200, true, true ); // Anzeige "nicht verfügbar" (max. 2 Sekunden anzeigen) |
break; |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################################# |
//# MK_Parameters() - Main-Function |
//############################################################################################# |
//-------------------------------------------------------------- |
// changed = MK_Parameters( setting, settingname ) |
//-------------------------------------------------------------- |
uint8_t MK_Parameters( uint8_t setting, char *settingname ) |
{ |
int changed; |
unsigned char *org_parameters; |
uint8_t size = paramsetSize(); |
//----------------------------------------------------------------- |
// Erkennung ob Aenderungen durch den Benutzer vorgenommen wurde |
// -> das aktuelle Paramset wird gespeichert um es spaeter mit |
// der bearbeiteten Version via memcmp zu vergleichen |
//----------------------------------------------------------------- |
org_parameters = malloc( size+1 ); // +1 fuer das erste settings-byte |
if( !org_parameters ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return false; |
} |
memcpy( org_parameters, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
//----------------------------------------------------------------- |
// MK-Parameter bearbeiten |
//----------------------------------------------------------------- |
MK_Parameters_MenuMain( setting, settingname ); |
//----------------------------------------------------------------- |
// Vergleich: Orginal-Daten <-> ggf. geaenderte Daten |
//----------------------------------------------------------------- |
changed = memcmp( org_parameters, (unsigned char *)(pRxData), size+1 ); |
free( org_parameters ); |
return( changed!=0 ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/mkparameters.h |
---|
0,0 → 1,95 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters.h |
//# |
//# 27.03.2014 OG |
//# etliche neue Funktionen / Strukturen |
//# |
//# 23.02.2014 OG |
//# - chg: MK_Parameters_Menu() umbenannt zu MK_Parameters() |
//# |
//# 12.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
// fuer cmd-Parameter der Edit-Funktionen |
#define MKPARAM_EDIT 1 // Wert bearbeiten |
#define MKPARAM_SHORTVALUE 2 // erzeugt einen 3-Zeichen String der den Wert darstellt (z.B. "Dis"); Ergebnis in strValueBuffer |
#define MKPARAM_LONGVALUE 3 // erzeugt einen String der den Wert in langer Form darstellt (z.B. "Disabled"); Ergebnis in strValueBuffer |
#define MKPARAM_STRBUFFER_LEN 20 |
//-------------------------------------------- |
// paramEditItem_t |
// deklariert die Edit-Daten einer paramID |
//-------------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID aus paramset.h |
void (*editfunc)(unsigned char paramID, uint8_t cmd); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
const char *format; // Parameter: String (PROGMEM) (vorallem fuer editGeneric() ) |
unsigned char min; // Parameter: min (P1) |
unsigned char max; // Parameter: max (P2) |
const char *title_de; // Text in PROGMEM - Menuetext und Beschreibung im Edit-Screen |
const char *title_en; // Text in PROGMEM |
} paramEditItem_t; |
extern paramEditItem_t paramEditItem; // RAM Buffer: fuer ein Element von paramEditDef |
extern char paramEditFormat[MKPARAM_STRBUFFER_LEN]; // RAM Buffer: fuer 'str' von paramEdit (Format; editGeneric) |
extern char mkparam_strValueBuffer[MKPARAM_STRBUFFER_LEN]; // Anzeige eines Values als Klartext; Kurz (fuer das Menue) oder Lang (in der Edit-Funktion) |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char find_paramEditItem( unsigned char paramID ); |
void editGeneric( unsigned char paramID, uint8_t cmd ); |
void editBitmask( unsigned char paramID, uint8_t cmd ); |
void editDisableDeclCalc( unsigned char paramID, uint8_t cmd ); |
void editCompassOffset( unsigned char paramID, uint8_t cmd ); |
void editACCZLandingPulse( unsigned char paramID, uint8_t cmd ); |
void editNA( unsigned char paramID, uint8_t cmd ); |
uint8_t MK_Parameters( uint8_t setting, char *settingname ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/mkparameters_messages.h |
---|
0,0 → 1,618 |
/***************************************************************************** |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mkparameters_messages.h |
//# |
//# 16.07.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.11a inkl. der ID's |
//# const char param_singleWpControlChannel_de |
//# const char param_MenuKeyChannel_de |
//# |
//# 09.04.2015 Cebra |
//# - add: Texte für neue Parameter in FC 2.09j |
//# const char param_ServoNickFails_de[],const char param_ServoRollFails_de[], const char param_Servo3Fails_de[], |
//# const char param_Servo4Fails_de[], const char param_Servo5Fails_de[] |
//# |
//# 26.09.2014 Cebra |
//# - add: Text Höhe Tilt Compensation (FC207f) |
//# |
//# 04.06.2014 OG |
//# - chg: Text von ID_MENU_FAVORITEN etwas gekuerzt |
//# |
//# 07.05.2014 OG |
//# - chg: ID_MENU_FAVORITEN_en |
//# |
//# 18.04.2014 OG |
//# - chg: Textanpassung: param_NaviGpsMinSat_de, param_NaviStickThreshold_de |
//# |
//# 17.04.2014 OG |
//# - add: param_Servo3OnValue, param_Servo3OffValue, param_Servo4OnValue |
//# param_Servo4OffValue |
//# - add: param_NaviMaxFlyingRange, param_NaviDescendRange |
//# - chg: Textanpassung: param_WARN_J16_Bitmask_de, param_WARN_J17_Bitmask_de |
//# - chg: Textanpassung: param_J16Bitmask, param_J16Timing |
//# - chg: Textanpassung: param_J17Bitmask, param_J17Timing |
//# - chg: Textanpassung: param_SingleWpSpeed_de |
//# - chg: Textanpassung: param_ServoNickRefresh_de, param_ServoManualControlSpeed_de |
//# - chg: Textanpassung: param_ServoRollControl_de |
//# |
//# 07.04.2014 OG |
//# - chg: kleine Aenderungen an englischen Texten |
//# |
//# 06.04.2014 CB |
//# - add: englische Menütexte ergänzt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# |
//# 28.03.2014 OG |
//# - add: paramID-Texte fuer Rev. 100 |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 16.03.2014 OG |
//# erste groesstenteils fertige Version |
//# |
//# 23.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_MESSAGES_H |
#define _MKPARAMETERS_MESSAGES_H |
//############################################################################################# |
//# Texte: Menues von mkparameters |
//############################################################################################# |
static const char ID_MENU_TEST_de[] PROGMEM = "TEST/DEBUG"; |
#define ID_MENU_TEST_en ID_MENU_TEST_de |
static const char ID_MENU_FAVORITEN_de[] PROGMEM = "Favoriten"; |
static const char ID_MENU_FAVORITEN_en[] PROGMEM = "Favorites"; |
static const char ID_MENU_KANAELE_de[] PROGMEM = "Kanäle"; |
static const char ID_MENU_KANAELE_en[] PROGMEM = "Channel"; |
static const char ID_MENU_KONFIGURATION_de[] PROGMEM = "Konfiguration"; |
static const char ID_MENU_KONFIGURATION_en[] PROGMEM = "Configuration"; |
static const char ID_MENU_STICK_de[] PROGMEM = "Stick"; |
#define ID_MENU_STICK_en ID_MENU_STICK_de |
static const char ID_MENU_LOOPING_de[] PROGMEM = "Looping"; |
#define ID_MENU_LOOPING_en ID_MENU_LOOPING_de |
static const char ID_MENU_HOEHE_de[] PROGMEM = "Höhe"; |
static const char ID_MENU_HOEHE_en[] PROGMEM = "Altitude"; |
static const char ID_MENU_KAMERA_de[] PROGMEM = "Kamera"; |
static const char ID_MENU_KAMERA_en[] PROGMEM = "Camera"; |
static const char ID_MENU_NAVICTRL_de[] PROGMEM = "Navi-Ctrl"; |
#define ID_MENU_NAVICTRL_en ID_MENU_NAVICTRL_de |
static const char ID_MENU_AUSGAENGE_de[] PROGMEM = "Ausgänge"; |
static const char ID_MENU_AUSGAENGE_en[] PROGMEM = "Outputs"; |
static const char ID_MENU_VERSCHIEDENES_de[] PROGMEM = "Verschiedenes"; |
static const char ID_MENU_VERSCHIEDENES_en[] PROGMEM = "Miscellaneous"; |
static const char ID_MENU_GYRO_de[] PROGMEM = "Gyro"; |
#define ID_MENU_GYRO_en ID_MENU_GYRO_de |
static const char ID_MENU_BENUTZER_de[] PROGMEM = "Benutzer"; |
static const char ID_MENU_BENUTZER_en[] PROGMEM = "User"; |
static const char ID_MENU_ACHSKOPPLUNG_de[] PROGMEM = "Achskopplung"; |
static const char ID_MENU_ACHSKOPPLUNG_en[] PROGMEM = "Coupl Axes"; |
/* |
static const char ID_MENU_MIXERSETUP_de[] PROGMEM = "Mixer-Setup"; |
static const char ID_MENU_MIXERSETUP_en[] PROGMEM = "Config Mix"; |
*/ |
static const char ID_MENU_EASYSETUP_de[] PROGMEM = "Easy Setup"; |
#define ID_MENU_EASYSETUP_en ID_MENU_EASYSETUP_de |
//############################################################################################# |
//# Bezeichnungen der paramID's |
//############################################################################################# |
// Hier ist erstmal alles deklariert was auch in paramset.h vorhanden ist. |
// Wenn der Compiler richtig eingestellt ist, dann wird alles nicht benoetigte wegoptimiert! |
static const char param_Hoehe_MinGas_de[] PROGMEM = "Min. Gas"; |
static const char param_Hoehe_MinGas_en[] PROGMEM = "min. throttle"; |
static const char param_Luftdruck_D_de[] PROGMEM = "Luftdruck D"; |
static const char param_Luftdruck_D_en[] PROGMEM = "barometric D"; |
static const char param_HoeheChannel_de[] PROGMEM = "Höhe Sollwert"; |
static const char param_HoeheChannel_en[] PROGMEM = "hight setpoint"; |
static const char param_Hoehe_P_de[] PROGMEM = "Höhe P"; |
static const char param_Hoehe_P_en[] PROGMEM = "altitude P"; |
static const char param_Hoehe_Verstaerkung_de[] PROGMEM = "Verstärk./Rate"; |
static const char param_Hoehe_Verstaerkung_en[] PROGMEM = "gain rate"; |
static const char param_Hoehe_ACC_Wirkung_de[] PROGMEM = "Z-ACC"; |
#define param_Hoehe_ACC_Wirkung_en param_Hoehe_ACC_Wirkung_de |
static const char param_Hoehe_HoverBand_de[] PROGMEM = "Schwebe-Gas"; |
static const char param_Hoehe_HoverBand_en[] PROGMEM = "hoover throttle"; |
static const char param_Hoehe_GPS_Z_de[] PROGMEM = "GPS Z"; |
#define param_Hoehe_GPS_Z_en param_Hoehe_GPS_Z_de |
static const char param_Hoehe_Tilt_Comp_de[] PROGMEM = "Tilt Compensation"; |
#define param_Hoehe_Tilt_Comp_en param_Hoehe_Tilt_Comp_de |
static const char param_Hoehe_StickNeutralPoint_de[] PROGMEM = "Stick Neutr.Punkt"; |
static const char param_Hoehe_StickNeutralPoint_en[] PROGMEM = "stick neutr.point"; |
static const char param_Stick_P_de[] PROGMEM = "Nick/Roll P"; |
static const char param_Stick_D_de[] PROGMEM = "Nick/Roll D"; |
static const char param_StickGier_P_de[] PROGMEM = "Gier P"; |
#define param_Stick_P_en param_Stick_P_de |
#define param_Stick_D_en param_Stick_D_de |
#define param_StickGier_P_en param_StickGier_P_de |
static const char param_Gas_Min_de[] PROGMEM = "Min. Gas"; |
static const char param_Gas_Min_en[] PROGMEM = "min. throttle"; |
static const char param_Gas_Max_de[] PROGMEM = "Max. Gas"; |
static const char param_Gas_Max_en[] PROGMEM = "max. throttle"; |
static const char param_GyroAccFaktor_de[] PROGMEM = "ACC/Gyro Faktor"; |
#define param_GyroAccFaktor_en param_GyroAccFaktor_de |
static const char param_KompassWirkung_de[] PROGMEM = "Kompasswirkung"; |
static const char param_KompassWirkung_en[] PROGMEM = "compass effect"; |
static const char param_Gyro_P_de[] PROGMEM = "Gyro P"; |
static const char param_Gyro_I_de[] PROGMEM = "Gyro I"; |
static const char param_Gyro_D_de[] PROGMEM = "Gyro D"; |
static const char param_Gyro_Gier_P_de[] PROGMEM = "Gier P"; |
static const char param_Gyro_Gier_I_de[] PROGMEM = "Gier I"; |
static const char param_Gyro_Stability_de[] PROGMEM = "Gyro stab."; |
#define param_Gyro_P_en param_Gyro_P_de |
#define param_Gyro_I_en param_Gyro_I_de |
#define param_Gyro_D_en param_Gyro_D_de |
#define param_Gyro_Gier_P_en param_Gyro_Gier_P_de |
#define param_Gyro_Gier_I_en param_Gyro_Gier_I_de |
#define param_Gyro_Stability_en param_Gyro_Stability_de |
static const char param_UnterspannungsWarnung_de[] PROGMEM = "Unterspannung [0.1V]"; |
static const char param_UnterspannungsWarnung_en[] PROGMEM = "undervoltage [0.1V]"; |
static const char param_NotGas_de[] PROGMEM = "NOT-Gas"; |
static const char param_NotGas_en[] PROGMEM = "Emerg.Throttle"; |
static const char param_NotGasZeit_de[] PROGMEM = "NOT-Gas Zeit [0.1s]"; |
static const char param_NotGasZeit_en[] PROGMEM = "Emerg.Thro.Time[0.1s]"; |
static const char param_Receiver_de[] PROGMEM = "Receiver"; |
#define param_Receiver_en param_Receiver_de |
static const char param_I_Faktor_de[] PROGMEM = "Hauptregler I"; |
static const char param_I_Faktor_en[] PROGMEM = "main I"; |
static const char param_UserParam1_de[] PROGMEM = "Parameter 1"; |
static const char param_UserParam2_de[] PROGMEM = "Parameter 2"; |
static const char param_UserParam3_de[] PROGMEM = "Parameter 3"; |
static const char param_UserParam4_de[] PROGMEM = "Parameter 4"; |
#define param_UserParam1_en param_UserParam1_de |
#define param_UserParam2_en param_UserParam2_de |
#define param_UserParam3_en param_UserParam3_de |
#define param_UserParam4_en param_UserParam4_de |
static const char param_ServoNickControl_de[] PROGMEM = "Nick Ansteuerung"; |
static const char param_ServoNickControl_en[] PROGMEM = "nick servo ctrl"; |
static const char param_ServoNickComp_de[] PROGMEM = "Nick Kompensation"; |
static const char param_ServoNickComp_en[] PROGMEM = "nick servo comp."; |
static const char param_ServoNickFails_de[] PROGMEM = "Nick Failsave Wert"; |
static const char param_ServoNickFails_en[] PROGMEM = "nick failsave value"; |
static const char param_ServoNickMin_de[] PROGMEM = "Nick Servo Minimum"; |
static const char param_ServoNickMax_de[] PROGMEM = "Nick Servo Maximum"; |
#define param_ServoNickMin_en param_ServoNickMin_de |
#define param_ServoNickMax_en param_ServoNickMax_de |
static const char param_ServoRollControl_de[] PROGMEM = "Roll Ansteuerung"; |
static const char param_ServoRollControl_en[] PROGMEM = "roll servo ctrl."; |
static const char param_ServoRollComp_de[] PROGMEM = "Roll Kompensation"; |
static const char param_ServoRollComp_en[] PROGMEM = "roll compensation"; |
static const char param_ServoRollFails_de[] PROGMEM = "Roll Failsave Wert"; |
static const char param_ServoRollFails_en[] PROGMEM = "roll failsave value"; |
static const char param_ServoRollMin_de[] PROGMEM = "Roll Minimum"; |
static const char param_ServoRollMax_de[] PROGMEM = "Roll Maximum"; |
#define param_ServoRollMin_en param_ServoRollMin_de |
#define param_ServoRollMax_en param_ServoRollMax_de |
//static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschw."; // ALT |
static const char param_ServoNickRefresh_de[] PROGMEM = "Anst. Geschwindigkeit"; |
static const char param_ServoNickRefresh_en[] PROGMEM = "servo refresh"; |
static const char param_ServoManualControlSpeed_de[] PROGMEM = "Manu. Geschwindigkeit."; |
static const char param_ServoManualControlSpeed_en[] PROGMEM = "manuell Speed"; |
static const char param_CamOrientation_de[] PROGMEM = "Cam Richtung"; |
static const char param_CamOrientation_en[] PROGMEM = "cam orientation"; |
static const char param_Servo3_de[] PROGMEM = "Servo 3"; |
#define param_Servo3_en param_Servo3_de |
static const char param_Servo3Fails_de[] PROGMEM = "Servo 3 Failsave Wert"; |
static const char param_Servo3Fails_en[] PROGMEM = "Servo 3 failsave value"; |
static const char param_Servo4_de[] PROGMEM = "Servo 4"; |
#define param_Servo4_en param_Servo4_de |
static const char param_Servo4Fails_de[] PROGMEM = "Servo 4 Failsave Wert"; |
static const char param_Servo4Fails_en[] PROGMEM = "Servo 4 failsave value"; |
static const char param_Servo5_de[] PROGMEM = "Servo 5"; |
#define param_Servo5_en param_Servo5_de |
static const char param_Servo5Fails_de[] PROGMEM = "Servo 5 Failsave Wert"; |
static const char param_Servo5Fails_en[] PROGMEM = "Servo 5 failsave value"; |
static const char param_Servo3OnValue_de[] PROGMEM = "Servo 3 On Out1/2"; |
static const char param_Servo3OffValue_de[] PROGMEM = "Servo 3 Off Out1/2"; |
static const char param_Servo4OnValue_de[] PROGMEM = "Servo 4 On Out1/2"; |
static const char param_Servo4OffValue_de[] PROGMEM = "Servo 4 Off Out1/2"; |
#define param_Servo3OnValue_en param_Servo3OnValue_de |
#define param_Servo3OffValue_en param_Servo3OffValue_de |
#define param_Servo4OnValue_en param_Servo4OnValue_de |
#define param_Servo4OffValue_en param_Servo4OffValue_de |
static const char param_LoopGasLimit_de[] PROGMEM = "Looping Gas Limit"; |
static const char param_LoopGasLimit_en[] PROGMEM = "loop throttle limit"; |
static const char param_LoopThreshold_de[] PROGMEM = "Ansprechschwelle"; |
static const char param_LoopThreshold_en[] PROGMEM = "response threshold"; |
static const char param_LoopHysterese_de[] PROGMEM = "Hysterese"; |
static const char param_LoopHysterese_en[] PROGMEM = "hysteresis"; |
static const char param_AchsKopplung1_de[] PROGMEM = "Gier pos. Kopplung"; |
static const char param_AchsKopplung1_en[] PROGMEM = "gier pos. coupling"; |
static const char param_AchsKopplung2_de[] PROGMEM = "Nick/Roll Kopplung"; |
static const char param_AchsKopplung2_en[] PROGMEM = "nick/roll coupling"; |
static const char param_CouplingYawCorrection_de[] PROGMEM = "Gier Korrektur"; |
static const char param_CouplingYawCorrection_en[] PROGMEM = "gier correction"; |
static const char param_WinkelUmschlagNick_de[] PROGMEM = "Nick Umkehrpunkt"; |
static const char param_WinkelUmschlagNick_en[] PROGMEM = "nick turnover"; |
static const char param_WinkelUmschlagRoll_de[] PROGMEM = "Roll Umkehrpunkt"; |
static const char param_WinkelUmschlagRoll_en[] PROGMEM = "roll turnover"; |
static const char param_GyroAccAbgleich_de[] PROGMEM = "ACC/Gyro Komp. [1/x]"; |
static const char param_GyroAccAbgleich_en[] PROGMEM = "ACC/Gyro Comp. [1/x]"; |
static const char param_Driftkomp_de[] PROGMEM = "Drift-Kompensation"; |
static const char param_Driftkomp_en[] PROGMEM = "drift compensation"; |
static const char param_DynamicStability_de[] PROGMEM = "Dynamische Stabilität"; |
static const char param_DynamicStability_en[] PROGMEM = "dynamic stabiliy"; |
static const char param_UserParam5_de[] PROGMEM = "Parameter 5"; |
static const char param_UserParam6_de[] PROGMEM = "Parameter 6"; |
static const char param_UserParam7_de[] PROGMEM = "Parameter 7"; |
static const char param_UserParam8_de[] PROGMEM = "Parameter 8"; |
#define param_UserParam5_en param_UserParam5_de |
#define param_UserParam6_en param_UserParam6_de |
#define param_UserParam7_en param_UserParam7_de |
#define param_UserParam8_en param_UserParam8_de |
static const char param_J16Bitmask_de[] PROGMEM = "Out1: Bitmaske"; |
static const char param_J16Bitmask_en[] PROGMEM = "Out1: bitmask"; |
static const char param_J16Timing_de[] PROGMEM = "Out1: Timing [10ms]"; |
#define param_J16Timing_en param_J16Timing_de |
static const char param_J17Bitmask_de[] PROGMEM = "Out2: Bitmaske"; |
static const char param_J17Bitmask_en[] PROGMEM = "Out2: bitmask"; |
static const char param_J17Timing_de[] PROGMEM = "Out2: Timing [10ms]"; |
#define param_J17Timing_en param_J17Timing_de |
static const char param_WARN_J16_Bitmask_de[] PROGMEM = "Out1 Warn UBat"; |
static const char param_WARN_J16_Bitmask_en[] PROGMEM = "Out1 undervolt warn"; |
static const char param_WARN_J17_Bitmask_de[] PROGMEM = "Out2 Warn UBat"; |
static const char param_WARN_J17_Bitmask_en[] PROGMEM = "Out2 undervolt warn"; |
static const char param_AutoPhotoDistance_de[] PROGMEM = "Auto Trigger Dist.[m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoDistance_en param_AutoPhotoDistance_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_AutoPhotoAtitudes_de[] PROGMEM = "Auto Trigger Alt. [m]"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_AutoPhotoAtitudes_en param_AutoPhotoAtitudes_de // ab Rev. 100 (z.B. FC 2.05e) |
#define param_NaviOut1Parameter_de param_AutoPhotoDistance_de // bis Rev. 98 |
#define param_NaviOut1Parameter_en param_AutoPhotoDistance_de // bis Rev. 98 |
static const char param_SingleWpSpeed_de[] PROGMEM = "SingleWP Speed 0.1m/s"; // ab Rev. 100 (z.B. FC 2.05e) |
#define param_SingleWpSpeed_en param_SingleWpSpeed_de // ab Rev. 100 (z.B. FC 2.05e) |
static const char param_LandingPulse_de[] PROGMEM = "ACC Z Landing Pulse"; // ab Rev. 104 (FC 2.09d) |
#define param_LandingPulse_en param_LandingPulse_de // ab Rev. 104 (FC 2.09d) |
static const char param_NaviGpsModeChannel_de[] PROGMEM = "GPS Modus Steuerung"; |
static const char param_NaviGpsModeChannel_en[] PROGMEM = "GPS mode control"; |
static const char param_NaviGpsGain_de[] PROGMEM = "GPS Verstärkung [%]"; |
static const char param_NaviGpsGain_en[] PROGMEM = "GPS gain [%]"; |
static const char param_NaviGpsP_de[] PROGMEM = "GPS-P"; |
static const char param_NaviGpsI_de[] PROGMEM = "GPS-I"; |
static const char param_NaviGpsD_de[] PROGMEM = "GPS-D"; |
static const char param_NaviGpsPLimit_de[] PROGMEM = "GPS-P Limit"; |
static const char param_NaviGpsILimit_de[] PROGMEM = "GPS-I Limit"; |
static const char param_NaviGpsDLimit_de[] PROGMEM = "GPS-D Limit"; |
static const char param_NaviGpsA_de[] PROGMEM = "GPS Acc"; |
#define param_NaviGpsP_en param_NaviGpsP_de |
#define param_NaviGpsI_en param_NaviGpsI_de |
#define param_NaviGpsD_en param_NaviGpsD_de |
#define param_NaviGpsPLimit_en param_NaviGpsPLimit_de |
#define param_NaviGpsILimit_en param_NaviGpsILimit_de |
#define param_NaviGpsDLimit_en param_NaviGpsDLimit_de |
#define param_NaviGpsA_en param_NaviGpsA_de |
static const char param_NaviGpsMinSat_de[] PROGMEM = "Min. Satelliten"; |
static const char param_NaviGpsMinSat_en[] PROGMEM = "minimum satelite"; |
static const char param_NaviStickThreshold_de[] PROGMEM = "GPS Stick-Schwelle"; |
static const char param_NaviStickThreshold_en[] PROGMEM = "GPS stick threshold"; |
static const char param_NaviWindCorrection_de[] PROGMEM = "GPS Windkorrektur [%]"; |
static const char param_NaviWindCorrection_en[] PROGMEM = "GPS wind correct. [%]"; |
static const char param_NaviAccCompensation_de[] PROGMEM = "ACC Kompensation"; |
static const char param_NaviAccCompensation_en[] PROGMEM = "ACC compensation"; |
static const char param_NaviOperatingRadius_de[] PROGMEM = "max. Radius"; |
static const char param_NaviOperatingRadius_en[] PROGMEM = "max. radius"; |
static const char param_NaviAngleLimitation_de[] PROGMEM = "Winkel Limit"; |
static const char param_NaviAngleLimitation_en[] PROGMEM = "angle limit"; |
static const char param_NaviPH_LoginTime_de[] PROGMEM = "PH Login Zeit [s]"; |
static const char param_NaviPH_LoginTime_en[] PROGMEM = "PH login time [s]"; |
static const char param_ExternalControl_de[] PROGMEM = "Ext. Kontrolle"; |
static const char param_ExternalControl_en[] PROGMEM = "ext. control"; |
static const char param_OrientationAngle_de[] PROGMEM = "OrientationAngle"; |
#define param_OrientationAngle_en param_OrientationAngle_de |
static const char param_CareFreeChannel_de[] PROGMEM = "CareFree Steuerung"; |
static const char param_CareFreeChannel_en[] PROGMEM = "careFree control"; |
static const char param_MotorSafetySwitch_de[] PROGMEM = "Motor Sicherh.Schalt."; |
static const char param_MotorSafetySwitch_en[] PROGMEM = "motor safety switch"; |
static const char param_MotorSmooth_de[] PROGMEM = "Motor Glättung"; |
static const char param_MotorSmooth_en[] PROGMEM = "motor smooth"; |
static const char param_ComingHomeAltitude_de[] PROGMEM = "Com.Home Höhe [m]"; |
static const char param_ComingHomeAltitude_en[] PROGMEM = "coming home alti. [m]"; |
static const char param_FailSafeTime_de[] PROGMEM = "Fails. CH Zeit [s]"; |
static const char param_FailSafeTime_en[] PROGMEM = "fails. CH time [s]"; |
static const char param_MaxAltitude_de[] PROGMEM = "Max. Höhe [m]"; |
static const char param_MaxAltitude_en[] PROGMEM = "max. altitude [m]"; |
static const char param_FailsafeChannel_de[] PROGMEM = "Fails. Channel 0=Aus"; |
static const char param_FailsafeChannel_en[] PROGMEM = "fails. channel 0=off"; |
static const char param_ServoFilterNick_de[] PROGMEM = "Nick Filter"; |
static const char param_ServoFilterNick_en[] PROGMEM = "nick filter"; |
static const char param_ServoFilterRoll_de[] PROGMEM = "Roll Filter"; |
static const char param_ServoFilterRoll_en[] PROGMEM = "roll filter"; |
static const char param_StartLandChannel_de[] PROGMEM = "Auto StartLand Kanal"; |
static const char param_StartLandChannel_en[] PROGMEM = "auto start/land chan."; |
static const char param_LandingSpeed_de[] PROGMEM = "Landing Speed 0.1m/s"; |
#define param_LandingSpeed_en param_LandingSpeed_de |
static const char param_CompassOffset_de[] PROGMEM = "Compass Offset [\x0B]"; |
#define param_CompassOffset_en param_CompassOffset_de |
static const char param_AutoLandingVoltage_de[] PROGMEM = "Autoland. Volt [0.1V]"; |
#define param_AutoLandingVoltage_en param_AutoLandingVoltage_de |
static const char param_ComingHomeVoltage_de[] PROGMEM = "Coming H. Volt [0.1V]"; |
#define param_ComingHomeVoltage_en param_ComingHomeVoltage_de |
static const char param_BitConfig_de[] PROGMEM = "BitConfig"; |
static const char param_ServoCompInvert_de[] PROGMEM = "ServoCompInvert"; |
static const char param_ExtraConfig_de[] PROGMEM = "ExtraConfig"; |
static const char param_GlobalConfig3_de[] PROGMEM = "GlobalConfig3"; |
static const char param_Name_de[] PROGMEM = "Setting Name"; |
#define param_BitConfig_en param_BitConfig_de |
#define param_ServoCompInvert_en param_ServoCompInvert_de |
#define param_ExtraConfig_en param_ExtraConfig_de |
#define param_GlobalConfig3_en param_GlobalConfig3_de |
#define param_Name_en param_Name_de |
static const char param_ComingHomeOrientation_de[] PROGMEM = "CH Ausrichtung"; |
static const char param_ComingHomeOrientation_en[] PROGMEM = "CH orientation"; |
static const char param_SingleWpControlChannel_de[] PROGMEM = "Single WP Ctrl Chan."; |
#define param_SingleWpControlChannel_en param_SingleWpControlChannel_de |
static const char param_MenuKeyChannel_de[] PROGMEM = "Next WP Channel"; |
#define param_MenuKeyChannel_en param_MenuKeyChannel_de |
// subitems (Bit / Byte-Felder) |
static const char param_ServoCompInvert_SVNick_de[] PROGMEM = "Nick Umkehren"; |
static const char param_ServoCompInvert_SVNick_en[] PROGMEM = "nick inv. direction"; |
static const char param_ServoCompInvert_SVRoll_de[] PROGMEM = "Roll Umkehren"; |
static const char param_ServoCompInvert_SVRoll_en[] PROGMEM = "Roll inv. direction"; |
static const char param_ServoCompInvert_SVRelMov_de[] PROGMEM = "Nick Relativ"; |
static const char param_ServoCompInvert_SVRelMov_en[] PROGMEM = "nick relativ"; |
static const char param_ExtraConfig_HeightLimit_de[] PROGMEM = "Höhenbegrenzung"; |
static const char param_ExtraConfig_HeightLimit_en[] PROGMEM = "heigth limitation"; |
static const char param_ExtraConfig_HeightVario_de[] PROGMEM = "Vario-Höhe"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_HeightVario_en[] PROGMEM = "vario heigth"; // negiert param_ExtraConfig_HeightLimit |
static const char param_ExtraConfig_VarioBeep_de[] PROGMEM = "akust. Vario"; |
#define param_ExtraConfig_VarioBeep_en param_ExtraConfig_VarioBeep_de |
static const char param_ExtraConfig_SensitiveRc_de[] PROGMEM = "Erw. Empf.Sig.Prüfung"; |
static const char param_ExtraConfig_SensitiveRc_en[] PROGMEM = "enh. rec. sign. check"; |
static const char param_ExtraConfig_33vReference_de[] PROGMEM = "33vReference"; |
#define param_ExtraConfig_33vReference_en param_ExtraConfig_33vReference_de |
static const char param_ExtraConfig_NoRcOffBeeping_de[] PROGMEM = "k.Summer o.Sender"; |
static const char param_ExtraConfig_NoRcOffBeeping_en[] PROGMEM = "no beep without TX"; |
static const char param_ExtraConfig_GpsAid_de[] PROGMEM = "Dynamic PH"; |
#define param_ExtraConfig_GpsAid_en param_ExtraConfig_GpsAid_de |
static const char param_ExtraConfig_LearnableCarefree_de[] PROGMEM = "Teachable CF"; |
#define param_ExtraConfig_LearnableCarefree_en param_ExtraConfig_LearnableCarefree_de |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_de[] PROGMEM = "Kompass Fehler ignor."; |
static const char param_ExtraConfig_IgnoreMagErrAtStartup_en[] PROGMEM = "ignore compass error"; |
static const char param_BitConfig_LoopOben_de[] PROGMEM = "Looping Oben"; |
static const char param_BitConfig_LoopOben_en[] PROGMEM = "looping up"; |
static const char param_BitConfig_LoopUnten_de[] PROGMEM = "Looping Unten"; |
static const char param_BitConfig_LoopUnten_en[] PROGMEM = "looping down"; |
static const char param_BitConfig_LoopLinks_de[] PROGMEM = "Looping Links"; |
static const char param_BitConfig_LoopLinks_en[] PROGMEM = "looping left"; |
static const char param_BitConfig_LoopRechts_de[] PROGMEM = "Looping Rechts"; |
static const char param_BitConfig_LoopRechts_en[] PROGMEM = "looping right"; |
static const char param_BitConfig_MotorBlink1_de[] PROGMEM = "nur wenn Motor An"; |
static const char param_BitConfig_MotorBlink1_en[] PROGMEM = "only with motor on"; |
static const char param_BitConfig_MotorOffLed1_de[] PROGMEM = " \x19 sofort an"; |
static const char param_BitConfig_MotorOffLed1_en[] PROGMEM = " \x19 immediately on"; |
#define param_BitConfig_MotorOffLed2_de param_BitConfig_MotorOffLed1_de |
#define param_BitConfig_MotorBlink2_de param_BitConfig_MotorBlink1_de |
#define param_BitConfig_MotorOffLed2_en param_BitConfig_MotorOffLed1_en |
#define param_BitConfig_MotorBlink2_en param_BitConfig_MotorBlink1_en |
static const char param_GlobalConfig3_NoSdCardNoStart_de[] PROGMEM = "k.Start o.SD-Karte"; |
static const char param_GlobalConfig3_NoSdCardNoStart_en[] PROGMEM = "no start w/o SD-card"; |
static const char param_GlobalConfig3_DphMaxRadius_de[] PROGMEM = "Max.Radius dPH"; |
#define param_GlobalConfig3_DphMaxRadius_en param_GlobalConfig3_DphMaxRadius_de |
static const char param_GlobalConfig3_VarioFailsafe_de[] PROGMEM = "NOT-Gas Vario Höhe"; |
static const char param_GlobalConfig3_VarioFailsafe_en[] PROGMEM = "Emerg.thr.vario h."; |
static const char param_GlobalConfig3_MotorSwitchMode_de[] PROGMEM = "Motor Swi.Mode"; |
#define param_GlobalConfig3_MotorSwitchMode_en param_GlobalConfig3_MotorSwitchMode_de |
static const char param_GlobalConfig3_NoGpsFixNoStart_de[] PROGMEM = "k.Start o.GPS Fix"; |
static const char param_GlobalConfig3_NoGpsFixNoStart_en[] PROGMEM = "no start w/o GPS Fix"; |
static const char param_GlobalConfig3_UseNcForOut1_de[] PROGMEM = "mit WP-Event verkn."; |
static const char param_GlobalConfig3_UseNcForOut1_en[] PROGMEM = "combine with WP-Event"; |
static const char param_GlobalConfig3_SpeakAll_de[] PROGMEM = "Alles Ansagen"; |
static const char param_GlobalConfig3_SpeakAll_en[] PROGMEM = "speak all"; |
static const char param_GlobalConfig3_ServoNickCompOff_de[] PROGMEM = "Nick Komp. Aus"; |
static const char param_GlobalConfig3_ServoNickCompOff_en[] PROGMEM = "nick compensation off"; |
static const char param_GlobalConfig_HoehenRegelung_de[] PROGMEM = "Höhenregelung"; |
static const char param_GlobalConfig_HoehenRegelung_en[] PROGMEM = "height control"; |
static const char param_GlobalConfig_HoehenSchalter_de[] PROGMEM = "Schalter Höhe"; |
static const char param_GlobalConfig_HoehenSchalter_en[] PROGMEM = "height switch"; |
static const char param_GlobalConfig_HeadingHold_de[] PROGMEM = "Heading Hold"; |
#define param_GlobalConfig_HeadingHold_en param_GlobalConfig_HeadingHold_de |
static const char param_GlobalConfig_KompassAktiv_de[] PROGMEM = "Kompass Aktiv"; |
static const char param_GlobalConfig_KompassAktiv_en[] PROGMEM = "compass aktiv"; |
static const char param_GlobalConfig_KompassFix_de[] PROGMEM = "Kompass Fix"; |
static const char param_GlobalConfig_KompassFix_en[] PROGMEM = "compass fix"; |
static const char param_GlobalConfig_GpsAktiv_de[] PROGMEM = "GPS Aktiv"; |
#define param_GlobalConfig_GpsAktiv_en param_GlobalConfig_GpsAktiv_de |
static const char param_GlobalConfig_AchsenkopplungAktiv_de[] PROGMEM = "Achs(ent)kopplung"; |
static const char param_GlobalConfig_AchsenkopplungAktiv_en[] PROGMEM = "(De)Coupl Axes"; |
static const char param_GlobalConfig_DrehratenBegrenzer_de[] PROGMEM = "Drehratenbegrenzung"; |
static const char param_GlobalConfig_DrehratenBegrenzer_en[] PROGMEM = "rotary rate limit."; |
static const char param_Kanalbelegung_Nick_de[] PROGMEM = "Nick"; |
static const char param_Kanalbelegung_Roll_de[] PROGMEM = "Roll"; |
#define param_Kanalbelegung_Nick_en param_Kanalbelegung_Nick_de |
#define param_Kanalbelegung_Roll_en param_Kanalbelegung_Roll_de |
static const char param_Kanalbelegung_Gas_de[] PROGMEM = "Gas"; |
static const char param_Kanalbelegung_Gas_en[] PROGMEM = "throttle"; |
static const char param_Kanalbelegung_Gear_de[] PROGMEM = "Gear"; |
#define param_Kanalbelegung_Gear_en param_Kanalbelegung_Gear_de |
static const char param_Kanalbelegung_Poti1_de[] PROGMEM = "Poti 1"; |
static const char param_Kanalbelegung_Poti2_de[] PROGMEM = "Poti 2"; |
static const char param_Kanalbelegung_Poti3_de[] PROGMEM = "Poti 3"; |
static const char param_Kanalbelegung_Poti4_de[] PROGMEM = "Poti 4"; |
static const char param_Kanalbelegung_Poti5_de[] PROGMEM = "Poti 5"; |
static const char param_Kanalbelegung_Poti6_de[] PROGMEM = "Poti 6"; |
static const char param_Kanalbelegung_Poti7_de[] PROGMEM = "Poti 7"; |
static const char param_Kanalbelegung_Poti8_de[] PROGMEM = "Poti 8"; |
#define param_Kanalbelegung_Poti1_en param_Kanalbelegung_Poti1_de |
#define param_Kanalbelegung_Poti2_en param_Kanalbelegung_Poti2_de |
#define param_Kanalbelegung_Poti3_en param_Kanalbelegung_Poti3_de |
#define param_Kanalbelegung_Poti4_en param_Kanalbelegung_Poti4_de |
#define param_Kanalbelegung_Poti5_en param_Kanalbelegung_Poti5_de |
#define param_Kanalbelegung_Poti6_en param_Kanalbelegung_Poti6_de |
#define param_Kanalbelegung_Poti7_en param_Kanalbelegung_Poti7_de |
#define param_Kanalbelegung_Poti8_en param_Kanalbelegung_Poti8_de |
static const char param_CompassOffset_DisableDeclCalc_de[] PROGMEM = "Disable Decl. Calc"; |
#define param_CompassOffset_DisableDeclCalc_en param_CompassOffset_DisableDeclCalc_de |
static const char param_NaviMaxFlyingRange_de[] PROGMEM = "Max. Flugradius [10m]"; |
static const char param_NaviMaxFlyingRange_en[] PROGMEM = "Max. Flightrad. [10m]"; |
static const char param_NaviDescendRange_de[] PROGMEM = "Fail SinkRadius [10m]"; |
#define param_NaviDescendRange_en param_NaviDescendRange_de |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/mksettings.c |
---|
0,0 → 1,707 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.c |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 11.05.2014 OG |
//# - chg: MKSettings_Menu() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 29.03.2014 OG |
//# - chg: versch. Funktionen: del: MenuCtrl_SetShowBatt() wegen Aenderung |
//# des Defaults auf true |
//# |
//# 26.03.2014 OG |
//# - add: etliche Aenderungen in allen Bereichen fuer das erste Release |
//# der neuen MK-Settings |
//# |
//# 27.02.2014 OG |
//# - chg: MKSettings_AskAction() Unterstuetzung von param_DUMMY |
//# |
//# 26.02.2014 OG |
//# - chg: MKSettings_Copy() auf KEYLINE2 geaendert |
//# |
//# 23.02.2014 OG |
//# - chg: MKSettings_Menu() Aufruf von MK_Parameters() geaendert |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
//#include "../lipo/lipo.h" |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
//#include "../menu.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
#include "mkparameters.h" |
#include "mksettings.h" |
//--------------------------------------------------------------------------------------------- |
#define F_EXIT 0 |
#define F_REFRESH 1 |
char mksettings_menu_names[6][18]; // befuellt von: MKSettings_read_Names() |
//------------------------------------- |
//------------------------------------- |
typedef struct |
{ |
unsigned char paramsetRevision; // Revision FC-Parameterset fuer die das Temp-Setting gueltig ist |
unsigned char *paramset; // malloc: Pointer auf kopierte Parameter-Daten |
} MKSetting_TMP_t; |
MKSetting_TMP_t MKSetting_TMP; |
#define ID_SETTING_COPY 10 // fuer MKSettings_Menu() |
#define ID_EDIT 20 // fuer MKSettings_AskAction() |
#define ID_ACTIVATE 21 // fuer MKSettings_AskAction() |
#define ID_COPY 22 // fuer MKSettings_AskAction() |
#define ID_SAVE 30 // fuer MKSettings_AskSaveSetting() |
#define ID_DISCARD 31 // fuer MKSettings_AskSaveSetting() |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init0() |
// |
// nur fuer main.c |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init0( void ) |
{ |
memset( &MKSetting_TMP, 0, sizeof(MKSetting_TMP_t) ); |
strcpy( mksettings_menu_names[5], "PKT: --empty--"); |
} |
//-------------------------------------------------------------- |
// MKSettings_TMP_Init() |
// |
// loeschen / initialisieren vom PKT Temp-Setting |
//-------------------------------------------------------------- |
void MKSettings_TMP_Init( void ) |
{ |
if( MKSetting_TMP.paramset != NULL ) |
{ |
free( MKSetting_TMP.paramset ); |
} |
MKSettings_TMP_Init0(); |
} |
//-------------------------------------------------------------- |
// from_setting = 6 : von TMP zu einem MK-Setting |
// from_setting <= 5: von MK-Setting zu TMP |
//-------------------------------------------------------------- |
uint8_t MKSettings_TMP_copy( uint8_t to_setting, uint8_t timeout ) |
{ |
uint8_t written; |
uint8_t size = paramsetSize(); |
if( MKSetting_TMP.paramset == NULL ) |
{ |
MKSetting_TMP.paramset = malloc( size+1 ); // +1 fuer das erste settings-byte |
} |
if( !MKSetting_TMP.paramset ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("NO RAM!"), true, 800, true, true ); // FEHLER! (NO RAM) |
return 0; // -> EXIT |
} |
// von MK-Setting 1..5 zu TMP |
if( to_setting == 6 ) |
{ |
MKSetting_TMP.paramsetRevision = MKVersion.paramsetRevision; |
memcpy( MKSetting_TMP.paramset, (unsigned char *)(pRxData), size+1 ); // memcpy( dst, src, size)) |
return 6; |
} |
// von TMP zu MK-Setting 1..5 |
memcpy( (unsigned char *)(pRxData), MKSetting_TMP.paramset , size+1 ); // memcpy( dst, src, size)) |
written = MK_Setting_write( to_setting, timeout); |
return written; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_read_Names( void ) |
{ |
uint8_t setting; |
// die 5 Settings vom Kopter laden um die |
// Setting-Namen zu speichern |
for( setting=1; setting <= 5; setting++ ) |
{ |
if( !MK_Setting_load( setting, 20 ) ) |
return(0); // Fehler - setting konnte nicht geladen werden (timeout = 20) |
xsnprintf( mksettings_menu_names[setting-1], 16, "%1d: %s", setting, paramGet_p(param_Name) ); |
} |
// TMP-Setting |
if( MKSetting_TMP.paramset != NULL ) |
{ |
if( MKSetting_TMP.paramsetRevision != MKVersion.paramsetRevision ) |
{ |
// wenn die FC-Revision vom TMP-Setting abweicht vom zuletzt geladenen |
// dann wurde ggf. der Kopter gewechselt -> TMP-Setting verwerfen |
MKSettings_TMP_Init(); |
} |
else |
{ |
// den Namen aus dem TMP-Setting holen |
paramsetInit( MKSetting_TMP.paramset ); |
xsnprintf( mksettings_menu_names[5], 16, "PKT: %s", paramGet_p(param_Name) ); |
} |
} |
setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
return setting; |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskAction( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_EDIT, ID_ACTIVATE, ID_COPY |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskAction( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
if( !paramExist(param_DUMMY) ) // param_DUMMY -> das Parameterset wird nicht zum bearbeiten unterstuetzt |
MenuCtrl_Push_P( ID_EDIT , MENU_ITEM, NOFUNC, strGet(EDIT_SETTING) ); // "Setting ändern" |
if( setting != 6) |
MenuCtrl_Push_P( ID_ACTIVATE, MENU_ITEM, NOFUNC, strGet(PARA_AKTIVI) ); // "aktivieren" |
MenuCtrl_Push_P( ID_COPY , MENU_ITEM, NOFUNC, strGet(STR_COPY) ); // "kopieren" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
// wahl = MKSettings_AskSaveSetting( setting) |
// |
// Rueckgabe: |
// 0 (==Ende), ID_SAVE, ID_DISCARD |
//-------------------------------------------------------------- |
uint8_t MKSettings_AskSaveSetting( uint8_t setting ) |
{ |
uint8_t wahl = 0; |
//----------------- |
// Menue erstellen |
//----------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle( mksettings_menu_names[setting-1] ); // Menuetitel ist der Settingname |
MenuCtrl_ShowLevel(false); |
MenuCtrl_SetTopSpace(1); // oben beim Menue eine Leerzeile einfuegen |
//----------------- |
// Menueeintraege |
//----------------- |
MenuCtrl_Push_P( ID_SAVE , MENU_ITEM, NOFUNC, strGet(STR_SAVE) ); // "speichern" |
MenuCtrl_Push_P( ID_DISCARD , MENU_ITEM, NOFUNC, strGet(STR_DISCARD) ); // "verwerfen" |
//----------------- |
// Menue Control |
//----------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() != KEY_ESC ) |
wahl = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID_CHANGE, ID_ACTIVATE) |
MenuCtrl_Destroy(); |
MenuCtrl_ShowLevel(true); |
return wahl; // 0=Ende; ID_EDIT; ID_ACTIVATE, ID_COPY |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t MKSettings_Copy( uint8_t from_setting ) |
{ |
const char *mask = PSTR("%15s"); |
uint8_t redraw = true; |
uint8_t loaded = 0; |
uint8_t to_setting; |
uint8_t written; |
uint8_t key; |
int8_t yoffs; |
lcd_cls(); |
to_setting = from_setting; |
while( true ) |
{ |
//------------------------ |
// anzeigen |
//------------------------ |
if( redraw ) |
{ |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
lcd_printp_at( 1, 0, strGet(PARA_COPY), MINVERS); |
yoffs = -1; |
lcdx_printp_at( 0, 2, strGet(STR_VON), MNORMAL, 0,yoffs); // "von" |
lcdx_printf_at_P( 5, 2, MNORMAL, 3,yoffs, mask, mksettings_menu_names[from_setting-1] ); |
yoffs = -7; |
lcd_frect( 5*6, 4*8+yoffs, 15*6, 7, 0); |
lcdx_printp_at( 0, 4, strGet(STR_NACH), MNORMAL, 0,yoffs); // "nach" |
lcdx_printf_at_P( 5, 4, MNORMAL, 3,yoffs, mask, mksettings_menu_names[to_setting-1] ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); // Keyline: <- -> Ende OK |
redraw = false; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press(1 << KEY_ESC) ) |
{ |
return 99; // nur "ENDE" |
} |
if( get_key_press(1 << KEY_PLUS) ) |
{ |
if( to_setting == 6 ) to_setting = 1; |
else to_setting++; |
redraw = true; |
} |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
if( to_setting == 1 ) to_setting = 6; |
else to_setting--; |
redraw = true; |
} |
//------------------------------- |
// Taste: OK = Setting kopieren? |
//------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
lcdx_printp_center( 5, strGet(PARA_COPYQ), MNORMAL, 0,1); // "Wirklich kopieren?" (zentriert) |
lcd_rect_round( 0, 5*8-3, 127, 7+7, 1, R2); // Rahmen um die Frage |
lcd_frect( 0, 7*8, 127, 7, 0); // Keyline loeschen |
lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // neue Keyline: "Nein Ja" |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep |
key = 0; |
while( !key ) // Abfrage: Ja / Nein |
{ |
key = get_key_press(1 << KEY_ENTER) ? KEY_ENTER : key; // => "Ja" (=Setting kopieren) |
key = get_key_press(1 << KEY_ESC) ? KEY_ESC : key; // => "Nein" |
PKT_CtrlHook(); |
} |
//--------------------------- |
// "Ja" -> Setting kopieren! |
//--------------------------- |
if( key == KEY_ENTER ) // => "Ja" -> Setting kopieren! |
{ |
if( from_setting != 6 ) // kein PKT TMP-Setting |
{ |
loaded = MK_Setting_load( from_setting, 20 ); // timeout = 20 |
if( loaded != from_setting ) // Fehler beim laden - Datenverlust? |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return 0; // 0 = Ende/Abbruch |
} |
} |
if( to_setting == 6 ) // von Setting 1..5 nach TMP speichern |
{ |
MKSettings_TMP_copy( 6, 10 ); |
return loaded; |
} |
// Anzeige "speichern..." |
lcd_frect( 0, (8*4), 128, (8*4), 0); // Anzeigebereich löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
if( from_setting == 6 ) // von TMP nach Setting 1..5 |
{ |
written = MKSettings_TMP_copy( to_setting, 30 ); |
return written; |
} |
if( to_setting <= 5 ) // 'echtes' MK Setting speichern |
{ |
written = MK_Setting_write( to_setting, 30 ); // Timeout = 40 |
return written; |
} |
} |
if( key == KEY_ESC ) // => "Nein" -> nicht kopieren |
{ |
lcd_cls(); |
redraw = true; |
} |
} //end: if( get_key_press(1 << KEY_ENTER) ) |
//------------------------------------------ |
// Pruefe PKT-Update oder andere PKT-Aktion |
//------------------------------------------ |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
redraw = true; |
} |
} // end: while( true ) |
return 0; |
} |
//-------------------------------------------------------------- |
// ret = MKSettings_Menu() |
// |
// Rueckgabe: |
// 0 = Ende/Fehler/Abbruch |
// 1 = Refresh |
//-------------------------------------------------------------- |
uint8_t MKSettings_Menu( void ) |
{ |
uint8_t i; |
uint8_t active_setting; |
uint8_t setting; |
uint8_t wahl; |
uint8_t wahl2; |
uint8_t changed; |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_LOADSETTINGS), false, 0, true, true ); // "lade Settings..." |
active_setting = MKSettings_read_Names(); |
if( !active_setting ) // Fehler: settings konnten nicht geladen werden... |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // "Datenverlust!" (max. 8 Sekunden anzeigen) |
return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "MK Settings" |
//MenuCtrl_SetTitle_P( PSTR("MK Settings") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menue-Punkte |
//--------------- |
for( i=0; i< ( MKSetting_TMP.paramset!=NULL ? 6 : 5); i++ ) |
{ |
MenuCtrl_Push( i+1, MENU_SUB, NOFUNC, mksettings_menu_names[i] ); // Setting 1..5 |
} |
MenuCtrl_ItemSelect( active_setting ); // Menucursor auf aktives Setting setzen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) // Ende? |
{ |
break; // Ende |
} |
setting = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( setting >=1 && setting <= 6 ) |
{ |
wahl = MKSettings_AskAction( setting ); |
//-------------- |
// bearbeiten |
//-------------- |
if( wahl == ID_EDIT ) |
{ |
// TODO: Fehler abfangen! |
MK_Setting_load( setting, 25 ); // timeout = 25 |
changed = MK_Parameters( setting, mksettings_menu_names[setting-1] ); |
if( changed && (setting!=6) ) |
{ |
wahl2 = MKSettings_AskSaveSetting( setting ); |
if( wahl2 == ID_SAVE ) |
{ |
lcd_frect( 0, (8*7), 128, 7, 0); // Keyline löschen |
lcdx_printp_center( 4, strGet(STR_SAVING), MNORMAL, 0,9); // Text zentriert; String in PROGMEM |
lcd_rect_round( 0, 37, 127, 14, 1, R2); // Rahmen |
setting = MK_Setting_write( setting, 50); |
if( !setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
} |
} |
} |
//-------------- |
// aktivieren |
//-------------- |
if( wahl == ID_ACTIVATE ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(MSG_ACTIVATESETTING), false, 0, true, true ); // "aktiviere Setting..." |
active_setting = MK_Setting_change( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
else |
{ |
// neues Setting markieren |
for( i=1; i<=5; i++ ) MenuCtrl_ItemMark( i, false); // Markierungen loeschen |
MenuCtrl_ItemMark( active_setting, true); // aktives Setting markieren |
} |
} // end: if( wahl == ID_ACTIVATE ) |
//-------------- |
// kopieren |
//-------------- |
if( wahl == ID_COPY ) |
{ |
active_setting = MKSettings_Copy( setting ); |
if( !active_setting ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(ERROR_NODATA), true, 800, true, true ); // FEHLER! nodata (max. 8 Sekunden anzeigen) |
//MenuCtrl_Destroy(); |
//return F_EXIT; // F_EXIT = Ende/Abbruch |
} |
if( active_setting != 99 ) // =99 bedeutet: User hat die Funktion abgebrochen... |
{ |
MenuCtrl_Destroy(); // ok, kein Abbruch durch den User -> Menue neu aufbauen |
return F_REFRESH; // da sich ggf. Settings-Namen geaendert haben |
} |
} // end: if( wahl == ID_COPY ) |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
return F_EXIT; |
} |
//############################################################################################# |
//# |
//############################################################################################# |
//-------------------------------------------------------------- |
// das muss ueberarbeitet werden.... |
//-------------------------------------------------------------- |
static uint8_t check_motorOff(void) |
{ |
NaviData_t *naviData; |
if( hardware == NC ) // Prüfung funktioniert nur mit NC |
{ |
SwitchToNC(); |
SendOutData( 'o', ADDRESS_NC, 1, 10, 1); |
mode = 'O'; |
rxd_buffer_locked = FALSE; |
timer = 200; |
while( !rxd_buffer_locked && timer>0 ); |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
// timer = MK_TIMEOUT; |
Decode64(); |
naviData = (NaviData_t *) pRxData; |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
return false; |
else |
return true; |
} |
return false; |
} |
return true; // hmm, wenn man nur eine FC hat dann wird hier immer gemeldet "Motoren sind aus" ? |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MK_Settings( void ) |
{ |
//MKSettings_TMP_Init(); |
//if( true ) |
if( !check_motorOff() ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( strGet(STR_SWITCHMOTOROFF), false, 400, true, true ); // "Motoren ausschalten!" |
return; |
} |
while( MKSettings_Menu() == F_REFRESH ); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/mksettings.h |
---|
0,0 → 1,55 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY mksettings.h |
//# |
//# 19.02.2014 OG - NEU |
//# - add: MKSettings_TMP_Init(), MKSettings_TMP_Init0() |
//# |
//# 18.02.2014 OG - NEU |
//############################################################################ |
#ifndef _MKPARAMETERS_H |
#define _MKPARAMETERS_H |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
void MK_Settings( void ); |
void MKSettings_TMP_Init0( void ); |
void MKSettings_TMP_Init( void ); |
#endif // _MKPARAMETERS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/paramset.c |
---|
0,0 → 1,2583 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.c |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: unsigned char SingleWpControlChannel; (FC2.11a) |
//# unsigned char MenuKeyChannel; (FC2.11a) |
//# - add: paramset_106 (FC2.11a) |
//# |
//# 09.04.2015 Cebra (PKT384a) |
//# - add: unsigned char ServoFS_Pos[5] (FC 2.09i) |
//# - add: paramset_105 (FC2.09j) |
//# |
//# 19.03.2015 Cebra (PKT383a) |
//# - add: param_LandingPulse (FC 2.09d) |
//# |
//# 26.01.2015 Cebra |
//# - add: Function transform_ComingHomeOrientation |
//# Ändert die Bits 4+5 für ServoCompInvert |
//# in Konfigurationstabelle der paramSubID: param_ComingHomeOrientation neu |
//# |
//# 26.09.2014 Cebra |
//# - add: Parameterset Rev. 103 (FC 2.07f) |
//# - chg: param_Hoehe_GPS_Z zu param_Hoehe_Verstaerkung im Paramset 103 |
//# |
//# 14.05.2014 OG |
//# - chg: include "mkbase.h" geaendert auf "../mk/mkbase.h" |
//# |
//# 10.05.2014 OG |
//# - add: transform_CompassOffset_DisableDeclCalc() - transformiert true/false |
//# in das Byte von param_CompassOffset |
//# |
//# 09.05.2014 OG |
//# - chg: paramSet() - angepasst auf Transform-Funktionen |
//# - chg: paramGet() - angepasst auf Transform-Funktionen |
//# - add: eine neue Transformations-Zwischenschicht implementiert - abgebildet |
//# ueber die neue Tabelle paramTransform. |
//# Hier koennen Transform-Funktionen hinterlegt werden die einen |
//# ubyte8 Wert veraendern. |
//# Die Transformation wird von paramSet()/paramGet() aufgerufen. |
//# |
//# 17.04.2014 OG |
//# - FIX: _paramset_getsubitemid(): Pruefung von_Revision/bis_Revision |
//# korrigiert - fuehrte vorher ggf. zu falschen Anzeigen von SubItems! |
//# - chg: param_BitConfig_MotorOffLed1 auf bis_Revision 101 begrenzt |
//# - chg: param_BitConfig_MotorOffLed2 auf bis_Revision 101 begrenzt |
//# |
//# 09.04.2014 OG |
//# - chg: Rev. 101 auf param_DUMMY umgestellt um Platz zu sparen |
//# |
//# 08.04.2014 OG |
//# - add: Parameterset Rev. 102 (FC 2.05g) |
//# - fix: _paramset_getsubitemid() - Vergleich bzgl. von/bis_Revision umgedreht |
//# und equal zugefuegt |
//# |
//# 06.04.2014 OG |
//# - add: Parameterset Rev. 101 (FC 2.05f) |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: Parameterset Rev. 100 (FC 2.05e) |
//# (param_AutoPhotoDistance, param_AutoPhotoAtitudes, param_SingleWpSpeed) |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 24.03.2014 OG |
//# - chg: _paramset_getsubitemid() erweitert Unterstuetzung von/bis FC-Revisionen |
//# - add: paramSubItem_t mit Unterstuetzung fuer von/bis FC-Revisionen |
//# |
//# 27.02.2014 OG |
//# - chg: die Revisions-Tabellen 90 bis 94 mittels param_DUMMY gekuerzt |
//# -> keine Parameter-Bearbeitung bei denen moeglich |
//# - add: vollstaendige Unterstuetzung von paramSubID's via paramGet(), paramSet() |
//# |
//# 26.02.2014 OG |
//# - add: paramSubItem[] - Zugriffstabelle auf Bit- und Bytefelder innerhalb |
//# einer paramID |
//# - chg: bei den paramset_nnn[] Tabellen 'const' ergaenzt |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 19.02.2014 OG |
//# - fix: paramsetInit() Parameter pData wurde nicht konsequent verwendet |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# - add: Rev-Tabellen von FC v0.88e bis FC v2.03d |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
//############################################################################ |
//# INHALT |
//# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//# 3a. interne Zugriffsfunktionen |
//# 3b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# 3c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# x. TEST / DEBUG |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lipo/lipo.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../pkt/pkt.h" |
#include "../mk/mkbase.h" |
#include "paramset.h" |
//--------------------------- |
// fuer transform_... |
//--------------------------- |
#define GETVALUE 1 // Wert setzen |
#define SETVALUE 2 // Wert lesen |
paramRevItem_t *paramsetRevTable; // Zeiger auf aktive Revision-Table oder 0 wenn nicht gesetzt -> wird gesetzt druch paramsetInit() / paramset.c |
unsigned char *mkparamset; // nur temp. gueltig solange der PKT-RX-Buffer (pRxData) noch nicht wieder ueberschrieben -> wird gesetzt druch paramsetInit() / paramset.c |
//############################################################################################# |
//# 1a. Konfigurationtabellen fuer verschiedene FC-Revisionen |
//############################################################################################# |
//--------------------------------------------------------------------------------------- |
// die nachfolgenden Tabellen entsprechen der Struktur von paramRevItem_t aus paramset.h |
//--------------------------------------------------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 90 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88e |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_091 |
// |
// STRUKTUR-DIFF zu 0: |
// |
// DEFINE-DIFF zu 0: |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 91 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.88m, 0.88n |
// |
// STRUKTUR-DIFF zu 090: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 090: |
// - add: #define PID_SPEAK_HOTT_CFG |
// - add: #define CFG3_MOTOR_SWITCH_MODE |
// - add: #define CFG3_NO_GPSFIX_NO_START |
//----------------------------------------------- |
// |
// |
// + paramID (paramRevItem_t -> paramID) |
// | |
// | + size in Bytes (paramRevItem_t -> size) |
// | | |
// | | |
// | | |
paramRevItem_t const paramset_091[] PROGMEM = |
{ |
{ param_DUMMY , 111 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_091[] |
//############################################################################################################################################################ |
//----------------------------------------------- |
// FC-Parameter Revision: 92 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90d |
// |
// Tabelle ist definiert in der |
// nachfolgenden paramset_093 |
// |
// STRUKTUR-DIFF zu 091: |
// -add: param_NaviOut1Parameter |
//# |
// DEFINE-DIFF zu 091: |
// -add: #define CFG3_USE_NC_FOR_OUT1 0x20 |
// -add: #define CFG3_SPEAK_ALL |
// -add: #define SERVO_NICK_INV 0x01 |
// -add: #define SERVO_ROLL_INV 0x02 |
// -add: #define SERVO_RELATIVE 0x04 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 93 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90e, 0.90g, 0.90j |
// |
// STRUKTUR-DIFF zu 092: |
// - keine Aenderung der internen Datenstruktur erkennbar! |
// |
// DEFINE-DIFF zu 092: |
// - keine Aenderung erkennbar! |
//----------------------------------------------- |
paramRevItem_t const paramset_093[] PROGMEM = |
{ |
{ param_DUMMY , 112 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_093[] |
//----------------------------------------------- |
// FC-Parameter Revision: 94 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.91a |
// |
// STRUKTUR-DIFF zu 093: |
// - add: param_StartLandChannel |
// - add: param_LandingSpeed |
// |
// DEFINE-DIFF zu 093: |
// etwas undurchsichtig... |
// - add: #define EE_DUMMY |
// - add: #define PID_HARDWARE_VERSION |
//----------------------------------------------- |
paramRevItem_t const paramset_094[] PROGMEM = |
{ |
{ param_DUMMY , 114 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_094[] |
//----------------------------------------------- |
// FC-Parameter Revision: 95 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in : 0.90L, 2.00a, , 2.00e |
// |
// STRUKTUR-DIFF zu 094: |
// - rename: param_MaxHoehe -> param_HoeheChannel |
// - rename: param_NaviGpsModeControl -> param_NaviGpsModeChannel |
// - rename: param_NaviGpsACC -> param_NaviGpsA |
// - rename: param_CareFreeModeControl -> param_CareFreeChannel |
// |
// DEFINE-DIFF zu 094: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_095[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_095[] |
//----------------------------------------------- |
// FC-Parameter Revision: 96 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: ??? |
// |
// STRUKTUR-DIFF zu 095: |
// - KEINE AENDERUNG der INTERNEN DATEN-STRUKTUR! |
// |
// DEFINE-DIFF zu 095: |
// GlobalConfig3 |
// - add: #define CFG3_SERVO_NICK_COMP_OFF 0x80 |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 97 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.02b |
// |
// STRUKTUR-DIFF zu 096: |
// - add: param_CompassOffset |
// - add: param_AutoLandingVoltage |
// |
// DEFINE-DIFF zu 096: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_097[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_097[] |
//----------------------------------------------- |
// FC-Parameter Revision: 98 |
// |
// ab FC-Version: ab ??? bis ??? |
// gefunden in: 2.03d |
// |
// STRUKTUR-DIFF zu 097: |
// - add: param_ComingHomeVoltage |
// |
// DEFINE-DIFF zu 097: |
// - keine Aenderung |
//----------------------------------------------- |
paramRevItem_t const paramset_098[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char Hoehe_StickNeutralPoint;// Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char ServoManualControlSpeed;// |
{ param_CamOrientation , 1 }, // unsigned char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char WARN_J17_Bitmask; // for the J17 Output |
{ param_NaviOut1Parameter , 1 }, // unsigned char NaviOut1Parameter; // for the J16 Output |
{ param_NaviGpsModeChannel , 1 }, // unsigned char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_BitConfig , 1 }, // unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_098[] |
//----------------------------------------------- |
// FC-Parameter Revision: 99 |
// keine Inormationen zu Rev. 99 vorhanden. |
// |
// Kommentar dazu von Holger Buss: |
// "Die Revision 0.99 kannst Du ignorieren." |
//----------------------------------------------- |
//----------------------------------------------- |
// FC-Parameter Revision: 100 |
// |
// ab FC-Version: ab ??? bis 2.05e |
// gefunden in: 2.05e |
// |
// STRUKTUR-DIFF zu 098: |
// - del: NaviOut1Parameter |
// - add: AutoPhotoDistance (ersetzt NaviOut1Parameter) |
// - add: AutoPhotoAtitudes |
// - add: SingleWpSpeed |
// |
// DEFINE-DIFF zu 098: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine kurze Zeit in Betaversionen |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_100[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviOperatingRadius , 1 }, // unsigned char char NaviOperatingRadius; |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_100[] |
//----------------------------------------------- |
// FC-Parameter Revision: 101 |
// |
// ab FC-Version: ab 2.05f bis 2.05f |
// gefunden in: 2.05f |
// |
// STRUKTUR-DIFF zu 100: |
// - add: Servo3OnValue |
// - add: Servo3OffValue |
// - add: Servo4OnValue |
// - add: Servo4OffValue |
// |
// DEFINE-DIFF zu 100: |
// - keine Aenderung |
// |
// ANMERKUNG OG 06.04.2014: |
// Die Tabelle kann ggf. demnaechst geloescht werden |
// da sie nur fuer eine einzige Betaversion |
// vorhanden war! |
//----------------------------------------------- |
paramRevItem_t const paramset_101[] PROGMEM = |
{ |
{ param_DUMMY , 123 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_Name , 12 }, // char Name[12]; |
{ param_DUMMY , 1 }, // n-Bytes Fueller... keine Unterstuetzung fuer paramEdit |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_101[] |
//----------------------------------------------- |
// FC-Parameter Revision: 102 |
// |
// ab FC-Version: ab 2.05g bis ??? (min. 2.06b) |
// gefunden in: 2.05g, 2.06a |
// |
// STRUKTUR-DIFF zu 101: |
// - add: NaviMaxFlyingRange (ersetzt NaviOperatingRadius) |
// - add: NaviDescendRange |
// - del: NaviOperatingRadius |
// |
// DEFINE-DIFF zu 101: |
// - del: CFG3_DPH_MAX_RADIUS |
//----------------------------------------------- |
paramRevItem_t const paramset_102[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_GPS_Z , 1 }, // unsigned char char Hoehe_GPS_Z; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_102[] |
//----------------------------------------------- |
// FC-Parameter Revision: 103 |
// |
// ab FC-Version: ab 2.07f bis ??? |
// gefunden in: 2.07f |
// |
// STRUKTUR-DIFF zu 102: |
// keine |
// |
// DEFINE-DIFF zu 102: |
// - chg: Hoehe_GPS_Z zu Hoehe_TiltCompensation |
//----------------------------------------------- |
paramRevItem_t const paramset_103[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_103[] |
//----------------------------------------------- |
// FC-Parameter Revision: 104 |
// |
// ab FC-Version: ab 2.09d bis ??? |
// gefunden in: 2.09d |
// |
// STRUKTUR-DIFF zu 103: |
// add: LandingPulse |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_104[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_104[] |
//----------------------------------------------- |
// FC-Parameter Revision: 105 |
// |
// ab FC-Version: ab 2.09j bis ??? |
// gefunden in: 2.09j |
// |
// STRUKTUR-DIFF zu 104: |
// add: unsigned char ServoFS_Pos[5]; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_105[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_105[] |
//----------------------------------------------- |
// FC-Parameter Revision: 106 |
// |
// ab FC-Version: ab 2.11a bis ??? |
// gefunden in: 2.11a |
// |
// STRUKTUR-DIFF zu 105: |
// add: unsigned char SingleWpControlChannel; |
// unsigned char MenuKeyChannel; |
// |
//----------------------------------------------- |
paramRevItem_t const paramset_106[] PROGMEM = |
{ |
{ param_Revision , 1 }, // unsigned char Revision; |
{ param_Kanalbelegung , 12 }, // unsigned char char Kanalbelegung[12]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
{ param_GlobalConfig , 1 }, // unsigned char char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
{ param_Hoehe_MinGas , 1 }, // unsigned char char Hoehe_MinGas; // Wert : 0-100 |
{ param_Luftdruck_D , 1 }, // unsigned char char Luftdruck_D; // Wert : 0-250 |
{ param_HoeheChannel , 1 }, // unsigned char char HoeheChannel; // Wert : 0-32 |
{ param_Hoehe_P , 1 }, // unsigned char char Hoehe_P; // Wert : 0-32 |
{ param_Hoehe_Verstaerkung , 1 }, // unsigned char char Hoehe_Verstaerkung; // Wert : 0-50 |
{ param_Hoehe_ACC_Wirkung , 1 }, // unsigned char char Hoehe_ACC_Wirkung; // Wert : 0-250 |
{ param_Hoehe_HoverBand , 1 }, // unsigned char char Hoehe_HoverBand; // Wert : 0-250 |
{ param_Hoehe_TiltCompensation , 1 }, // unsigned char char Hoehe_TiltCompensation; // Wert : 0-250 |
{ param_Hoehe_StickNeutralPoint , 1 }, // unsigned char char Hoehe_StickNeutralPoint; // Wert : 0-250 |
{ param_Stick_P , 1 }, // unsigned char char Stick_P; // Wert : 1-6 |
{ param_Stick_D , 1 }, // unsigned char char Stick_D; // Wert : 0-64 |
{ param_StickGier_P , 1 }, // unsigned char char StickGier_P; // Wert : 1-20 |
{ param_Gas_Min , 1 }, // unsigned char char Gas_Min; // Wert : 0-32 |
{ param_Gas_Max , 1 }, // unsigned char char Gas_Max; // Wert : 33-250 |
{ param_GyroAccFaktor , 1 }, // unsigned char char GyroAccFaktor; // Wert : 1-64 |
{ param_KompassWirkung , 1 }, // unsigned char char KompassWirkung; // Wert : 0-32 |
{ param_Gyro_P , 1 }, // unsigned char char Gyro_P; // Wert : 10-250 |
{ param_Gyro_I , 1 }, // unsigned char char Gyro_I; // Wert : 0-250 |
{ param_Gyro_D , 1 }, // unsigned char char Gyro_D; // Wert : 0-250 |
{ param_Gyro_Gier_P , 1 }, // unsigned char char Gyro_Gier_P; // Wert : 10-250 |
{ param_Gyro_Gier_I , 1 }, // unsigned char char Gyro_Gier_I; // Wert : 0-250 |
{ param_Gyro_Stability , 1 }, // unsigned char char Gyro_Stability; // Wert : 0-16 |
{ param_UnterspannungsWarnung , 1 }, // unsigned char char UnterspannungsWarnung; // Wert : 0-250 |
{ param_NotGas , 1 }, // unsigned char char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
{ param_NotGasZeit , 1 }, // unsigned char char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
{ param_Receiver , 1 }, // unsigned char char Receiver; // 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
{ param_I_Faktor , 1 }, // unsigned char char I_Faktor; // Wert : 0-250 |
{ param_UserParam1 , 1 }, // unsigned char char UserParam1; // Wert : 0-250 |
{ param_UserParam2 , 1 }, // unsigned char char UserParam2; // Wert : 0-250 |
{ param_UserParam3 , 1 }, // unsigned char char UserParam3; // Wert : 0-250 |
{ param_UserParam4 , 1 }, // unsigned char char UserParam4; // Wert : 0-250 |
{ param_ServoNickControl , 1 }, // unsigned char char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoNickComp , 1 }, // unsigned char char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
{ param_ServoNickMin , 1 }, // unsigned char char ServoNickMin; // Wert : 0-250 // Anschlag |
{ param_ServoNickMax , 1 }, // unsigned char char ServoNickMax; // Wert : 0-250 // Anschlag |
{ param_ServoRollControl , 1 }, // unsigned char char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
{ param_ServoRollComp , 1 }, // unsigned char char ServoRollComp; // Wert : 0-250 |
{ param_ServoRollMin , 1 }, // unsigned char char ServoRollMin; // Wert : 0-250 |
{ param_ServoRollMax , 1 }, // unsigned char char ServoRollMax; // Wert : 0-250 |
{ param_ServoNickRefresh , 1 }, // unsigned char char ServoNickRefresh; // Speed of the Servo |
{ param_ServoManualControlSpeed , 1 }, // unsigned char char ServoManualControlSpeed; // |
{ param_CamOrientation , 1 }, // unsigned char char CamOrientation; // |
{ param_Servo3 , 1 }, // unsigned char char Servo3; // Value or mapping of the Servo Output |
{ param_Servo4 , 1 }, // unsigned char char Servo4; // Value or mapping of the Servo Output |
{ param_Servo5 , 1 }, // unsigned char char Servo5; // Value or mapping of the Servo Output |
{ param_LoopGasLimit , 1 }, // unsigned char char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
{ param_LoopThreshold , 1 }, // unsigned char char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
{ param_LoopHysterese , 1 }, // unsigned char char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
{ param_AchsKopplung1 , 1 }, // unsigned char char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
{ param_AchsKopplung2 , 1 }, // unsigned char char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_CouplingYawCorrection , 1 }, // unsigned char char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
{ param_WinkelUmschlagNick , 1 }, // unsigned char char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
{ param_WinkelUmschlagRoll , 1 }, // unsigned char char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
{ param_GyroAccAbgleich , 1 }, // unsigned char char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
{ param_Driftkomp , 1 }, // unsigned char char Driftkomp; |
{ param_DynamicStability , 1 }, // unsigned char char DynamicStability; |
{ param_UserParam5 , 1 }, // unsigned char char UserParam5; // Wert : 0-250 |
{ param_UserParam6 , 1 }, // unsigned char char UserParam6; // Wert : 0-250 |
{ param_UserParam7 , 1 }, // unsigned char char UserParam7; // Wert : 0-250 |
{ param_UserParam8 , 1 }, // unsigned char char UserParam8; // Wert : 0-250 |
{ param_J16Bitmask , 1 }, // unsigned char char J16Bitmask; // for the J16 Output |
{ param_J16Timing , 1 }, // unsigned char char J16Timing; // for the J16 Output |
{ param_J17Bitmask , 1 }, // unsigned char char J17Bitmask; // for the J17 Output |
{ param_J17Timing , 1 }, // unsigned char char J17Timing; // for the J17 Output |
{ param_WARN_J16_Bitmask , 1 }, // unsigned char char WARN_J16_Bitmask; // for the J16 Output |
{ param_WARN_J17_Bitmask , 1 }, // unsigned char char WARN_J17_Bitmask; // for the J17 Output |
{ param_AutoPhotoDistance , 1 }, // unsigned char char AutoPhotoDistance; // Auto Photo // ab Rev. 100 (ehemals NaviOut1Parameter) |
{ param_NaviGpsModeChannel , 1 }, // unsigned char char NaviGpsModeChannel; // Parameters for the Naviboard |
{ param_NaviGpsGain , 1 }, // unsigned char char NaviGpsGain; |
{ param_NaviGpsP , 1 }, // unsigned char char NaviGpsP; |
{ param_NaviGpsI , 1 }, // unsigned char char NaviGpsI; |
{ param_NaviGpsD , 1 }, // unsigned char char NaviGpsD; |
{ param_NaviGpsPLimit , 1 }, // unsigned char char NaviGpsPLimit; |
{ param_NaviGpsILimit , 1 }, // unsigned char char NaviGpsILimit; |
{ param_NaviGpsDLimit , 1 }, // unsigned char char NaviGpsDLimit; |
{ param_NaviGpsA , 1 }, // unsigned char char NaviGpsA; |
{ param_NaviGpsMinSat , 1 }, // unsigned char char NaviGpsMinSat; |
{ param_NaviStickThreshold , 1 }, // unsigned char char NaviStickThreshold; |
{ param_NaviWindCorrection , 1 }, // unsigned char char NaviWindCorrection; |
{ param_NaviAccCompensation , 1 }, // unsigned char char NaviAccCompensation; // New since 0.86 -> was: SpeedCompensation |
{ param_NaviMaxFlyingRange , 1 }, // unsigned char char NaviMaxFlyingRange; // in 10m |
{ param_NaviAngleLimitation , 1 }, // unsigned char char NaviAngleLimitation; |
{ param_NaviPH_LoginTime , 1 }, // unsigned char char NaviPH_LoginTime; |
{ param_NaviDescendRange , 1 }, // unsigned char char NaviDescendRange; |
{ param_ExternalControl , 1 }, // unsigned char char ExternalControl; // for serial Control |
{ param_OrientationAngle , 1 }, // unsigned char char OrientationAngle; // Where is the front-direction? |
{ param_CareFreeChannel , 1 }, // unsigned char char CareFreeChannel; // switch for CareFree |
{ param_MotorSafetySwitch , 1 }, // unsigned char char MotorSafetySwitch; |
{ param_MotorSmooth , 1 }, // unsigned char char MotorSmooth; |
{ param_ComingHomeAltitude , 1 }, // unsigned char char ComingHomeAltitude; |
{ param_FailSafeTime , 1 }, // unsigned char char FailSafeTime; |
{ param_MaxAltitude , 1 }, // unsigned char char MaxAltitude; |
{ param_FailsafeChannel , 1 }, // unsigned char char FailsafeChannel; // if the value of this channel is > 100, the MK reports "RC-Lost" |
{ param_ServoFilterNick , 1 }, // unsigned char char ServoFilterNick; |
{ param_ServoFilterRoll , 1 }, // unsigned char char ServoFilterRoll; |
{ param_Servo3OnValue , 1 }, // unsigned char char Servo3OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo3OffValue , 1 }, // unsigned char char Servo3OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OnValue , 1 }, // unsigned char char Servo4OnValue; // ab Rev. 101 (FC 2.05f) |
{ param_Servo4OffValue , 1 }, // unsigned char char Servo4OffValue; // ab Rev. 101 (FC 2.05f) |
{ param_ServoFS_Pos , 5 }, // unsigned char ServoFS_Pos[5]; // ab Rev. 105 (FC 2.09i) |
{ param_StartLandChannel , 1 }, // unsigned char char StartLandChannel; |
{ param_LandingSpeed , 1 }, // unsigned char char LandingSpeed; |
{ param_CompassOffset , 1 }, // unsigned char char CompassOffset; |
{ param_AutoLandingVoltage , 1 }, // unsigned char char AutoLandingVoltage; // in 0,1V 0 -> disabled |
{ param_ComingHomeVoltage , 1 }, // unsigned char char ComingHomeVoltage; // in 0,1V 0 -> disabled |
{ param_AutoPhotoAtitudes , 1 }, // unsigned char char AutoPhotoAtitudes; // ab Rev. 100 |
{ param_SingleWpSpeed , 1 }, // unsigned char char SingleWpSpeed; // ab Rev. 100 |
{ param_LandingPulse , 1 }, // unsigned char char LandingPulse; // ab Rev. 104 (FC 2.09d) |
{ param_SingleWpControlChannel , 1 }, // unsigned char SingleWpControlChannel; // ab Rev. 106 (FC2.11a) |
{ param_MenuKeyChannel , 1 }, // unsigned char MenuKeyChannel; // ab Rev. 106 (FC2.11a) |
{ param_BitConfig , 1 }, // unsigned char char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
{ param_ServoCompInvert , 1 }, // unsigned char char ServoCompInvert; // 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
{ param_ExtraConfig , 1 }, // unsigned char char ExtraConfig; // bitcodiert |
{ param_GlobalConfig3 , 1 }, // unsigned char char GlobalConfig3; // bitcodiert |
{ param_Name , 12 }, // char Name[12]; |
{ param_crc , 1 }, // unsigned char char crc; // must be the last byte! |
{ param_EOF , 0 } // END OF PARAM-LIST - MUST BE THE LAST!! |
}; // end: paramset_106[] |
//############################################################################################# |
//# 1b. Zuweisungstabelle: Revision -> paramset_xxx |
//############################################################################################# |
typedef struct |
{ |
unsigned char Revision; |
const paramRevItem_t *RevisionTable; |
} paramsetRevMap_t; |
#define MAPEOF 255 |
//---------------------------------------------------------------- |
// Mappingtabelle zwischen FC-Revision und Paramset-Tabelle |
// |
// Anmerkung bzgl. Groesse: |
// Angenommen man wuerde die Rev 97 bis 90 weglassen, dann |
// wuerde man ca. 1 KByte sparen |
//---------------------------------------------------------------- |
paramsetRevMap_t paramsetRevMap[] = |
{ |
{ 106 , paramset_106 }, // 2.11a |
{ 105 , paramset_105 }, // 2.09j |
{ 104 , paramset_104 }, // 2.09d |
{ 103 , paramset_103 }, // 2.07f |
{ 102 , paramset_102 }, // 2.05g (bis min. 2.06b) |
{ 101 , paramset_101 }, // 2.05f (eingeschraenkte Unterstuetzung - ) nur in einer einzigen Betaversion vorhanden!) |
{ 100 , paramset_100 }, // 2.05e (Anmerkung OG 06.04.2014: ggf. spaeter loeschen um Platz zu sparen da diese Version nur in einigen wenigen Betaversionen vorhanden ist) |
// Rev. 99: nicht vorhanden; ignorieren |
{ 98 , paramset_098 }, // 2.03d |
{ 97 , paramset_097 }, // 2.02b |
{ 96 , paramset_095 }, // ??? (keine Struktur-Aenderung zu 095) |
{ 95 , paramset_095 }, // 0.90L, 2.00a, 2.00e |
{ 94 , paramset_094 }, // eingeschraenkte Unterstuetzung; 0.91a |
{ 93 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90e, 0.90g, 0.90j |
{ 92 , paramset_093 }, // eingeschraenkte Unterstuetzung; 0.90d (keine Struktur-Aenderung zu 093) |
{ 91 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88m, 0.88n |
{ 90 , paramset_091 }, // eingeschraenkte Unterstuetzung; 0.88e (keine Struktur-Aenderung zu 091) |
{ MAPEOF, 0 } // END OF LIST - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 2. Konfigurationstabelle der paramSubID's (Bit- und Bytefelder) |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramSubID; // groesser/gleich PARAMSUBITEMS |
unsigned char paramID; // -> mapping auf entsprechende paramID |
unsigned char subType; // SUBTYPE_BIT oder SUBTYPE_BYTE |
unsigned char subIndex; // bei SUBTYPE_BIT: Bitmask z.b. 0x02 |
// bei SUBTYPE_BYTE: index des Bytes (siehe param_Kanalbelegung) |
unsigned char von_Revision; // ab welcher FC-Revision ist das SubItem vorhanden (0=immer) |
unsigned char bis_Revision; // bis zu welcher FC-Revision ist das SubItem vorhanden (0=immer) |
} paramSubItem_t; |
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
// +- von_Revision |
// | |
// +- paramSubID +- gehoert zu? +- Bit/Byte? +- Bit/Byte der paramID | +- bis_Revision |
// | | | | | | |
//--|-----------------------------------------|----------------------|--------------|----------------------------------|--|---------------------------------------------- |
paramSubItem_t const paramSubItem[] PROGMEM = |
{ |
{ param_ServoCompInvert_SERVO_NICK_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_NICK_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
{ param_ServoCompInvert_SERVO_ROLL_INV , param_ServoCompInvert, SUBTYPE_BIT, SERVO_ROLL_INV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
{ param_ServoCompInvert_SERVO_RELATIVE , param_ServoCompInvert, SUBTYPE_BIT, SERVO_RELATIVE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
{ param_ExtraConfig_HeightLimit , param_ExtraConfig, SUBTYPE_BIT, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
{ param_ExtraConfig_HeightVario , param_ExtraConfig, SUBTYPE_BITN, CFG2_HEIGHT_LIMIT , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 negiert zu param_ExtraConfig_HeightLimit |
{ param_ExtraConfig_VarioBeep , param_ExtraConfig, SUBTYPE_BIT, CFG2_VARIO_BEEP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
{ param_ExtraConfig_SensitiveRc , param_ExtraConfig, SUBTYPE_BIT, CFG_SENSITIVE_RC , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
{ param_ExtraConfig_33vReference , param_ExtraConfig, SUBTYPE_BIT, CFG_3_3V_REFERENCE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
{ param_ExtraConfig_NoRcOffBeeping , param_ExtraConfig, SUBTYPE_BIT, CFG_NO_RCOFF_BEEPING , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
{ param_ExtraConfig_GpsAid , param_ExtraConfig, SUBTYPE_BIT, CFG_GPS_AID , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
{ param_ExtraConfig_LearnableCarefree , param_ExtraConfig, SUBTYPE_BIT, CFG_LEARNABLE_CAREFREE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
{ param_ExtraConfig_IgnoreMagErrAtStartup , param_ExtraConfig, SUBTYPE_BIT, CFG_IGNORE_MAG_ERR_AT_STARTUP , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
{ param_BitConfig_LoopOben , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_OBEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
{ param_BitConfig_LoopUnten , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_UNTEN , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
{ param_BitConfig_LoopLinks , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_LINKS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
{ param_BitConfig_LoopRechts , param_BitConfig, SUBTYPE_BIT, CFG_LOOP_RECHTS , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
{ param_BitConfig_MotorBlink1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
{ param_BitConfig_MotorOffLed1 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED1 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
{ param_BitConfig_MotorOffLed2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_OFF_LED2 , 0,101}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
{ param_BitConfig_MotorBlink2 , param_BitConfig, SUBTYPE_BIT, CFG_MOTOR_BLINK2 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
{ param_GlobalConfig3_NoSdCardNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_SDCARD_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
{ param_GlobalConfig3_DphMaxRadius , param_GlobalConfig3, SUBTYPE_BIT, CFG3_DPH_MAX_RADIUS , 0,100}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 |
{ param_GlobalConfig3_VarioFailsafe , param_GlobalConfig3, SUBTYPE_BIT, CFG3_VARIO_FAILSAFE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
{ param_GlobalConfig3_MotorSwitchMode , param_GlobalConfig3, SUBTYPE_BIT, CFG3_MOTOR_SWITCH_MODE , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
{ param_GlobalConfig3_NoGpsFixNoStart , param_GlobalConfig3, SUBTYPE_BIT, CFG3_NO_GPSFIX_NO_START , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
{ param_GlobalConfig3_UseNcForOut1 , param_GlobalConfig3, SUBTYPE_BIT, CFG3_USE_NC_FOR_OUT1 , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
{ param_GlobalConfig3_SpeakAll , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SPEAK_ALL , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
{ param_GlobalConfig3_ServoNickCompOff , param_GlobalConfig3, SUBTYPE_BIT, CFG3_SERVO_NICK_COMP_OFF , 96, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
{ param_GlobalConfig_HoehenRegelung , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHENREGELUNG , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
{ param_GlobalConfig_HoehenSchalter , param_GlobalConfig, SUBTYPE_BIT, CFG_HOEHEN_SCHALTER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
{ param_GlobalConfig_HeadingHold , param_GlobalConfig, SUBTYPE_BIT, CFG_HEADING_HOLD , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
{ param_GlobalConfig_KompassAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
{ param_GlobalConfig_KompassFix , param_GlobalConfig, SUBTYPE_BIT, CFG_KOMPASS_FIX , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
{ param_GlobalConfig_GpsAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_GPS_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
{ param_GlobalConfig_AchsenkopplungAktiv , param_GlobalConfig, SUBTYPE_BIT, CFG_ACHSENKOPPLUNG_AKTIV , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
{ param_GlobalConfig_DrehratenBegrenzer , param_GlobalConfig, SUBTYPE_BIT, CFG_DREHRATEN_BEGRENZER , 0, 0}, // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
{ param_Kanalbelegung_Nick , param_Kanalbelegung, SUBTYPE_BYTE, 0 , 0, 0}, // -> Kanalbelegung[12] |
{ param_Kanalbelegung_Roll , param_Kanalbelegung, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Kanalbelegung_Gas , param_Kanalbelegung, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Kanalbelegung_Gear , param_Kanalbelegung, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Kanalbelegung_Poti1 , param_Kanalbelegung, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_Kanalbelegung_Poti2 , param_Kanalbelegung, SUBTYPE_BYTE, 5 , 0, 0}, |
{ param_Kanalbelegung_Poti3 , param_Kanalbelegung, SUBTYPE_BYTE, 6 , 0, 0}, |
{ param_Kanalbelegung_Poti4 , param_Kanalbelegung, SUBTYPE_BYTE, 7 , 0, 0}, |
{ param_Kanalbelegung_Poti5 , param_Kanalbelegung, SUBTYPE_BYTE, 8 , 0, 0}, |
{ param_Kanalbelegung_Poti6 , param_Kanalbelegung, SUBTYPE_BYTE, 9 , 0, 0}, |
{ param_Kanalbelegung_Poti7 , param_Kanalbelegung, SUBTYPE_BYTE, 10 , 0, 0}, |
{ param_Kanalbelegung_Poti8 , param_Kanalbelegung, SUBTYPE_BYTE, 11 , 0, 0}, |
{ param_CompassOffset_DisableDeclCalc , param_CompassOffset, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 8 und 7 von CompassOffset kodiert |
{ param_ComingHomeOrientation , param_ServoCompInvert, SUBTYPE_BYTE, 0 , 0, 0}, // ist in Bit 5 und 4 von CervoCompInvert |
{ param_ServoNickFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 0 , 0, 0}, |
{ param_ServoRollFailsave , param_ServoFS_Pos, SUBTYPE_BYTE, 1 , 0, 0}, |
{ param_Servo3Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 2 , 0, 0}, |
{ param_Servo4Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 3 , 0, 0}, |
{ param_Servo5Failsave , param_ServoFS_Pos, SUBTYPE_BYTE, 4 , 0, 0}, |
{ param_EOF , 0,0,0,0,0 } // END - MUST BE THE LAST!! |
}; // end: paramSubItem[] |
//############################################################################################# |
//# 3a. Transformations-Tabelle |
//############################################################################################# |
//--------------------------------------- |
// struct: einzelnes Parameter-SubItem |
//--------------------------------------- |
typedef struct |
{ |
unsigned char paramID; // paramID oder paramSubID |
unsigned char (*transformfunc)( uint8_t cmd, unsigned char value, unsigned char newvalue ); // Edit-Funktion (z.B. editGeneric()); cmd = CMD_EDIT oder CMD_SHORTVALUE |
} paramTransform_t; |
//--------------------------------------- |
// forward Deklarationen fuer transform |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ); |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, uint16_t newvalue ); |
//--------------------------------------- |
//--------------------------------------- |
paramTransform_t const paramTransform[] = |
{ |
{ param_CompassOffset_DisableDeclCalc , &transform_CompassOffset_DisableDeclCalc }, |
{ param_ComingHomeOrientation , &transform_ComingHomeOrientation }, |
// { param_LandingPulse, &transform_ValueACCZ}, |
{ param_EOF , NULL } // END - MUST BE THE LAST!! |
}; |
//############################################################################################# |
//# 3b. Transformations-Funktionen |
//############################################################################################# |
//--------------------------------------- |
// transform_CompassOffset_DisableDeclCalc() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// Der true/false (Ja/Nein) Wert von param_CompassOffset_DisableDeclCalc |
// ist in dem Byte von param_CompassOffset in den Bits 7 & 8 einkodiert. |
//--------------------------------------- |
unsigned char transform_CompassOffset_DisableDeclCalc( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
uint8_t bit7; |
uint8_t bit8; |
bit7 = ((value & 0x40) ? true : false); |
bit8 = ((value & 0x80) ? true : false); |
if( cmd == GETVALUE ) |
{ |
// Bit 8 == Bit 7: Disable dec. Calc AUS |
// Bit 8 != Bit 7: Disable dec. Calc AN |
value = ((bit8 == bit7) ? 0 : 1); |
} |
if( cmd == SETVALUE ) |
{ |
if( newvalue ) bit8 = !bit7; // Bit 8 != Bit 7: Disable dec. Calc AN |
else bit8 = bit7; // Bit 8 == Bit 7: Disable dec. Calc AUS |
if( bit8 ) value = (value | 0x80); // Bit 8 setzen |
else value = (value & (0x80 ^ 0xff)); // Bit 8 loeschen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
//--------------------------------------- |
// transform_ComingHomeOrientation() |
// |
// PARAMETER: |
// cmd : GETVALUE || SETVALUE |
// value : |
// newvalue: nur wenn cmd == SETVALUE |
// |
// |
// ist in dem Byte von param_ServoCompInvert in den Bits 4 & 5 einkodiert, Bits 1-3 bleiben unverändert |
//--------------------------------------- |
uint8_t save =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
unsigned char transform_ComingHomeOrientation( uint8_t cmd, unsigned char value, unsigned char newvalue ) |
{ |
if( cmd == GETVALUE ) |
{ |
save = value; // altes "gemeinsames" Byte ServoCompInvert sichern |
value =((value & 0x18) >> 3); // CominghomeOrientation Bits nach rechts verschieben |
} |
if( cmd == SETVALUE ) |
{ |
value = ((newvalue << 3)|(save & 0x07)); // CominghomeOrientation Bits nach links verschieben und "alte" Bits 1-3 wieder dazufügen |
} |
//lcdx_printf_at_P( 0, 7, MINVERS, 0,0 , PSTR(" %d => %d "), value, newvalue ); |
return value; |
} |
// |
// |
////--------------------------------------- |
//// transform_ValueACCZ() |
//// |
//// PARAMETER: |
//// cmd : GETVALUE || SETVALUE |
//// value : |
//// newvalue: nur wenn cmd == SETVALUE |
//// |
//// |
//// ACC Z Landing Pulse Wert für die Anzeige x 4 |
////--------------------------------------- |
// |
// |
//unsigned char transform_ValueACCZ( uint16_t cmd, uint16_t value, unint16_t newvalue ) |
// |
//uint16_t save16 =0; // globale Variable zum Sichern des alten Byte ServoCompInvert Wertes |
// |
//{ |
// if( cmd == GETVALUE ) |
// { |
// save16 = value; // altes Byte ACC Z Landing Pulse sichern |
// value = (value * 4); // Wert x 4 |
// } |
// |
// if( cmd == SETVALUE ) |
// { |
// value = (newvalue / 4); // Neuer Wert ACC Z Landingpulse /4 |
// } |
// |
// |
// return value; |
//} |
//############################################################################################# |
//# 4a. interne Zugriffsfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// paramSubID: 'echter' Parameter zum Suchen in paramSubItem[] zum |
// nachfolgeden Parameter geben das Suchergebnis zurueck |
// und sind alle 0 wenn nichts gefunden wurde |
// |
// Rueckgabe: |
// true/false |
//-------------------------------------------------------------- |
unsigned char _paramset_getsubitemid( unsigned char paramSubID, unsigned char *paramID, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char id; |
unsigned char *p; |
unsigned char von_Revision; |
unsigned char bis_Revision; |
*paramID = 0; |
*subType = 0; |
*subIndex = 0; |
p = (unsigned char *) paramSubItem; |
while( true ) |
{ |
id = pgm_read_byte(p+0); // paramSubItem[..].paramSubID - die aktuelle paramSubID |
if( (id == paramSubID) ) // gefunden? |
{ |
von_Revision = pgm_read_byte(p+4); // paramSubItem[..].von_Revision; |
bis_Revision = pgm_read_byte(p+5); // paramSubItem[..].bis_Revision; |
// in aktueller FC-Revision vorhanden? |
if( von_Revision && (MKVersion.paramsetRevision < von_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
if( bis_Revision && (MKVersion.paramsetRevision > bis_Revision) ) return false; // passt nicht zur aktuellen FC-Revision -> exit |
*paramID = pgm_read_byte(p+1); // paramSubItem[..].paramID; |
*subType = pgm_read_byte(p+2); // paramSubItem[..].subType; |
*subIndex = pgm_read_byte(p+3); // paramSubItem[..].subIndex; |
return true; |
} |
if( id == param_EOF ) break; |
p += sizeof( paramSubItem_t ); |
} |
return false; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize: false = offset zurueckgeben (normaler Modus) |
// true = size von paramID zurueckgeben (fuer paramSize()) |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffsetX( unsigned char paramID, unsigned char retItemSize, unsigned char *subType, unsigned char *subIndex ) |
{ |
unsigned char *p; |
unsigned char offset; |
unsigned char size; |
unsigned char this_paramID; |
unsigned char paramSubID; |
*subType = SUBTYPE_NO; // =0 |
*subIndex = 0; |
size = 0; |
offset = param_NOTFOUND; |
if( MKVersion.paramsetOK ) |
{ |
//----------------------- |
// eine paramSubID wurde uebergeben |
// -> ermittle zugehoerige paramID! |
//----------------------- |
if( (paramID >= param_SUBITEM) && (paramID != param_EOF) ) |
{ |
paramSubID = paramID; |
if( !_paramset_getsubitemid( paramSubID, ¶mID, subType, subIndex ) ) |
{ |
// keine paramID gefunden... |
if( retItemSize ) return size; // size == 0 |
else return offset; // offset == param_NOTFOUND |
} |
} |
//----------------------- |
// paramID suchen |
//----------------------- |
offset = 0; |
p = (unsigned char *) paramsetRevTable; |
while( true ) |
{ |
this_paramID = pgm_read_byte(p); // die aktuelle paramID |
size = pgm_read_byte(p+1); // size von paramID |
// gefunden oder Ende |
if( (this_paramID == paramID) || (this_paramID == param_EOF) ) |
{ |
break; |
} |
offset += size; |
p += sizeof( paramRevItem_t ); |
} |
// paramID nicht gefunden? |
if( (this_paramID == param_EOF) && (paramID != param_EOF) ) |
{ |
offset = param_NOTFOUND; |
size = 0; |
} |
} |
if( retItemSize ) return size; |
else return offset; |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// Parameter: |
// retItemSize == false: offset zurueckgeben (normaler Modus) |
// retItemSize == true : size von paramID zurueckgeben |
// --> fuer paramSize() |
//-------------------------------------------------------------- |
unsigned char _paramset_getoffset( unsigned char paramID, unsigned char retItemSize ) |
{ |
unsigned char subType; |
unsigned char subIndex; |
return _paramset_getoffsetX( paramID, retItemSize, &subType, &subIndex ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// sucht/prueft ob eine Transform-Funktion vorhanden ist |
// |
// RUECKGABE: |
// Index: (Num 0..n) auf paramTransform[] |
// => param_EOF (=255) wenn keine transform-Funktion vorhanden |
//-------------------------------------------------------------- |
unsigned char _paramset_gettransformIndex( unsigned char paramID ) |
{ |
unsigned char i; |
i = 0; |
while( paramTransform[i].paramID != param_EOF ) |
{ |
if( paramTransform[i].paramID == paramID ) return i; |
if( paramTransform[i].paramID == param_EOF ) return param_EOF; |
i++; |
} |
return param_EOF; |
} |
//############################################################################################# |
//# 4b. oeffentliche Zugriffsfunktionen fuer paramID / paramSubID Elemente |
//# |
//# Hier sind die Getter/Setter um auf die einzelnen Werte der paramset-Struktur zuzugreifen. |
//# Dazu muss vorher das richtige paramset initialisert worden sein! |
//# |
//# ACHTUNG! |
//# Fuer die Daten wird direkt auf den RX-Buffer vom PKT zugegriffen! |
//# ==> der Buffer darf NICHT durch andere Datenpakete bereits wieder ueberschrieben worden sein! |
//# |
//# Anmerkung: |
//# Das obige 'Achtung' koennte man aendern durch eine Kopie des RX-Buffers. Das verbraucht |
//# zusaetzlichen RAM-Speicher -> probieren wir es erstmal so... |
//############################################################################################# |
//-------------------------------------------------------------- |
// size = paramSize( paramID ) |
//-------------------------------------------------------------- |
unsigned char paramSize( unsigned char paramID ) |
{ |
return( _paramset_getoffset(paramID,true) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// exist = paramExist( paramID ) |
// |
// Rueckgabe: |
// true : die paramID existiert im aktuellen paramset |
// false : die paramID existiert nicht |
//-------------------------------------------------------------- |
unsigned char paramExist( unsigned char paramID ) |
{ |
return( (_paramset_getoffset(paramID,true) != 0) ); // true = size (in Bytes) der paramID zurueckgeben (nicht offset) |
} |
//-------------------------------------------------------------- |
// pointer = paramGet_p( paramID ) |
// |
// ACHTUNG! Nicht faehig fuer Transform! |
// |
// RUECKGABE: |
// pointer direkt auf~ein Byte im Parameterset |
// (paramSubID nicht moeglich!) |
//-------------------------------------------------------------- |
unsigned char *paramGet_p( unsigned char paramID ) |
{ |
unsigned char offset; |
offset = _paramset_getoffset( paramID, false ); // false = offset zurueckgeben |
if( offset != param_NOTFOUND ) |
{ |
return( (unsigned char *) (mkparamset + offset) ); // Rueckgabe: Pointer auf die Daten |
} |
return 0; |
} |
//-------------------------------------------------------------- |
// value = paramGet( paramID ) |
// |
// holt den Wert von paramID. paramSubID's werden dabei |
// unterstuetzt (Bit- und Bytefelder). |
// |
// Bei Bit-Feldern (z.B. GlobalConfig3) wird eine enstprechende |
// Bit-Maskierung automatisch durchgefuehrt |
// Ergebnis: true (=1) oder false (=0)false (=0) |
// |
// Bei Byte-Feldern (z.B. Kanalbelegung) wird der Wert des |
// entsprechenden Byte's zurueckgegeben |
// |
// Hinweis: anhand des Rueckgabewertes kann nicht ueberprueft |
// werden ob die paramID gefunden wurde bzw. existiert! |
// -> ggf. erst mit paramExist() pruefen ob der Wert existiert! |
//-------------------------------------------------------------- |
unsigned char paramGet( unsigned char paramID ) |
{ |
unsigned char offset; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char value; |
unsigned char transformIdx; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
value = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen (paramID) |
//------------------- |
// subIndex: Bit |
//------------------- |
if( subType == SUBTYPE_BIT ) |
{ |
value = ((value & subIndex) ? true : false); // Rueckgabe: true/false des gewaehlten Bit's |
} |
//------------------- |
// subIndex: Bit negiert |
//------------------- |
if( subType == SUBTYPE_BITN ) |
{ |
value = ((value & subIndex) ? false : true); // Rueckgabe: true/false des gewaehlten Bit's (negiert) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
value = (unsigned char) *(mkparamset + (offset+subIndex)); // subIndex Inhalt lesen |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
value = paramTransform[transformIdx].transformfunc( GETVALUE, value, value); |
} |
return value; // ubyte zurueckgeben |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ) |
{ |
unsigned char offset; |
unsigned char subOffset = 0; |
unsigned char subType; |
unsigned char subIndex; |
unsigned char byte; |
unsigned char value; |
unsigned char transformIdx; |
value = newvalue; |
offset = _paramset_getoffsetX( paramID, false, &subType, &subIndex ); // false = offset zurueckgeben |
if( offset == param_NOTFOUND ) return false; // paramID nicht gefunden / nicht unterstuetzt |
//------------------- |
// subIndex: Bit |
//------------------- |
if( (subType == SUBTYPE_BIT) || (subType == SUBTYPE_BITN) ) |
{ |
byte = (unsigned char) *(mkparamset + offset); // offset Inhalt lesen |
if( subType == SUBTYPE_BITN ) // SUBTYPE_BITN = negiertes BIT |
value = (value ? false : true); // negieren |
if(value) byte = byte | subIndex; // Bit setzen |
else byte = byte & (subIndex ^ 0xff); // Bit loeschen |
value = byte; // neues Value fuer das gesamte Byte (mit eingerechnetem BIT) |
} |
//------------------- |
// subIndex: Byte |
//------------------- |
if( subType == SUBTYPE_BYTE ) |
{ |
subOffset = subIndex; // das 'verschobene' Byte (hier nur den Offset verschieben) |
} |
//------------------- |
// Transform |
//------------------- |
transformIdx = _paramset_gettransformIndex( paramID ); |
if( transformIdx != param_EOF ) |
{ |
byte = (unsigned char) *(mkparamset + offset + subOffset); // offset Inhalt lesen |
value = paramTransform[transformIdx].transformfunc( SETVALUE, byte, newvalue); |
} |
//------------------- |
// Byte speichern |
//------------------- |
*(mkparamset + offset + subOffset) = value; |
return true; // OK |
} |
//############################################################################################# |
//# 4c. oeffentliche Zugriffsfunktionen fuer das gesamte Paramset (Int usw.) |
//# |
//# Init und Abfrage der Groesse in Bytes ddes gesamten Paramset's |
//############################################################################################# |
//-------------------------------------------------------------- |
// ok = paramsetInit( *pData ) |
// |
// Ermittelt zu einer empfangenen Revision die passende paramset-Tabelle |
// und setzt entsprechende Werte in MKVersion |
// |
// Wird von MK_load_setting() aufgerufen nach dem Einlesen der |
// MK-Parameter (Setting-Request 'q') |
// |
// Parameter: |
// *pData : Zeiger direkt auf den RX-Buffer des PKT |
// |
// Rueckgabe: |
// true : OK - Paramset ist initialisert; die aktuelle FC-Revision |
// steht in MKVersion.paramsetRevision |
// false : Fehler - keine passende paramset-Tabelle gefunden |
//-------------------------------------------------------------- |
unsigned char paramsetInit( unsigned char *pData ) |
{ |
unsigned char i; |
unsigned char *p; |
paramsetRevTable = 0; |
mkparamset = 0; |
MKVersion.paramsetOK = false; |
MKVersion.paramsetRevision = 0; |
MKVersion.mksetting = 0; |
MKVersion.mksettingName[0] = 0; |
//-------------------------------------------------- |
// pData == 0 -> nur 'reset' der Daten |
//-------------------------------------------------- |
if( pData == 0 ) |
{ |
return 0; |
} |
//-------------------------------------------------- |
// diese beiden Werte koennen in jedem Fall gesetzt werden |
// unabhaengig ob das gefundene Parameterset vom PKT |
// unterstuetzt wird oder nicht |
//-------------------------------------------------- |
MKVersion.mksetting = (unsigned char) *pData; |
MKVersion.paramsetRevision = (unsigned char) *(pData + 1); |
//-------------------------------------------------- |
// FC-Revision in paramsetRevMap suchen |
//-------------------------------------------------- |
i = 0; |
while( true ) |
{ |
if( (paramsetRevMap[i].Revision == MKVersion.paramsetRevision) || (paramsetRevMap[i].Revision == MAPEOF) ) |
break; |
i++; |
} |
//-------------------------------------- |
// Revision nicht gefunden -> return 0 |
//-------------------------------------- |
if( paramsetRevMap[i].Revision == MAPEOF ) |
{ |
return 0; |
} |
//-------------------------------------- |
// Revision gefunden |
//-------------------------------------- |
mkparamset = (unsigned char *) (pData + 1); |
paramsetRevTable = (paramRevItem_t *) paramsetRevMap[i].RevisionTable; |
MKVersion.paramsetOK = true; |
//-------------------------------------- |
// den aktuellen Setting-Namen kopieren |
// wird u.a. vom OSD verwendet |
//-------------------------------------- |
p = paramGet_p( param_Name ); |
if( p ) memcpy( MKVersion.mksettingName, p, 12 ); |
return MKVersion.paramsetOK; |
} |
//-------------------------------------------------------------- |
// size = paramsetSize() |
// |
// Gibt die Groesse des gesamten aktuellen Parameterset's |
// zurueck. Fuer Revision 098 z.B. 130 = 130 Bytes. |
// |
// Rueckgabe: |
// =0 : Fehler; nicht unterstuetzte FC-Revision oder nicht |
// initialisiert |
//-------------------------------------------------------------- |
unsigned char paramsetSize( void ) |
{ |
if( MKVersion.paramsetOK ) |
return _paramset_getoffset( param_EOF, false ); // false = offset zurueckgeben; der offset von param_EOF entspricht der Groesse des paramSet's! |
else |
return 0; |
} |
//############################################################################################# |
//# x. TEST / DEBUG |
//############################################################################################# |
//-------------------------------------------------------------- |
// TEST / DEBUG |
//-------------------------------------------------------------- |
#ifdef DEBUG_PARAMSET |
void paramsetDEBUG( void ) |
{ |
lcd_cls(); |
//lcdx_printp_at( 0, 0, PSTR("NEW PARAM TEST..."), MNORMAL, 0,0); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
//memcpy( &MKVersion.NCVersion, (Version_t *) (pRxData), sizeof(MKVersion_t) ); |
/* |
typedef struct |
{ |
Version_t NCVersion; // |
Version_t FCVersion; // |
unsigned char paramsetRevision; // von der FC |
unsigned char paramsetOk; // true wenn Revision in paramset.c vorhanden |
} MKVersion_t; |
//------------------------------------- |
// zur Orientierung: Version_t |
//------------------------------------- |
//typedef struct |
//{ |
// unsigned char SWMajor; |
// unsigned char SWMinor; |
// unsigned char ProtoMajor; |
// unsigned char ProtoMinor; |
// unsigned char SWPatch; |
// unsigned char HardwareError[5]; |
//} __attribute__((packed)) Version_t; |
*/ |
//setting = MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
MK_Setting_load( 0xff, 20); // aktuelles MK Setting ermitteln |
//---------------------------------- |
// Anzeige: FC/NC Version |
//---------------------------------- |
lcdx_printp_at( 0, 0, PSTR("FC:"), MNORMAL, 0,0); |
MKVersion_print_at( 3, 0, FC, MNORMAL, 0,0); |
lcdx_printp_at( 13, 0, PSTR("NC:"), MNORMAL, 0,0); |
MKVersion_print_at( 16, 0, NC, MNORMAL, 0,0); |
//---------------------------------- |
// Anzeige: FC Revision |
//---------------------------------- |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("Rev: %03u"), MKVersion.paramsetRevision ); |
if( MKVersion.paramsetOK ) |
lcdx_printp_at( 8, 1, PSTR(" ok!"), MINVERS, 0,0); |
else |
lcdx_printp_at( 8, 1, PSTR(" err"), MINVERS, 0,0); |
// Size von paramset |
lcd_printf_at_P( 13, 1, MNORMAL, PSTR("Size=%3u"), paramsetSize() ); |
//---------------------------------- |
// Anzeige: aktuelles MK Setting |
//---------------------------------- |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("Set:%2u %s"), MKVersion.mksetting, MKVersion.mksettingName ); |
/* |
unsigned char *p; |
p = paramGet_p( param_Name ); |
if( p ) |
{ |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 1, 3, MNORMAL, PSTR("%c"), *p ); |
p++; |
lcd_printf_at_P( 2, 3, MNORMAL, PSTR("%c"), *p ); |
} |
*/ |
//---------------------------------- |
//---------------------------------- |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("Size Name:%3u"), paramSize(param_Name) ); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
paramSet_UByte( param_Name, 'x' ); |
lcd_printf_at_P( 3, 5, MNORMAL, PSTR("%c"), paramGet_UByte( param_Name ) ); |
*/ |
/* |
uint8_t v; |
v = MKVersion_Cmp( MKVersion.FCVer, 2,0,'f' ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("v:%3u"), v ); |
*/ |
/* |
unsigned char paramID = 1; |
unsigned char paramSubType = 1; |
unsigned char paramSubIndex =1; |
_paramset_getsubitemid( param_GlobalConfig_DrehratenBegrenzer, ¶mID, ¶mSubType, ¶mSubIndex ); |
//lcd_printf_at_P( 0, 4, MNORMAL, PSTR("ID:%u T:%u I:%u"), paramID, paramSubType, paramSubIndex ); |
*/ |
//#define param_ExtraConfig_NoRcOffBeeping 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
//#define param_ExtraConfig_GpsAid 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
//#define param_ExtraConfig_LearnableCarefree 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
//#define param_ExtraConfig_IgnoreMagErrAtStartup 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
//#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
//#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
//paramGet( unsigned char paramID ); |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
//#define param_Kanalbelegung_Gas 242 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
//#define param_Kanalbelegung_Gear 243 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
//#define param_Kanalbelegung_Nick 240 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
//#define param_Kanalbelegung_Roll 241 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
// lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), paramGet(param_Kanalbelegung_Gas), paramGet(param_Kanalbelegung_Gear), paramGet(param_Kanalbelegung_Nick), paramGet(param_Kanalbelegung_Roll) ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u"), paramGet(param_Kanalbelegung) ); |
/* |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
unsigned char v; |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
v = paramGet(param_GlobalConfig3_NoSdCardNoStart); |
v = (v ? false : true); |
paramSet(param_GlobalConfig3_NoSdCardNoStart, v); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("%u %u %u"), paramGet(param_ExtraConfig_NoRcOffBeeping), paramGet(param_GlobalConfig3_NoSdCardNoStart), paramGet(param_GlobalConfig3_NoGpsFixNoStart) ); |
*/ |
/* |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("%u %u"), |
paramGet( param_Kanalbelegung_Gas ), |
paramGet( param_Kanalbelegung_Gear ) |
); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti1 ), |
paramGet( param_Kanalbelegung_Poti2 ), |
paramGet( param_Kanalbelegung_Poti3 ), |
paramGet( param_Kanalbelegung_Poti4 ) |
); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("%u %u %u %u"), |
paramGet( param_Kanalbelegung_Poti5 ), |
paramGet( param_Kanalbelegung_Poti6 ), |
paramGet( param_Kanalbelegung_Poti7 ), |
paramGet( param_Kanalbelegung_Poti8 ) |
); |
*/ |
// lcd_printf_at_P( 0, 3, MNORMAL, PSTR("HR:%u H-Limit:%u"), |
// paramGet( param_GlobalConfig_HoehenRegelung ), |
// paramGet( param_ExtraConfig_HeightLimit ) |
// ); |
// lcd_printf_at_P( 0, 5, MNORMAL, PSTR("Auto-S-L:%u"), |
// paramGet( param_StartLandChannel ) |
// ); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("CH Orientation:%x"), paramGet( param_ServoCompInvert)); |
lcd_printf_at_P( 0, 5, MNORMAL, PSTR("ACC Land pulse:%x"), paramGet( param_LandingPulse)); |
; |
while( true ) |
{ |
PKT_CtrlHook(); |
if (get_key_press (1 << KEY_ESC)) |
{ |
break; |
} |
} |
} |
#endif // #ifdef DEBUG_PARAMSET |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings/paramset.h |
---|
0,0 → 1,357 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY paramset.h |
//# |
//# 16.07.2015 Cebra (PKT385a) |
//# - add: #define param_SingleWpControlChannel (FC2.11a) |
//# #define param_MenuKeyChannel (FC2.11a) |
//# |
//# 09.04.2015 Cebra |
//# - add: #define param_ServoNickFailsave, #define param_ServoRollFailsave, #define param_Servo3Failsave |
//# #define param_Servo4Failsave, #define param_Servo5Failsave |
//# |
//# 26.01.2015 Cebra |
//# - add: #define param_ComingHomeOrientation 241 ab FC 209a im Wert ServoCompInvert, Bit4 + Bit5 |
//# |
//# 26.09.2014 Cebra |
//# - add: paramID fuer Rev.103 (FC 2.07f) |
//# -> param_Hoehe_TiltCompensation |
//# |
//# 08.04.2014 OG |
//# - add: paramID's fuer Rev.102 (FC 2.05g) |
//# -> param_NaviMaxFlyingRange, param_NaviDescendRange |
//# |
//# 06.04.2014 OG |
//# - add: paramID's fuer Rev.101 |
//# -> param_Servo3OnValue, param_Servo3OffValue |
//# -> param_Servo4OnValue, param_Servo4OffValue |
//# |
//# 28.03.2014 OG |
//# - add: paramID's fuer Rev.100 |
//# -> param_AutoPhotoDistance, param_AutoPhotoAtitudes |
//# -> param_SingleWpSpeed |
//# |
//# 26.03.2014 OG |
//# - add: param_CompassOffset_DisableDeclCalc (Sub-Item) |
//# |
//# 27.02.2014 OG |
//# - del: paramID's fuer FC-Revisionen < 95 entfernt |
//# |
//# 27.02.2014 OG |
//# - chg: paramGet(), paramSet() - Unterstuetzen auch Bit- und Bytefelder |
//# |
//# 26.02.2014 OG |
//# - add: defines fuer die Unterstuetzung von paramSubID's ergaenzt |
//# paramSubItems bieten direkten Zugriff auf Bit- und Bytefelder |
//# von paramID's - die paramSubItems wurde in die defines der |
//# paramID's integriert indem sie am Ende gelistet werden (>=160) |
//# - chg: paramsetTest() umbenannt zu paramsetDEBUG() |
//# |
//# 25.02.2014 OG |
//# - add: defines zu param_Kanalbelegung_xyz (240 bis 251) |
//# |
//# 14.02.2014 OG |
//# - add: diverse Zugriffsfunktionen fuer paramID's und paramSet's |
//# |
//# 05.02.2014 OG - NEU |
//############################################################################ |
#ifndef _PARAMSET_H |
#define _PARAMSET_H |
//--------------------------- |
// Typ einer paramSubID |
//--------------------------- |
#define SUBTYPE_NO 0 // keine paramSubID |
#define SUBTYPE_BIT 1 // Bit-Feld (z.B. param_GlobalConfig) |
#define SUBTYPE_BITN 2 // Bit-Feld (z.B. param_GlobalConfig) negiert! |
#define SUBTYPE_BYTE 3 // Byte-Feld (z.B. Kanalbelegung) |
//######################################################################################################################################################## |
//---------------------------------------------------------------------------------------- |
// Die Reihenfolge ist egal - aber(!) darauf achten das keine ID doppelt vergeben wird! |
// Wenn neue Feleder hinzukommen an das Ende (vor EOF - keine ID 255!) hinzufuegen! |
// Bitte die Datentypen im Kommentar notieren! |
// |
// -> paramID |
//---------------------------------------------------------------------------------------- |
#define param_Revision 1 // unsigned char |
#define param_Kanalbelegung 2 // unsigned char [12] GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
#define param_GlobalConfig 3 // unsigned char 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
#define param_Hoehe_MinGas 4 // unsigned char Wert : 0-100 |
#define param_Luftdruck_D 5 // unsigned char Wert : 0-250 |
#define param_HoeheChannel 6 // unsigned char Wert : 0-32 |
#define param_Hoehe_P 7 // unsigned char Wert : 0-32 |
#define param_Hoehe_Verstaerkung 8 // unsigned char Wert : 0-50 |
#define param_Hoehe_ACC_Wirkung 9 // unsigned char Wert : 0-250 |
#define param_Hoehe_HoverBand 10 // unsigned char Wert : 0-250 |
#define param_Hoehe_GPS_Z 11 // unsigned char Wert : 0-250 |
#define param_Hoehe_StickNeutralPoint 12 // unsigned char Wert : 0-250 |
#define param_Stick_P 13 // unsigned char WERT : 0..20 |
#define param_Stick_D 14 // unsigned char WERT : 0..20 |
#define param_StickGier_P 15 // unsigned char Wert : |
#define param_Gas_Min 16 // unsigned char Wert : 0-32 |
#define param_Gas_Max 17 // unsigned char Wert : 33-250 |
#define param_GyroAccFaktor 18 // unsigned char Wert : 1-64 |
#define param_KompassWirkung 19 // unsigned char Wert : 0-32 |
#define param_Gyro_P 20 // unsigned char Wert : 10-250 |
#define param_Gyro_I 21 // unsigned char Wert : 0-250 |
#define param_Gyro_D 22 // unsigned char Wert : 0-250 |
#define param_Gyro_Gier_P 23 // unsigned char Wert : 10-250 |
#define param_Gyro_Gier_I 24 // unsigned char Wert : 0-250 |
#define param_Gyro_Stability 25 // unsigned char Wert : 0-16 |
#define param_UnterspannungsWarnung 26 // unsigned char Wert : 0-250 |
#define param_NotGas 27 // unsigned char Wert : 0-250 //Gaswert bei Empängsverlust |
#define param_NotGasZeit 28 // unsigned char Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
#define param_Receiver 29 // unsigned char 0= Summensignal, 1= Spektrum, 2 =Jeti, 3=ACT DSL, 4=ACT S3D |
#define param_I_Faktor 30 // unsigned char Wert : 0-250 |
#define param_UserParam1 31 // unsigned char Wert : 0-250 |
#define param_UserParam2 32 // unsigned char Wert : 0-250 |
#define param_UserParam3 33 // unsigned char Wert : 0-250 |
#define param_UserParam4 34 // unsigned char Wert : 0-250 |
#define param_ServoNickControl 35 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoNickComp 36 // unsigned char Wert : 0-250 // Einfluss Gyro/Servo |
#define param_ServoNickMin 37 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoNickMax 38 // unsigned char Wert : 0-250 // Anschlag |
#define param_ServoRollControl 39 // unsigned char Wert : 0-250 // Stellung des Servos |
#define param_ServoRollComp 40 // unsigned char Wert : 0-250 |
#define param_ServoRollMin 41 // unsigned char Wert : 0-250 |
#define param_ServoRollMax 42 // unsigned char Wert : 0-250 |
#define param_ServoNickRefresh 43 // unsigned char Speed of the Servo |
#define param_ServoManualControlSpeed 44 // unsigned char |
#define param_CamOrientation 45 // unsigned char |
#define param_Servo3 46 // unsigned char Value or mapping of the Servo Output |
#define param_Servo4 47 // unsigned char Value or mapping of the Servo Output |
#define param_Servo5 48 // unsigned char Value or mapping of the Servo Output |
#define param_LoopGasLimit 49 // unsigned char Wert: 0-250 max. Gas während Looping |
#define param_LoopThreshold 50 // unsigned char Wert: 0-250 Schwelle für Stickausschlag |
#define param_LoopHysterese 51 // unsigned char Wert: 0-250 Hysterese für Stickausschlag |
#define param_AchsKopplung1 52 // unsigned char Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
#define param_AchsKopplung2 53 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_CouplingYawCorrection 54 // unsigned char Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
#define param_WinkelUmschlagNick 55 // unsigned char Wert: 0-250 180°-Punkt |
#define param_WinkelUmschlagRoll 56 // unsigned char Wert: 0-250 180°-Punkt |
#define param_GyroAccAbgleich 57 // unsigned char 1/k (Koppel_ACC_Wirkung) |
#define param_Driftkomp 58 // unsigned char |
#define param_DynamicStability 59 // unsigned char |
#define param_UserParam5 60 // unsigned char Wert : 0-250 |
#define param_UserParam6 61 // unsigned char Wert : 0-250 |
#define param_UserParam7 62 // unsigned char Wert : 0-250 |
#define param_UserParam8 63 // unsigned char Wert : 0-250 |
#define param_J16Bitmask 64 // unsigned char for the J16 Output |
#define param_J16Timing 65 // unsigned char for the J16 Output |
#define param_J17Bitmask 66 // unsigned char for the J17 Output |
#define param_J17Timing 67 // unsigned char for the J17 Output |
#define param_WARN_J16_Bitmask 68 // unsigned char for the J16 Output |
#define param_WARN_J17_Bitmask 69 // unsigned char for the J17 Output |
#define param_NaviOut1Parameter 70 // unsigned char for the J16 Output |
#define param_NaviGpsModeChannel 71 // unsigned char Parameters for the Naviboard |
#define param_NaviGpsGain 72 // unsigned char |
#define param_NaviGpsP 73 // unsigned char |
#define param_NaviGpsI 74 // unsigned char |
#define param_NaviGpsD 75 // unsigned char |
#define param_NaviGpsPLimit 76 // unsigned char |
#define param_NaviGpsILimit 77 // unsigned char |
#define param_NaviGpsDLimit 78 // unsigned char |
#define param_NaviGpsA 79 // unsigned char |
#define param_NaviGpsMinSat 80 // unsigned char |
#define param_NaviStickThreshold 81 // unsigned char |
#define param_NaviWindCorrection 82 // unsigned char |
#define param_NaviAccCompensation 83 // unsigned char New since 0.86 -> was: SpeedCompensation |
#define param_NaviOperatingRadius 84 // unsigned char bis Rev. 101 |
#define param_NaviAngleLimitation 85 // unsigned char |
#define param_NaviPH_LoginTime 86 // unsigned char |
#define param_ExternalControl 87 // unsigned char for serial Control |
#define param_OrientationAngle 88 // unsigned char Where is the front-direction? |
#define param_CareFreeChannel 89 // unsigned char switch for CareFree |
#define param_MotorSafetySwitch 90 // unsigned char |
#define param_MotorSmooth 91 // unsigned char |
#define param_ComingHomeAltitude 92 // unsigned char |
#define param_FailSafeTime 93 // unsigned char |
#define param_MaxAltitude 94 // unsigned char |
#define param_FailsafeChannel 95 // unsigned char if the value of this channel is > 100, the MK reports "RC-Lost" |
#define param_ServoFilterNick 96 // unsigned char |
#define param_ServoFilterRoll 97 // unsigned char |
#define param_StartLandChannel 98 // unsigned char |
#define param_LandingSpeed 99 // unsigned char |
#define param_CompassOffset 100 // unsigned char |
#define param_AutoLandingVoltage 101 // unsigned char in 0,1V 0 -> disabled |
#define param_ComingHomeVoltage 102 // unsigned char in 0,1V 0 -> disabled |
#define param_BitConfig 103 // unsigned char (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
#define param_ServoCompInvert 104 // unsigned char 0x01 = Nick, 0x02 = Roll, 0x04 = relative moving // WICHTIG!!! am Ende lassen |
#define param_ExtraConfig 105 // unsigned char bitcodiert |
#define param_GlobalConfig3 106 // unsigned char bitcodiert |
#define param_Name 107 // char [12] Name vom Parameterset |
#define param_crc 108 // unsigned char |
#define param_AutoPhotoDistance 109 // unsigned char ab Rev. 100 (ersetzt NaviOut1Parameter) // Auto Photo |
#define param_AutoPhotoAtitudes 110 // unsigned char ab Rev. 100 |
#define param_SingleWpSpeed 111 // unsigned char ab Rev. 100 |
#define param_Servo3OnValue 112 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo3OffValue 113 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OnValue 114 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_Servo4OffValue 115 // unsigned char ab Rev. 101 (FC 2.05f) |
#define param_NaviMaxFlyingRange 116 // unsigned char ab Rev. 102 (FC 2.05g) (ersetzt NaviOperatingRadius) // in 10m |
#define param_NaviDescendRange 117 // unsigned char ab Rev. 102 (FC 2.05g) |
#define param_Hoehe_TiltCompensation 118 // unsigned char ab Rev. 103 (FC 2.07f) |
#define param_LandingPulse 119 // unsigned char ab Rev. 104 (FC 2.09d) |
#define param_ServoFS_Pos 120 // unsigned char [5] ServoNickFailsave[0],ServoRollFailsave[1],Servo3Failsave[2],Servo4Failsave[3],Servo5Failsave[4] ab Rev. 105 (FC 2.09i) |
#define param_SingleWpControlChannel 121 // unsigned char ab Rev. 106 (FC 2.11a) |
#define param_MenuKeyChannel 122 // unsigned char ab Rev. 106 (FC 2.11a) |
//------- ^^^ HIER NEUE paramID's einfuegen! ^^^ |
// ab hier sind Sub-Items zu einer paramID definiert |
// - einige paramID's werden nochmals untergliedert in Bit- oder Bytefelder |
// - die Zuordnung der Sub-Items zu paramID's erfolgt in paramset.c |
#define param_SUBITEM 160 // <- ab diesem Wert handelt es sich um ein paramSubID und nicht um eine paramID |
#define param_Kanalbelegung_Nick 170 // SUBTYPE_BYTE - Kanalbelegung [0] (unsigned char) |
#define param_Kanalbelegung_Roll 171 // SUBTYPE_BYTE - Kanalbelegung [1] (unsigned char) |
#define param_Kanalbelegung_Gas 172 // SUBTYPE_BYTE - Kanalbelegung [2] (unsigned char) |
#define param_Kanalbelegung_Gear 173 // SUBTYPE_BYTE - Kanalbelegung [3] (unsigned char) |
#define param_Kanalbelegung_Poti1 174 // SUBTYPE_BYTE - Kanalbelegung [4] (unsigned char) |
#define param_Kanalbelegung_Poti2 175 // SUBTYPE_BYTE - Kanalbelegung [5] (unsigned char) |
#define param_Kanalbelegung_Poti3 176 // SUBTYPE_BYTE - Kanalbelegung [6] (unsigned char) |
#define param_Kanalbelegung_Poti4 177 // SUBTYPE_BYTE - Kanalbelegung [7] (unsigned char) |
#define param_Kanalbelegung_Poti5 178 // SUBTYPE_BYTE - Kanalbelegung [8] (unsigned char) |
#define param_Kanalbelegung_Poti6 179 // SUBTYPE_BYTE - Kanalbelegung [9] (unsigned char) |
#define param_Kanalbelegung_Poti7 180 // SUBTYPE_BYTE - Kanalbelegung [10] (unsigned char) |
#define param_Kanalbelegung_Poti8 181 // SUBTYPE_BYTE - Kanalbelegung [11] (unsigned char) |
#define param_ServoCompInvert_SERVO_NICK_INV 190 // SUBTYPE_BIT - mk-data-structs.h: SVNick 0x01 |
#define param_ServoCompInvert_SERVO_ROLL_INV 191 // SUBTYPE_BIT - mk-data-structs.h: SVRoll 0x02 |
#define param_ServoCompInvert_SERVO_RELATIVE 192 // SUBTYPE_BIT - mk-data-structs.h: SVRelMov 0x04 |
#define param_ExtraConfig_HeightLimit 200 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 |
#define param_ExtraConfig_HeightVario 201 // SUBTYPE_BIT - mk-data-structs.h: CFG2_HEIGHT_LIMIT 0x01 ist negiert zu param_ExtraConfig_HeightLimit |
#define param_ExtraConfig_VarioBeep 202 // SUBTYPE_BIT - mk-data-structs.h: CFG2_VARIO_BEEP 0x02 |
#define param_ExtraConfig_SensitiveRc 203 // SUBTYPE_BIT - mk-data-structs.h: CFG_SENSITIVE_RC 0x04 |
#define param_ExtraConfig_33vReference 204 // SUBTYPE_BIT - mk-data-structs.h: CFG_3_3V_REFERENCE 0x08 |
#define param_ExtraConfig_NoRcOffBeeping 205 // SUBTYPE_BIT - mk-data-structs.h: CFG_NO_RCOFF_BEEPING 0x10 |
#define param_ExtraConfig_GpsAid 206 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AID 0x20 |
#define param_ExtraConfig_LearnableCarefree 207 // SUBTYPE_BIT - mk-data-structs.h: CFG_LEARNABLE_CAREFREE 0x40 |
#define param_ExtraConfig_IgnoreMagErrAtStartup 208 // SUBTYPE_BIT - mk-data-structs.h: CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
#define param_BitConfig_LoopOben 210 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_OBEN 0x01 |
#define param_BitConfig_LoopUnten 211 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_UNTEN 0x02 |
#define param_BitConfig_LoopLinks 212 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_LINKS 0x04 |
#define param_BitConfig_LoopRechts 213 // SUBTYPE_BIT - mk-data-structs.h: CFG_LOOP_RECHTS 0x08 |
#define param_BitConfig_MotorBlink1 214 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK1 0x10 |
#define param_BitConfig_MotorOffLed1 215 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED1 0x20 |
#define param_BitConfig_MotorOffLed2 216 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_OFF_LED2 0x40 |
#define param_BitConfig_MotorBlink2 217 // SUBTYPE_BIT - mk-data-structs.h: CFG_MOTOR_BLINK2 0x80 |
#define param_GlobalConfig3_NoSdCardNoStart 220 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_SDCARD_NO_START 0x01 |
#define param_GlobalConfig3_DphMaxRadius 221 // SUBTYPE_BIT - mk-data-structs.h: CFG3_DPH_MAX_RADIUS 0x02 nur bis Rev 101 |
#define param_GlobalConfig3_VarioFailsafe 222 // SUBTYPE_BIT - mk-data-structs.h: CFG3_VARIO_FAILSAFE 0x04 |
#define param_GlobalConfig3_MotorSwitchMode 223 // SUBTYPE_BIT - mk-data-structs.h: CFG3_MOTOR_SWITCH_MODE 0x08 |
#define param_GlobalConfig3_NoGpsFixNoStart 224 // SUBTYPE_BIT - mk-data-structs.h: CFG3_NO_GPSFIX_NO_START 0x10 |
#define param_GlobalConfig3_UseNcForOut1 225 // SUBTYPE_BIT - mk-data-structs.h: CFG3_USE_NC_FOR_OUT1 0x20 |
#define param_GlobalConfig3_SpeakAll 226 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SPEAK_ALL 0x40 |
#define param_GlobalConfig3_ServoNickCompOff 227 // SUBTYPE_BIT - mk-data-structs.h: CFG3_SERVO_NICK_COMP_OFF 0x80 |
#define param_GlobalConfig_HoehenRegelung 230 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHENREGELUNG 0x01 |
#define param_GlobalConfig_HoehenSchalter 231 // SUBTYPE_BIT - mk-data-structs.h: CFG_HOEHEN_SCHALTER 0x02 |
#define param_GlobalConfig_HeadingHold 232 // SUBTYPE_BIT - mk-data-structs.h: CFG_HEADING_HOLD 0x04 |
#define param_GlobalConfig_KompassAktiv 233 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_AKTIV 0x08 |
#define param_GlobalConfig_KompassFix 234 // SUBTYPE_BIT - mk-data-structs.h: CFG_KOMPASS_FIX 0x10 |
#define param_GlobalConfig_GpsAktiv 235 // SUBTYPE_BIT - mk-data-structs.h: CFG_GPS_AKTIV 0x20 |
#define param_GlobalConfig_AchsenkopplungAktiv 236 // SUBTYPE_BIT - mk-data-structs.h: CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define param_GlobalConfig_DrehratenBegrenzer 237 // SUBTYPE_BIT - mk-data-structs.h: CFG_DREHRATEN_BEGRENZER 0x80 |
#define param_ServoNickFailsave 238 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[0] ab FC 2.09j |
#define param_ServoRollFailsave 239 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[1] ab FC 2.09j |
#define param_Servo3Failsave 240 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[2] ab FC 2.09j |
#define param_Servo4Failsave 241 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[3] ab FC 2.09j |
#define param_Servo5Failsave 242 // SUBTYPE_BYTE- mk-data-structs.h: ServoFS_Pos[4] ab FC 2.09j |
// hier: spezielle Subitems |
#define param_CompassOffset_DisableDeclCalc 243 // wird gemappt auf: param_CompassOffset - der Wert ist dort in Bit 8 und 7 kodiert |
#define param_ComingHomeOrientation 244 // unsigned char 0x08, 0x10, Bits im Feld ServoCompInvert |
//--------------------------------------------------------------------------------------- |
// IMMER am Ende!! |
#define param_DUMMY 254 // DO NOT CHANGE! DUMMY => spacer werte |
#define param_EOF 255 // DO NOT CHANGE! EOF => markiert das Ende von paramID-Listen |
#define param_NOTFOUND 255 // DO NOT CHANGE! NOTFOUND => kein paramID gefunden |
//######################################################################################################################################################## |
//--------------------------- |
// struct: einzelnes Parameter-Item |
//--------------------------- |
typedef struct |
{ |
unsigned char paramID; |
unsigned char size; |
} paramRevItem_t; |
//--------------------------- |
// exportierte Funktionen |
//--------------------------- |
unsigned char paramsetInit( unsigned char *pData ); |
unsigned char paramsetSize( void ); |
unsigned char paramExist( unsigned char paramID ); |
unsigned char paramSize( unsigned char paramID ); |
unsigned char paramGet( unsigned char paramID ); |
unsigned char paramSet( unsigned char paramID, unsigned char newvalue ); |
unsigned char *paramGet_p( unsigned char paramID ); |
// TEST / DEBUG |
void paramsetDEBUG( void ); |
#endif // _PARAMSET_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/mksettings |
---|
Property changes: |
Added: svn:ignore |
+_FC_eeprom_Versions_Archiv |
+ |
+_old_source |
+ |
+mkparameters.c_sic |
/Transportables_Koptertool/PKT/trunk/motortest/motortest.c |
---|
0,0 → 1,396 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY motortest.c |
//# |
//# 13.05.2014 OG |
//# - chg: motor_test() - #ifdef USE_I2CMOTORTEST erweitert um unsigned int SerLoop; |
//# wegen Warning: variable set but not used |
//# |
//# 12.02.2014 OG |
//# - chg: motor_test() Verschiebung von var 'buffer' wegen "unused variable 'buffer'" |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "motortest.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../motortest/twimaster.h" |
//#include "menu.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../messages.h" |
uint8_t m; |
uint8_t mmode; // 0=Value 1=Motor |
uint8_t v; |
volatile uint8_t i2c_state; |
volatile uint8_t motor_addr = 0; |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// Senden der Motorwerte per I2C-Bus |
// |
void SendMotorData(uint8_t m,uint8_t v) |
{ |
if (m==0) |
for(m=0;m<MAX_MOTORS;m++) // alle Motoren |
{ |
// Motor[m].SetPoint = MotorTest[m]; |
Motor[m].SetPoint = v; |
Motor[m].SetPointLowerBits = 0; |
// Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution |
// Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
} |
else |
{ |
Motor[m-1].SetPoint = v; |
Motor[m-1].SetPointLowerBits = 0; |
} |
if(I2C_TransferActive) |
I2C_TransferActive = 0; // enable for the next time |
else |
{ |
motor_write = 0; |
I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
} |
} |
//-------------------------------------------------------------- |
// |
void Search_BL (void) |
{ |
uint8_t i = 0; |
unsigned int timer; |
lcd_cls (); |
MotorenEin =0; |
MotorTest[i] = 0; |
lcd_printp (PSTR("Suche BL-Ctrl"), 0); |
// Check connected BL-Ctrls |
BLFlags |= BLFLAG_READ_VERSION; |
motor_read = 0; // read the first I2C-Data |
SendMotorData(0,0); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
timer = SetDelay(1); |
for(i=0; i < MAX_MOTORS; i++) |
{ |
SendMotorData(i,0); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
{ |
lcd_printp_at (0, 1, PSTR("Found BL-Ctrl:"), 0); |
lcd_print_hex_at (18,1,i,0); |
lcd_printp_at (0, 2, PSTR("Version:"), 0); |
lcd_print_hex_at (8,2,Motor[i].Version,0); |
lcd_printp_at (11, 2, PSTR("SetPoi:"), 0); |
lcd_print_hex_at (18,2,Motor[i].SetPoint,0); |
lcd_printp_at (0, 3, PSTR("SetPoiL:"), 0); |
lcd_print_hex_at (8,3,Motor[i].SetPointLowerBits,0); |
lcd_printp_at (11, 3, PSTR("State :"), 0); |
lcd_print_hex_at (18,3,Motor[i].State,0); |
lcd_printp_at (0, 4, PSTR("ReadMod:"), 0); |
lcd_print_hex_at (8,4,Motor[i].ReadMode,0); |
lcd_printp_at (11, 4, PSTR("Currnt:"), 0); |
lcd_print_hex_at (18,4,Motor[i].Current,0); |
lcd_printp_at (0, 5, PSTR("MaxPWM :"), 0); |
lcd_print_hex_at (8,5,Motor[i].MaxPWM,0); |
lcd_printp_at (11, 5, PSTR("Temp :"), 0); |
write_ndigit_number_u (18,5,Motor[i].Temperature,3,1,0); |
} |
} //End For I |
} |
#endif |
//-------------------------------------------------------------- |
// |
void motor (uint8_t m,uint8_t v) |
{ |
memset (buffer, 0, 16); |
if(m == 0) |
{ |
memset (buffer, v, 16); |
} |
else |
{ |
buffer[m-1] = v; |
} |
SendOutData('t', ADDRESS_FC, 1, buffer, 16); |
} |
//-------------------------------------------------------------- |
// |
void motor_test (uint8_t MotorMode) |
{ |
lcd_cls (); |
mmode = 1; // 1=Motor |
m = 1; |
v = 0; |
#ifdef USE_I2CMOTORTEST |
unsigned int SerLoop; |
SerLoop = 10; |
char buffer[7]; |
if (MotorMode == I2C_Mode) |
{ |
Search_BL(); |
do |
{ |
lcd_printp_at (11, 7, PSTR("Ende Check"), 0); |
if (get_key_press (1 << KEY_ESC)) |
{ |
get_key_press(KEY_ALL); |
return; |
} |
} |
while (!get_key_press (1 << KEY_ENTER)); |
} |
#endif |
lcd_cls(); |
lcd_printp (PSTR(" BL-Ctrl Test "), 2); |
lcd_printp_at (2, 2, PSTR("Motor: 1"), 0); |
lcd_printp_at (2, 3, PSTR("Value: 0"), 0); |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x1a \x1b"), 0); |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
uart1_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r"); |
#endif |
if (MotorMode == FC_Mode) |
{ |
if (hardware == NC && current_hardware == NC) |
{ |
SwitchToFC(); |
} |
} |
do |
{ |
// mmode 0=Value 1=Motor |
if ((mmode == 0) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (v < 254)) |
{ |
v++; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 0) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (v > 0)) |
{ |
if (MotorMode == FC_Mode) |
lcd_frect (((v * (14*8) / 255) + 8), (8*5), ((14*8) / 255), 6, 0); |
v--; |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
if (MotorMode == FC_Mode) |
lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (m < 16)) |
{ |
m++; |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
} |
if ((mmode == 1) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (m > 0)) |
{ |
m--; |
if(m > 0) |
write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) |
lcd_printp_at (9, 2, PSTR("All"), 0); |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
if (v > 0) |
{ |
m = 0; |
v=0; |
lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1); |
lcd_cls_line (0, 5, 21); |
if(m > 0) write_ndigit_number_u (9, 2, m, 3, 0,0); |
if(m == 0) lcd_printp_at (9, 2, PSTR("All"), 0); |
write_ndigit_number_u (9, 3, v, 3, 0,0); |
SendMotorData(m,v); |
timer = SetDelay(1); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
} |
#endif |
if(mmode == 0) // 0=Value |
{ |
lcd_printp_at (0, 2, PSTR("\x1d"), 0); |
lcd_printp_at (0, 3, PSTR(" "), 0); |
mmode = 1; // 1=Motor |
} |
else |
{ |
lcd_printp_at (0, 2, PSTR(" "), 0); |
lcd_printp_at (0, 3, PSTR("\x1d"), 0); |
mmode = 0; // 0=Value |
} |
} |
//if (get_key_press (1 << KEY_ENTER))// |
#ifdef USE_I2CMOTORTEST |
if (MotorMode == I2C_Mode) |
{ |
SendMotorData(m,v); |
timer = SetDelay(1); |
lcd_printp_at (0, 3, PSTR("SetPoint :"), 0); |
write_ndigit_number_u (13,3,Motor[m-1].SetPoint,3,0,0); |
lcd_printp_at (0, 4, PSTR("Current :"), 0); |
lcd_print_hex_at (13,4,Motor[m-1].Current,0); |
write_ndigit_number_u (13,4,Motor[m-1].Current,3,0,0); |
lcd_printp_at (0, 5, PSTR("Temperature:"), 0); |
write_ndigit_number_u (13,5,Motor[m-1].Temperature,3,0,0); |
lcd_printp_at (0, 6, PSTR("Version:"), 0); |
lcd_print_hex_at (8,6,Motor[m-1].Version,0); |
lcd_printp_at (11, 6, PSTR("State :"), 0); |
lcd_print_hex_at (18,6,Motor[m-1].State,0); |
if (Motor[m-1].SetPoint > 0) |
{ |
if (SerLoop == 0) |
{ |
itoa( m-1, buffer, 10); // convert interger into string (decimal format) |
uart1_puts(buffer); // and transmit string to UART |
uart1_puts(";"); |
itoa( Motor[m-1].Version, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPoint, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].SetPointLowerBits, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].State, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].ReadMode, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Current, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].MaxPWM, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts(";"); |
itoa( Motor[m-1].Temperature, buffer, 10); // |
uart1_puts(buffer); |
uart1_puts("\r"); |
uart1_puts("\n"); |
SerLoop =200; |
} |
else |
SerLoop--; |
} |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
else |
#endif |
motor (m,v); //if (MotorMode == I2C_Mode)// |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
if (MotorMode == FC_Mode) |
{ |
motor(0,0); // switch all engines off at exit |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/motortest/motortest.h |
---|
0,0 → 1,46 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _MOTORTEST_H |
#define _MOTORTEST_H |
#define I2C_Mode 1 // Motortest Lokal |
#define FC_Mode 2 // Motortest ueber FC |
void motor_test (uint8_t MotorMode); |
void SendMotorData(uint8_t m,uint8_t v); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/motortest/twimaster.c |
---|
0,0 → 1,523 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) Holger Buss, Ingo Busker |
// + Nur f?r den privaten Gebrauch |
// + www.MikroKopter.com |
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt f?r das gesamte Projekt (Hardware, Software, Bin?rfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur f?r den privaten (nicht-kommerziellen) Gebrauch zul?ssig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best?ckung und Verkauf von Platinen oder Baus?tzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver?ffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m?ssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien ver?ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gew?hr auf Fehlerfreiheit, Vollst?ndigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir ?bernehmen keinerlei Haftung f?r direkte oder indirekte Personen- oder Sachsch?den |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zul?ssig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY twimaster.c |
//# |
//# 13.05.2013 Cebra |
//# - chg: #define USE_I2CMOTORTEST, I2C Funktionen schaltbar |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/twi.h> |
#include <util/delay.h> |
#include "../eeprom/eeprom.h" |
#include "../motortest/twimaster.h" |
#include "../timer/timer.h" |
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint8_t I2C_TransferActive = 0; |
volatile uint16_t I2CTimeout = 100; |
uint8_t MissingMotor = 0; |
uint8_t RequiredMotors = 1; |
char MotorenEin = 0; |
volatile uint8_t BLFlags = 0; |
MotorData_t Motor[MAX_MOTORS]; |
// bit mask for witch BL the configuration should be sent |
volatile uint16_t BLConfig_WriteMask = 0; |
// bit mask for witch BL the configuration should be read |
volatile uint16_t BLConfig_ReadMask = 0; |
// buffer for BL Configuration |
BLConfig_t BLConfig; |
#define I2C_WriteByte(byte) {TWDR = byte; TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define I2C_ReceiveByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);} |
#define I2C_ReceiveLastByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);} |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define TWI_BASE_ADDRESS 0x52 |
uint8_t RAM_Checksum(uint8_t* pBuffer, uint16_t len) |
{ |
uint8_t crc = 0xAA; |
uint16_t i; |
for(i=0; i<len; i++) |
{ |
crc += pBuffer[i]; |
} |
return crc; |
} |
//-------------------------------------------------------------- |
// Initialize I2C (TWI) |
// |
void I2C_Init(char clear) |
{ |
#ifdef USE_I2CMOTORTEST |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
// SDA is INPUT |
DDRC &= ~(1<<DDC1); |
// SCL is output |
DDRC |= (1<<DDC0); |
// pull up SDA |
//PORTC |= (1<<PORTC0)|(1<<PORTC1); |
// TWI Status Register |
// prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
// set TWI Bit Rate Register |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; |
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
if(clear) for(i=0; i < MAX_MOTORS; i++) |
{ |
Motor[i].Version = 0; |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
Motor[i].State = 0; |
Motor[i].ReadMode = BL_READMODE_STATUS; |
Motor[i].Current = 0; |
Motor[i].MaxPWM = 0; |
Motor[i].Temperature = 0; |
} |
sei(); |
SREG = sreg; |
#endif |
} |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
void I2C_Reset(void) |
{ |
// stop i2c bus |
I2C_Stop(TWI_STATE_MOTOR_TX); |
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
I2C_TransferActive = 0; |
I2C_Init(0); |
I2C_WriteByte(0); |
BLFlags |= BLFLAG_READ_VERSION; |
} |
#endif |
#ifdef USE_I2CMOTORTEST |
//-------------------------------------------------------------- |
// I2C ISR |
// |
ISR (TWI_vect) |
{ |
static uint8_t missing_motor = 0, motor_read_temperature = 0; |
static uint8_t *pBuff = 0; |
static uint8_t BuffLen = 0; |
switch (twi_state++) |
{ |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
I2C_TransferActive = 1; |
// skip motor if not used in mixer |
// while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
BLConfig_WriteMask = 0; // reset configuration bitmask |
motor_write = 0; // reset motor write counter for next cycle |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(TWI_BASE_ADDRESS + TW_READ + (motor_read<<1) ); // select slave address in rx mode |
} |
else I2C_WriteByte(TWI_BASE_ADDRESS + TW_WRITE + (motor_write<<1) ); // select slave address in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint |
// if old version has been detected |
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
{ |
twi_state = 4; //jump over sending more data |
} |
// the new version has been detected |
else if(!( (Motor[motor_write].SetPointLowerBits && (RequiredMotors < 7)) || BLConfig_WriteMask || BLConfig_ReadMask ) ) |
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
twi_state = 4; //jump over sending more data |
} |
break; |
case 2: // lower bits of setpoint (higher resolution) |
if ((0x0001<<motor_write) & BLConfig_ReadMask) |
{ |
Motor[motor_write].ReadMode = BL_READMODE_CONFIG; // configuration request |
} |
else |
{ |
Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request |
} |
// send read mode and the lower bits of setpoint |
I2C_WriteByte((Motor[motor_write].ReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
// configuration tranmission request? |
if((0x0001<<motor_write) & BLConfig_WriteMask) |
{ // redirect tx pointer to configuration data |
pBuff = (uint8_t*)&BLConfig; // select config for motor |
BuffLen = sizeof(BLConfig_t); |
} |
else |
{ // jump to end of transmission for that motor |
twi_state = 4; |
} |
break; |
case 3: // send configuration |
I2C_WriteByte(*pBuff); |
pBuff++; |
if(--BuffLen > 0) |
twi_state = 3; // if there are some bytes left |
break; |
case 4: // repeat case 0-4 for all motors |
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received |
{ |
if(!missing_motor) |
missing_motor = motor_write + 1; |
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) |
Motor[motor_write].State++; // increment error counter and handle overflow |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive Data |
case 5: // TWI_STATE_MOTOR_RX |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted but no ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
if(++motor_read >= MAX_MOTORS) |
{ // all motors read |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
} |
else |
{ // motor successfully addressed |
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
{ |
// new BL found |
switch(Motor[motor_read].ReadMode) |
{ |
case BL_READMODE_CONFIG: |
pBuff = (uint8_t*)&BLConfig; |
BuffLen = sizeof(BLConfig_t); |
break; |
case BL_READMODE_STATUS: |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
else BuffLen = 1;// read Current only |
break; |
} |
} |
else // old BL version |
{ |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if((BLFlags & BLFLAG_READ_VERSION) || (motor_read == motor_read_temperature)) BuffLen = 2; // Current & MaxPwm |
else BuffLen = 1; // read Current only |
} |
if(BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else |
{ |
I2C_ReceiveByte(); // read next byte |
} |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 6: // receive bytes |
*pBuff = TWDR; |
pBuff++; |
BuffLen--; |
if(BuffLen>1) |
{ |
I2C_ReceiveByte(); // read next byte |
} |
else if (BuffLen == 1) |
{ |
I2C_ReceiveLastByte(); // read last byte |
} |
else // nothing left |
{ |
if(BLFlags & BLFLAG_READ_VERSION) |
{ |
// if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN) && (Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
if((Motor[motor_read].MaxPWM == 250) ) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
else Motor[motor_read].Version = 0; |
} |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart from beginning |
BLConfig_ReadMask = 0; // reset read configuration bitmask |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2C_TransferActive = 0; |
return; |
} |
twi_state = 6; // if there are some bytes left |
break; |
case 21: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
case 22: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2C_TransferActive = 0; |
I2CTimeout = 10; |
// repeat case 18...22 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ |
dac_channel = 0; // reset dac channel counter |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
break; |
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
I2C_TransferActive = 0; |
break; |
} |
} |
//-------------------------------------------------------------- |
uint8_t I2C_WriteBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
// return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor) |
{ |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
} |
// check BL configuration to send |
if(BLConfig.Revision != BLCONFIG_REVISION) |
return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) |
return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
if(!motor) // 0 means all |
{ |
BLConfig_WriteMask = 0xFF; // all motors at once with the same configuration |
} |
else //only one specific motor |
{ |
BLConfig_WriteMask = 0x0001<<(motor-1); |
} |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_WriteMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_write = 0; |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
} |
while(BLConfig_WriteMask && !CheckDelay(timer)); // repeat until the BL config has been sent |
if(BLConfig_WriteMask) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); |
return(BLCONFIG_SUCCESS); |
} |
//-------------------------------------------------------------- |
uint8_t I2C_ReadBLConfig(uint8_t motor) |
{ |
uint8_t i; |
uint16_t timer; |
// if(MotorenEin || PC_MotortestActive) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(MotorenEin) |
return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) |
return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor == 0) |
return (BLCONFIG_ERR_READ_NOT_POSSIBLE); |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) |
return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
timer = SetDelay(2000); |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
// prepare the bitmask |
BLConfig_ReadMask = 0x0001<<(motor-1); |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_ReadMask) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
} |
} |
motor_read = 0; |
BLConfig.Revision = 0; // bad revision |
BLConfig.crc = 0; // bad checksum |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
{ |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer |
}while(BLConfig_ReadMask && !CheckDelay(timer)); // repeat until the BL config has been received from all motors |
// validate result |
if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
return(BLCONFIG_SUCCESS); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/motortest/twimaster.h |
---|
0,0 → 1,80 |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
+ |
+#include <inttypes.h> |
+#include "../mk-data-structs.h" |
+ |
+#define TWI_STATE_MOTOR_TX 0 |
+#define TWI_STATE_MOTOR_RX 5 |
+#define TWI_STATE_GYRO_OFFSET_TX 18 |
+ |
+extern volatile uint8_t twi_state; |
+extern volatile uint8_t motor_write; |
+extern volatile uint8_t motor_read; |
+extern volatile uint8_t I2C_TransferActive; |
+ |
+extern uint8_t MissingMotor; |
+ |
+#define MAX_MOTORS 12 |
+#define MOTOR_STATE_PRESENT_MASK 0x80 |
+#define MOTOR_STATE_ERROR_MASK 0x7F |
+#define MOTOR_STATE_NEW_PROTOCOL_MASK 0x01 |
+#define BLFLAG_TX_COMPLETE 0x01 |
+#define BLFLAG_READ_VERSION 0x02 |
+ |
+extern volatile uint8_t BLFlags; |
+extern char MotorenEin; |
+unsigned char MotorTest[16]; |
+#define BL_READMODE_STATUS 0 |
+#define BL_READMODE_CONFIG 16 |
+ |
+ |
+ |
+extern MotorData_t Motor[MAX_MOTORS]; |
+ |
+#define BLCONFIG_REVISION 2 |
+ |
+#define MASK_SET_PWM_SCALING 0x01 |
+#define MASK_SET_CURRENT_LIMIT 0x02 |
+#define MASK_SET_TEMP_LIMIT 0x04 |
+#define MASK_SET_CURRENT_SCALING 0x08 |
+#define MASK_SET_BITCONFIG 0x10 |
+#define MASK_RESET_CAPCOUNTER 0x20 |
+#define MASK_SET_DEFAULT_PARAMS 0x40 |
+#define MASK_SET_SAVE_EEPROM 0x80 |
+ |
+#define BITCONF_REVERSE_ROTATION 0x01 |
+#define BITCONF_RES1 0x02 |
+#define BITCONF_RES2 0x04 |
+#define BITCONF_RES3 0x08 |
+#define BITCONF_RES4 0x10 |
+#define BITCONF_RES5 0x20 |
+#define BITCONF_RES6 0x40 |
+#define BITCONF_RES7 0x80 |
+ |
+ |
+ |
+extern BLConfig_t BLConfig; |
+ |
+extern volatile uint16_t I2CTimeout; |
+ |
+void I2C_Init(char); // Initialize I2C |
+#define I2C_Start(start_state) {twi_state = start_state; BLFlags &= ~BLFLAG_TX_COMPLETE; TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);} |
+#define I2C_Stop(start_state) {twi_state = start_state; TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);} |
+void I2C_Reset(void); // Reset I2C |
+ |
+#define BLCONFIG_SUCCESS 0 |
+#define BLCONFIG_ERR_MOTOR_RUNNING 1 |
+#define BLCONFIG_ERR_MOTOR_NOT_EXIST 2 |
+#define BLCONFIG_ERR_HW_NOT_COMPATIBLE 3 |
+#define BLCONFIG_ERR_SW_NOT_COMPATIBLE 4 |
+#define BLCONFIG_ERR_CHECKSUM 5 |
+#define BLCONFIG_ERR_READ_NOT_POSSIBLE 6 |
+ |
+uint8_t I2C_WriteBLConfig(uint8_t motor); |
+uint8_t I2C_ReadBLConfig(uint8_t motor); |
+ |
+#endif |
+ |
+ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osd.c |
---|
0,0 → 1,4919 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
* Oliver Gemesi "olli42" for additions und changes in OSDScreen * |
*****************************************************************************/ |
//-------------------- |
// Debug |
//-------------------- |
//#define DEBUG_OSD_TIME // mit dem NC-Simulator koennen keine brauchbaren Zeit/Datum Daten ermittelt werden |
// am Anfang von osd() wird mit dieser Einstellung eine gueltige Fake-Zeit/Date gesetzt |
//#define DEBUG_OSD_STAT_MOTORRUN // erzwingt Statistik Aufzeichnung auch wenn die Motoren nicht laufen |
//############################################################################ |
//# HISTORY osd.c |
//# |
//# 05.04.2015 Cebra |
//# - chg: SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00,1) ergänzt um 2. Parameter wegen Fehlfunktion mit NC 2.09h |
//# |
//# 21.06.2014 OG |
//# - chg: osd() - bei auftreten von mk_timeout wieder OSD_MK_Connect(MK_CONNECT) |
//# aktiviert -> dadurch wird u.a. wieder auf die NC umgeschaltet falls |
//# etwas anderes (z.B. ein anderes PKT) auf die FC umgeschaltet hat |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 02.06.2014 OG |
//# - fix: Beep_Waypoint() Target-Reach Beep auch bei Single-WP's (WP-Listen |
//# die nur aus einem WP bestehen) |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# - add: include "../osd/osdold_screens.h" |
//# - add: include "../osd/osdold_messages.h" |
//# - chg: osd() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
//# aufrufende Menue erledigt wird |
//# - add: OSD_Screen_MKDisplay() um Beep_Waypoint() ergaenzt |
//# - chg: OSD_Screen_Waypoints() umgestellt auf Beep_Waypoint() |
//# - add: Beep_Waypoint() |
//# |
//# 31.05.2014 OG |
//# - chg: OSD_Screen_MKDisplay() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 26.05.2014 OG |
//# - add: etliche Aenderungen/Erweiterung am Screen "Waypoints" (auch neues Layout) |
//# |
//# 24.05.2014 OG |
//# - fix: OSD_Screen_Waypoints(), OSD_Screen_MKDisplay() - Anzeige des akt. WP-Index |
//# - chg: OSD_Screen_3DLage() - optisches Facelift |
//# - fix: OSD_Screen_3DLage() - es wurden ggf. Kreise gezeichnet die ungueltig |
//# und in undefinierten Speicher gingen - wurde korrigiert |
//# - chg: OSD0,1,2 angepasst an OSD_Element_CompassDirection() mit xoffs,yoffs |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 20.05.2014 OG |
//# - chg: OSD_Element_Flag_Label() - Anpassung zur Rahmenbestimmung eines Flags |
//# |
//# 19.05.2014 OG |
//# - fix: osd() - Tastensteuerung bei MK-Empfangsverlust |
//# - chg: osd() - wenn MK-Setting nicht ermittelt werden konnte dann exit |
//# - add: OSD_Screen_MKDisplay() - Anzeige von akt. Waypoint und Anzahl der Waypoints |
//# - chg: OSD_Popup_MKSetting() - gibt true/false fuer Ok zurueck und Beep bei Fehler |
//# - chg: OSD_Popup_MKSetting() - timeout von 15 auf 9 reduziert |
//# - chg: OSD_Popup_MKSetting(), OSD_Popup_MKError() - Layout, Multi-Sprachenunterstuetzung |
//# |
//# 02.05.2014 OG |
//# - del: Popup_Draw() (jetzt in lcd.c) |
//# |
//# 02.05.2014 OG |
//# - chg: kleine Aenderung an Codereihenfolge von MkError_Save() wegen Compiler-Warning |
//# |
//# 28.04.2014 OG |
//# Anmerkung OSD-MK-Display: wenn der MK-Display Modus eingeschaltet wird, dann werden die Display Daten |
//# angefordert und die BL-Daten etwas reduziert. Hin und wieder kann es passieren das ein Tastendruck im |
//# OSD-MK-Display-Modus nicht durch kommt (aber nicht allzu stoerend) - kann evtl. verbessert werden wenn |
//# auch die OSD-Daten reduziert werden (waere jedoch einiges mehr Aufwand). |
//# - Unterstuetzung von MK-Display (nur NC-Modus) im OSD |
//# (einschalten mit langem Druck dritte Taste von links, abschalten ebenso) |
//# - Info-Anzeige bzgl. langem Tastendruck fuer MK-Display erweitert und deutsche Uebersetzung von "long press" |
//# - Umstrukturierung von osd.c fuer MK-Display |
//# - Aenderungen/Erweiterungen an verschiedenen weiteren Funktionen wie OSD_MK_GetData() fuer MK-Display |
//# - add: OSD_MK_ShowTimeout() - neues Design/Layout |
//# - del: OSD_Timeout() (ersetzt durch OSD_MK_ShowTimeout()) |
//# - chg: verschiedene Aenderungen an Timings |
//# - add: neuer Timer fuer MK-Display: timer_get_displaydata |
//# - fix: es gab bei einem Datenpaket-Timer einen Konflikt mit 'timer' - ein neuer Timer wurde aktiviert (timer_mk_timeout) |
//# |
//# 22.04.2014 OG |
//# - add: OSD_Element_HomeCircleX() - soll das alte OSD_Element_HomeCircle() ersetzen, wenn |
//# die alten OSD-Screens endgueltig rausfliegen |
//# - chg: Aenderung Anzeige OSD_Screen_UserGPS() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_MKStatus() (abgerundete Ecken und Code Einsparung) |
//# - chg: Aenderung Anzeige OSD_Screen_Electric() (abgerundete Ecken und Code Einsparung) |
//# |
//# 01.03.2014 OG |
//# - chg: OSD_Popup_MKError() auf lcdx_printp_center() umgestellt |
//# |
//# 16.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MK_Setting_load() |
//# |
//# 13.02.2014 OG |
//# - chg: OSD_MK_GetData() prueft mit MKVersion_Cmp() auf NC-Version v0.30g |
//# ob GPS-Zeit-Datenpakete ('t') von der NC angefordert werden koennen |
//# |
//# 12.02.2014 OG |
//# - chg: define MKVERSIONnnn rund um OSD_MK_UTCTime() entfernt |
//# -> TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//# -> keine Unterstuetzung mehr durch zu alte NC-Versionen |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 10.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf MKVersion_Setting_print() |
//# |
//# 08.02.2014 OG |
//# - chg: OSD_Popup_MKSetting() umgestellt auf neue MKVersion Struktur |
//# |
//# 04.02.2014 OG |
//# - fix: OSD_Element_BattLevel2() Aufruf von writex_ndigit_number_u_100th() |
//# |
//# 03.02.2014 OG |
//# - chg: OSD_Element_BattLevel2() unterstuetzt Config.OSD_ShowCellU zur Anzeige |
//# der MK-Spannung als Einzelzelle (kalk. auf Basis der ermittelten Zellenzahl) |
//# |
//# 29.01.2014 OG |
//# - add: neue MK-Errorcodes 32,33,34 |
//# - add: #include "mk/mkbase.h" |
//# |
//# 24.01.2014 OG |
//# - chg: OSD_Popup_MKSetting(): MK-Settings wird beim Start vom OSD wieder angezeigt |
//# auch wenn die falsche FC-Revision vorhanden ist mit folgender Regel: |
//# a) wenn falsche FC-Settings-Revision dann nur die Nummer |
//# b) wenn richtige FC-Revision dann mit Nummer und Namen |
//# |
//# 06.01.2014 Cebra |
//# - add: MK-Settingsname wird bei falscher FC Version nicht angezeigt |
//# |
//# 07.07.2013 OG |
//# - add: OSD-Screens koennen vom Benutzer an-/ausgeschaltet werden (osd(), ScreenCtrl_Push()) |
//# - chg: OSD-Screen Namen jetzt in messages.c |
//# - fix: MK-Error Check prueft ob die Fehlernummer gueltig ist |
//# - del: alten Screen Electric (der ohne Nachkomma bei den Stroemen) |
//# |
//# 03.07.2013 OG |
//# - chg: OSD_Popup_MKSetting() - zentrierte Anzeige des Setting-Namens |
//# - chg: OSD_Popup_MKSetting() - timing |
//# - chg: osd() - Screen-Umschaltung beschleunigt |
//# - chg: timinigs bei OSD_MK_Connect(), osd() (werden Verbindungsfehler reduziert?) |
//# |
//# 02.07.2013 OG |
//# - chg: CheckMKLipo() - MK-Unterspannungs-Beep nur wenn Motoren laufen |
//# - del: unbenutzte Variablen |
//# |
//# 30.06.2013 OG |
//# - add: grafische Akku-Anzeige auch fuer Screen Navigation und ElectricN |
//# |
//# 30.06.2013 OG |
//# - add: Screen General: das Batt-Symbol zeigt grafisch den Fuellstand |
//# des MK-Lipos |
//# - add: Mittelwert fuer Gesamtstrom |
//# - add: calc_avg() - Mittelwertberechnung |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - add: USE_OSD_PKTHOOK in osd() - aktuell noch nicht klar ob PKT_Hook() |
//# die Kommunikation stoert |
//# |
//# 27.06.2013 OG |
//# - chg: numerische Winkelanzeige im Screen "Navigation" von -180 bis 180 Grad |
//# 0 Grad = vorderer Ausleger zeigt zur Startposition |
//# - fix: Homesicht des MK's (Winkelanzeige, Grafik) |
//# ab nun: keine Benutzereinstellung mehr dafuer |
//# es gilt: 0 Grad bzw. Strich nach unten -> der vordere Ausleger des MK |
//# zeigt auf seine Startposition |
//# |
//# 19.06.2013 OG |
//# - fix: redundante PKT-Error 40 eliminiert durch merken via old_PKTErrorcode |
//# -> hilft wenn der MK ausgeschaltet wird waehrend man in der PKT-OSD-Anzeige ist |
//# - fix: last_FlyTime in osd() |
//# |
//# 18.06.2013 OG |
//# - chg: OSD_Timeout() erweitert um Errorlogging (Code 40 = "PKT RX lost") |
//# - add: Fehlerliste "PKT RX lost" (Code: 40) |
//# |
//# 15.06.2013 OG |
//# - chg: Anzeige OSD_Popup_MKSetting() kann durch Benutzer via Setup an/ausgeschaltet werden |
//# |
//# 15.06.2013 OG |
//# - chg: OSD_Popup_MKSetting() optimiert, Layout, fix |
//# - chg: PKT_CtrlHook erstmal wieder disabled in osd() (evtl. Probleme mit Timings?) |
//# |
//# 13.06.2013 cebra |
//# - add: Settings Popup beim Start des OSD-Screens |
//# |
//# 11.06.2013 OG |
//# - add: Mittelwertberechnung BL-Strom [MK_GetData()] |
//# - chg: Config.OSD_ScreenMode wird vor PKT_CtrlHook() gesetzt [osd()] |
//# |
//# 27.05.2013 OG |
//# - fix: OSD_MK_UTCTime() (neue "t" Version) Problem wegen cast fuer Sekunden |
//# |
//# 19.05.2013 OG |
//# - chg: osd() erweitert um PKT_CtrlHook() um u.a. PKT-Updates zu ermoeglichen |
//# |
//# 16.05.2013 OG |
//# - add: USE_OSD_SCREEN_NAVIGATION, USE_OSD_SCREEN_ELECTRIC, USE_OSD_SCREEN_ELECTRIC_N, |
//# USE_OSD_SCREEN_MKSTATUS, USE_OSD_SCREEN_USERGPS, USE_OSD_SCREEN_3DLAGE, |
//# USE_OSD_SCREEN_STATISTIC |
//# - add: OSD_Screen_ElectricN() - zeigt BL-Stroeme mit Nachkomma an |
//# - del: define SHOW_OSD_BLCURRENT_10TH (ersetzt durch neue Screen-Variante) |
//# - chg: osd() umgestellt auf OSD Screen Controler; Anpassungen dafuer an allen Screens |
//# - add: Funktionen fuer Screen Controler (Screen_*()) |
//# |
//# 15.05.2013 OG |
//# - chg: OSD_Screen_Electric() zeigt BL-Strom mit Nachkomma an |
//# (einstellbar via define SHOW_OSD_BLCURRENT_10TH bzgl. neue/alte Anzeige) |
//# - add: define DEBUG_OSD_STAT_MOTORRUN erzwingt Statistik Aufzeichnung auch wenn |
//# die Motoren nicht laufen |
//# - fix: osd() Statistik-Ende Zeit wird nach Landung gesetzt |
//# - chg: diverse Aufraeumarbeiten |
//# - chg: OSD_Screen_Statistics() umgetsellt auf calc_BLmax(() |
//# - add: calc_BLmax() ermittelt max. BL Current & Temp |
//# - del: draw_symbol_degree(), draw_symbol_rc() - wurden ersetzt durch |
//# Zeichen im font 8x6 (SYMBOL_SMALLDEGREE, SYMBOL_RCQUALITY) |
//# |
//# 14.05.2013 OG |
//# - add: OSD_MK_Connect() vereinheitlicht MK-Verbindungsinitialisierung |
//# und MK-Abo-Renew |
//# - chg: OSD_MK_UTCTime() atomisiert via ATOMIC_BLOCK() |
//# - chg: OSD_Screen_Debug_RX() verschiedene Optimierungen und Beschreibungen |
//# |
//# 14.05.2013 OG |
//# - chg: OSD_MK_UTCTime() wird mittels define jetzt mit neuem oder mit altem |
//# Algorithmus einkompiliert. |
//# ALT bei: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
//# NEU bei: alles andere (also neuer): die neue Zeit/Datumsermittlung |
//# |
//# 13.05.2013 OG |
//# - fix: an allen Stellen mit naviData->CompassHeading ein Modulo 360 ergaenzt |
//# um Winkelangaben der FC geradezuziehen falls der Kopter beim Mixersetup |
//# 'verdreht' wurde (Problem von helitron im Forum - Winkelanzeige > 360 Grad) |
//# Ist ggf. ab Firmware NC 0.30h nicht mehr notwendig da Holger das |
//# korrigieren wollte (muesste noch geprueft werden) |
//# |
//# 11.05.2013 OG |
//# - add: OSD_MK_UTCTime_NEW() - ab NC 0.30g (Alpha, noch nicht im Einsatz) |
//# |
//# 05.05.2013 OG |
//# - chg: OSD_Popup_Info() & OSD_Popup_MKError() wurden vereinheitlicht |
//# mittels Popup_Draw() |
//# - chg: Anzeigezeiten der Popup's etwas verkuerzt |
//# |
//# 03.05.2013 OG |
//# - fix: OSD_MK_GetData() - griff beim lesen der BL's auf naviData fuer |
//# die Statistik - relevante naviData-Werte werden nun vor dem |
//# wechseln auf BL-Data gemerkt |
//# - chg: Erfassung der Statistik Start/Ende-Zeit verbessert |
//# - chg: bei Aufrufen von writex_datetime_time()/writex_datetime_time() den |
//# Parameter 'timeoffset' entfernt |
//# - add: define DEBUG_OSD_TIME um eine Fake-Zeit/Datum bei Gebrauch des |
//# NC-Simulators zu setzen |
//# |
//# 29.04.2013 OG |
//# - chg: Plausibilitaetscheck in OSD_MK_UTCTime() bzgl. des Datums |
//# |
//# 28.04.2013 OG |
//# - chg: die alten OSD-Screens angepasst auf define USE_FONT_BIG |
//# |
//# 24.04.2013 OG |
//# - chg: directions_p[] geaendert auf Kompatibilitaet mit Atmel Studio 6 |
//# |
//# 21.04.2013 OG |
//# - chg: define AltimeterAdjust nach osd.h verschoben |
//# |
//# 14.04.2013 OG |
//# - add: weitere Statistik-Aufzeichnungen |
//# - fix: MK_GetData() - Statistik-Aufzeichnung nur fuer wirklich vorhandene BL's |
//# |
//# 04.04.2013 OG |
//# - fix: 3 warnings |
//# |
//# 04.04.2013 OG |
//# - chg: MK-Errortext-Anzeige umgestellt auf naviData->Errorcode |
//# weniger Datenkomminkation mit dem MK und schnelleres Ansprechverhalten |
//# |
//# 03.04.2013 OG |
//# - chg: defines OSD_DEMO, OSD_SCREEN_DEBUG, OSD_SCREEN_OLD |
//# umbenannt nach: USE_OSD_DEMO, USE_OSD_SCREEN_DEBUG, USE_OSD_SCREEN_OLD |
//# und verschoben nach: main.h |
//# |
//# 28.03.2013 OG |
//# - fix: MKErr_Log_Init() - es fehlte Multiplikation mit MAX_MKERR_LOG |
//# - chg: Code Formatierungen |
//# |
//# 28.03.2013 CB |
//# - add: replace OSD_Statistic, GPS_User, MKErr_Log in EEProm structure variable |
//# |
//# 28.03.2013 OG |
//# - fix: abo_timer wieder aktiviert fuer 'o' und 'k' |
//# |
//# 28.03.2013 OG |
//# - chg: osd_statistic_t - erweitert um diverse Werte und Timestamps (werden apeter implementiert) |
//# - add: mkerror_t MKErr_Log[MAX_MKERR_LOG] - Vorbereitung um MK-Errors zu loggen |
//# |
//# 27.03.2013 OG |
//# - add: Datum / Zeit vom MK lesen |
//# - chg: Kommunikation mit dem MK optimiert |
//# - chg: auf neue messages.c angepasst |
//# - struct von den Statistiken auf PKTdatetime PKTdatetime_t |
//# - verschiedene andere Aenderungen |
//# |
//# 22.03.2013 OG |
//# erhebliche Aenderungen / Ergaenzungen - hier nur das Wichtigste |
//# - neue Funktion "User GPS" (UGPS) - mit langem Druck auf Taste KEY_ENTER werden die aktuellen GPS-Daten abgespeichert |
//# * gespeichert wird nur wenn wenn der MK GPS-Ok meldet |
//# * wenn Ok erfolgt ein bestaetigungs Beep; wenn nicht Ok erfolgt ein Error-Beep |
//# - neuer Screen "User GPS" - Anzeige der letzten der 3 UGPS-Koordinaten (intern wird mehr gespeichert) |
//# - Anzeige von MK-Error Messages als Popup mit einstellbarer Anzeigezeit (kann mit Taste abgebrochen werden) |
//# * kann getestet werden z.B. durch Ausschalten der Funke (= "RC Signal lost") |
//# * siehe: http://www.mikrokopter.de/ucwiki/ErrorCodes |
//# - OSD-Info ist nun ein Popup - autom. ausblenden nach einstellbarer Zeit; im Hintergrund werden weiterhin MK-Daten empfangen und ausgewertet |
//# - neuer Debug-Screen "Debug RXPackages" - zeigt die Anzahl empfangener Datenpakete der verschiedenen Bereiche (um z.B. Timings einzustellen) |
//# - erweiterte Kommunikation mit dem MK fuer BL-Daten und Error-Messages |
//# - Timings der MK-Datenkommunikation weitreichend einstellbar durch defines |
//# - Datenstrukturen fuer OSD-Statistiken und BL-Statistiken |
//# - neue timer: timer_get_erdata, timer_get_bldata, timer_osd_refresh, timer_pkt_uptime |
//# - mit dem define OSD_SCREEN_OLD koennen ggf. die alten OSD-Screens ausgeblendet werden (spart ein paar KByte's) |
//# - verschiedene Optimierungen |
//# |
//# 12.03.2013 OG |
//# - add: Get_BL_Data() - BL-Ctrl via NC auslesen (BETA) (siehe dort auch Kommentare im func-Header) |
//# - add: neuer Screen "Electric" |
//# - chg: Layout von Screen "Navigation" - Pixelverschiebungen |
//# - chg: Layout von Screen "MK-Status" - Pixelverschiebungen und Anzeige 'Strom' ersetzt durch 'entn. Kapazitaet' |
//# - add: in osd() LiPO-Cell Erkennung hinzugefuegt (fuer Screen "Electric") |
//# |
//# 10.03.2013 OG |
//# - fix: doppelte Degree-Anzeige in OSD_Element_CompassDegree() |
//# - add: neuer Screen "MK-Status" |
//# - add: 7 neue OSD-Flags |
//# - chg: Screen-Refresh Zeit via timer2 (einstellbar durch define TIME_OSD_REFRESH) |
//# - chg: mit define OSD_DEBUG_SCREEN kann ein zusaetzlicher Screen verwendet werden zum testen/entwickeln |
//# - del: entfernt CFG2_HEIGHT_LIMIT in OSD_Element_AltitudeControl() (bis bessere Loesung gefunden ist) |
//# |
//# 08.03.2013 OG |
//# - del: OSD_Screen_Element() und cleanup in osd.h |
//# - add: OSD_Element_UpDown() (steigend/sinken via Pfeilen) |
//# - chg: OSD_Element_UpDown() in Screen "General" und "Navigation" hinzugefuegt (rechts neben der Hoehenanzeige) |
//# - chg: Screen "General" die Sat-Warnung wurde auf OSD_Element_Flag(OSD_FLAG_S0) geaendert |
//# - chg: Anzeige von Flag 'nicht genug GPS-Sateliten' (OSD_FLAG_S0) auf "S!" geändert |
//# |
//# 07.03.2013 OG |
//# - Hinweis bzgl. LowBatt-Anzeige in den Screens "General" und "Navigation": |
//# Es gibt zwei unabhängige LowBatt-Warnungen. |
//# 1. die PKT LowBatt-Warnung: sie arbeitet mit der im PKT hinterlegten |
//# LowBatt Spannung und stellt den Spannungswert inkl. der Einheit "V" |
//# invers dar wenn der Warnwert erreicht wurde (inkl. lautem PKT-Peepen) |
//# 2. die MK-LowBatt Warnung: hierbeit wird das Flag "BA" angezeigt wenn |
//# der MK eine LowBatt Warnung sendet |
//# Dadurch hat man nun zwei verschiedene LowBatt Warnungen die man auf Wunsch |
//# verschieden einstellen kann. Bei mir ist die PKT-LowBatt etwas niedriger |
//# eingestellt als die MK-Warnung und bedeutet "es ist aller hoechste Zeit zu landen". |
//# Die Spannung der MK-LowBat ist ein wenig hoeher eingestellt und |
//# zeigt mir "es ist bald Zeit zu landen". |
//# - del: Kommunikation zu FC - siehe Kommentare in osd() |
//# - chg: Kommunikation zu NC - siehe Kommentare in osd() |
//# - add: neuer Screen "Navigation" |
//# - chg: Layout Screen "Statistics" - Einheiten um zwei Pixel nach rechts verschoben |
//# - chg: Layout von Screen "General" modifiziert (u.a. xoffs,yoffs Pixelverschiebungen) |
//# - add: OSD_FLAG_BA in Screen "General" |
//# - add: die OSD_Element_xyz() Funktionen in osd.h aufgenommen |
//# - chg: an verschiedenen Stellen die Char-Drawmode defines MNORMAL, MINVERS, usw. eingebaut |
//# - del: Kompatibilitaetscode fuer "3D-Lage" ueber Hauptmenue entfernt |
//# - chg: die Funktionen OSD_Element_Switch() und OSD_Element_SwitchLabel() heissen |
//# nun OSD_Element_Flag() und OSD_Element_Flag_Label() |
//# - chg: die defines OSD_SWITCH_xy heissen jetzt OSD_FLAG_xy |
//# - fix: letzte GPS-Koordinaten werden jetzt permanent Config.LastLatitude, Config.LastLongitude gespeichert |
//# |
//# 03.03.2013 OG |
//# - add: delay in Mainloop von osd() um springende Werte abzudaempfen (TEST) |
//# - add: Startverzoegerung der Screens bis NaviData sich etwas stabilisiert hat (TEST) |
//# - add: OSD Startup Message "connecting MK..." |
//# - add: 'Emergency Landing' (EL) Anzeige in Screen "General" |
//# - del: OUT1/OUT2 Anzeige in Screen "General" |
//# - add: RC-Quality in Screen "General" |
//# - add: func: draw_symbol_rc() (alternative RC-Quality Symbol) |
//# - fix: Hoehenanzeige fuer Screens "OSD0" und "OSD1" |
//# - fix: OSD_Element_SwitchLabel() angepasst fuer x=0 und y=0 |
//# - add: OSD_Element_Switch/Label() erweitert um OSD_SWITCH_FS |
//# - fix: Screen-Redraw nach OSD_MK_TIMEOUT() und anderen Fehlermeldungen |
//# - chg: messages.c: STATS_ITEM_0 bis STATS_ITEM_6 angepasst (1 char kuerzer) |
//# - chg: Layout von OSD_Info() - mehr background-clear und etwas kleiner |
//# |
//# 02.03.2013 OG |
//# - chg: keine internal func in Screen's wegen warnings bei anderen |
//# - del: Screen "OSD3" |
//# - fix: Hoehenanzeige in Screen "General" (Minuszeichen) |
//# - add: MK LowBat Warning in Screen "General" |
//# - add: neues Degree Symbol (als func) in Screen General (kleiner als das Char 0x1E) |
//# - add: weitere Flags in OSD_Element_Flag() |
//# |
//# 01.03.2013 OG |
//# - Reskrukturierung Code (+ neuer OSD-Screens und einiges mehr) |
//############################################################################ |
//############################################################################ |
//# HINWEISE: |
//# |
//# 1. define: USE_OSD_DEMO (main.h) |
//# mit define OSD_DEMO wird ein Demo-Modus bei den neuen Screens einge- |
//# schaltet - damit werden u.a. alle Flag's angezeigt fuer Scree-Fotos |
//# |
//# 2. define: USE_OSD_SCREEN_DEBUG (main.h) |
//# mit diesem define wird ein zusaetzlicher Screen "Debug" einkompiliert |
//# fuer Test / Experimente / Debug von OSD-Elementen |
//# |
//# 3. define: USE_OSD_SCREEN_OLD (main.h) |
//# ein-/ ausschalten der alten OSD-Screens OSD0, OSD1, OSD2 (spart ca. 3 KByte) |
//# |
//# 4. Timings der MK-Datenkommunikation |
//# die Timings sind weitreichend enstellbar ueber die define im Abschnitt 'Timings' |
//# der Debug-Screen 'Debug RXPackages' informiert ueber die Anzahl eingegangener |
//# Datenpakete der verschiedenen Bereiche (OSD, BL-Daten, Error-Message) |
//# |
//# 5. Informationen zum Display |
//# DOG: 128 x 64 Pixel with 6x8 Font => 21 x 8 |
//# |
//# 6. MK-Kommunikationsprotokoll Referenz |
//# http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=%28%28----%28-*%29%28\r%29%3F\n%29%28.*%29CategoryCoding%29#en.2BAC8-SerialCommands.Flight-Ctrl |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
//-------------------- |
// Funktionen ein-/ausbinden |
//-------------------- |
// -> siehe main.h |
//-------------------- |
// Timings |
//-------------------- |
#define TIME_OSD_REFRESH 45 // Screen Refresh (Steuerung via 'timer_osd_refresh') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_POPUP_INFO 400 // 4 Sekunden Popup-Info zeigen (kann mit Taste abgebrochen werden) |
#define TIME_POPUP_MKERROR 700 // 7 Sekunden Popup-MK-Error zeigen (kann mit Taste abgebrochen werden) |
#define TXINTERVAL_OSDDATA 10 // Intervall mit der der MK OSD-Daten senden soll (n*10 = ms) |
#define TIME_GET_BLDATA 35 // Zeitintervall in der BL-Daten vom MK geholt werden (Steuerung via 'timer_get_bldata') (n*10 = ms; 100 entspricht 1 Sekunde) |
#define TIME_READ_BLDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
#define TXINTERVAL_BLDATA 7 // Intervall mit der der MK BL-Daten senden soll (n*10 = ms) |
#define TIME_GET_TIDATA 12000 // alles 120 Sekunden Zeit/Datum vom MK lesen (die Zwischenzeit wird von einem PKT-Timer uebernommen) |
#define TIME_GET_DISPLAYDATA 20 // fuer n Zeit werden BL-Daten vom MK gelesen (Steuerung via timer) (n*10 = ms; 100 entspricht 1 Sekunde)) |
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
//-------------------- |
// weiteres |
//-------------------- |
#define OSD_POPUP_NONE 0 |
#define OSD_POPUP_INFO 1 |
#define OSD_POPUP_MKERROR 2 |
#define MK_CONNECT 1 |
#define MK_ABORENEW 2 |
// global definitions and global vars |
NaviData_t *naviData; |
uint16_t heading_home; |
uint8_t drawmode; |
uint8_t OSDScreenRefresh; |
// flags from last round to check for changes |
uint8_t old_AngleNick = 0; |
uint8_t old_AngleRoll = 0; |
uint16_t old_hh = 0; |
// aktuell nicht benoetigt - siehe Kommentar in osd.c |
//mk_param_struct_t *mk_param_struct; |
//uint8_t Flags_ExtraConfig; |
//uint8_t Flags_GlobalConfig; |
//uint8_t Flags_GlobalConfig3; |
// cache old vars for blinking attribute, checkup is faster than full |
// attribute write each time |
volatile uint8_t OSD_active = 0; |
uint8_t Vario_Beep_Up = 0; |
uint8_t Vario_Beep_Down = 0; |
uint8_t Vario_Beep_Up_Interval = 9; |
uint8_t Vario_Beep_Down_Interval = 6; |
uint8_t Vario_Threshold = 0; |
uint8_t Vario_Threshold_Value = 7; |
uint8_t OldWP = 0; |
uint8_t NextWP = 0; |
uint8_t WP_old = 0; // Screen: "Waypoints" |
uint8_t WP_last = false; // Screen: "Waypoints" |
#define MAX_CELL_VOLTAGE 43 // max cell voltage for LiPO |
#define MIN_CELL_VOLTAGE 32 // min cell voltage for LiPO |
// Flags |
volatile uint8_t error = 0; |
uint8_t cells = 0; |
uint8_t BattLowVoltageWarning = 0; |
uint8_t CellIsChecked = 0; |
uint8_t AkkuWarnThreshold = 0; |
uint16_t duration = 0; |
//----------------------------------------------------------- |
// Buffer |
//----------------------------------------------------------- |
BLData_t blData[OSD_MAX_MOTORS]; // speichert gelesene BL-Datenpakete |
pkt_gpspos_t GPS_Current; // aktuelle GPS-Position |
u8 old_PKTErrorcode; // speichert den letzen Errorcode vom PKT damit dieser nicht wiederholt gespeichert wird |
u8 old_MKErrorcode; // speichert den letzen Errorcode vom MK damit dieser nicht wiederholt angezeigt wird |
//----------------------------------------------------------- |
// OSD Daten |
//----------------------------------------------------------- |
NaviData_t osdData; // Buffer |
//----------------------------------------------------------- |
// MK-DISPLAY |
//----------------------------------------------------------- |
uint8_t mkdisplayMode = false; |
uint8_t mkdisplayCmd = 0xff; |
char mkdisplayData[81]; // Buffer (80 +1 fuer term. Char) |
//--------------------- |
// DEBUG |
//--------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
uint16_t readCounterOSD; // Anzahl gelesener Datenpakete von NC Modus 'o' (Request OSD-Data) |
uint16_t readCounterTIME; // Anzahl gelesener Datenpakete von NC (Time) |
uint16_t readCounterDISPLAY; |
uint16_t readCounterBL[OSD_MAX_MOTORS]; // Anzahl gelesener Datenpakete pro BL via NC Modus 'k' (BL Ctrl Status) |
#endif // USE_OSD_SCREEN_DEBUG |
//--------------------- |
// Strings & Co. |
//--------------------- |
static const char mkerror00[] PROGMEM = "No Error"; |
static const char mkerror01[] PROGMEM = "FC not compatible"; |
static const char mkerror02[] PROGMEM = "MK3Mag not compati."; |
static const char mkerror03[] PROGMEM = "no FC communication"; |
static const char mkerror04[] PROGMEM = "no MK3Mag communic."; |
static const char mkerror05[] PROGMEM = "no GPS communication"; |
static const char mkerror06[] PROGMEM = "bad compass value"; |
static const char mkerror07[] PROGMEM = "RC Signal lost"; |
static const char mkerror08[] PROGMEM = "FC spi rx error"; |
static const char mkerror09[] PROGMEM = "no NC communication"; |
static const char mkerror10[] PROGMEM = "FC Nick Gyro"; |
static const char mkerror11[] PROGMEM = "FC Roll Gyro"; |
static const char mkerror12[] PROGMEM = "FC Yaw Gyro"; |
static const char mkerror13[] PROGMEM = "FC Nick ACC"; |
static const char mkerror14[] PROGMEM = "FC Roll ACC"; |
static const char mkerror15[] PROGMEM = "FC Z-ACC"; |
static const char mkerror16[] PROGMEM = "Pressure sensor"; |
static const char mkerror17[] PROGMEM = "FC I2C"; |
static const char mkerror18[] PROGMEM = "Bl Missing"; |
static const char mkerror19[] PROGMEM = "Mixer Error"; |
static const char mkerror20[] PROGMEM = "Carefree Error"; |
static const char mkerror21[] PROGMEM = "GPS lost"; |
static const char mkerror22[] PROGMEM = "Magnet Error"; |
static const char mkerror23[] PROGMEM = "Motor restart"; |
static const char mkerror24[] PROGMEM = "BL Limitation"; |
static const char mkerror25[] PROGMEM = "Waypoint range"; |
static const char mkerror26[] PROGMEM = "No SD-Card"; |
static const char mkerror27[] PROGMEM = "SD Logging aborted"; |
static const char mkerror28[] PROGMEM = "Flying range!"; |
static const char mkerror29[] PROGMEM = "Max Altitude"; |
static const char mkerror30[] PROGMEM = "No GPS Fix"; |
static const char mkerror31[] PROGMEM = "compass not calibr."; |
static const char mkerror32[] PROGMEM = "BL selftest"; |
static const char mkerror33[] PROGMEM = "no ext. compass"; |
static const char mkerror34[] PROGMEM = "compass sensor"; |
#define MAX_MKERROR_NUM 34 // maximale Error-Nummer vom MK (verwendet in osd()) |
static const char mkerror35[] PROGMEM = ""; // free for MK |
static const char mkerror36[] PROGMEM = ""; // free for MK |
static const char mkerror37[] PROGMEM = ""; // free for MK |
static const char mkerror38[] PROGMEM = ""; // free for MK |
static const char mkerror39[] PROGMEM = ""; // free for MK |
static const char pkterror40[] PROGMEM = "PKT RX lost"; |
//--------------------------------------- |
// wenn die Liste erweitert wird, |
// MAX_MKERROR_NUM in osd.h anpassen! |
//--------------------------------------- |
const char * const mkerrortext[] PROGMEM= |
{ |
mkerror00, |
mkerror01, |
mkerror02, |
mkerror03, |
mkerror04, |
mkerror05, |
mkerror06, |
mkerror07, |
mkerror08, |
mkerror09, |
mkerror10, |
mkerror11, |
mkerror12, |
mkerror13, |
mkerror14, |
mkerror15, |
mkerror16, |
mkerror17, |
mkerror18, |
mkerror19, |
mkerror20, |
mkerror21, |
mkerror22, |
mkerror23, |
mkerror24, |
mkerror25, |
mkerror26, |
mkerror27, |
mkerror28, |
mkerror29, |
mkerror30, |
mkerror31, |
mkerror32, |
mkerror33, |
mkerror34, |
mkerror35, |
mkerror36, |
mkerror37, |
mkerror38, |
mkerror39, |
pkterror40 |
}; |
//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W"; |
const char rose[48] PROGMEM = { |
0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', |
0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', |
0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', |
0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', |
}; |
// the center is char 19 (north), we add the current heading in 8th |
// which would be 22.5 degrees, but float would bloat up the code |
// and *10 / 225 would take ages... so we take the uncorrect way |
static const char str_NE[] PROGMEM = "NE"; |
static const char str_E[] PROGMEM = "E "; |
static const char str_SE[] PROGMEM = "SE"; |
static const char str_S[] PROGMEM = "S "; |
static const char str_SW[] PROGMEM = "SW"; |
static const char str_W[] PROGMEM = "W "; |
static const char str_NW[] PROGMEM = "NW"; |
static const char str_N[] PROGMEM = "N "; |
const char * const directions_p[] PROGMEM= |
{ |
str_NE, |
str_E, |
str_SE, |
str_S, |
str_SW, |
str_W, |
str_NW, |
str_N |
}; |
//########################################################### |
//# OSD Screen Controler |
//########################################################### |
#define SCREENCTRL_MAX 15 // Anzahl max. zu verwaltender Screens |
//--------------------------- |
// typedef: einzelner Screen |
//--------------------------- |
typedef struct |
{ |
const char *screenname; |
void (*screen)(void); |
} screen_t; |
//--------------------------- |
// typedef: Screenliste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; |
uint8_t count; |
screen_t screen[SCREENCTRL_MAX]; |
} screenlist_t; |
screenlist_t osdscreens; |
//-------------------------------------------------------------- |
// ScreenCtrl_Init() |
// |
// initialisiert die Screenliste - muss vor dem ersten |
// ScreenCtrl_Push() aufgerufen werden |
//-------------------------------------------------------------- |
void ScreenCtrl_Init( void ) |
{ |
osdscreens.active = 0; |
osdscreens.count = 0; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Push( *screenname, *screenfunc) |
// |
// fuegt einen Screen der Screenliste hinzu (siehe osd()) |
//-------------------------------------------------------------- |
void ScreenCtrl_Push( uint8_t screenid, const char *screenname, void (*screenfunc)(void)) |
{ |
if( osdscreens.count < SCREENCTRL_MAX ) |
{ |
// wenn screenid = 0 dann immer anzeigen (Screen ist nicht vom Benutzer auswaehlbar) |
if( (screenid==0) || ((Config.OSD_UseScreen & (1 << screenid)) != 0) ) |
{ |
osdscreens.screen[osdscreens.count].screenname = screenname; |
osdscreens.screen[osdscreens.count].screen = screenfunc; |
osdscreens.count++; |
} |
} |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Set( screennum) |
// |
// aktiviert einen bestimmten Screen aus der Screenliste |
//-------------------------------------------------------------- |
void ScreenCtrl_Set( uint8_t screennum ) |
{ |
osdscreens.active = 0; |
if( screennum < osdscreens.count ) |
{ |
osdscreens.active = screennum; |
} |
} |
//-------------------------------------------------------------- |
// num = ScreenCtrl_GetNum() |
// |
// gibt die Nummer des aktuell aktivierten Screens zurueck |
//-------------------------------------------------------------- |
uint8_t ScreenCtrl_GetNum( void ) |
{ |
return osdscreens.active; |
} |
//-------------------------------------------------------------- |
// name = ScreenCtrl_GetName() |
// |
// gibt den Namen des aktuell aktivierten Screens zurueck |
// Rueckgabe ist ein Pointer auf einen String im PROGMEM |
//-------------------------------------------------------------- |
const char * ScreenCtrl_GetName( void ) |
{ |
return osdscreens.screen[osdscreens.active].screenname; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Next() |
// |
// zum naechsten Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Next( void ) |
{ |
osdscreens.active++; |
if( osdscreens.active >= osdscreens.count ) |
{ |
osdscreens.active = 0; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Previous() |
// |
// zum vorherigen Screen wechseln |
//-------------------------------------------------------------- |
void ScreenCtrl_Previous( void ) |
{ |
if( osdscreens.active == 0 ) |
{ |
osdscreens.active = osdscreens.count-1; |
} |
else |
{ |
osdscreens.active--; |
} |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
//-------------------------------------------------------------- |
// ScreenCtrl_Show() |
// |
// ruft den aktuell Screen auf |
//-------------------------------------------------------------- |
void ScreenCtrl_Show( void ) |
{ |
osdscreens.screen[osdscreens.active].screen(); |
} |
//########################################################### |
//########################################################### |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_BLmax( osd_BLmax_t *blmax ) |
{ |
uint8_t i; |
memset( blmax, 0, sizeof(osd_BLmax_t) ); |
for( i=0; i<Config.OSD_Statistic.BL_Count; i++) |
{ |
if( Config.OSD_Statistic.BL[i].max_Current > blmax->max_BL_Current ) |
{ |
blmax->max_BL_Current = Config.OSD_Statistic.BL[i].max_Current; |
blmax->max_BL_Current_Index = i; |
} |
if( Config.OSD_Statistic.BL[i].max_Temp > blmax->max_BL_Temp ) |
{ |
blmax->max_BL_Temp = Config.OSD_Statistic.BL[i].max_Temp; |
blmax->max_BL_Temp_Index = i; |
} |
} |
} |
//-------------------------------------------------------------- |
// STAT_Init() |
// |
// initialisiert die Statistic Werte neu |
//-------------------------------------------------------------- |
void STAT_Init(void) |
{ |
// init: statistic |
memset( &Config.OSD_Statistic, 0, sizeof(osd_statistic_t) ); |
Config.OSD_Statistic.min_UBat = 255; |
Config.OSD_Statistic.min_RCQuality = 255; |
Config.OSD_Statistic.min_AngleNick = 126; |
Config.OSD_Statistic.min_AngleRoll = 126; |
} |
//-------------------------------------------------------------- |
// MKErr_Log_Init() |
// |
// initialisiert die MK Errorcodes neu |
//-------------------------------------------------------------- |
void MKErr_Log_Init(void) |
{ |
// init: MK Errorlog |
memset( &Config.MKErr_Log, 0, sizeof(mkerror_t)*MAX_MKERR_LOG ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MkError_Save( uint8_t Errorcode ) |
{ |
uint8_t i; |
for( i=MAX_MKERR_LOG-1; i>0; i--) |
{ |
Config.MKErr_Log[i] = Config.MKErr_Log[i-1]; |
} |
Config.MKErr_Log[0].Errorcode = Errorcode; |
memcpy( &Config.MKErr_Log[0].set_Time, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
// OSD_MK_Connect() |
// |
// Verbindung zum MK herstellen oder ABO-Renew |
// |
// modus: MK_CONNECT oder MK_ABORENEW |
//-------------------------------------------------------------- |
void OSD_MK_Connect( uint8_t modus ) |
{ |
uint8_t tx_interval; |
if( modus == MK_CONNECT ) |
{ |
SwitchToNC(); |
// switch off: 3d data => kompatibel mit SmartOSD???? |
//tx_interval = 0; |
//SendOutData ('c', ADDRESS_ANY, 1, &tx_interval, 1); |
//_delay_ms(25); |
tx_interval = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tx_interval, 1); // switch off: debug |
timer = 3; while( timer > 0 ); // short delay |
} |
tx_interval = TXINTERVAL_OSDDATA; // switch on: sending osd-data |
SendOutData( 'o', ADDRESS_NC, 1, &tx_interval, 1); // request: OSD Data from NC every ..ms |
timer = 3; while( timer > 0 ); // short delay |
// switch on: sending bl-data |
tx_interval = TXINTERVAL_BLDATA; // 5 => 50 ms (send packet every n*10 ms) |
SendOutData( 'k', ADDRESS_ANY, 1, &tx_interval, 1); // request: BL Ctrl Status |
mode = 'O'; |
abo_timer = ABO_TIMEOUT; |
rxd_buffer_locked = FALSE; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_MK_ShowTimeout( void ) |
{ |
if( old_PKTErrorcode != 40 ) |
{ |
MkError_Save( 40 ); // Logge: "PKT RX lost" |
old_PKTErrorcode = 40; |
} |
lcd_cls (); |
lcd_frect_round( 0, 0, 127,10, 1, R1); // Rect: Invers (Titel) |
lcdx_printp_center( 0, strGet(OSD_ERROR), MINVERS, 1,2); // "FEHLER: Datenverlust!" |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,6); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,6); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+5, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,9); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,11); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,11); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+8, 127, (2*8)+4, 1); // Rahmen |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); // Keyline: "Ende" |
set_beep ( 0, 0, BeepOff); // Falls Spannungswarnung an war Beeper aus (ist das notwendig?) |
} |
/*************************************************************************************************************************************** |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// Fuer: defined MKVERSION088n || defined MKVERSION090b || defined MKVERSION090e |
// -> erstmal keine Unterstuetzung mehr... |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// ALTE Funktion fuer FC < 0.90h (und dementsprechend NC < 0.30h) |
// |
// Setzt die PKT globale UTCTime mithilfe des MK. |
// |
// Foraussetzung: |
// - der NC-Modus muss aktiv sein (SwitchToNC) |
// - aktuell nur fuer osd.c |
// |
// Rueckgabe: |
// true = ok, UTCTime gespeichert |
// false = Zeit/Datum nicht gelesen |
// |
// Hack: |
// Gelesen wird die Seite 3 des NC-Display's. Dort wird Zeit |
// und Datum an den entsprechnenden Positionen via Zeichen an- |
// gezeigt. Die Zeichen werden ausgelesen und in die interne UTCTime |
// gespeichert. |
// |
// HINWEIS: |
// Ab NC > v0.30b (evtl. v0.30c) wird die NC ein neues Datenpaket |
// senden ("T") mit strukturierten Informationen zu Datum/Zeit. |
// Das wurde von Holger eingebaut. |
// Wird spaeter auch im PKT implementiert. |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
uint32_t sec; |
uint32_t min; |
uint32_t hour; |
uint16_t year; |
uint8_t month; |
uint8_t day; |
uint8_t page; |
uint8_t ok = false; |
mode = 'L'; // read: MK-Display Page |
page = 3; // anfordern von Seite 3 (der NC) |
SendOutData ('l', ADDRESS_ANY, 1, &page, 1); // request: MK-Display Page 3 |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
sec = (uint32_t)(pRxData[66+7+2] - '0') + 10 * (uint32_t)(pRxData[66+6+2] - '0'); // seconds: sec |
min = (uint32_t)(pRxData[66+4+2] - '0') + 10 * (uint32_t)(pRxData[66+3+2] - '0'); // seconds: min |
hour = (uint32_t)(pRxData[66+1+2] - '0') + 10 * (uint32_t)(pRxData[66+0+2] - '0'); // seconds: hour |
sec += (min * 60) + (hour * 3600); |
year = (uint16_t)(pRxData[46+9+2] - '0') + 10 * (uint16_t)(pRxData[46+8+2] - '0'); // year |
year += 100 * (uint16_t)(pRxData[46+7+2] - '0') + 1000 * (uint16_t)(pRxData[46+6+2] - '0'); // year |
day = (uint8_t)(pRxData[46+4+2] - '0') + 10 * (uint8_t)(pRxData[46+3+2] - '0'); // day |
month = (uint8_t)(pRxData[46+1+2] - '0') + 10 * (uint8_t)(pRxData[46+0+2] - '0'); // month |
// in der globalen UTCTime speichern (hoffentlich funkt kein timer dazwischen) |
if( year > 2000 && year < 2200 ) // plausibilitaets check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = sec; |
UTCTime.day = day; |
UTCTime.month = month; |
UTCTime.year = year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
***************************************************************************************************************************************/ |
//-------------------------------------------------------------- |
// OSD_MK_UTCTime() |
// |
// NEUE Ermittlung der Zeit vom MK |
// ab NC v0.30g (bzw. NC 0.30h fuer FC 0.90h) |
// |
// TODO OG: auf neue MKVersion bzgl. Versionsprüfung anpassen! |
//-------------------------------------------------------------- |
int OSD_MK_UTCTime( uint8_t readtime ) |
{ |
DateTime_t *rx_DateTime; |
uint8_t tx_interval; |
uint8_t ok = false; |
mode = 'T'; // read: MK-Display Page |
tx_interval = 1; // |
SendOutData ('t', ADDRESS_NC, 1, &tx_interval, 1); // request: DateTime |
rxd_buffer_locked = FALSE; |
timer = readtime; // fuer max. n*10 millisec versuchen Daten zu lesen |
while( timer>0 && !ok ); // lese Daten-Pakete fuer die angegebene Zeit oder gelesen |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
rx_DateTime = (DateTime_t *) (pRxData); |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterTIME++; |
#endif |
if( rx_DateTime->Year > 2000 && rx_DateTime->Year < 2200 ) // Plausibilitaets Check |
{ |
ATOMIC_BLOCK(ATOMIC_FORCEON) |
{ |
UTCTime.seconds = ((uint32_t)(rx_DateTime->Hour))*3600 + ((uint32_t)(rx_DateTime->Min))*60 + (uint32_t)(rx_DateTime->Sec); |
UTCTime.day = rx_DateTime->Day; |
UTCTime.month = rx_DateTime->Month; |
UTCTime.year = rx_DateTime->Year; |
} |
} |
ok = true; |
} |
} |
return ok; // wenn erfolgreich gelesen dann true (kein timeout) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
int32_t calc_avg( int32_t avg, int32_t nvalue, int32_t count, int32_t factor) |
{ |
avg = avg + (( ( nvalue * factor) - avg) / count); |
return avg; |
} |
//-------------------------------------------------------------- |
// OSD_MK_GetData() |
// |
// Holt Daten der BL-Ctrl via NC. |
// |
// Die Werte der BL's 1-12 kommen in mehr oder weniger beliebiger |
// Reihenfolge an. Die Daten fuer vorhandene BL's werden oefter |
// gesendet als die von nicht vorhandenen BL's. |
// |
// Diese Funktioen liest fuer die Zeit BL_READ_TIME die BL-Daten |
// vom MK ein und ordnet diese der PKT-internen Speichstruktur zu. |
// |
// Hierbei ist nicht gewaehrleistet, dass die Daten von jedem BL |
// in einem festen Zeitrahmen ermittelt werden. Die Folge ist |
// ein 'zufaelliger' Aufbau der Anzeige der BL-Daten und auch |
// eine nicht exakt bestimmbare Refreshtime der Werte. |
// |
// Optimieren kann man ggf. die Refreshzeit aller BL-Werte durch |
// tx_intervall fuer Kommando "k" und durch BL_READ_TIME. |
// Brauchbare Startwerte sind (noch experimentell): |
// tx_interval = 5 (fuer "k") = 50 ms |
// BL_READ_TIME = 25 = 250 ms |
// |
// Wenn dieses Verhalten verbessert werden soll muss ggf. |
// die ISR() (usart.c) fuer Kommando "k" angepasst werden um |
// in einem Schwung mehrere gesendete BL-Pakete aufeinmal |
// aufzunehmen und als Paket zur Verfuegung zu stellen. |
//-------------------------------------------------------------- |
void OSD_MK_GetData( void ) |
{ |
BLData_t *rx_blData; |
uint8_t blIndex; |
uint8_t FCStatusFlags; |
uint8_t v; |
uint8_t skipBL = false; |
FCStatusFlags = naviData->FCStatusFlags; // save naviData->FCStatusFlags for use with BL-Data |
//###################################### |
//# ZEIT/DATUM |
//###################################### |
// lese UTC-Time vom MK |
//-------------------------------------- |
if( timer_get_tidata == 0 ) |
{ |
//-------------------------------------------------- |
// das 'T' Datenpaket der NC fuer OSD_MK_UTCTime() |
// gibt es erst ab NC v0.30g (!) |
// |
// --> Versionspruefung der NC-Firmware |
//-------------------------------------------------- |
v = MKVersion_Cmp( MKVersion.NCVer, 0,30,'g' ); // pruefe auf NC-Version >= "0.30g" |
if( v && (v >= GIVEN_VERSION) ) // wenn aktuelle NC-Version >= "0.30g"... |
{ |
if( !OSD_MK_UTCTime(20) ) |
timer_get_tidata = 50; // erfolglos: versuche es nach einer 1/2 Sekunde erneut |
else |
timer_get_tidata = TIME_GET_TIDATA; // alle 60 Sekunden refresh - den Rest uebernimmt ein Timer des PKT |
} |
} |
//###################################### |
//# MK-DISPLAY |
//###################################### |
// switch to: (h) |
//-------------------------------------- |
//if( timer_get_displaydata == 0 ) |
if( mkdisplayMode && timer_get_displaydata == 0 ) |
{ |
mode = 'H'; |
rxd_buffer_locked = FALSE; |
/* |
if( mkdisplayCmd != 0xff ) |
{ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 1, &mkdisplayCmd, 1); |
} |
*/ |
if( mkdisplayCmd == 0 ) mkdisplayCmd = 0xff; |
SendOutData( 'h', ADDRESS_ANY, 2, &mkdisplayCmd, 1, 0x00 ,1); // 05.04.2015 Cebra, 2.er Parameter wg NC 2.09i |
mkdisplayCmd = 0xff; // 0xff = aktuelle Seite |
timer = 20; while( (timer>0) && !rxd_buffer_locked ); // |
if( rxd_buffer_locked ) |
{ |
Decode64(); |
memcpy( mkdisplayData, (const void *)&rxd_buffer[3+ 0], 80 ); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterDISPLAY++; |
#endif // USE_OSD_SCREEN_DEBUG |
} |
timer_get_displaydata = TIME_GET_DISPLAYDATA; // n*10 ms |
skipBL = true; |
} // end: if( mkdisplayMode && timer_get_displaydata == 0 ) |
//###################################### |
//# BL-Daten |
//###################################### |
// switch to: 'BL Ctrl Status' (k) |
//-------------------------------------- |
if( !skipBL && timer_get_bldata == 0 ) |
{ |
mode = 'K'; // read: BL Ctrl Status |
rxd_buffer_locked = FALSE; |
timer = TIME_READ_BLDATA; // fuer x Zeit werden BL-Daten gelesen |
while( timer>0 ) // lese Daten-Pakete fuer die angegebene Zeit |
{ |
if( rxd_buffer_locked ) |
{ |
Decode64 (); |
rx_blData = (BLData_t *) (pRxData); |
// die BL-Daten kommen in beliebiger Reihenfolge an |
// Hier werden sie entsprechend ihres Index gesichert |
blIndex = rx_blData->Index; |
if( blIndex >= 0 && blIndex < OSD_MAX_MOTORS ) |
{ |
memcpy( &blData[blIndex], rx_blData, sizeof(BLData_t)); // sichern... |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterBL[blIndex]++; |
#endif // USE_OSD_SCREEN_DEBUG |
// Statistiken |
if( (blData[blIndex].Status & 0xf0) && (FCStatusFlags & FC_STATUS_MOTOR_RUN) ) // nur wenn BL/Motor vorhanden und Motoren laufen |
{ |
// BL Statistik: Anzahl empf. Datenpakete (fuer Mittelwert) |
Config.OSD_Statistic.BL[blIndex].count++; |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t) calc_avg( (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current, |
(int32_t)blData[blIndex].Current, |
(int32_t)Config.OSD_Statistic.BL[blIndex].count, |
100); |
// ALT |
// BL Statistik: Mittelwert: Strom (*100 um Rundungsfehler zu reduzieren) |
//avg = (int32_t)Config.OSD_Statistic.BL[blIndex].avg_Current; |
//avg = avg + (( ( (int32_t)blData[blIndex].Current * 100) - avg) / (int32_t)Config.OSD_Statistic.BL[blIndex].count); |
//Config.OSD_Statistic.BL[blIndex].avg_Current = (uint16_t)avg; |
// BL Statistik: Max: Strom |
if( blData[blIndex].Current > Config.OSD_Statistic.BL[blIndex].max_Current) Config.OSD_Statistic.BL[blIndex].max_Current = blData[blIndex].Current; |
// BL Statistik: Max: Temperatur |
if( blData[blIndex].Temperature > Config.OSD_Statistic.BL[blIndex].max_Temp) Config.OSD_Statistic.BL[blIndex].max_Temp = blData[blIndex].Temperature; |
if( blIndex+1 > Config.OSD_Statistic.BL_Count ) Config.OSD_Statistic.BL_Count = blIndex+1; |
} |
} |
rxd_buffer_locked = FALSE; |
} |
} |
timer_get_bldata = TIME_GET_BLDATA; // n*10 ms |
} // end: if( timer_get_bldata == 0 ) |
//-------------------------------------- |
// back to: OSD-Data |
//-------------------------------------- |
mode = 'O'; // read: OSD-Data |
rxd_buffer_locked = FALSE; // ready to receive new data |
timer_mk_timeout = MK_TIMEOUT; // reset osd MK_TIMEOUT timer |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void variobeep(int16_t vario) |
{ |
#ifdef USE_SOUND |
{ //start Beep |
if (vario >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
// set_beep ( 100, 0xffff, BeepNormal); |
duration = 52 -vario; |
// if (duration =0); duration = 1; |
// write_ndigit_number_u (0,6,duration,5,0); |
playTone(300+vario*2,duration,Config.Volume); |
// playTone(300,duration,volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
if (vario <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
duration = 50 -vario; |
// write_ndigit_number_u (0,7,duration,5,0); |
// if (duration < vario) ; duration = 0; |
// playTone(300,50,volume); |
playTone(300+vario*2,duration,Config.Volume); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (vario == 0) Vario_Threshold = 0; //Startverzögerung löschen |
} // end Beep |
#endif |
} |
//-------------------------------------------------------------- |
// Diese Funktion Beept unabhaengig von der Einstellung Config.OSD_VarioBeep |
// Aufruf ggf. mit: if( Config.OSD_VarioBeep ) Beep_Vario(); |
// |
// Ansonten: |
// -> hier noch aufräumen in Zusammenhang mit func variobeep() |
//-------------------------------------------------------------- |
void Beep_Vario(void) |
{ |
if ( (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) && (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL)) |
{ //start Beep |
if (naviData->Variometer <0) // MK fällt |
{ |
Vario_Beep_Up = 0; // Up Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Down == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 300, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Down++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Down == Vario_Beep_Down_Interval) Vario_Beep_Down = 0; |
} |
if (naviData->Variometer == 0) Vario_Threshold = 0; //Startverzögerung löschen |
if (naviData->Variometer >0 ) // MK steigt |
{ |
Vario_Beep_Down = 0; // Down Beep freischalten |
Vario_Threshold++; |
if ((Vario_Beep_Up == 0) && (Vario_Threshold >= Vario_Threshold_Value)) |
{ |
if (!Config.HWSound) set_beep ( 100, 0xffff, BeepNormal); |
else variobeep(naviData->Variometer); |
Vario_Threshold = Vario_Threshold_Value; // auf Maximalwert begrenzen |
} |
Vario_Beep_Up++; // Interval hochzählen in dem nicht gepiept wird |
if (Vario_Beep_Up == Vario_Beep_Up_Interval) Vario_Beep_Up = 0; |
} |
} // end Beep |
} |
//-------------------------------------------------------------- |
// Quelle Mikrokopter FC-Software Holger + Ingo |
//-------------------------------------------------------------- |
void CheckMKLipo( void ) |
{ |
if( Config.MK_LowBat < 50 ) // automatische Zellenerkennung |
{ |
if( CellIsChecked <= 2 ) // nur beim Start 1x prüfen |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE) |
break; |
} |
BattLowVoltageWarning = cells * Config.MK_LowBat; |
CellIsChecked++; |
} |
} |
else BattLowVoltageWarning = Config.MK_LowBat; |
if( naviData->UBat < BattLowVoltageWarning) |
{ |
if( AkkuWarnThreshold <= 4) |
{ |
AkkuWarnThreshold++; |
} |
else if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) |
{ |
// MK-Unterspannungs-Beep nur wenn Motoren laufen |
set_beep ( 1000, 0x0020, BeepSevere); |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void calc_heading_home(void) |
{ |
int16_t degree; |
//--------------------------------------- |
// warum modulo 360: |
// |
// die NC kann unter gewissen Umstaenden Winkel > 360 Grad |
// senden -> fragt H&I warum |
//--------------------------------------- |
degree = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
if( degree < 0 ) |
heading_home = (int16_t)(360 + degree); |
else |
heading_home = (int16_t)degree; |
} |
//-------------------------------------------------------------- |
// convert the <heading> gotton from NC into an index |
uint8_t heading_conv (uint16_t heading) |
{ |
if (heading > 23 && heading < 68) |
return 0; //direction = "NE"; |
else if (heading > 67 && heading < 113) |
return 1; //direction = "E "; |
else if (heading > 112 && heading < 158) |
return 2; //direction = "SE"; |
else if (heading > 157 && heading < 203) |
return 3; //direction = "S "; |
else if (heading > 202 && heading < 248) |
return 4; //direction = "SW"; |
else if (heading > 247 && heading < 293) |
return 5; //direction = "W "; |
else if (heading > 292 && heading < 338) |
return 6; //direction = "NW"; |
return 7; //direction = "N "; |
} |
//-------------------------------------------------------------- |
// draw a compass rose at <x>/<y> for <heading> |
void draw_compass (uint8_t x, uint8_t y, uint16_t heading, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t front = 19 + (heading / 22); |
for (uint8_t i = 0; i < 9; i++) |
lcdx_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// variometer |
// x, y in Pixel! |
//-------------------------------------------------------------- |
void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
lcd_rect (x, y - ((hight) / 2), width, hight, 1); |
lcd_frect (x + 1, y - ((hight) / 2) + 1, width - 2, hight - 2, 0); |
lcd_line (x, y, x + width, y, 1); |
if (variometer > 0) // steigend |
{ |
switch (variometer / 5) |
{ |
case 0: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
break; |
case 1: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
break; |
case 2: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
break; |
default: |
lcd_line (x + 4, y - 1, x + 6, y - 1, 1); // 1 > 4 |
lcd_frect (x + 3, y - 3, 4, 1, 1); // 5 > 9 |
lcd_frect (x + 2, y - 5, 6, 1, 1); // 10 > 14 |
lcd_frect (x + 1, y - 6, 8, 1, 1); // 15 > |
break; |
} |
} |
else if (variometer < 0) // fallend |
{ |
switch ((variometer) / -5) |
{ |
case 0: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
break; |
case 1: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
break; |
case 2: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
break; |
default: |
lcd_line (x + 4, y + 1, x + 6, y + 1, 1); // - 1 > - 4 |
lcd_frect (x + 3, y + 2, 4, 1, 1); // - 5 > - 9 |
lcd_frect (x + 2, y + 4, 6, 1, 1); // -10 > -14 |
lcd_frect (x + 1, y + 5, 8, 1, 1); // -15 > |
break; |
} |
} |
} |
//-------------------------------------------------------------- |
// variometer 2 |
// |
// x, y in Pixel |
// x, y top, left |
//-------------------------------------------------------------- |
/* |
void draw_variometer2( uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
{ |
uint8_t max = 5; // max: 5 m/sec == 100% |
lcd_rect (x, y, width, hight, 1); |
lcd_frect(x + 1, y + 1, width - 2, hight - 2, 0); |
lcd_line (x, y + ((hight) / 2), x + width, y + ((hight) / 2), 1); |
} |
*/ |
//-------------------------------------------------------------- |
// Home symbol |
// draw Homesymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_homesymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_plot (x,y-4,1); |
lcd_line (x+1,y-1,x+1,y-5,1); |
lcd_plot (x+2,y-6,1); |
lcd_plot (x+3,y-7,1); |
lcd_plot (x+4,y-6,1); |
lcd_line (x+5,y-1,x+5,y-5,1); |
lcd_plot (x+6,y-4,1); |
lcd_plot (x+3,y-1,1); |
lcd_plot (x+3,y-2,1); |
lcd_line (x+1,y,x+5,y,1); |
} |
//-------------------------------------------------------------- |
// Target symbol |
// draw Targetsymbol at <x>/<y> |
//-------------------------------------------------------------- |
void draw_targetsymbol (uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
lcd_circle (x+3, y-3, 4, 1); |
lcd_line (x,y-3,x+6,y-3,1); |
lcd_line (x+3,y,x+3,y-6,1); |
lcd_circle (x+3, y-3, 2, 1); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ) |
{ |
if (Altimeter > (300 / AltimeterAdjust) || Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
writex_ndigit_number_s ( x, y, (Altimeter / (30 / AltimeterAdjust)), 4, 0, mode, xoffs,yoffs); |
else // up to 10m write meters.dm |
writex_ndigit_number_s_10th( x, y, (Altimeter / (3 / AltimeterAdjust)), 3, 0, mode, xoffs,yoffs); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcd_o_circle (uint16_t x, uint16_t y, int16_t breite, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 2; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x+xoffs, y+yoffs, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcd_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//----------------------------------------------------------- |
//-------------------------------------------------------------- |
void lcdx_o_circle( uint8_t x, uint8_t y, int8_t width, uint8_t mode) |
{ |
int8_t radius = width / 2; |
// 04.03.2012 OG: chg: x-radius von -3 auf -2 (runder auf dem display) |
//lcd_ellipse (x, y, radius - 3, radius - 5, mode); |
lcd_ellipse( x, y, radius - 2, radius - 5, mode); |
} |
//----------------------------------------------------------- |
// lcd_o_circ_line( x, y, breite, deg, rOffset, mode) |
// |
// x, y : in Chars |
// breite : in Chars |
// deg : in Pixel |
// rOffset: Beeinflusst den Schluss der Linie zum Huellkreis |
// 0 = Standard |
// >0 naeher zum Huellkreis |
// <0 entfernter vom Huellkreis |
// mode : siehe: lcd_ellipse_line() |
//----------------------------------------------------------- |
void lcdx_o_circ_line( uint16_t x, uint16_t y, uint8_t breite, uint16_t deg, int8_t rOffset, uint8_t mode, int8_t xoffs, int8_t yoffs) |
{ |
breite *= 6; |
int16_t radius = breite / 3; |
x += 2; |
x *= 6; |
x += 2; |
y += 1; |
y *= 8; |
y += 3; |
// 04.03.2013 OG: der Radius kann mit rOffset >0 vergroessert werden um zum Kreis aufzuschliessen |
lcd_ellipse_line( x+xoffs, y+yoffs, radius+rOffset, radius+rOffset, deg, mode); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_mk( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: rund |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_round( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_ellipse( x+5, y+5, 5+1, 5, 1); // Aussenkreis |
//lcd_line( x+2, y+5, x+8, y+5, 1); // Innenkreuz Horizontal (laenger) |
lcd_line( x+3, y+5, x+7, y+5, 1); // Innenkreuz Horizontal (kuerzer) |
lcd_line( x+5, y+3, x+5, y+7, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// Variante: eckig |
// |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_target_diamond( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach oben Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach oben Rechts |
lcd_line( x+5, y+10, x+0, y+5, 1); // Dach unten Links |
lcd_line( x+5, y+10, x+10, y+5, 1); // Dach unten Rechts |
lcd_line( x+4, y+5, x+6, y+5, 1); // Innenkreuz Horizontal |
lcd_line( x+5, y+4, x+5, y+6, 1); // Innenkreuz Vertikal |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_home( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+5, 10, 8, 1); // Mitte |
lcd_line( x+5, y+0, x+0, y+5, 1); // Dach Links |
lcd_line( x+5, y+0, x+10, y+5, 1); // Dach Rechts |
lcd_rect( x+4, y+10, 2, 3, 1); // Tuere |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_sat( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+0, y+2, 4, 2, 1); // linker Fluegel |
lcd_rect( x+8, y+2, 4, 2, 1); // rechter Fluegel |
lcd_rect( x+4, y+0, 4, 6, 1); // Mitte, oben |
lcd_line( x+6, y+7, x+2, y+11, 1); // Strahl Links |
lcd_line( x+6, y+7, x+10, y+11, 1); // Strahl Rechts |
lcd_line( x+1, y+12, x+11, y+12, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_satmini2( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
//lcd_line( x+3, y+3, x+0, y+6, 1); // Strahl Links |
lcd_line( x+2, y-1, x+4, y-1, 1); // |
//lcd_line( x+2, y+0, x+4, y+0, 1); // |
lcd_line( x+0, y+0, x+6, y+0, 1); // |
lcd_line( x+0, y+1, x+6, y+1, 1); // |
lcd_line( x+2, y+2, x+4, y+2, 1); // |
lcd_plot( x+3, y+0, 0); // |
lcd_plot( x+3, y+1, 0); // |
lcd_line( x+3, y+3, x+1, y+5, 1); // Strahl Links |
lcd_line( x+3, y+3, x+5, y+5, 1); // Strahl Rechts |
lcd_line( x+0, y+6, x+6, y+6, 1); // Strahl Unten |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// x,y : in Pixel |
//-------------------------------------------------------------- |
void draw_icon_battery( uint8_t x, uint8_t y) |
{ |
//lcd_plot( x+0, y+0, 1); // Referenz 0,0 |
lcd_rect( x+2, y+0, 2, 2, 1); // der kleine Knubbel oben |
lcd_rect( x+0, y+2, 6, 15, 1); // body |
} |
//-------------------------------------------------------------- |
// RC symbol |
// alternatives Symbol fuer RC-Quality |
//-------------------------------------------------------------- |
void draw_symbol_rc( uint8_t x, uint8_t y) |
{ |
x *= 6; |
y *= 8; |
y += 1; |
x += 1; |
lcd_plot( x+3, y+4, 1); |
lcd_line( x+2, y+2, x+4, y+2, 1); |
lcd_line( x+1, y+0, x+5, y+0, 1); |
} |
//############################################################## |
//# spezielle Beeps |
//############################################################## |
//-------------------------------------------------------------- |
// Beep_Waypoint() |
// |
// Beep bei Waypoint Wechsel und wenn die WP-Liste abgearbeitet |
// ist |
//-------------------------------------------------------------- |
void Beep_Waypoint( void ) |
{ |
//----------------- |
// BEEP: WP erreicht |
//----------------- |
if( naviData->WaypointIndex > WP_old ) |
{ |
set_beep( 30, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( (naviData->WaypointIndex == naviData->WaypointNumber) |
&& (naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
&& (!WP_last) |
) |
{ |
set_beep( 400, 0xffff, BeepNormal ); // langer Bestaetigungs-Beep |
WP_last = true; |
} |
if( naviData->WaypointIndex != WP_old ) |
{ |
WP_old = naviData->WaypointIndex; |
} |
if( (naviData->WaypointIndex != naviData->WaypointNumber) // wenn aktueller WP != WP-Anzahl -> WP_last abschalten |
|| !(naviData->NCFlags & NC_FLAG_TARGET_REACHED) // wenn kein TR mehr an ist -> WP_last abschalten |
) |
{ |
WP_last = false; |
} |
// alter Code zur Orientierung |
//if( OldWP != naviData->WaypointIndex ) |
//{ |
// OldWP = naviData->WaypointIndex; |
// NextWP = true; |
//} |
// |
//if( (NextWP==true) && (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ) |
//{ |
// set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
// NextWP = false; |
//} |
} |
//############################################################## |
//# OSD-ELEMENTS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs, yoffs) |
// |
// xC, yC : x,y in Characters |
// item : OSD_FLAG_AH, OSD_FLAG_PH, usw. |
// lOn : true / false |
// xoffs,yoffs : x,y Pixelverschiebung |
//-------------------------------------------------------------- |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs) |
{ |
int8_t x = (xC*6)-2; |
int8_t y = (yC*8)-1; |
uint8_t w = 14; |
uint8_t h = 8; |
const char *labels[OSD_FLAG_COUNT] = |
{ |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!"), // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
}; |
//lcd_plot( x-2, y-2, 1); // Referenz |
if( yC==0 && yoffs<=0 ) { y = 0; h = 7; } |
if( xC==0 && xoffs<=0 ) { x = 0; w = 12; } |
if( lOn ) |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 1); // Filler |
lcdx_printp_at( xC, yC, labels[item], MINVERS, xoffs,yoffs); // Label |
} |
else |
{ |
lcd_frect( x+xoffs, y+yoffs, w, h, 0); // clear |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs) |
{ |
uint8_t lOn = 0; |
// FC_StatusFlags 0.88 |
switch( item ) |
{ |
// Altitue Hold |
case OSD_FLAG_AH : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL); |
break; |
// Position Hold |
case OSD_FLAG_PH : lOn = (naviData->NCFlags & NC_FLAG_PH); |
break; |
// Coming Home |
case OSD_FLAG_CH : lOn = (naviData->NCFlags & NC_FLAG_CH); |
break; |
// Care Free |
case OSD_FLAG_CF : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE); |
break; |
// Out1 |
case OSD_FLAG_O1 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE); |
break; |
// Out2 |
case OSD_FLAG_O2 : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE); |
break; |
// LowBat warning (MK) |
case OSD_FLAG_BA : lOn = (naviData->FCStatusFlags & FC_STATUS_LOWBAT); |
break; |
// Calibrate |
case OSD_FLAG_CA : lOn = (naviData->FCStatusFlags & FC_STATUS_CALIBRATE); |
break; |
// Start |
case OSD_FLAG_ST : lOn = (naviData->FCStatusFlags & FC_STATUS_START); |
break; |
// Motor Run |
case OSD_FLAG_MR : lOn = (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN); |
break; |
// Fly |
case OSD_FLAG_FY : lOn = (naviData->FCStatusFlags & FC_STATUS_FLY); |
break; |
// Emergency Landing |
case OSD_FLAG_EL : lOn = (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING); |
break; |
// RC Failsave Active |
case OSD_FLAG_FS : lOn = (naviData->FCStatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE); |
break; |
// GPS ok |
case OSD_FLAG_GP : lOn = (naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// GPS-Sat not ok (GPS NOT ok) |
case OSD_FLAG_S0 : lOn = !(naviData->NCFlags & NC_FLAG_GPS_OK); |
break; |
// Vario Trim Up |
case OSD_FLAG_TU : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_UP); |
break; |
// Vario Trim Down |
case OSD_FLAG_TD : lOn = (naviData->FCStatusFlags & FC_STATUS_VARIO_TRIM_DOWN); |
break; |
// Free |
case OSD_FLAG_FR : lOn = (naviData->NCFlags & NC_FLAG_FREE); |
break; |
// Range Limit |
case OSD_FLAG_RL : lOn = (naviData->NCFlags & NC_FLAG_RANGE_LIMIT); |
break; |
// No Serial Link |
case OSD_FLAG_SL : lOn = (naviData->NCFlags & NC_FLAG_NOSERIALLINK); |
break; |
// Target Reached |
case OSD_FLAG_TR : lOn = (naviData->NCFlags & NC_FLAG_TARGET_REACHED); |
break; |
// Manual Control |
case OSD_FLAG_MC : lOn = (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL); |
break; |
} |
OSD_Element_Flag_Label( xC, yC, item, lOn, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
// Anzeige von Steigen / Sinken |
//-------------------------------------------------------------- |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs) |
{ |
x = (x*6) + xoffs; |
y = (y*8) + yoffs; |
lcd_frect( x, y, 6, 7, 0); // clear |
if( naviData->Variometer > 2 ) // steigen mehr als 0.2 m/sec (ein guter Wert muss noch in der Praxis ermittelt werden) |
{ |
lcd_line( x+2, y+0, x+0, y+2, 1); |
lcd_line( x+2, y+0, x+4, y+2, 1); |
} |
else if( naviData->Variometer < -2 ) // sinken mehr als 0.2 m/sec |
{ |
lcd_line( x+2, y+6, x+0, y+4, 1); |
lcd_line( x+2, y+6, x+4, y+4, 1); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Altitude( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
switch( nStyle ) |
{ |
case 0 : |
case 1 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
break; |
case 2 : //note:lephisto:according to several sources it's /30 |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x+4, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MBIG); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x+4, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MBIG); |
lcd_putc( x+8, y, 'm', MBIG); |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
break; |
} |
#else |
if( nStyle == 2 ) |
{ |
lcd_printp_at (x, y, PSTR("Höhe"), MNORMAL); |
x += 4; |
} |
else |
{ |
lcd_putc (x+5, y, 0x09, 0); // Hoehensymbol |
} |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s (x, y, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th (x, y, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0,MNORMAL); |
lcd_printp_at (x+4, y, PSTR("m"), MNORMAL); |
#endif |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==1 |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ) |
{ |
uint16_t Balken = 0; |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); |
if( cells > 0 ) // LipobargraphAnzeige nur wenn Anzahl der Lipozellen bekannt sind |
{ |
write_ndigit_number_u (x+6, y, cells, 1, 0, drawmode); |
lcd_printp_at (x+7, y, PSTR("S"), drawmode); |
if( cells==3 ) |
{ |
lcd_rect(x*6, y*8, 28, 7, 1); // Rahmen |
Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/12; |
if((Balken > 0) && (Balken <28)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if(Balken <= 26) lcd_frect(Balken+(x*6)+1, (y*8)+1, 26-Balken, 5, 0); // loeschen |
} |
if( cells==4 ||cells==5 ) |
{ |
lcd_rect(x*6, y*8, 30, 7, 1); // Rahmen |
if (cells == 4) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/15; |
if (cells == 5) Balken = ((naviData->UBat-(cells*MIN_CELL_VOLTAGE))*10)/19; |
if ((Balken > 0) && (Balken <=29)) lcd_frect((x*6)+1, (y*8)+1, Balken, 5, 1); // Fuellung |
if (Balken <= 27) lcd_frect(Balken+(x*6)+1, (y*8)+1, 28-Balken, 5, 0); // loeschen |
} |
} // end if: cells > 0 (TODO: Anzeige fuer cells==0 implementieren) |
} |
//-------------------------------------------------------------- |
// fuer die neuen OSD-Screens |
//-------------------------------------------------------------- |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
drawmode = (naviData->UBat < BattLowVoltageWarning ? MINVERS : MNORMAL); |
if( Config.OSD_ShowCellU ) |
{ |
// kalk. Einzelzellen Spannungsanzeige |
writex_ndigit_number_u_100th( x, y, (uint16_t)((naviData->UBat*10)/cells), 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("v"), drawmode, xoffs+1,yoffs); // Einheit (Einzelzellenanzeige ca., berechnet) |
} |
else |
{ |
// Gesamtspannungsanzeige |
writex_ndigit_number_u_10th( x, y, naviData->UBat, 3, 0, drawmode, xoffs,yoffs); |
lcdx_printp_at( x+4, y, PSTR("V"), drawmode, xoffs+1,yoffs); // Einheit (Gesamtspannung) |
} |
lcd_line( (x+4)*6, y*8, (x+4)*6, y*8+7, (drawmode==MINVERS ? 1 : 0) ); // filler zwischen Spannung und "V" |
} |
//-------------------------------------------------------------- |
// fuer: Config.OSD_LipoBar==0 |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
#ifdef USE_FONT_BIG |
if( nStyle <= 1) |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 4 : 3); // Fett-Schrift |
if( nStyle <= 1) |
{ |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
} |
else |
{ |
write_ndigit_number_u_10th (x-2, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR("V"), drawmode); |
} |
#else |
drawmode = (naviData->UBat < BattLowVoltageWarning ? 2 : 0); // Normal-Schrift |
if( nStyle == 2) |
x += 3; |
write_ndigit_number_u_10th (x, y, naviData->UBat, 3, 0, drawmode); |
lcd_printp_at (x+4, y, PSTR("V"), drawmode); |
#endif |
} |
//-------------------------------------------------------------- |
// OSD_Element_Battery_Bar( x, y, length, width, orientation) |
// |
// neue Variante (OG 06/2013) |
// |
// Parameter: |
// length : Gesamtlaenge des Bar's |
// width : Breite in Pixel (fest) |
// orientation: ORIENTATION_V - Vertikal (x,y oberer,linker Punkt) |
// ORIENTATION_H |
//-------------------------------------------------------------- |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation) |
{ |
uint8_t bat_umax; |
uint8_t bat_umin; |
uint8_t bat_uact; |
int8_t bat_p; |
// die Min/Max Spannungswerte sind jetzt erstmal fest vorgegeben bzw. berechnet, eine individuelle |
// Anpassung waere moeglich aber wenn besser ist es, wenn der User das nicht machen muesste... |
bat_umax = cells * 42; |
//bat_umin = cells * 32; |
bat_umin = cells * 34; // 3.4 Volt pro Zelle Minimum |
bat_uact = naviData->UBat; |
bat_p = length * (bat_uact-bat_umin) / (bat_umax-bat_umin); |
if( bat_p < 0 ) bat_p = 0; |
if( bat_p > length ) bat_p = length; |
if( width == 1 ) width = 0; // eine kleine Eigenart von frect um es zu ueberreden eine Linie zu zeichnen |
if( orientation == ORIENTATION_V ) |
{ |
lcd_frect( x, y , width, length-bat_p, 0); // clear: vertical |
lcd_frect( x, y+length-bat_p, width, bat_p, 1); // draw: vertical |
} |
else |
{ |
lcd_frect( x+bat_p, y, length-bat_p, width, 0); // clear: |
lcd_frect( x , y, bat_p , width, 1); // draw: |
} |
} |
//-------------------------------------------------------------- |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( Config.OSD_LipoBar ) |
OSD_Element_BatteryLevel_Bar( x, y); |
else |
OSD_Element_BatteryLevel_Text( x, y, nStyle); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Capacity( uint8_t x, uint8_t y ) |
{ |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? 2 : 0); |
write_ndigit_number_u (x, y, naviData->UsedCapacity, 5, 0, drawmode); |
lcd_printp_at (x+5, y, PSTR("mAh"), drawmode); |
// BeepTime = 3000; |
// BeepMuster = 0x0020; |
} |
//-------------------------------------------------------------- |
// OSD_Element_CompassDegree( x, y, nStyle) |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
x += 3; |
break; |
case 2 : |
#ifdef USE_FONT_BIG |
write_ndigit_number_u (x, y, (naviData->CompassHeading)%360, 3, 0,MBIG); |
#else |
write_ndigit_number_u (x+5, y, (naviData->CompassHeading)%360, 3, 0,MNORMAL); |
#endif |
x += 8; |
break; |
} |
lcd_putc( x, y, 0x1E, MNORMAL); // degree symbol |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ) |
{ |
lcdx_printp_at (x, y, (const char *) (pgm_read_word ( &(directions_p[heading_conv((naviData->CompassHeading)%360)]))), MNORMAL, xoffs,yoffs); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ) |
{ |
draw_compass (x, y, (naviData->CompassHeading)%360, 0,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_Current( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u_10th (x, y, naviData->Current, 3, 0,0); |
lcd_printp_at (x+4, y, PSTR("A"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ) |
{ |
write_time (x, y, naviData->FlyingTime); |
lcd_printp_at (x+5, y, PSTR("m"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,0); |
lcd_printp_at (x+3, y, PSTR("Kmh"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ) |
{ |
calc_heading_home(); |
lcd_o_circle( x, y, breite, 1, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (old_hh + 180), rOffset, 0, xoffs,yoffs); |
lcd_o_circ_line( x, y, breite, (heading_home + 180), rOffset, 1, xoffs,yoffs); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
// NEU! 22.04.2014 OG |
// soll das alte OSD_Element_HomeCircle() komplett ersetzen wenn |
// die alten OSD-Screens endgueltig rausfliegen |
//-------------------------------------------------------------- |
void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
{ |
calc_heading_home(); |
lcd_ellipse_line( px, py, rx, ry, (old_hh + 180), 0); |
lcd_ellipse_line( px, py, rx, ry, (heading_home + 180), 1); |
lcd_ellipse( px, py, rx, ry, 1); |
old_hh = heading_home; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, heading_home, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//-------------------------------------------------------------- |
// OSD_Element_HomeDistance( x, y, nStyle) |
//-------------------------------------------------------------- |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_homesymbol(x+4, y); |
break; |
case 2 : lcd_printp_at (x, y, PSTR("Home"), 0); |
write_ndigit_number_u (x+5, y, naviData->HomePositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_LEDOutput( x, y, bitmask) |
// |
// bitmask: LED1 = FC_STATUS2_OUT1_ACTIVE |
// LED2 = FC_STATUS2_OUT2_ACTIVE |
//-------------------------------------------------------------- |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ) |
{ |
uint8_t lOn; |
lOn = (naviData->FCStatusFlags2 & bitmask ? 1 : 0); // Bit gesetzt? |
lOn = (Config.OSD_InvertOut ? !lOn : lOn); // Invertieren? |
lOn = (lOn ? 1 : 0); // auf 0 oder 1 setzen |
lcd_fcircle (x * 6 + 5, y * 8 + 3, Config.OSD_LEDform, lOn); |
lcd_circle (x * 6 + 5, y * 8 + 3, 3, 1); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED1Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT1_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_LED2Output( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ) |
{ |
OSD_Element_LEDOutput( x, y, FC_STATUS2_OUT2_ACTIVE ); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Manuell( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Manuell( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL) |
lcd_putc (x, y, 'M', 0); // rc transmitter |
else |
lcd_putc (x, y, 'X', 0); // clear |
} |
//-------------------------------------------------------------- |
// OSD_Element_RCIntensity( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->RC_Quality, 3, 0,0); |
//lcd_printp_at (x+3, y, PSTR("\x1F"), 0); // RC-transmitter |
if (naviData->NCFlags & NC_FLAG_NOSERIALLINK) |
{ |
lcd_printpns_at(x+3, y, PSTR(" "), 0); // Clear |
} |
else |
{ |
lcd_printpns_at(x+3, y, PSTR("PC"), 0); |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_SatsInUse( x, y, nStyle) |
// |
// nStyle == 0: "00s" |
// nStyle == 1: wie 0 |
// nStyle == 2: "00 Sat" |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
//-------------------------------------------------------------- |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
drawmode = (naviData->NCFlags & NC_FLAG_GPS_OK ? 0 : 2); |
switch( nStyle ) |
{ |
case 0 : |
case 1 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_putc (x+2, y, 0x08, drawmode); |
break; |
case 2 : write_ndigit_number_u (x, y, naviData->SatsInUse, 2, 0, drawmode); |
lcd_printp_at (x+2, y, PSTR(" Sat"), drawmode); |
break; |
} |
} |
//-------------------------------------------------------------- |
// OSD_Element_Variometer( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_Variometer( uint8_t x, uint8_t y ) |
{ |
x *= 6; |
y *= 8; |
y += 7; |
draw_variometer (x, y, 10, 14, naviData->Variometer); |
} |
//-------------------------------------------------------------- |
// OSD_Element_Target( x, y, nStyle) |
// |
// nStyle entspricht dem ehemaligen 'Mode' |
// nStyle = 0,1: "000m" |
// nStyle = 2,3: "Ziel 000m -" |
//-------------------------------------------------------------- |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ) |
{ |
if( nStyle <= 1 ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_putc (x+3, y, 'm', 0); |
draw_targetsymbol(x+4,y); |
} |
else |
{ |
lcd_printp_at (x, y, PSTR("Ziel"), 0); |
write_ndigit_number_u (x+5, y, naviData->TargetPositionDeviation.Distance / 10, 3, 0,0); |
lcd_printp_at (x+8, y, PSTR("m -"), 0); |
} |
} |
//-------------------------------------------------------------- |
// TODO: |
// - pruefen ob beep hier an richtiger Stelle ist |
//-------------------------------------------------------------- |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ) |
{ |
uint8_t FC_Fallspeed; |
FC_Fallspeed = (unsigned int)naviData->Variometer; |
FC_Fallspeed = 255-FC_Fallspeed; |
drawmode = ( (naviData->Variometer < 0) && (FC_Fallspeed > Config.OSD_Fallspeed) ? 2 : 0); |
if( Config.OSD_VarioBeep ) |
Beep_Vario(); // Beep ??? |
if( drawmode == 2 ) |
{ |
if( !Config.HWSound ) |
set_beep ( 1000, 0x0060, BeepNormal); // muss ein Beep hier hin???? |
else |
variobeep(naviData->Variometer); // muss ein Beep hier hin???? |
} |
write_ndigit_number_s_10th (x, y, naviData->Variometer, 3,0, drawmode); |
lcd_printpns_at(x+4, y, PSTR("ms"), drawmode); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ) |
{ |
if (!OldWP == naviData->WaypointIndex) |
{ |
// BeepTime = 500; |
// BeepMuster = 0x0080; |
OldWP = naviData->WaypointIndex; |
NextWP = true; |
} |
if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
{ |
set_beep ( 500, 0x0080, BeepNormal); |
NextWP = false; |
} |
write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
lcd_printp_at (x, y, PSTR("WP"), 0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ) |
{ |
write_ndigit_number_u (x, y, naviData->TargetPositionDeviation.Bearing/ 10, 3, 0,0); |
lcd_putc (x+3, y, 0x1e, 0); // degree symbol |
} |
//############################################################## |
//# OSD-SCREENS |
//############################################################## |
//-------------------------------------------------------------- |
// OSD-Screen "General" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
void OSD_Screen_General( void ) |
{ |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// Display: 128 x 64 with 6x8 Font => 21 x 8 |
// Linien: Horizontal |
lcd_line (0, 28, 127, 28, 1); // mitte |
lcd_line (0, 51, 127, 51, 1); // unten |
// Linien: Vertikal |
lcd_line (65, 0, 65, 50, 1); // mitte |
//----------------------------------------- |
// Block: Oben - Links |
//----------------------------------------- |
draw_icon_battery(0,4); |
lcdx_printp_at( 7, 2, PSTR(" mA"), MNORMAL, 0,2); |
//----------------------------------------- |
// Block: Oben - Rechts |
//----------------------------------------- |
lcdx_printp_at( 12, 0, PSTR("Alt:") , MNORMAL, 0,0); |
lcdx_printp_at( 12, 1, PSTR("Dir:") , MNORMAL, 0,1); |
lcdx_putc( 20, 1, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
lcdx_printp_at( 12, 2, PSTR(" I:") , MNORMAL, 0,2); |
lcdx_putc( 20, 2, 'A', MNORMAL, 2,2); |
//----------------------------------------- |
// Block: Unten - Links |
//----------------------------------------- |
draw_icon_sat(0,33); |
lcdx_printp_at( 6, 5, PSTR(" kmh"), MNORMAL, 0,1); |
//----------------------------------------- |
// Block: Unten - Rechts |
//----------------------------------------- |
draw_icon_home( 70, 32); |
lcdx_putc( 20, 4, 'm', MNORMAL, 2,0); |
lcdx_putc( 20, 5, SYMBOL_SMALLDEGREE, MNORMAL, 1,1); |
//----------------------------------------- |
// unterste Zeile |
//----------------------------------------- |
draw_symbol_rc( 20, 7); // RC-transmitter |
//lcdx_putc( 20, 7, SYMBOL_RCQUALITY, MNORMAL, 2,0); |
} |
//----------------- |
// Batt Level (Graph) |
//----------------- |
OSD_Element_Battery_Bar( 2, 8, 11, 2, ORIENTATION_V); |
//----------------- |
// Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 2, 0, 0,0 ); |
//----------------- |
// LowBat Warnung MK |
//----------------- |
OSD_Element_Flag( 8, 0, OSD_FLAG_BA, 0,0 ); |
//----------------- |
// Flugzeit |
//----------------- |
writex_time(2, 1, naviData->FlyingTime, MNORMAL, 0,1); |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 2, 2, naviData->UsedCapacity, 5, 0, drawmode, 0,2); |
//----------------- |
// Höhe |
//----------------- |
writex_altimeter( 16, 0, naviData->Altimeter, MNORMAL, 0,0 ); |
/* |
if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust)) // above 10m only write full meters |
write_ndigit_number_s ( 16, 0, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0, MNORMAL); |
else // up to 10m write meters.dm |
write_ndigit_number_s_10th( 16, 0, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0, MNORMAL); |
*/ |
//----------------- |
// steigen / sinken |
//----------------- |
OSD_Element_UpDown( 20, 0, 2,0); |
//----------------- |
// Compass Degree |
//----------------- |
writex_ndigit_number_u (17, 1, (naviData->CompassHeading)%360, 3, 0,MNORMAL, 0,1); |
//----------------- |
// Strom |
//----------------- |
//write_ndigit_number_u_10th( 16, 2, naviData->Current, 3, 0,0); // alternativ mit Nachkomma |
writex_ndigit_number_u( 17, 2, naviData->Current/10, 3, 0,MNORMAL, 0,2); |
//----------------- |
// Sat Anzahl |
//----------------- |
write_ndigit_number_u (4, 4, naviData->SatsInUse, 2, 0,MNORMAL); |
//----------------- |
// Sat Warnung "!" |
//----------------- |
/* |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) |
lcd_printp_at( 9, 4, PSTR(" "), MNORMAL); |
else |
lcd_printp_at( 9, 4, PSTR("!"), MNORMAL); |
*/ |
OSD_Element_Flag( 8, 4, OSD_FLAG_S0, -1,0 ); // Sat Warnung (GPS not ok) |
//----------------- |
// Geschwindigkeit |
//----------------- |
writex_ndigit_number_u( 3, 5, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL, 0,1); |
//----------------- |
// Home Distance |
//----------------- |
write_ndigit_number_u( 17, 4, naviData->HomePositionDeviation.Distance / 10, 3, 0,MNORMAL); |
//----------------- |
// Home Winkel |
//----------------- |
writex_ndigit_number_u( 16, 5, heading_home, 4, 0,MNORMAL, 0,1); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 1, 7, OSD_FLAG_CF, 0,0 ); // Care Free |
OSD_Element_Flag( 4, 7, OSD_FLAG_AH, 0,0 ); // Altitude Hold |
OSD_Element_Flag( 7, 7, OSD_FLAG_PH, 0,0 ); // Position Hold |
OSD_Element_Flag( 10, 7, OSD_FLAG_CH, 0,0 ); // Coming Home |
OSD_Element_Flag( 13, 7, OSD_FLAG_EL, 0,0 ); // Emergency Landing |
//----------------- |
// RC-Quality (MK) |
//----------------- |
write_ndigit_number_u( 17, 7, naviData->RC_Quality, 3, 0,MNORMAL); |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 8, 0, OSD_FLAG_BA, true, 0,0 ); // DEMO: Batterie Warnung |
OSD_Element_Flag_Label( 8, 4, OSD_FLAG_S0, true, -1,0 ); // DEMO: Sat Warnung (GPS not ok) |
OSD_Element_Flag_Label( 1, 7, OSD_FLAG_CF, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 4, 7, OSD_FLAG_AH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 7, 7, OSD_FLAG_PH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 10, 7, OSD_FLAG_CH, true, 0,0 ); // DEMO |
OSD_Element_Flag_Label( 13, 7, OSD_FLAG_EL, true, 0,0 ); // DEMO |
#endif |
} |
//-------------------------------------------------------------- |
// OSD-Screen "Navigation" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
void OSD_Screen_Navigation( void ) |
{ |
int8_t xoffs, yoffs; |
int16_t degree; |
uint8_t minus; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcdx_printp_at( 0, 2, PSTR("Alt:"), MNORMAL, 0,2); // Hoehe |
lcdx_printp_at( 0, 5, PSTR("Home:"), MNORMAL, 0,3); // Home Distance |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Hoehe |
//----------------- |
xoffs = 0; |
yoffs = 3; |
writex_altimeter( 0, 3, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
//----------------- |
// Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 4, 3, 1,yoffs); |
//----------------- |
// Home Distance |
//----------------- |
yoffs = 3; |
writex_ndigit_number_u( 0, 6, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, 0,yoffs+1); |
lcdx_printp_at( 4, 6, PSTR("m"), MNORMAL, 2,yoffs+1); // Home |
//----------------- |
// Home Circle |
//----------------- |
//void OSD_Element_HomeCircleX( uint8_t px, uint8_t py, uint8_t rx, int8_t ry ) |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( 64, 38, 25, 22 ); // entspricht dem ehemaligem Huellkreis |
OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
lcd_frect( (9*6)-3+xoffs, (4*8)-2+yoffs, (3*6)+4, (1*8)+2, 0); // inner clear |
lcd_rect ( (9*6)-4+xoffs, (4*8)-3+yoffs, (3*6)+6, (1*8)+4, 1); // inner rect |
lcd_frect( 61+xoffs, 57+yoffs, 2, 2, 1); // bottom mini rect |
degree = (int16_t)heading_home; |
minus = false; |
if( degree >= 180 ) degree = 360 - degree; |
else minus = true; |
writex_ndigit_number_u( 9, 4, degree, 3, 0,MNORMAL, xoffs+1,yoffs); // Degree (Winkel) |
if( minus && degree != 0 ) |
lcd_line( (9*6)-2+xoffs, (4*8)+3+yoffs, (9*6)-1+xoffs, (4*8)+3+yoffs, 1); |
//----------------- |
// Variometer |
//----------------- |
// OG: Variometer wird erstmal nicht angezeigt weil es den Screen zu voll macht |
// wenn doch muessen erst grafische Anpassungen an den Variometer-Code gemacht |
// werden |
//void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer) |
//draw_variometer( 95, 38, 7, 30, naviData->Variometer); |
//draw_variometer( 94, 38, 7, 21, naviData->Variometer); |
//draw_variometer2( 94, 28, 7, 21, naviData->Variometer); |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3, 0); // MK Batt Warning |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0, 0); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0,-3); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0,-6); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0,-1); // Coming Home |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3,-1); // GPS-Sat not ok (GPS NOT ok) |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3,0); // DEMO |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0,0); // DEMO |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0,-3); // DEMO |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0,-6); // DEMO |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0,-1); // DEMO |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3,-1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_NAVIGATION |
//-------------------------------------------------------------- |
// OSD-Screen "OSD_Screen_Waypoints_OLD" |
// |
// alte, alternative Variante - wenn sich die neue Variante |
// durchsetzt kann das hier geloescht werden |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
/* |
void OSD_Screen_Waypoints_OLD( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, 127, 12+25, 1); // Trennlinie in der Mitte |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Kompass Rose |
//----------------- |
//draw_compass( 6, 0, (naviData->CompassHeading)%360, 2,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 6; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+2,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -2,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
xoffs = 2; |
yoffs = 3; |
//OSD_Element_HomeCircleX( px, py, rx, ry ) |
//OSD_Element_HomeCircleX( 64, 38, 26, 22 ); // leicht erweiterter Huellkreis |
OSD_Element_HomeCircleX( 112, 24, 10, 8 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-1, 33, 2, 1, 1); // bottom mini rect |
lcd_frect( 112-1, 33, 2, 0, 1); // bottom mini rect |
lcd_frect( 112-0, 33, 0, 1, 1); // bottom mini rect |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 6; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+2,yoffs); |
uint8_t wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//----------------- |
// Sat Anzahl |
//----------------- |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, 0,2); |
draw_icon_satmini( 117, 42); |
//----------------- |
// WP: Distance |
//----------------- |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+2,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// Oben: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
// Variante: TR neben Waypoint-Anzahl |
//OSD_Element_Flag( 13, 5, OSD_FLAG_TR, 6,2); // Target Reached |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
*/ |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "Waypoints" |
// |
// aktuelle Variante! |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
void OSD_Screen_Waypoints( void ) |
{ |
int8_t xoffs, yoffs; |
uint8_t v; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
//lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
lcd_rect_round( 0, 12, 127, 63-19+7, 1, R2); // Rahmen |
lcd_line( 0, 12+25, (15*6+5), 12+25, 1); // Trennlinie in der Mitte |
lcd_line( (15*6+5), 12+25, (15*6+5), 63, 1); // Linie Vertikal rechts, unten |
lcd_plot ((15*6+5), 12+25, 0); |
draw_icon_home( 7, 18); |
draw_icon_target_round( 7, 45); |
//draw_icon_target_diamond( 7, 45); // Alternative fuer draw_icon_target_round() |
} |
//----------------- |
// Oben,Links: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben,Links: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben,Rechts: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Oben,Mitte: Flags |
//----------------- |
// Variante 1: PH, CH, CF |
//OSD_Element_Flag( 7, 0, OSD_FLAG_PH, -2,1); // Position Hold |
//OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
//OSD_Element_Flag( 12, 0, OSD_FLAG_CF, 6,1); // Carefree |
// Variante 2: CF, CH, TR |
OSD_Element_Flag( 7, 0, OSD_FLAG_CF, -2,1); // Carefree |
OSD_Element_Flag( 9, 0, OSD_FLAG_CH, 5,1); // Coming Home |
OSD_Element_Flag( 12, 0, OSD_FLAG_TR, 6,1); // Target Reached |
//----------------- |
// Home: Hoehe |
//----------------- |
xoffs = 5; |
yoffs = 0; |
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
lcdx_printp_at( 3, 2, PSTR("Al:"), MNORMAL, xoffs+3,yoffs); |
//----------------- |
// Home: Steigen / Sinken |
//----------------- |
OSD_Element_UpDown( 13, 2, -3,yoffs); |
//----------------- |
// Home: Distance |
//----------------- |
yoffs = 2; |
lcdx_printp_at( 3, 3, PSTR("Ho:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 3, naviData->HomePositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at(11, 3, PSTR("m"), MNORMAL, xoffs+4,yoffs); // Home |
//----------------- |
// Home: Circle |
//----------------- |
// orginal |
//OSD_Element_HomeCircleX( 110, 25, 11, 9 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-3, 35, 2, 1, 1); // bottom mini rect |
// 1 pixel weiter links |
OSD_Element_HomeCircleX( 109, 25, 11, 9 ); // leicht erweiterter Huellkreis |
lcd_frect( 112-4, 35, 2, 1, 1); // bottom mini rect |
// etwas groesser |
//OSD_Element_HomeCircleX( 109, 26, 12, 10 ); // leicht erweiterter Huellkreis |
//lcd_frect( 112-4, 37, 2, 1, 1); // bottom mini rect |
//----------------- |
// Home: Sat Anzahl |
//----------------- |
//yoffs = -2; // alternativ |
yoffs = 0; |
//naviData->SatsInUse = 10; |
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,MNORMAL, -1,2+yoffs); |
draw_icon_satmini( 115, 42+yoffs); |
//----------------- |
// WP: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
xoffs = 5; |
yoffs = 2; |
lcdx_printp_at( 3, 5, PSTR("WP:"), MNORMAL, xoffs+3,yoffs); |
v = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) v = 0; |
lcdx_printf_at_P( 7, 5, MNORMAL, xoffs,yoffs, PSTR("%2d/%2d"), v, naviData->WaypointNumber ); |
//----------------- |
// WP: Countdown Target-Holdtime |
//----------------- |
xoffs = 6; |
yoffs = 2; |
v = naviData->TargetHoldTime; |
if( v > 0 ) |
{ |
lcdx_printf_at_P( 12, 5, MINVERS, xoffs+2,yoffs, PSTR("%2d"), v ); |
lcd_line( (12*6)+xoffs+2, (5*8)+yoffs-1, (12*6)+xoffs+2+11, (5*8)+yoffs-1, 1); |
lcd_line( (12*6)+xoffs+1, (5*8)+yoffs-1, (12*6)+xoffs+1, (5*8)+yoffs+7, 1); |
} |
else |
{ |
lcd_frect( (12*6)+xoffs+1, (5*8)+yoffs-1, (2*8)-3, 8, 0); |
} |
//----------------- |
// WP: Distance |
//----------------- |
xoffs = 5; |
yoffs = 4; |
lcdx_printp_at( 3, 6, PSTR("Di:"), MNORMAL, xoffs+3,yoffs); |
writex_ndigit_number_u( 7, 6, naviData->TargetPositionDeviation.Distance / 10, 4, 0,MNORMAL, xoffs,yoffs); |
lcdx_printp_at( 11, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// WP: Hoehe |
// Anmerkung OG: macht nicht so wirklich Sinn denke ich |
//----------------- |
//writex_ndigit_number_u( 14, 6, naviData->TargetPosition.Altitude / 1000, 4, 0,MNORMAL, xoffs,yoffs); |
//lcdx_printp_at( 18, 6, PSTR("m"), MNORMAL, xoffs+4,yoffs); // |
//----------------- |
// ggf. BEEP wenn WP erreicht |
//----------------- |
Beep_Waypoint(); |
} |
#endif // USE_OSD_SCREEN_WAYPOINTS |
//-------------------------------------------------------------- |
// OSD-Screen "User GPS" |
// |
// OSDScreenRefresh: OSD_SCREEN_REFESH = update values |
// OSD_SCREEN_REDRAW = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_USERGPS |
void OSD_Screen_UserGPS( void ) |
{ |
uint8_t y, i; |
int8_t yoffs; |
uint8_t show_gps_altitude = 0; // 1=GPS-Höhe anzeigen; 0=barymetrische Höhe anzeigen |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 9, 127, 63-9, 1, R2); // Rahmen |
lcdx_printp_at( 6, 0, PSTR("UGPS"), MNORMAL, 2,0); |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Sat Ok |
//----------------- |
OSD_Element_Flag( 12, 0, OSD_FLAG_S0, 5,0); // GPS-Sat not ok (GPS NOT ok) |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// die letzen 3 User-GPS Daten anzeigen |
//----------------- |
yoffs = -4; |
for(i=0; i<3; i++) |
{ |
y = (i*2)+2; |
lcd_frect( 0, (y*8)+yoffs-1, 127, 7, 1); // inverser Hintergrund |
writex_ndigit_number_u( 0, y+0, (i+1), 1, 0 , MINVERS, 4,yoffs); // Index |
writex_datetime_time( 4, y+0, Config.GPS_User[i].timestamp, MINVERS, 0,yoffs); |
//writex_time( 3, y+0, GPS_User[i].time_pkt, MINVERS, 0,yoffs); |
//lcdx_printp_at( 12, y+0, PSTR("Alt:") , MINVERS, 0,yoffs); |
lcdx_printp_at( 20, y+0, PSTR("m") , MINVERS, 0,yoffs); |
if( show_gps_altitude ) // GPS-Hoehe oder barymetrische Hoehe? |
{ |
// GPS Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("G"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_ndigit_number_s( 16, y+0, Config.GPS_User[i].GPSData.Altitude/1000, 4, 0, MINVERS, 0,yoffs); // GPS Hoehe in Meter |
} |
else |
{ |
// barymetrische Hoehe |
//lcdx_printp_at( 10, y+0, PSTR("B"), MINVERS, 2,yoffs); |
//lcd_frect( (12*6)-4, (y*8)+3+yoffs, 1, 1, 0); |
writex_altimeter( 16, y+0, Config.GPS_User[i].Altimeter, MINVERS, 0,yoffs ); |
} |
lcd_line( 1, (y*8)+yoffs+7, 125, (y*8)+yoffs+7, 0); // clear: Invers unterste Linie |
writex_gpspos( 1, y+1, Config.GPS_User[i].GPSData.Latitude , MNORMAL, 0,yoffs); // GPS Lat |
writex_gpspos( 12, y+1, Config.GPS_User[i].GPSData.Longitude, MNORMAL, 0,yoffs); // GPS Long |
//writex_gpspos( 1, y+1, GPS_User[i].GPSData.Latitude + 12867000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
//writex_gpspos( 12, y+1, GPS_User[i].GPSData.Longitude+ 8568000, MNORMAL, 0,yoffs); // DUMMY DATEN! DEMO! |
yoffs++; |
} |
} |
#endif // USE_OSD_SCREEN_USERGPS |
//-------------------------------------------------------------- |
// OSD-Screen "Status" |
// |
// OSDScreenRefresh: 0 = update values |
// 1 = redraw labels and update values |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_MKSTATUS |
void OSD_Screen_MKStatus( void ) |
{ |
int8_t xoffs, yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 9, 1); // Linie vertikal oben, links |
lcd_line( (15*6+3), 0, (15*6+3), 9, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 10, 127, 63-10, 1, R2); // Rahmen |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // "mAh" (entnommene Kapazität) |
} |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Strom |
//----------------- |
//writex_ndigit_number_u_10th( 7, 0, naviData->Current, 4, 0,MNORMAL, 1,0); // Strom mit Nachkomma |
//----------------- |
// entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
//writex_ndigit_number_u( 7, 0, naviData->UsedCapacity * 10, 5, 0, drawmode, -3,0); // DEBUG |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Flags |
//----------------- |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag( 19, 2, OSD_FLAG_CF, 0+xoffs, 0+yoffs); // Carefree |
OSD_Element_Flag( 19, 4, OSD_FLAG_AH, 0+xoffs,-3+yoffs); // Altitude Hold |
OSD_Element_Flag( 19, 6, OSD_FLAG_PH, 0+xoffs,-6+yoffs); // Position Hold |
OSD_Element_Flag( 19, 7, OSD_FLAG_CH, 0+xoffs,-1+yoffs); // Coming Home |
xoffs -= 4; |
OSD_Element_Flag( 16, 2, OSD_FLAG_BA, -3+xoffs, 0+yoffs); // MK Batt Warning |
OSD_Element_Flag( 16, 4, OSD_FLAG_EL, -3+xoffs,-3+yoffs); // Emergency Landing |
OSD_Element_Flag( 16, 6, OSD_FLAG_RL, -3+xoffs,-6+yoffs); // Range Limit |
OSD_Element_Flag( 16, 7, OSD_FLAG_S0, -3+xoffs,-1+yoffs); // GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag( 12, 2, OSD_FLAG_CA, 0+xoffs, 0+yoffs); // Calibrate |
OSD_Element_Flag( 12, 4, OSD_FLAG_ST, 0+xoffs,-3+yoffs); // Start |
OSD_Element_Flag( 12, 6, OSD_FLAG_MR, 0+xoffs,-6+yoffs); // Motor Run |
OSD_Element_Flag( 12, 7, OSD_FLAG_FY, 0+xoffs,-1+yoffs); // Fly |
xoffs -= 4; |
OSD_Element_Flag( 9, 2, OSD_FLAG_O1, -2+xoffs, 0+yoffs); // Out1 |
OSD_Element_Flag( 9, 4, OSD_FLAG_O2, -2+xoffs,-3+yoffs); // Out2 |
OSD_Element_Flag( 9, 6, OSD_FLAG_TR, -2+xoffs,-6+yoffs); // Target Reached |
OSD_Element_Flag( 9, 7, OSD_FLAG_MC, -2+xoffs,-1+yoffs); // Manual Control |
xoffs -= 4; |
OSD_Element_Flag( 6, 2, OSD_FLAG_TU, -4+xoffs, 0+yoffs); // Vario Trim Up |
OSD_Element_Flag( 6, 4, OSD_FLAG_TD, -4+xoffs,-3+yoffs); // Vario Trim Down |
OSD_Element_Flag( 6, 6, OSD_FLAG_FR, -4+xoffs,-6+yoffs); // Free |
OSD_Element_Flag( 6, 7, OSD_FLAG_SL, -4+xoffs,-1+yoffs); // No Serial Link |
#ifdef USE_OSD_DEMO |
//----------------- |
// Flags |
//----------------- |
/* |
PSTR("AH"), // OSD_FLAG_AH Altitue Hold |
PSTR("PH"), // OSD_FLAG_PH Position Hold |
PSTR("CF"), // OSD_FLAG_CF Care Free |
PSTR("CH"), // OSD_FLAG_CH Coming Home |
PSTR("o1"), // OSD_FLAG_O1 Out1 |
PSTR("o2"), // OSD_FLAG_O2 Out2 |
PSTR("BA"), // OSD_FLAG_BA LowBat warning (MK) |
PSTR("CA"), // OSD_FLAG_CA Calibrate |
PSTR("ST"), // OSD_FLAG_ST Start |
PSTR("MR"), // OSD_FLAG_MR Motor Run |
PSTR("FY"), // OSD_FLAG_FY Fly |
PSTR("EL"), // OSD_FLAG_EL Emergency Landing |
PSTR("FS"), // OSD_FLAG_FS RX Failsave Active |
PSTR("GP"), // OSD_FLAG_GP GPS Ok |
PSTR("S!") // OSD_FLAG_S0 GPS-Sat not ok (GPS NOT ok) |
PSTR("TU"), // OSD_FLAG_TU Vario Trim Up |
PSTR("TD"), // OSD_FLAG_TD Vario Trim Down |
PSTR("FR"), // OSD_FLAG_FR Free |
PSTR("RL"), // OSD_FLAG_RL Range Limit |
PSTR("SL"), // OSD_FLAG_SL No Serial Link |
PSTR("TR"), // OSD_FLAG_TR Target Reached |
PSTR("MC") // OSD_FLAG_MC Manual Control |
*/ |
yoffs = -2; |
xoffs = -7; |
OSD_Element_Flag_Label( 19, 2, OSD_FLAG_CF, true, 0+xoffs, 0+yoffs); // DEMO: Carefree |
OSD_Element_Flag_Label( 19, 4, OSD_FLAG_AH, true, 0+xoffs,-3+yoffs); // DEMO: Altitude Hold |
OSD_Element_Flag_Label( 19, 6, OSD_FLAG_PH, true, 0+xoffs,-6+yoffs); // DEMO: Position Hold |
OSD_Element_Flag_Label( 19, 7, OSD_FLAG_CH, true, 0+xoffs,-1+yoffs); // DEMO: Coming Home |
xoffs -= 4; |
OSD_Element_Flag_Label( 16, 2, OSD_FLAG_BA, true, -3+xoffs, 0+yoffs); // DEMO: MK Batt Warning |
OSD_Element_Flag_Label( 16, 4, OSD_FLAG_EL, true, -3+xoffs,-3+yoffs); // DEMO: Emergency Landing |
OSD_Element_Flag_Label( 16, 6, OSD_FLAG_RL, true, -3+xoffs,-6+yoffs); // DEMO: Range Limit |
OSD_Element_Flag_Label( 16, 7, OSD_FLAG_S0, true, -3+xoffs,-1+yoffs); // DEMO: GPS-Sat not ok (GPS NOT ok) |
xoffs -= 4; |
OSD_Element_Flag_Label( 12, 2, OSD_FLAG_CA, true, 0+xoffs, 0+yoffs); // DEMO: Calibrate |
OSD_Element_Flag_Label( 12, 4, OSD_FLAG_ST, true, 0+xoffs,-3+yoffs); // DEMO: Start |
OSD_Element_Flag_Label( 12, 6, OSD_FLAG_MR, true, 0+xoffs,-6+yoffs); // DEMO: Motor Run |
OSD_Element_Flag_Label( 12, 7, OSD_FLAG_FY, true, 0+xoffs,-1+yoffs); // DEMO: Fly |
xoffs -= 4; |
OSD_Element_Flag_Label( 9, 2, OSD_FLAG_O1, true, -2+xoffs, 0+yoffs); // DEMO: Out1 |
OSD_Element_Flag_Label( 9, 4, OSD_FLAG_O2, true, -2+xoffs,-3+yoffs); // DEMO: Out2 |
OSD_Element_Flag_Label( 9, 6, OSD_FLAG_TR, true, -2+xoffs,-6+yoffs); // DEMO: Target Reached |
OSD_Element_Flag_Label( 9, 7, OSD_FLAG_MC, true, -2+xoffs,-1+yoffs); // DEMO: Manual Control |
xoffs -= 4; |
OSD_Element_Flag_Label( 6, 2, OSD_FLAG_TU, true, -4+xoffs, 0+yoffs); // DEMO: Vario Trim Up |
OSD_Element_Flag_Label( 6, 4, OSD_FLAG_TD, true, -4+xoffs,-3+yoffs); // DEMO: Vario Trim Down |
OSD_Element_Flag_Label( 6, 6, OSD_FLAG_FR, true, -4+xoffs,-6+yoffs); // DEMO: Free |
OSD_Element_Flag_Label( 6, 7, OSD_FLAG_SL, true, -4+xoffs,-1+yoffs); // DEMO: No Serial Link |
#endif |
} |
#endif // USE_OSD_SCREEN_MKSTATUS |
//-------------------------------------------------------------- |
// OSD_Screen_Electric() |
// |
// Anzeige BL-Temperaturen und Stroeme |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_ELECTRIC |
void OSD_Screen_Electric( void ) |
{ |
uint8_t x, y, x0, y0; |
int8_t yoffs; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line (33, 0, 33, 19, 1); // Linie vertikal oben, links |
lcd_line (93, 0, 93, 19, 1); // Linie vertikal oben, rechts |
lcd_rect_round( 0, 19, 127, 63-19, 1, R2); // Rahmen |
lcd_line (0, 41, 127, 41, 1); // Linie horizontal mitte |
lcdx_printp_at( 12,0 , PSTR("mAh"), MNORMAL, -1,0); // entnommene Kapazität |
lcdx_printp_at( 12,1 , PSTR("A") , MNORMAL, -1,2); // aktueller Strom |
writex_ndigit_number_u( 19, 1, cells, 1, 0, MNORMAL, 2,2); // LiPO Cells Wert |
lcdx_printp_at( 20,1 , PSTR("s") , MNORMAL, 2,2); // LiPO Cells "s" |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: entnommene Kapazitaet (mAh) |
//----------------- |
drawmode = (naviData->UsedCapacity > Config.OSD_mAh_Warning ? MINVERS : MNORMAL); |
writex_ndigit_number_u( 7, 0, naviData->UsedCapacity, 5, 0, drawmode, -3,0); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
//writex_time(16, 0, naviData->FlyingTime+900, MNORMAL, 2,0); // DEBUG |
writex_time( 16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//----------------- |
// Strom |
//----------------- |
writex_ndigit_number_u_10th( 7, 1, naviData->Current, 4, 0,MNORMAL, -3,2); // Strom mit Nachkomma |
//----------------- |
// BL 1-8 Temp & Strom |
//----------------- |
x0 = 1; |
y0 = 4; |
for( y=0; y<2; y++) // 2 Zeilen (mit je 4 BL's/Motoren) |
{ |
if( y==0 ) yoffs = -9; |
else yoffs = -3; |
for( x=0; x<4; x++) // und 4 BL's/Motoren pro Zeile |
{ |
if( blData[y*4+x].Status & 0xf0 ) // BL/Motor vorhanden? |
{ |
if( blData[y*4+x].Temperature != 0 ) // Anzeige nur wenn Temp != 0 wegen BL-Ctrl v1 die keine Temperatur senden |
{ |
writex_ndigit_number_u( (x*5)+x0, (y*2)+y0+0, ( blData[y*4+x].Temperature ), 3, 0,MNORMAL, 0,yoffs); // Temperatur |
lcdx_putc ( (x*5)+3+x0, (y*2)+y0+0, SYMBOL_SMALLDEGREE ,MNORMAL, 1,yoffs); |
} |
// Variante: mit Nachkommastellen |
writex_ndigit_number_u_10th ( (x*5)+x0, (y*2)+y0+1, ( blData[y*4+x].Current), 3, 0, MNORMAL, 0,yoffs); // Strom |
} |
} |
} |
#ifdef USE_OSD_DEMO |
OSD_Element_Flag_Label( 19, 1, OSD_FLAG_BA, true, 0,1); // DEMO |
#endif |
} |
#endif // USE_OSD_SCREEN_ELECTRIC_N |
//-------------------------------------------------------------- |
// OSD_Screen_Statistics() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_STATISTIC |
void OSD_Screen_Statistics( void ) |
{ |
osd_BLmax_t blmax; |
uint8_t line = 0; |
// max. der BL's ermitteln |
calc_BLmax( &blmax ); |
//--------------------------- |
// max Altitude |
lcd_printp_at (0, line, strGet(STATS_ITEM_0), MNORMAL); |
write_ndigit_number_s (14, line, Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust), 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// max Speed |
// max_GroundSpeed = 1; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_1), MNORMAL); |
write_ndigit_number_u (15, line, (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("kmh"), MNORMAL, 2,0); |
//--------------------------- |
// max Distance |
// max_Distance = 64512; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_2), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Distance / 10, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'm', MNORMAL, 2,0); |
//--------------------------- |
// min voltage |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_3), MNORMAL); |
if( Config.OSD_Statistic.min_UBat==255 ) |
lcd_printp_at(14, line, PSTR(" 0"), MNORMAL); |
else |
write_ndigit_number_u_10th (14, line, Config.OSD_Statistic.min_UBat, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'V', MNORMAL, 2,0); |
//--------------------------- |
// Used Capacity |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_6), MNORMAL); |
write_ndigit_number_u (14, line, Config.OSD_Statistic.max_Capacity, 4, 0,MNORMAL); |
lcdx_printp_at(18, line, PSTR("mAh"), MNORMAL, 2,0); |
//--------------------------- |
// max Current |
// max_Current = 1000; |
lcd_printp_at (0, ++line, strGet(STATS_ITEM_5), MNORMAL); |
write_ndigit_number_u_10th (13, line, Config.OSD_Statistic.max_Current, 4, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Current |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Curr:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Current_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u_10th (14, line, blmax.max_BL_Current, 3, 0,MNORMAL); |
lcdx_putc (18, line, 'A', MNORMAL, 2,0); |
//--------------------------- |
// max BL-Temp |
line++; |
lcd_printp_at( 0, line, PSTR("max BL Temp:"), MNORMAL); |
write_ndigit_number_u( 6, line, blmax.max_BL_Temp_Index+1, 1, 0,MNORMAL); |
write_ndigit_number_u (14, line, blmax.max_BL_Temp, 4, 0,MNORMAL); |
lcdx_printp_at( 18, line, PSTR("\013C"), MNORMAL, 2,0); |
} |
#endif // USE_OSD_SCREEN_STATISTIC |
//-------------------------------------------------------------- |
// OSD_Screen_3DLage() |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_3DLAGE |
void OSD_Screen_3DLage( void ) |
{ |
uint16_t head_home; |
uint8_t Nick; |
uint8_t Roll; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_rect_round( 0, 0, 127, 63-0, 1, R2); // Rahmen |
} |
head_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_line(26,32,100,32,1); // horizontal // |
lcd_line(63,0,63,63,1); // vertical // |
// 45' Angel |
lcd_line(61,11,65,11,1); // -- // |
lcd_line(40,30,40,34,1); // | // |
lcd_line(86,30,86,34,1); // | // |
lcd_line(61,53,65,53,1); // -- // |
lcdx_printp_at( 9, 0, strGet(OSD_3D_V), MNORMAL, 0,4); // V |
lcdx_printp_at( 3, 3, strGet(OSD_3D_L), MNORMAL, 0,0); // L |
lcdx_printp_at(17, 3, strGet(OSD_3D_R), MNORMAL, 0,0); // R |
lcdx_printp_at( 9, 7, strGet(OSD_3D_H), MNORMAL, 0,-3); // H |
// Oben, Links: Ni |
lcdx_printp_at(0, 0, strGet(OSD_3D_NICK), MNORMAL, 5,4); // Ni |
writex_ndigit_number_s (2, 0, naviData->AngleNick, 3, 0, MNORMAL, 7,4); |
lcdx_putc (5, 0, SYMBOL_SMALLDEGREE, MNORMAL, 7,4); |
// Unten, Links: Ro |
lcdx_printp_at(0, 7, strGet(OSD_3D_ROLL), MNORMAL, 5,-3); // Ro |
writex_ndigit_number_s (2, 7, naviData->AngleRoll, 3, 0, MNORMAL, 7,-3); |
lcdx_putc (5, 7, SYMBOL_SMALLDEGREE, MNORMAL, 7,-3); |
// Oben, Rechts: Ko |
//lcdx_printp_at(13, 0, strGet(OSD_3D_COMPASS), MNORMAL, -4,3); // Ko |
writex_ndigit_number_u (15, 0, (naviData->CompassHeading)%360, 3, 0, MNORMAL, -4,4); |
lcdx_putc (18, 0, SYMBOL_SMALLDEGREE, MNORMAL, -4,4); |
OSD_Element_CompassDirection( 19, 0, -2,4 ); |
Roll = ((-naviData->AngleRoll/2)+63); |
Nick = ((-naviData->AngleNick/2)+32); |
if( Roll < (9+1) ) Roll = (9+1); // nicht ausserhalb des Screens zeichnen! |
if( Roll > 127-(9+1) ) Roll = 127-(9+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick < (8+1) ) Nick = (8+1); // nicht ausserhalb des Screens zeichnen! |
if( Nick > 63-(8+1) ) Nick = 63-(8+1); // nicht ausserhalb des Screens zeichnen! |
if( old_AngleRoll != 0 ) // nicht ausserhalb des Screens zeichnen! |
{ |
lcd_ellipse ( old_AngleRoll, old_AngleNick, 9, 8, 0); |
lcd_ellipse_line( old_AngleRoll, old_AngleNick, 8, 7, old_hh, 0); |
} |
lcd_ellipse ( Roll, Nick, 9, 8, 1); |
lcd_ellipse_line( Roll, Nick, 8, 7, head_home, 1); |
// remember last values (3DL) |
old_hh = head_home; |
old_AngleNick = Nick; |
old_AngleRoll = Roll; |
} |
#endif // USE_OSD_SCREEN_3DLAGE |
//-------------------------------------------------------------- |
// OSD_Screen_MKDisplay() |
// |
// das ist ein Spezialscreen der ausserhalb der regulaeren |
// OSD-Screens aufgerufen und bedient wird! |
//-------------------------------------------------------------- |
//#ifdef USE_OSD_SCREEN_MKDISPLAY |
void OSD_Screen_MKDisplay( void ) |
{ |
uint8_t wpindex; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal Oben: links |
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal Oben: rechts |
lcd_rect_round( 0, 2*7-2, 127, 5*7+3+3, 1, R2); // Rahmen unten fuer Inhalt Display |
//lcd_frect_round( 6*6+0, 0, 9*6+2, 9, 1, R1); // Umrahmung fuer "OSD-Displ" |
//lcdx_printp_at( 7, 0, PSTR("OSD-Disp"), MINVERS, -3,1); // "OSD-Displ" |
//lcdx_printp_at(15, 0, PSTR("l"), MINVERS, -4,1); // das "l" von "OSD-Displ" (1 Pixel nach links) |
lcdx_printp_at( 2, 7, PSTR("\x18 \x19"), MNORMAL, 0,0); // Keyline: Links / Rechts |
PKT_KeylineUpDown( 18, 13, 0,0); // Keyline: Down / Up |
} // end: if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
//----------------------------------------- |
//----------------- |
// Oben: Waypoint-Index und Anzahl der Waypoint's |
//----------------- |
wpindex = naviData->WaypointIndex; |
if( naviData->WaypointNumber==0 ) wpindex = 0; |
lcdx_printf_at_P( 7, 0, MNORMAL, -1,0, PSTR("WP:%2d/%2d"), wpindex, naviData->WaypointNumber ); |
//lcdx_printf_at_P(13, 0, MNORMAL, -3,1, PSTR("R:%d"), (naviData->NCFlags & NC_FLAG_TARGET_REACHED) ); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//lcd_printf_at_P( 0, 2, MNORMAL, PSTR("WP:%d"), naviData->WaypointIndex); |
//write_ndigit_number_u (x+2, y, naviData->WaypointIndex , 2, 0,0); |
//lcd_printp_at (x, y, PSTR("WP"), 0); |
//if ((NextWP==true)&& naviData->NCFlags & NC_FLAG_TARGET_REACHED) |
//----------------- |
// Oben: Batt Level (Volt) |
//----------------- |
OSD_Element_BattLevel2( 0, 0, 0,0 ); |
//----------------- |
// Oben: Batt Level Bar |
//----------------- |
//OSD_Element_Battery_Bar( x, y, length, width, orientation) |
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
//----------------- |
// Oben: Navi-Kreis |
//----------------- |
//OSD_Element_HomeCircleX( 64, 5, 6, 5, true ); |
//----------------- |
// Oben: Flugzeit |
//----------------- |
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
//------------------------------------------ |
// Ausgabe auf PKT-Anzeige |
// 4 Zeilen a 20 Zeichen |
//------------------------------------------ |
mkdisplayData[80] = 0; |
lcdx_print_at( 0,5, (uint8_t *) &mkdisplayData[60], MNORMAL, 5,3); |
mkdisplayData[60] = 0; |
lcdx_print_at( 0,4, (uint8_t *) &mkdisplayData[40], MNORMAL, 5,2); |
mkdisplayData[40] = 0; |
lcdx_print_at( 0,3, (uint8_t *) &mkdisplayData[20], MNORMAL, 5,1); |
mkdisplayData[20] = 0; |
lcdx_print_at( 0,2, (uint8_t *) &mkdisplayData[0], MNORMAL, 5,0); |
Beep_Waypoint(); |
} |
//#endif // USE_OSD_SCREEN_MKDISPLAY |
//############################################################## |
#ifdef USE_OSD_SCREEN_DEBUG |
//############################################################## |
//************************************************************** |
//* OSD_DEBUG_SCREEN - Experimental-Code usw. |
//* - nicht fuer die Oeffentlichkeit bestimmt |
//* - gesteuert ueber define OSD_DEBUG_SCREEN |
//************************************************************** |
//-------------------------------------------------------------- |
// OSD_Screen_Debug() |
//-------------------------------------------------------------- |
void OSD_Screen_Debug( void ) |
{ |
//char buffer[80]; |
static uint16_t debug_count = 0; |
//char buffer[30]; |
//uint8_t y, i; |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_printp_at( 0, 0, PSTR("Debug"), 0); |
//timer_pkt_uptime = 0; |
} |
debug_count++; |
write_ndigit_number_u( 17, 0, (debug_count), 4, 0,MNORMAL); // |
// act. Current |
lcd_printf_at_P( 0, 2, MNORMAL, PSTR("act Current%3.1u A"), naviData->Current); |
// max. Current |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("max Current%3.1u A"), Config.OSD_Statistic.max_Current); |
// avg. Current |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("avg Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Current/100)); |
// avg. Current DEBUG |
//lcd_printf_at_P( 0, 5, MNORMAL, PSTR("avgD Current%3.1u A"), (uint8_t)(Config.OSD_Statistic.avg_Altimeter/100)); |
// stat. Packages |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("stat Pkg's%7u"), Config.OSD_Statistic.count_osd); |
/* |
// DEBUG: Heading |
lcd_printf_at_P( 0, 1, MNORMAL, PSTR("NC-Errorcode:%3u"), naviData->Errorcode); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
lcd_printf_at_P( 0, 3, MNORMAL, PSTR("CH:%6d%6d"), naviData->CompassHeading, naviData->CompassHeading%360); |
lcd_printf_at_P( 0, 4, MNORMAL, PSTR("HD:%6d%6d"), naviData->HomePositionDeviation.Bearing, naviData->HomePositionDeviation.Bearing%360); |
//heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)) + 180; |
//heading_home = ((naviData->CompassHeading % 360) - (naviData->HomePositionDeviation.Bearing % 360)); |
calc_heading_home(); |
lcd_printf_at_P( 0, 6, MNORMAL, PSTR("xx:%6d"), heading_home); |
*/ |
/* |
lcd_putc( 0, 3, 0x08, MNORMAL); // ASCII - 8 08 SAT Symbol |
lcd_putc( 2, 3, 0x09, MNORMAL); // ASCII - 9 09 Altitude Symbol |
lcd_putc( 4, 3, 0x0C, MNORMAL); // ASCII - 12 0C Enter Symbol |
lcd_putc( 6, 3, 0x1F, MNORMAL); // ASCII - 31 1F Antenne |
lcd_putc( 8, 3, 10, MNORMAL); // 'o' |
lcd_putc(10, 3, 13, MNORMAL); // 'o' |
//lcd_putc(10, 3, 0x06, MNORMAL); |
//lcd_putc(12, 3, 0x07, MNORMAL); |
lcd_putc( 0, 5, 0x1E, MNORMAL); |
lcd_putc( 4, 5, 0x7e, MNORMAL); |
lcd_putc( 6, 5, 0x7f, MNORMAL); |
lcd_putc( 8, 5, 0x18, MNORMAL); |
lcd_putc(10, 5, 0x19, MNORMAL); |
*/ |
//lcd_printp_at( 0, 1, PSTR("Free RAM:"), 0); |
//writex_ndigit_number_u ( 9, 1, get_freeRam(), 9, 0,MNORMAL, 0,0); // |
} |
//-------------------------------------------------------------- |
// OSD_Screen_Debug_RX() |
// |
// Anzeige gelesener Datenpakete (fuer Feinabstimmung) und weitere |
// Werte wie Zeit/Datum (mit/ohne Abgleich zur NC) |
// |
// Anzeige oben 1. Zeile: |
// Screenname : "Debug-RX" |
// PKT-Uptime : Minuten, Sekunden die das PKT aktuell eingeschaltet ist |
// Screen-Count: Anzahl der Aufrufe des Screens (abhaengig von der Refreshtime) |
// |
// Anzeige Zeit: |
// "N" oder "O": neuer(N) oder alter(O) Zeit-Algo fuer die NC (OSD_MK_UTCTime()) |
// 00:00:00 : Stunde, Minute, Sekunde (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// dd.mm.yyyy : Tag, Monat, Jahr (korrigiert mittels PKT-Einstellung bzgl. Zeitzone/Sommerzeit) |
// Solange keine richtige Zeit von der NC gemeldet wird, wird die PKT-Uptime |
// seit einschalten des PKT angezeigt. |
// |
// Anzeige unten: |
// OSD: Anzahl gelesener OSD-Pakete der NC |
// Time: Anzahl gelesener Time-Pakete der NC (Aktualisierung ca. jede Minute) |
// |
// BL: es werden die gelesenen Datenpakete der BL's angezeigt |
// von BL1 (links oben) bis BL8 (rechts unten) |
//-------------------------------------------------------------- |
void OSD_Screen_Debug_RX( void ) |
{ |
static uint16_t debug_count = 0; |
uint8_t y; |
int8_t yoffs; |
//uint8_t status; // FC Kommunikation |
//----------------------------------------- |
// REDRAW |
// statische Screen Elemente die nicht |
// jedesmal neu gezeichnet werden muessen |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
lcd_frect( 0, 0, 1, 8, 1); // title spacer |
lcdx_printp_at( 0, 0, PSTR("Debug-RX"), MINVERS, 1,0); // title |
lcd_line (0, 9, 127, 9, 1); // Linie horizontal |
lcd_line (0, 22, 127, 22, 1); // Linie horizontal |
//lcd_line (0, 31, 127, 31, 1); // Linie horizontal |
} |
//----------------------- |
// Zeile 0 |
//----------------------- |
writex_time( 11, 0, timer_pkt_uptime/100, MNORMAL, 0,0); |
debug_count++; |
write_ndigit_number_u ( 17, 0, (debug_count), 4, 0,MNORMAL); // |
//----------------------- |
// Anzeige Zeit / Datum |
//----------------------- |
yoffs = 5; |
writex_datetime_time( 1, 1, UTCTime, MNORMAL, 5,yoffs); // Zeit |
writex_datetime_date( 11, 1, UTCTime, MNORMAL, 0,yoffs); // Datum |
//----------------------- |
// gelesene Pakete: OSD und Zeit |
//----------------------- |
y = 4; |
yoffs = -5; |
lcdx_printp_at( 0, y+0, PSTR("OSD:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+0, ( readCounterOSD), 5, 0,MNORMAL, 0,yoffs); // |
lcdx_printp_at( 13, y+0, PSTR("Time:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 18, y+0, ( readCounterTIME), 3, 0,MNORMAL, 0,yoffs); // |
//---- |
lcdx_printp_at( 0, y+1, PSTR("Dis:"), MNORMAL, 0,yoffs); |
writex_ndigit_number_u ( 4, y+1, ( readCounterDISPLAY), 5, 0,MNORMAL, 0,yoffs); // |
//----------------------- |
// gelesene Pakete: BL |
//----------------------- |
y = 6; |
lcd_frect( 0, y*8-1, 8, 17, 1); // Box |
lcdx_printp_at( 0, y+0, PSTR("B"), MINVERS, 2,0); |
lcdx_printp_at( 0, y+1, PSTR("L"), MINVERS, 2,0); |
write_ndigit_number_u ( 2, y+0, ( readCounterBL[0]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+0, ( readCounterBL[1]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+0, ( readCounterBL[2]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+0, ( readCounterBL[3]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 2, y+1, ( readCounterBL[4]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 7, y+1, ( readCounterBL[5]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 12, y+1, ( readCounterBL[6]), 4, 0,MNORMAL); // |
write_ndigit_number_u ( 17, y+1, ( readCounterBL[7]), 4, 0,MNORMAL); // |
} |
//############################################################## |
#endif // USE_OSD_SCREEN_DEBUG |
//############################################################## |
//----------------------------------------------------------- |
// ok = OSD_Popup_MKSetting() |
// |
// zeigt das aktuelle FC-Setting beim Start vom OSD an |
// |
// RUECKGABE: |
// true = Setting konnte gelesen werden |
// false = Fehler |
//----------------------------------------------------------- |
uint8_t OSD_Popup_MKSetting( void ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printp_center( 2, PSTR("PKT OSD") , MNORMAL, 0, 0); |
lcdx_printp_center( 6, PSTR("MK Setting"), MINVERS, 0,-8); |
MK_Setting_load( 0xff, 9 ); // 0xff == aktuelles Parameterset holen; 9 == timeout |
MKVersion_Setting_print( 7, MINVERS, 0,-4); // aus: mkbase.c |
if( MKVersion.mksetting == 0 ) |
set_beep( 500, 0xffff, BeepNormal ); // kein Setting - langer Beep ERROR |
clear_key_all(); |
timer = 300; // ca. 3 Sekunden zeigen; Abbruch mit einer Taste moeglich |
while( timer > 0 && !get_key_press(0xff) ); |
clear_key_all(); |
lcd_cls(); |
return (MKVersion.mksetting != 0); |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_MKError( u8 mkerrorcode ) |
{ |
Popup_Draw( 3 ); // 3 Zeilen von unten nach oben fuer's Popup |
lcdx_printf_center_P( 6, MINVERS, 0,-8, PSTR("** MK-%S %02d **"), strGet(STR_ERROR), mkerrorcode ); // "MK-FEHLER" und Fehlernummer |
lcdx_printp_center( 7, (const char*) pgm_read_word(&mkerrortext[mkerrorcode]), MINVERS, 0,-4); // MK-Fehlertext |
} |
//----------------------------------------------------------- |
//----------------------------------------------------------- |
void OSD_Popup_Info( uint8_t ScreenNum, const char *ScreenName) |
{ |
Popup_Draw( 5 ); // 5 Zeilen von unten nach oben fuer's Popup |
lcd_line( 3, 53-21, 124, 53-21, 0); // Linie: oben |
lcd_line( 3, 53, 124, 53, 0); // Linie: unten |
//----------------------- |
// ScreenNummer: ScreenName |
//----------------------- |
lcdx_printf_at_P( 0, 3, MINVERS, 5,-2, PSTR("%02d: %S"), ScreenNum, ScreenName ); |
//----------------------- |
// longpress Key's |
//----------------------- |
lcdx_printp_at( 0, 4, strGet(STR_LONGPRESS), MINVERS, 6,3); // "langer Tastendruck:" |
lcdx_printp_at(12, 5, PSTR("Disp"), MINVERS, 0,4); |
lcdx_printp_at(17, 5, PSTR("UGps"), MINVERS, 0,4); |
//----------------------- |
// shortpress Key's |
//----------------------- |
lcd_printp_at( 0, 7, strGet(KEYLINE3), MINVERS); |
lcd_printp_at(17, 7, PSTR("Info") , MINVERS); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_Pos_Save( pkt_gpspos_t *pGPS ) |
{ |
pGPS->Altimeter = naviData->Altimeter; // barymetrische Hoehe |
pGPS->HomeDistance = naviData->HomePositionDeviation.Distance; // Entfernung Home |
memcpy( &pGPS->GPSData, &naviData->CurrentPosition, sizeof(GPS_Pos_t) ); // sichern... |
memcpy( &pGPS->timestamp, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // sichern... |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
// GPS_User_Init() |
// |
// initialisiert die GPS Positionen neu |
//-------------------------------------------------------------- |
void GPS_User_Init( void ) |
{ |
memset( Config.GPS_User, 0, sizeof(pkt_gpspos_t)*MAX_GPS_USER ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void GPS_User_Save( void ) |
{ |
uint8_t i; |
if( naviData->NCFlags & NC_FLAG_GPS_OK ) // nur wenn MK-GPS ok ist |
{ |
for( i=MAX_GPS_USER-1; i>0; i--) |
{ |
Config.GPS_User[i] = Config.GPS_User[i-1]; |
} |
GPS_Pos_Save( &Config.GPS_User[0] ); |
set_beep( 160, 0xffff, BeepNormal ); // Beep Ok |
} |
else |
{ |
set_beep( 600, 0x000f, BeepNormal ); // Beep Error (keine gueeltigen GPS-Daten) |
} |
} |
//############################################################## |
//############################################################## |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Init( void ) |
{ |
CellIsChecked = 0; |
cells = 0; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MKLiPoCells_Check( void ) |
{ |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
} |
//############################################################## |
//# OSD MAIN LOOP |
//############################################################## |
//-------------------------------------------------------------- |
// OSD MAIN LOOP |
//-------------------------------------------------------------- |
void osd( void ) |
{ |
uint8_t osdexit = false; |
uint8_t mktimeout = false; |
uint8_t flying = false; |
uint8_t popup_state = OSD_POPUP_NONE; |
lcd_cls(); |
#ifdef DEBUG_OSD_TIME |
// Fake-Zeit/Datum setzen wenn der NC-Simulator verwendet wird |
if( UTCTime.year < 2000 ) |
{ |
UTCTime.seconds = ((uint32_t)13*3600)+(15*60)+42; // 13:15:42 |
UTCTime.day = 01; |
UTCTime.month = 05; |
UTCTime.year = 2013; |
} |
#endif |
//---------------------------------------- |
// Anzeige: aktuelles MK-Setting |
//---------------------------------------- |
if( (Config.OSD_ShowMKSetting) ) |
{ |
if( !OSD_Popup_MKSetting() ) |
return; |
} |
/* |
//----------------------------------------------------------------------------------------------- |
// 07.03.2013 OG: del |
// Dieser Teil hat immer wieder Probleme bereitet bei der Verbindung des PKT-OSD zum MK da |
// MK_TIMEOUTs zustande kamen. Eine Recherche im Code ergab, dass die Nutzdaten die |
// hierueber bezogen werden sich lediglich auf Flags_ExtraConfig beschraenkten (CFG2_HEIGHT_LIMIT). |
// Siehe dazu untere Kommentare. |
// |
// Der negative Effekt moeglicher MK_TIMEOUTs und Verzoegerungen sind es aktuell nicht Wert |
// CFG2_HEIGHT_LIMIT zu unterstuetzen. Dieses Feature ist erstmal raus. |
// |
// Falls gewuenscht wird, dass CFG2_HEIGHT_LIMIT wieder in das PKT-OSD kommt muss |
// es zuverlaessig an anderer Stelle implementiert werden - und zwar nicht in osd.c |
// weil es eine statische FC-Information ist (ggf. beim Verbindungsaufbau PKT <-> MK). |
// |
// Hat auch aktuell Auswirkung auf den Code OSD_Element_AltitudeControl() |
//----------------------------------------------------------------------------------------------- |
//lcd_printp_at( 0, 3, PSTR("connecting MK..."), 0); |
// |
//SwitchToFC(); |
// |
//status = load_setting(0xff); |
// |
//if( status == 255 ) |
//{ |
// lcd_printp_at(0, 0, PSTR("Keine Settings !!"), 0); // Keine Settings |
// _delay_ms(2000); |
//} |
//Flags_ExtraConfig = mk_param_struct->ExtraConfig; // OG: wird in osd.c nur verwendet von: OSD_Element_AltitudeControl() |
//Flags_GlobalConfig = mk_param_struct->GlobalConfig; // OG: wird nicht in osd.c verwendet |
//Flags_GlobalConfig3 = mk_param_struct->GlobalConfig3; // OG: wird nicht in osd.c verwendet |
*/ |
//------------------------- |
// MK-Display initialisieren |
//------------------------- |
memset( mkdisplayData, 0, 81 ); |
mkdisplayMode = false; |
mkdisplayCmd = 0xff; |
//------------------------- |
// BL-DATA initialisieren |
//------------------------- |
memset( blData, 0, sizeof(BLData_t)*OSD_MAX_MOTORS ); |
//------------------------- |
// Debug initialisieren |
//------------------------- |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD = 0; |
readCounterTIME = 0; |
readCounterDISPLAY = 0; |
memset( readCounterBL, 0, sizeof(uint16_t)*OSD_MAX_MOTORS ); |
#endif // USE_OSD_SCREEN_DEBUG |
//------------------------- |
// NC Datenkommunikation starten |
//------------------------- |
OSD_MK_Connect( MK_CONNECT ); |
OSD_active = true; // benoetigt fuer Navidata Ausgabe an SV2 |
//------------------------- |
// Clear statistics |
//------------------------- |
//STAT_Init(); |
CellIsChecked = 0; |
cells = 0; |
AkkuWarnThreshold = 0; |
OldWP = 0; |
NextWP = false; |
old_PKTErrorcode = 0; |
old_MKErrorcode = 0; |
//------------------------- |
// Init: OSD-Screens |
//------------------------- |
ScreenCtrl_Init(); |
ScreenCtrl_Push( OSDSCREEN_GENERAL , strGet(STR_OSDSCREEN_GENERAL) , &OSD_Screen_General ); |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScreenCtrl_Push( OSDSCREEN_NAVIGATION , strGet(STR_OSDSCREEN_NAVIGATION), &OSD_Screen_Navigation ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints ); |
#endif |
// *ALTERNATIVE* |
//#ifdef USE_OSD_SCREEN_WAYPOINTS |
//ScreenCtrl_Push( OSDSCREEN_WAYPOINTS , strGet(STR_OSDSCREEN_WAYPOINTS) , &OSD_Screen_Waypoints0 ); |
//#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScreenCtrl_Push( OSDSCREEN_ELECTRIC , strGet(STR_OSDSCREEN_ELECTRIC) , &OSD_Screen_Electric ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScreenCtrl_Push( OSDSCREEN_MKSTATUS , strGet(STR_OSDSCREEN_MKSTATUS) , &OSD_Screen_MKStatus ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScreenCtrl_Push( OSDSCREEN_USERGPS , strGet(STR_OSDSCREEN_USERGPS) , &OSD_Screen_UserGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScreenCtrl_Push( OSDSCREEN_3DLAGE , strGet(STR_OSDSCREEN_3DLAGE) , &OSD_Screen_3DLage ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScreenCtrl_Push( OSDSCREEN_STATISTICS , strGet(STR_OSDSCREEN_STATISTIC) , &OSD_Screen_Statistics ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScreenCtrl_Push( OSDSCREEN_OSD0 , strGet(STR_OSDSCREEN_OSD0) , &OSD_Screen_OSD0 ); |
ScreenCtrl_Push( OSDSCREEN_OSD1 , strGet(STR_OSDSCREEN_OSD1) , &OSD_Screen_OSD1 ); |
ScreenCtrl_Push( OSDSCREEN_OSD2 , strGet(STR_OSDSCREEN_OSD2) , &OSD_Screen_OSD2 ); |
#endif |
#ifdef USE_OSD_SCREEN_DEBUG |
ScreenCtrl_Push( 0 , PSTR("Debug") , &OSD_Screen_Debug ); |
ScreenCtrl_Push( 0 , PSTR("Debug-RX") , &OSD_Screen_Debug_RX ); |
#endif |
ScreenCtrl_Set( Config.OSD_ScreenMode ); |
//------------------------- |
// Init: Timer & Flags |
//------------------------- |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
timer_mk_timeout = MK_TIMEOUT; |
timer_osd_refresh = TIME_OSD_REFRESH; |
timer_get_bldata = 0; |
timer_get_tidata = 0; |
timer_get_displaydata = 0; |
//-------------------------------- |
// OSD Main-Loop |
//-------------------------------- |
while( !osdexit ) |
{ |
//################################ |
//# Empfange/verarbeite: OSD-Daten |
//################################ |
if( rxd_buffer_locked ) // naviData Ok? |
{ |
Decode64(); |
memcpy( &osdData, (const void *)pRxData, sizeof(NaviData_t) ); // sichern in: osdData |
naviData = &osdData; |
#ifdef USE_OSD_SCREEN_DEBUG |
readCounterOSD++; // gelesene Datenpakete |
#endif |
mktimeout = false; |
//---------------------------------- |
// LiPo Cell Check |
//---------------------------------- |
if( cells == 0 ) // Zellenzahl noch nicht ermittelt? |
{ |
// up to 6s LiPo, less than 2s is technical impossible |
for( cells = 2; cells < 7; cells++) |
{ |
if( naviData->UBat < cells * MAX_CELL_VOLTAGE ) break; |
} |
Config.OSD_Statistic.LiPoCells = cells; |
} |
//---------------------------------- |
// Winkel zu Home |
//---------------------------------- |
calc_heading_home(); |
//---------------------------------- |
// speichere letzte GPS-Positionen |
//---------------------------------- |
GPS_Pos_Save( &GPS_Current ); |
Config.LastLatitude = GPS_Current.GPSData.Latitude; // speichere letzte Position in Config |
Config.LastLongitude = GPS_Current.GPSData.Longitude; // speichere letzte Position in Config |
//---------------------------------- |
// PKT Fehler reset |
//---------------------------------- |
if( old_PKTErrorcode == 40 ) // 40 = PKT Empfangsausfall ("PKT RX lost") |
{ |
// PKT-Verbindungsfehler zuruecksetzen |
// da an dieser Stelle ja bereits wieder ein gueltiges Datenpaket |
// von der NaviCtrl empfangen wurde |
old_PKTErrorcode = 0; |
clear_key_all(); |
} |
//---------------------------------- |
// remember statistics (only when engines running) |
//---------------------------------- |
#ifdef DEBUG_OSD_STAT_MOTORRUN |
if( true ) |
#else |
if( naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN ) // AM FLIEGEN -> Statistik aufzeichnen |
#endif |
{ |
flying = true; |
// --- gueltige Zeit von der NC vorhanden und noch keine Start-Zeit gesetzt? |
if( UTCTime.year != 0 && Config.OSD_Statistic.begin_StatTime.year == 0 ) |
{ |
memcpy( &Config.OSD_Statistic.begin_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Start Zeit/Datum sichern... |
} |
Config.OSD_Statistic.last_FlyTime = naviData->FlyingTime; |
Config.OSD_Statistic.count_osd++; // Anzahl OSD-Statistik Pakete |
// int32_t calc_avg( int32_t avg, int32_t value, int32_t count, int32_t factor) |
Config.OSD_Statistic.avg_Current = (uint16_t)calc_avg( (int32_t)Config.OSD_Statistic.avg_Current, |
(int32_t)naviData->Current, |
(int32_t)Config.OSD_Statistic.count_osd, |
(int32_t)100 ); |
if( naviData->Altimeter > Config.OSD_Statistic.max_Altimeter ) Config.OSD_Statistic.max_Altimeter = naviData->Altimeter; |
if( naviData->GroundSpeed > Config.OSD_Statistic.max_GroundSpeed ) Config.OSD_Statistic.max_GroundSpeed = naviData->GroundSpeed; |
if( naviData->HomePositionDeviation.Distance > Config.OSD_Statistic.max_Distance ) Config.OSD_Statistic.max_Distance = naviData->HomePositionDeviation.Distance; |
if( naviData->Current > Config.OSD_Statistic.max_Current ) Config.OSD_Statistic.max_Current = naviData->Current; |
if( naviData->UsedCapacity > Config.OSD_Statistic.max_Capacity ) Config.OSD_Statistic.max_Capacity = naviData->UsedCapacity; |
if( naviData->UBat < Config.OSD_Statistic.min_UBat ) Config.OSD_Statistic.min_UBat = naviData->UBat; |
if( naviData->TopSpeed > Config.OSD_Statistic.max_TopSpeed ) Config.OSD_Statistic.max_TopSpeed = naviData->TopSpeed; |
if( naviData->RC_Quality > Config.OSD_Statistic.max_RCQuality ) Config.OSD_Statistic.max_RCQuality = naviData->RC_Quality; |
if( naviData->RC_Quality < Config.OSD_Statistic.min_RCQuality ) Config.OSD_Statistic.min_RCQuality = naviData->RC_Quality; |
if( naviData->AngleNick < Config.OSD_Statistic.min_AngleNick ) Config.OSD_Statistic.min_AngleNick = naviData->AngleNick; |
if( naviData->AngleNick > Config.OSD_Statistic.max_AngleNick ) Config.OSD_Statistic.max_AngleNick = naviData->AngleNick; |
if( naviData->AngleRoll < Config.OSD_Statistic.min_AngleRoll ) Config.OSD_Statistic.min_AngleRoll = naviData->AngleRoll; |
if( naviData->AngleRoll > Config.OSD_Statistic.max_AngleRoll ) Config.OSD_Statistic.max_AngleRoll = naviData->AngleRoll; |
} |
else if( flying && UTCTime.year != 0 ) // GELANDET -> Statistik beenden |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
flying = false; |
} |
//----------------------- |
// Check: Akku Warnung |
//----------------------- |
CheckMKLipo(); |
//---------------------------------- |
// Show: OSD-Screens |
//---------------------------------- |
if( popup_state == OSD_POPUP_NONE && (timer_osd_refresh == 0 || OSDScreenRefresh == OSD_SCREEN_REDRAW) ) |
{ |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) lcd_cls(); |
if( !mkdisplayMode ) |
ScreenCtrl_Show(); |
else |
OSD_Screen_MKDisplay(); |
timer_osd_refresh = TIME_OSD_REFRESH; |
} |
OSDScreenRefresh = OSD_SCREEN_REFRESH; |
//---------------------------------- |
// Check: MK-Error |
//---------------------------------- |
if( naviData->Errorcode != old_MKErrorcode && naviData->Errorcode <= MAX_MKERROR_NUM ) |
{ |
if( naviData->Errorcode > 0 ) // raise Error |
{ |
// Fehler aufzeichnen |
MkError_Save( naviData->Errorcode ); |
// Fehler Anzeigen |
OSD_Popup_MKError( naviData->Errorcode ); |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error (MK-Error vorhanden) |
timer2 = TIME_POPUP_MKERROR; |
popup_state = OSD_POPUP_MKERROR; |
} |
else // reset Error |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
} |
old_MKErrorcode = naviData->Errorcode; |
} |
rxd_buffer_locked = FALSE; // ready to receive new data |
//------------------------------------------- |
// hole weitere Daten vom MK (BL, Time, ...) |
// |
// muss am Schluss stehen da naviData-Buffer |
// dabei ueberschrieben wird! |
//------------------------------------------- |
OSD_MK_GetData(); // holt BL-Daten und NC-Zeit |
timer_mk_timeout = MK_TIMEOUT; |
} //end: if( rxd_buffer_locked ) // OSD-Daten |
//################################ |
//# der Rest... |
//################################ |
//-------------------------------- |
// TASTEN: MK-Timeout |
//-------------------------------- |
if( mktimeout ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
} |
//-------------------------------- |
// Popup beenden |
// wenn irgendeine Taste gedrückt oder Popup-Timeout |
//-------------------------------- |
if( !mktimeout && (popup_state != OSD_POPUP_NONE) && (get_key_press(255) || !timer2) ) // get_key_press(255) == alles an Tasten abfangen was moeglich ist |
{ |
popup_state = OSD_POPUP_NONE; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
clear_key_all(); |
} |
//-------------------------------- |
// TASTEN: KEIN mkdisplay (OSD Modus) |
//-------------------------------- |
if( !osdexit && !mktimeout && !mkdisplayMode ) |
{ |
if( get_key_short(1 << KEY_ESC) ) // PKT OSD EXIT |
{ |
osdexit = true; |
} |
if( get_key_long(1 << KEY_ESC) ) // ÊINSCHALTEN: mkdisplayMode |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = true; |
mkdisplayCmd = 0; // lesen display-Daten erzwingen |
} |
if( get_key_long (1 << KEY_ENTER) ) // User GPS-Position sichern |
{ |
GPS_User_Save(); |
} |
if( get_key_short (1 << KEY_ENTER) ) // Popup: Info |
{ |
if( popup_state == OSD_POPUP_NONE ) |
{ |
popup_state = OSD_POPUP_INFO; |
OSD_Popup_Info( ScreenCtrl_GetNum(), ScreenCtrl_GetName() ); |
timer2 = TIME_POPUP_INFO; |
} |
} |
if( get_key_press (1 << KEY_MINUS)) // previous screen |
{ |
ScreenCtrl_Previous(); |
} |
if( get_key_press (1 << KEY_PLUS)) // next Screen |
{ |
ScreenCtrl_Next(); |
} |
} |
//-------------------------------- |
// TASTEN: mkdisplay AKTIV |
//-------------------------------- |
if( !osdexit && !mktimeout && mkdisplayMode ) |
{ |
/* |
if( get_key_long(1 << KEY_ENTER) // ABSCHALTEN mkdisplayMode: longpress ENTER, ESC, MINUS, PLUS schaltet mkdisplay aus |
|| get_key_long(1 << KEY_ESC) |
|| get_key_long(1 << KEY_MINUS) |
|| get_key_long(1 << KEY_PLUS) ) |
{ |
*/ |
if( get_key_long(1 << KEY_ESC) ) // ABSCHALTEN mkdisplayMode: longpress ESC (Taste 3 von links) |
{ |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep beim Modus-Wechsel |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
mkdisplayMode = false; |
clear_key_all(); |
} |
if( get_key_short (1 << KEY_MINUS) ) mkdisplayCmd = 0xfe; // MK-Key: rechts (next page) |
if( get_key_short (1 << KEY_PLUS) ) mkdisplayCmd = 0xfd; // MK-Key: links (previous page) |
if( get_key_short (1 << KEY_ESC) ) mkdisplayCmd = 0xfb; // MK-Key: runter |
if( get_key_short (1 << KEY_ENTER) ) mkdisplayCmd = 0xf7; // MK-Key: hoch |
if( mkdisplayCmd != 0xff ) // wenn eine MK-Display Taste gedrueckt worde sofort Daten |
{ // holen und darstellen um Anzeigereaktion fuer Benutzer zu verbessern |
timer_get_displaydata = 0; // lesen erzwingen |
OSD_MK_GetData(); // Daten holen |
OSD_Screen_MKDisplay(); // MK-Display Anzeigen |
} |
} |
//----------------------- |
// abo_timer |
//----------------------- |
if( abo_timer == 0 ) |
{ |
OSD_MK_Connect( MK_ABORENEW ); |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
if( timer_mk_timeout == 0 ) |
{ |
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
set_beep ( 200, 0x0080, BeepNormal); // Beep |
mktimeout = true; |
timer_mk_timeout = MK_TIMEOUT; |
OSDScreenRefresh = OSD_SCREEN_REDRAW; |
OSD_MK_Connect( MK_CONNECT ); // 21.06.2014 OG: wieder aktviert wegen Umschaltung auf NC |
} |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
#ifdef USE_OSD_PKTHOOK |
if( PKT_CtrlHook() ) OSDScreenRefresh = OSD_SCREEN_REDRAW; // Update vom Updatetool angefordert? |
#endif |
} // END: while( !osdexit ) |
//------------------------------------------ |
// PKT-OSD beenden |
//------------------------------------------ |
Config.OSD_ScreenMode = ScreenCtrl_GetNum(); // merken letzter Screen |
//------------------------------------------ |
// ggf. Statistik Ende Zeit/Datum sichern |
//------------------------------------------ |
if( flying && UTCTime.year != 0 ) // wenn noch am fliegen |
{ |
// --- Ende Zeit/Datum Statistik sichern |
memcpy( &Config.OSD_Statistic.end_StatTime, (char *)&UTCTime, sizeof(PKTdatetime_t) ); // Ende Zeit/Datum sichern... |
} |
OSD_active = false; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osd.h |
---|
0,0 → 1,309 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osd.h |
//# |
//# 21.06.2014 OG |
//# - add: writex_altimeter() |
//# - add: draw_icon_mk() |
//# |
//# 18.06.2014 OG |
//# - add: weitere Exporte von Funktionen draw_icon...() |
//# - add: MKLiPoCells_Init(), MKLiPoCells_Check() |
//# - add: OSD_Element_Battery_Bar() |
//# - chg: define ORIENTATION_H, ORIENTATION_V verschoben nach osd.h |
//# |
//# 01.06.2014 OG |
//# Beginn von Auslagerungen von Code alter OSD-Screens nach osdold_screens.c/h |
//# |
//# 26.05.2014 OG |
//# - add: #define OSDSCREEN_WAYPOINTS |
//# |
//# 24.05.2014 OG |
//# - chg: OSD_Element_CompassDirection() - erweitert um xoffs,yoffs |
//# |
//# 28.04.2014 OG |
//# - del: OSD_Timeout() |
//# |
//# 07.07.2013 OG |
//# - add: defines fuer Screen-ID's (verwendet in setup.c, osd.c) |
//# |
//# 30.06.2013 OG |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# |
//# 31.05.2013 OG |
//# Achtung! Aenderung eeprom-Kompatibilitaet wegen typedef Erweiterung! |
//# - chg: typedef: osd_statistic_BL_t fuer Mittelwerte |
//# - chg: typedef: osd_statistic_t fuer Mittelwerte |
//# |
//# 15.05.2013 OG |
//# - add: calc_BLmax() |
//# - add: struct osd_BLmax_t |
//# |
//# 04.05.2013 OG |
//# - chg: aktualisiert Kommentare in 'osd_statistic_t' |
//# - del: OSD_Debug_XX() |
//# |
//# 02.05.2013 OG |
//# - fix: struct osd_statistic_t: max_Distance von int16_t nach uint16_t |
//# |
//# 28.04.2013 OG |
//# - chg: osd(OSDMode) -> osd(void) |
//# - del: OSDDATA_Statistics() |
//# |
//# vorheriges: siehe osd.c |
//############################################################################ |
#ifndef _OSD_H |
#define _OSD_H |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#define OSD_MAX_MOTORS 8 // max. Anzahl vom PKT unterstuetzer Motoren (absolut MAX ist 12 da der MK nicht mehr unterstuetzt) |
#define MAX_GPS_USER 10 // max. Anzahl der GPS-Positionen durch Benutzer gespeichert |
#define MAX_MKERR_LOG 20 // max. Anzahl Eintraege im MK-Error-Log |
#define OSD_SCREEN_REFRESH 0 // Screen: Werte anzeigen |
#define OSD_SCREEN_REDRAW 1 // Screen: Labels und statischer Elemente neu zeichen, Werte anzeigen |
#define ORIENTATION_V 1 // fuer OSD_Element_Battery_Bar() |
#define ORIENTATION_H 2 |
// OSD-Screen ID's |
// maximal: 31 (!) wegen Bitcodierung in Config.OSD_UseScreen |
#define OSDSCREEN_GENERAL 0 |
#define OSDSCREEN_NAVIGATION 1 |
#define OSDSCREEN_ELECTRIC 2 |
#define OSDSCREEN_MKSTATUS 3 |
#define OSDSCREEN_USERGPS 4 |
#define OSDSCREEN_3DLAGE 5 |
#define OSDSCREEN_STATISTICS 6 |
#define OSDSCREEN_OSD0 7 |
#define OSDSCREEN_OSD1 8 |
#define OSDSCREEN_OSD2 9 |
#define OSDSCREEN_WAYPOINTS 10 |
// Flags |
#define OSD_FLAG_AH 0 // Altitue Hold |
#define OSD_FLAG_PH 1 // Position Hold |
#define OSD_FLAG_CF 2 // Care Free |
#define OSD_FLAG_CH 3 // Coming Home |
#define OSD_FLAG_O1 4 // Out1 (LED 1) |
#define OSD_FLAG_O2 5 // Out2 (LED 2) |
#define OSD_FLAG_BA 6 // LowBat warning (MK) |
#define OSD_FLAG_CA 7 // Calibrate |
#define OSD_FLAG_ST 8 // Start |
#define OSD_FLAG_MR 9 // Motor Run |
#define OSD_FLAG_FY 10 // Fly |
#define OSD_FLAG_EL 11 // Emergency Landing |
#define OSD_FLAG_FS 12 // RS Failsave Active |
#define OSD_FLAG_GP 13 // GPS ok |
#define OSD_FLAG_S0 14 // GPS-Sat not ok (GPS NOT ok) |
#define OSD_FLAG_TU 15 // Vario Trim Up |
#define OSD_FLAG_TD 16 // Vario Trim Down |
#define OSD_FLAG_FR 17 // Free |
#define OSD_FLAG_RL 18 // Range Limit |
#define OSD_FLAG_SL 19 // No Serial Link |
#define OSD_FLAG_TR 20 // Target Reached |
#define OSD_FLAG_MC 21 // Manual Control |
#define OSD_FLAG_COUNT 22 |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
//----------------------------------------------------------- |
// typedef: Statistiken |
//----------------------------------------------------------- |
typedef struct |
{ |
uint16_t count; // Anzahl Werte BL-Daten (fuer Mittelwertberechnung) |
uint8_t max_Current; // in 0.1 A steps |
uint16_t avg_Current; // Mittelwert Current (*100 fuer Rechengenauigkeit) |
uint8_t max_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_statistic_BL_t; |
typedef struct |
{ |
PKTdatetime_t begin_StatTime; // Datum/Zeit |
PKTdatetime_t end_StatTime; // Datum/Zeit |
uint16_t total_FlyTime; // gesamt Flugzeit seit Stat-Init |
uint16_t last_FlyTime; // letzte Flugzeit |
uint16_t count_osd; // TODO: Anzahl Werte OSD-Daten (fuer Mittelwertberechnung) |
uint16_t count_Errorcode; // TODO: Anzahl gemeldeter MK-Errors |
int16_t max_Altimeter; // max. Hoehe |
int16_t avg_Altimeter; // TODO: Mittelwert Hoehe () |
s16 max_Variometer; // TODO: ... |
uint16_t max_GroundSpeed; // max. Geschwindigkeit |
uint16_t avg_GroundSpeed; // TODO: Mittelwert Geschwindigkeit () |
s16 max_TopSpeed; // max. velocity in vertical direction in cm/s |
uint16_t max_Distance; // max. Entfernung |
uint16_t avg_Distance; // TODO: Mittelwert Entfernung () |
uint16_t max_Current; // max. Strom |
uint16_t avg_Current; // Mittelwert Strom () |
u8 max_RCQuality; // max. Empfangsqualitaet |
u8 min_RCQuality; // min. Empfangsqualitaet |
uint16_t avg_RCQuality; // TODO: Mittelwert Empfangsqualitaet () |
uint16_t max_Capacity; // max. entnommene Kapazitaet |
s8 max_AngleNick; // max. Nick |
s8 min_AngleNick; // min. Nick |
s8 max_AngleRoll; // max. Roll |
s8 min_AngleRoll; // min. Roll |
uint8_t min_UBat; // min. Spannung (V) |
uint8_t LiPoCells; // Anzahl der LiPo Zellen |
uint8_t BL_Count; // Anzahl erkannter BL's (Motoren) |
osd_statistic_BL_t BL[OSD_MAX_MOTORS]; // Werte der einzelnen BL's |
} osd_statistic_t; |
typedef struct |
{ |
uint8_t max_BL_Current_Index; // BL-Nummer |
unsigned char max_BL_Current; // in 0.1 A steps |
uint8_t max_BL_Temp_Index; // BL-Nummer |
unsigned char max_BL_Temp; // old BL-Ctrl will return a 255 here, the new version (>= V2.0) the temp. in °C |
} osd_BLmax_t; |
//----------------------------------------------------------- |
// typedef: Aufzeichnung von MK-Errors |
//----------------------------------------------------------- |
typedef struct |
{ |
u8 Errorcode; // 0 --> okay |
PKTdatetime_t set_Time; // Datum/Zeit |
PKTdatetime_t clear_Time; // Datum/Zeit |
} mkerror_t; |
//----------------------------------------------------------- |
// typedef: PKT GPS-Positionen |
//----------------------------------------------------------- |
typedef struct |
{ |
PKTdatetime_t timestamp; // Zeitstempel: UTC |
s16 Altimeter; // barymetrische Hoehe (entspricht: naviData->Altimeter) |
u16 HomeDistance; // distance to home in cm (entspricht: naviData->HomePositionDeviation.Distance) |
GPS_Pos_t GPSData; // GPS-Position (mk-data-structs.h) |
} pkt_gpspos_t; |
//----------------------------------------------------------- |
// global var's |
//----------------------------------------------------------- |
extern volatile uint8_t OSD_active; |
extern volatile uint8_t error; |
//----------------------------------------------------------- |
// strings |
//----------------------------------------------------------- |
extern const char * const mkerrortext[]; |
//----------------------------------------------------------- |
// Funktionen |
//----------------------------------------------------------- |
void osd( void ); |
void vario_beep_output (void); |
void CheckMKLipo(void); |
void STAT_Init(void); |
void GPS_User_Init(void); |
void MKErr_Log_Init(void); |
void calc_BLmax( osd_BLmax_t *blmax ); |
void OSD_Element_Flag_Label( uint8_t xC, uint8_t yC, uint8_t item, uint8_t lOn, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Flag( uint8_t xC, uint8_t yC, uint8_t item, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Altitude( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BattLevel2( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_BatteryLevel_Bar( uint8_t x, uint8_t y ); |
void OSD_Element_BatteryLevel_Text( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_BatteryLevel( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Capacity( uint8_t x, uint8_t y ); |
void OSD_Element_CompassDegree( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_CompassDirection( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_CompassRose( uint8_t x, uint8_t y ); |
void OSD_Element_Current( uint8_t x, uint8_t y ); |
void OSD_Element_FlyingTime( uint8_t x, uint8_t y ); |
void OSD_Element_GroundSpeed( uint8_t x, uint8_t y ); |
void OSD_Element_HomeCircle( uint8_t x, uint8_t y, uint8_t breite, int8_t rOffset, int8_t xoffs, int8_t yoffs ); |
void OSD_Element_HomeDegree( uint8_t x, uint8_t y ); |
void OSD_Element_HomeDistance( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_LEDOutput( uint8_t x, uint8_t y, uint8_t bitmask ); |
void OSD_Element_LED1Output( uint8_t x, uint8_t y ); |
void OSD_Element_LED2Output( uint8_t x, uint8_t y ); |
void OSD_Element_Manuell( uint8_t x, uint8_t y ); |
void OSD_Element_RCIntensity( uint8_t x, uint8_t y ); |
void OSD_Element_SatsInUse( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_Variometer( uint8_t x, uint8_t y ); |
void OSD_Element_Target( uint8_t x, uint8_t y, uint8_t nStyle ); |
void OSD_Element_VarioWert( uint8_t x, uint8_t y ); |
void OSD_Element_WayPoint( uint8_t x, uint8_t y ); |
void OSD_Element_TargetDegree( uint8_t x, uint8_t y ); |
void OSD_Element_UpDown( uint8_t x, uint8_t y, int8_t xoffs, int8_t yoffs); |
void OSD_Element_Battery_Bar( uint8_t x, uint8_t y, uint8_t length, uint8_t width, uint8_t orientation); |
void writex_altimeter( uint8_t x, uint8_t y, s32 Altimeter, uint8_t mode, int8_t xoffs, int8_t yoffs ); |
void MKLiPoCells_Init( void ); |
void MKLiPoCells_Check( void ); |
void draw_icon_satmini( uint8_t x, uint8_t y); |
void draw_icon_satmini2( uint8_t x, uint8_t y); |
void draw_icon_home( uint8_t x, uint8_t y); |
void draw_icon_target_diamond( uint8_t x, uint8_t y); |
void draw_icon_target_round( uint8_t x, uint8_t y); |
void draw_icon_mk( uint8_t x, uint8_t y); |
void OSD_MK_ShowTimeout( void ); |
//----------------------------------------------------------- |
// EXPORTS NUR FUER osdold_screens.c |
//----------------------------------------------------------- |
extern NaviData_t *naviData; |
extern uint8_t OSDScreenRefresh; |
#endif // _OSD_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osddata.c |
---|
0,0 → 1,838 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.c |
//# |
//# 14.06.2014 OG |
//# - chg: Menu_OSDData() - Bezeichnung von "OSD Daten" und "BL Daten" geaendert |
//# zu "OSD Statistik" und "BL Statistik" |
//# |
//# 11.05.2014 OG |
//# - chg: Text von SHOWMKERROR_de, SHOWMKERROR_en etwas gekuerzt |
//# - chg: Titel von Scrollbox-Listen angepasst |
//# - chg: Menu_OSDData() umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 29.03.2014 OG |
//# - chg: Menu_OSDData() del: MenuCtrl_SetShowBatt() wegen Aenderung Default auf true |
//# - chg: ask_delete_data() - Layout und Optimierung |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# - del: OSDDATA_ShowLastGPS() |
//# |
//# 12.02.2014 OG |
//# - del: die includes zu den alten parameter... entfernt |
//# |
//# 24.01.2014 OG |
//# - fix: OSDDATA_ClearAllData(): Beep wenn Daten geloescht werden |
//# (wie bei den anderen Löschungen) |
//# |
//# 30.06.2013 OG |
//# - add: Gesamtstrom-Mittelwert in OSDDATA_Statistics() und OSDDATA_BLStatistics() |
//# - chg: Benamung Statistik-Var's von mid_* auf avg_* geaendert |
//# - chg: Funktionsnamen geaendert |
//# |
//# 19.06.2013 OG |
//# - add: last Flytime in OSDDATA_Statistics() |
//# - chg: Reihenfolge in OSDDATA_Statistics() und zusaetzliche Trennline |
//# - add: max Current (Gesamt) in OSDDATA_Statistics() |
//# |
//# 16.06.2013 OG |
//# - fix: "min. Voltage" Anzeige in OSDDATA_Statistics() (war veschoben) |
//# |
//# 13.06.2013 OG |
//# - chg: Uebersetzungen von CB in ask_delete_data() aufgenommen |
//# - chg: GPS-Menuetitel geaendert |
//# - fix: include pkt.h |
//# |
//# 11.06.2013 OG |
//# - chg: ask_delete_data() erweitert um Info was geloescht wird & Layout (TODO: Uebersetzungen) |
//# - add: Mittelwertanzeige fuer BL-Strom |
//# - add: Anzahl BL-Statistik-Datenpakete (nur wenn USE_OSD_SCREEN_DEBUG) |
//# - add: OSDDATA_BLStatistic() - die BL-Daten wurden in eigene Anzeigefunktion verschoben |
//# - add: OSDDATA_ClearAllData() - alle Daten loeschen |
//# - add: OSDDATA_ShowLastGPS() - letzte GPS-Position anzeigen |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - chg: PKT_CtrlHook() noch an einigen Stellen eingefuegt |
//# - fix: ctrl_osddata() clear screen vor ScrollBox_Refresh |
//# |
//# 15.05.2013 OG |
//# - chg: OSDDATA_Statistics() umgestellt auf calc_BLmax() (aus osd.c) |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# - add: weitere Anzeigen in OSD_Statistics() |
//# - add: Content in OSDDATA_UserGPS() und OSDDATA_MkError() |
//# |
//# 28.04.2013 OG |
//# - chg: auf die neuen Features von xprintf angepasst (siehe utils/xstring.c) |
//# |
//# 21.04.2013 Cebra |
//# - chg: Texte "Datenlöschen" in messages.c aufgenommen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../osd/osd.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#include "../lipo/lipo.h" |
#include "../mk/mkbase.h" |
#define INIT_STATISTIC 1 |
#define INIT_MKERROR 2 |
#define INIT_USERGPS 3 |
#define INIT_LASTPOS 4 |
#define INIT_ALLDATA 5 |
//----------------------- |
// Menu_OSDDATA |
//----------------------- |
#define ID_SHOWSTATISTIK 1 |
#define ID_SHOWBLSTATISTIK 2 |
#define ID_SHOWMKERROR 3 |
#define ID_SHOWUSERGPS 4 |
#define ID_SHOWLASTGPS 5 |
#define ID_CLEARALLDATA 6 |
//static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Daten"; |
//static const char SHOWSTATISTIK_en[] PROGMEM = "OSD data"; |
//static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Daten"; |
//static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL data"; |
static const char SHOWSTATISTIK_de[] PROGMEM = "OSD Statistik"; |
static const char SHOWSTATISTIK_en[] PROGMEM = "OSD statistics"; |
static const char SHOWBLSTATISTIK_de[] PROGMEM = "BL Statistik"; |
static const char SHOWBLSTATISTIK_en[] PROGMEM = "BL statistics"; |
static const char SHOWMKERROR_de[] PROGMEM = "MK Fehler"; |
static const char SHOWMKERROR_en[] PROGMEM = "MK errors"; |
static const char SHOWUSERGPS_de[] PROGMEM = "GPS Userdaten"; |
static const char SHOWUSERGPS_en[] PROGMEM = "GPS userdata"; |
static const char SHOWLASTGPS_de[] PROGMEM = "GPS letzte Pos."; |
static const char SHOWLASTGPS_en[] PROGMEM = "GPS last pos."; |
static const char CLEARALLDATA_de[] PROGMEM = "LÖSCHE alle Daten"; |
static const char CLEARALLDATA_en[] PROGMEM = "DELETE all data"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t ask_delete_data( uint8_t initcode ) |
{ |
uint8_t refresh = true; |
clear_key_all(); |
set_beep ( 60, 0xffff, BeepNormal); |
while( true ) |
{ |
//-------------------------------------- |
// screen refresh |
//-------------------------------------- |
if( refresh ) |
{ |
lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Headline: Box fill Black |
lcdx_printp_at( 1, 0, strGet(DELETE), MINVERS, 0,0); // Titel |
show_Lipo(); // LiPo anzeigen |
lcd_rect_round( 0, 20, 127, 27, 1, R2); // Rahmen fuer Benutzerfrage |
switch( initcode ) |
{ |
case INIT_STATISTIC: lcdx_printp_center( 2, strGet(STAT_OSDBL), MNORMAL, 0,9); break; |
case INIT_MKERROR : lcdx_printp_center( 2, strGet(STAT_ERROR), MNORMAL, 0,9); break; |
case INIT_USERGPS : lcdx_printp_center( 2, strGet(STAT_GPS) , MNORMAL, 0,9); break; |
case INIT_LASTPOS : lcdx_printp_center( 2, strGet(STAT_POS) , MNORMAL, 0,9); break; |
case INIT_ALLDATA : lcdx_printp_center( 2, strGet(STAT_ALL) , MNORMAL, 0,9); break; |
} |
lcdx_printp_center( 4, strGet(DELETE), MNORMAL, 0,4); // "löschen?" |
lcd_printp_at( 0, 7, strGet(START_LASTPOS2) , MNORMAL); // Keyline: "löschen Nein" |
refresh = false; |
} |
//-------------------------------------- |
// Update vom Updatetool angefordert? |
//-------------------------------------- |
if( PKT_CtrlHook() ) |
{ |
refresh = true; |
} |
//-------------------------------------- |
// loeschen |
//-------------------------------------- |
if( get_key_press(1 << KEY_MINUS) ) |
{ |
clear_key_all(); |
return true; // Rueckgabe: true = loeschen |
} |
//-------------------------------------- |
// Ende |
//-------------------------------------- |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return false; // Rueckgabe: false = nicht loeschen |
} |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void ctrl_osddata( uint8_t isdata, uint8_t initcode ) |
{ |
uint8_t key = 0; |
if( !isdata ) ScrollBox_Push_P( MNORMAL, PSTR(" no data...") ); |
ScrollBox_SetKey( KEY_ENTER, " Del" ); |
do |
{ |
key = ScrollBox_Show(); |
if( key == KEY_ENTER ) |
{ |
if( ask_delete_data(initcode) ) |
{ |
switch( initcode ) |
{ |
case INIT_STATISTIC: STAT_Init(); break; |
case INIT_MKERROR: MKErr_Log_Init(); break; |
case INIT_USERGPS: GPS_User_Init(); break; |
} |
set_beep ( 400, 0xffff, BeepNormal); |
key = 0; // exit |
} |
else |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
} |
} while( key ); |
} |
//-------------------------------------------------------------- |
// add_statistic_head() |
// |
// fuegt Basisdaten den OSD- und BL-Listen hinzu |
//-------------------------------------------------------------- |
void add_statistic_head( void ) |
{ |
PKTdatetime_t dtlocal; |
//--------------------------- |
// begin: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.begin_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("Begin: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
//lcd_printp_at( 3, 3, strGet(DELETEDATA), MNORMAL); // "Daten loeschen?" |
dtlocal.day, dtlocal.month |
); |
//--------------------------- |
// end: date/time |
//--------------------------- |
UTCdatetime2local( &dtlocal, &Config.OSD_Statistic.end_StatTime ); |
ScrollBox_Push_P( MNORMAL, PSTR("End: %02u:%02u %02u.%02u."), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), |
dtlocal.day, dtlocal.month |
); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete OSD-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_Statistics( void ) |
{ |
int16_t v; |
osd_BLmax_t blmax; |
lcd_cls(); |
if( !ScrollBox_Create(25) ) |
return; // kein RAM mehr |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" OSD Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// last Flytime |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Last Fly %02u:%02u min"), (Config.OSD_Statistic.last_FlyTime/60), (Config.OSD_Statistic.last_FlyTime%60) ); |
//--------------------------- |
// max. Altitude |
//--------------------------- |
v = Config.OSD_Statistic.max_Altimeter / (30 / AltimeterAdjust); |
ScrollBox_Push_P( MNORMAL, PSTR("%cAltitude%7d m"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Distance |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cDistance%7u m"), SYMBOL_MAX, Config.OSD_Statistic.max_Distance/10 ); |
//--------------------------- |
// max. Ground-Speed |
//--------------------------- |
v = (uint16_t) (((uint32_t) Config.OSD_Statistic.max_GroundSpeed * (uint32_t) 90) / (uint32_t) 250); |
ScrollBox_Push_P( MNORMAL, PSTR("%cGrnd.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//--------------------------- |
// max. Vert.-Speed |
//--------------------------- |
v = Config.OSD_Statistic.max_TopSpeed; |
v = (v*36/100); // cm/s -> km/h*10 |
ScrollBox_Push_P( MNORMAL, PSTR("%cVert.Speed%3.1d kmh"), SYMBOL_MAX, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// Used Capacity |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Capacity%7d mAh"), Config.OSD_Statistic.max_Capacity ); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR(" BL Detected%4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Curr.%4.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC"), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//--------------------------- |
// min. Voltage |
//--------------------------- |
v = (Config.OSD_Statistic.min_UBat == 255) ? 0 : Config.OSD_Statistic.min_UBat; |
ScrollBox_Push_P( MNORMAL, PSTR("%cVoltage%6.1d V"), SYMBOL_MIN, v ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// max. RC-Quality |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MAX, Config.OSD_Statistic.max_RCQuality ); |
//--------------------------- |
// min. RC-Quality |
//--------------------------- |
v = (Config.OSD_Statistic.min_RCQuality==255) ? 0 : Config.OSD_Statistic.min_RCQuality; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRC-Quality%5d"), SYMBOL_MIN, v ); |
//--------------------------- |
// max. Nick |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleNick, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Nick |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleNick==126) ? 0 : Config.OSD_Statistic.min_AngleNick; |
ScrollBox_Push_P( MNORMAL, PSTR("%cNick%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// max. Roll |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MAX, Config.OSD_Statistic.max_AngleRoll, SYMBOL_SMALLDEGREE ); |
//--------------------------- |
// min. Roll |
//--------------------------- |
v = (Config.OSD_Statistic.min_AngleRoll==126) ? 0 : Config.OSD_Statistic.min_AngleRoll; |
ScrollBox_Push_P( MNORMAL, PSTR("%cRoll%11d %c"), SYMBOL_MIN, v, SYMBOL_SMALLDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete BL-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_StatisticsBL( void ) |
{ |
uint8_t i; |
osd_BLmax_t blmax; |
lcd_cls(); |
#ifdef USE_OSD_SCREEN_DEBUG |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*6)) ) |
return; // kein RAM mehr |
#else |
if( !ScrollBox_Create(10 + (Config.OSD_Statistic.BL_Count*4)) ) |
return; // kein RAM mehr |
#endif |
//+++++++++++++++++++++++++++ |
// max. der BL's ermitteln |
//+++++++++++++++++++++++++++ |
calc_BLmax( &blmax ); |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
//--------------------------- |
// Basisanzeige der Stat-Liste |
//--------------------------- |
add_statistic_head(); |
//--------------------------- |
// max. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_MAX, Config.OSD_Statistic.max_Current ); |
//--------------------------- |
// avg. Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cCurrent%6.1d A"), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.avg_Current/100) ); |
//--------------------------- |
// BL-Detected |
//--------------------------- |
//ScrollBox_Push_P( MNORMAL, PSTR("BL Detected %4d"), Config.OSD_Statistic.BL_Count ); |
//--------------------------- |
// max. BL-Current |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Current%2.1u A"), SYMBOL_MAX, blmax.max_BL_Current_Index+1, blmax.max_BL_Current ); |
//--------------------------- |
// max. BL-Temp |
//--------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%cBL%1d Temp. %5u %cC" ), SYMBOL_MAX, blmax.max_BL_Temp_Index+1, blmax.max_BL_Temp, SYMBOL_BIGDEGREE ); |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
//--------------------------- |
// LIST: BL-Data |
//--------------------------- |
if( Config.OSD_Statistic.BL_Count > 0 ) |
{ |
ScrollBox_Push_P( MINVERS, PSTR(" BL Data") ); |
for( i=0; i < Config.OSD_Statistic.BL_Count; i++) |
{ |
ScrollBox_Push_P( MNORMAL, PSTR("%1d:%cCurrent%4.1u A"), (i+1), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Current) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cCurrent%4.1u A" ), SYMBOL_AVG, (uint8_t)(Config.OSD_Statistic.BL[i].avg_Current/100) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" %cTemp.%8u %cC" ), SYMBOL_MAX, (uint8_t)(Config.OSD_Statistic.BL[i].max_Temp), SYMBOL_BIGDEGREE ); |
#ifdef USE_OSD_SCREEN_DEBUG |
ScrollBox_Push_P( MNORMAL, PSTR(" RX-Pkg.%7u"), Config.OSD_Statistic.BL[i].count ); |
//ScrollBox_Push_P( MNORMAL, PSTR(" MC%12u A" ), Config.OSD_Statistic.BL[i].avg_Current ); |
#endif |
//+++++++++++++++++++++++++++ |
// TRENNER |
//+++++++++++++++++++++++++++ |
ScrollBox_PushLine(); |
} |
} |
ctrl_osddata( true, INIT_STATISTIC ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// zeigt aufgezeichnete Benutzer-GPS-Daten an |
//-------------------------------------------------------------- |
void OSDDATA_UserGPS( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_GPS_USER*4) + 5 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" GPS Userdata") ); |
for( i=0; i<MAX_GPS_USER; i++) |
{ |
if( Config.GPS_User[i].GPSData.Latitude != 0) // nur gueltige GPS-Koordinaten anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Nummer / Datum / Zeit |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.GPS_User[i].timestamp ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
dtlocal.day, dtlocal.month, dtlocal.year, |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60) |
); |
/* |
sec = (uint16_t)(Config.GPS_User[i].timestamp.seconds); // Sekunden |
ScrollBox_Push_P( MNORMAL, PSTR("%02u.%02u.%04u %02u:%02u:%02u"), |
Config.MKErr_Log[i].set_Time.day, Config.MKErr_Log[i].set_Time.month, Config.MKErr_Log[i].set_Time.year, |
(uint16_t)(sec/3600), (uint16_t)(sec/60%60), (uint16_t)(sec%60) |
); |
// Alternative Anzeige von Datum/Zeit: |
min = (uint16_t)(Config.GPS_User[i].timestamp.seconds/60); // Minuten |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%02u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year%100 |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u:%02u %02u.%02u.%04u"), i+1, |
(uint16_t)(min/60), (uint16_t)(min%60), |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year |
); |
// Alternative Anzeige von Datum/Zeit: |
ScrollBox_Push_P( MNORMAL, PSTR("%02u: %02u.%02u.%04u %02u:%02u"), i+1, |
Config.GPS_User[i].timestamp.day, Config.GPS_User[i].timestamp.month, Config.GPS_User[i].timestamp.year, |
(uint16_t)(min/60), (uint16_t)(min%60) |
); |
*/ |
//----------------------------------- |
// Zeile 2: Hoehe (GPS/Barymetrisch) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%7d mG%5.1d mB"), |
(Config.GPS_User[i].GPSData.Altitude/1000), // GPS-Hoehe in m |
(Config.GPS_User[i].Altimeter / (3 / AltimeterAdjust)) ); // Barymetrische-Hoehe in dm |
//----------------------------------- |
// Zeile 3: Koordinaten (Lat/Long) |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%3.6ld%3.6ld"), |
Config.GPS_User[i].GPSData.Latitude/10, |
Config.GPS_User[i].GPSData.Longitude/10 ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_USERGPS ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_MkError() |
// |
// zeigt aufgezeichnete MK-Fehler an |
//-------------------------------------------------------------- |
void OSDDATA_MkError( void ) |
{ |
uint8_t i; |
uint8_t isdata = 0; |
PKTdatetime_t dtlocal; |
lcd_cls(); |
if( !ScrollBox_Create( (MAX_MKERR_LOG*4) + 3 ) ) // Speicher reservieren |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" MK Errors") ); |
for( i=0; i<MAX_MKERR_LOG; i++) |
{ |
if( Config.MKErr_Log[i].Errorcode != 0) // nur vorhandene Errorcodes anzeigen |
{ |
isdata = 1; |
ScrollBox_PushLine(); |
//----------------------------------- |
// Zeile 1: Zeit / Datum |
//----------------------------------- |
UTCdatetime2local( &dtlocal, &Config.MKErr_Log[i].set_Time ); |
ScrollBox_Push_P( MNORMAL, PSTR("%02u:%02u:%02u %02u.%02u.%04u"), |
(uint16_t)(dtlocal.seconds/3600), (uint16_t)(dtlocal.seconds/60%60), (uint16_t)(dtlocal.seconds%60), |
dtlocal.day, dtlocal.month, dtlocal.year |
); |
//----------------------------------- |
// Zeile 2: Error Code |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR(" Code %02u"), Config.MKErr_Log[i].Errorcode ); |
//----------------------------------- |
// Zeile 3: Error Text |
//----------------------------------- |
ScrollBox_Push_P( MNORMAL, PSTR("%20S"), pgm_read_word(&mkerrortext[Config.MKErr_Log[i].Errorcode]) ); |
} |
} |
ScrollBox_PushLine(); |
ctrl_osddata( isdata, INIT_MKERROR ); |
ScrollBox_Destroy(); // free memory |
} |
//-------------------------------------------------------------- |
// OSDDATA_ShowLastGPSPosition() |
// |
// zeigt die letzte aufgezeichnete GPS-Position an. |
// Möglichkeit zum loeschen der GPS-Pos ist vorhanden. |
//-------------------------------------------------------------- |
void OSDDATA_ShowLastGPSPosition( void ) |
{ |
uint8_t redraw = true; |
clear_key_all(); |
while( true ) |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearcsreen) |
lcdx_printp_at(3, 1, strGet(START_LASTPOS) , MNORMAL, 0,4); // "Letzte Position" |
lcdx_printp_at(3, 2, strGet(START_LASTPOS3), MNORMAL, 0,4); // "Google Eingabe" |
//---- |
lcd_frect( 0, (4*7)+4, 127, 7, 1); // Rect: Invers |
lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8); // "Breitengr Längengr" |
writex_gpspos( 1, 4, Config.LastLatitude , MNORMAL, 0,10); // Anzeige: Breitengrad |
writex_gpspos( 12, 4, Config.LastLongitude, MNORMAL, -1,10); // Anzeige: Laengengrad |
lcd_rect( 0, (3*8)+7, 127, (2*8)+4, 1); // Rahmen |
//lcd_printp_at(0, 7, strGet(START_LASTPOS2), MNORMAL); // Keyline |
lcd_printp_at(12, 7, strGet(KEYLINE4) , MNORMAL); // Keyline |
lcd_printp_at(18, 7, PSTR("Del") , MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_press (1 << KEY_ESC) ) // Ende |
{ |
break; |
} |
if( (get_key_press (1 << KEY_ENTER)) ) // Del |
{ |
if( ask_delete_data(INIT_LASTPOS) ) |
{ |
set_beep ( 400, 0xffff, BeepNormal); |
WriteLastPosition( 0x00000000, 0x00000000 ); // letzte GPS Position loeschen |
break; // und beenden |
} |
redraw = true; |
} |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// OSDDATA_ClearAllData() |
// |
// löscht alle erhobenen Daten |
//-------------------------------------------------------------- |
void OSDDATA_ClearAllData( void ) |
{ |
if( ask_delete_data( INIT_ALLDATA ) ) |
{ |
STAT_Init(); |
MKErr_Log_Init(); |
GPS_User_Init(); // loeschen: User GPS |
WriteLastPosition( 0x00000000, 0x00000000 ); // loeschen: letzte GPS Position |
set_beep ( 400, 0xffff, BeepNormal); |
} |
} |
//-------------------------------------------------------------- |
// Menue fuer osddata |
//-------------------------------------------------------------- |
#ifdef USE_OSDDATA |
void Menu_OSDData( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Daten" |
//MenuCtrl_SetTitle_P( PSTR("OSD Daten") ); |
//MenuCtrl_SetShowBatt( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( ID_SHOWSTATISTIK , MENU_ITEM, &OSDDATA_Statistics, SHOWSTATISTIK_de , SHOWSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWBLSTATISTIK, MENU_ITEM, &OSDDATA_StatisticsBL, SHOWBLSTATISTIK_de, SHOWBLSTATISTIK_en ); |
MenuCtrl_PushML2_P( ID_SHOWMKERROR , MENU_ITEM, &OSDDATA_MkError, SHOWMKERROR_de , SHOWMKERROR_en ); |
MenuCtrl_PushML2_P( ID_SHOWUSERGPS , MENU_ITEM, &OSDDATA_UserGPS, SHOWUSERGPS_de , SHOWUSERGPS_en ); |
MenuCtrl_PushML2_P( ID_SHOWLASTGPS , MENU_ITEM, &OSDDATA_ShowLastGPSPosition, SHOWLASTGPS_de , SHOWLASTGPS_en ); |
MenuCtrl_PushML2_P( ID_CLEARALLDATA , MENU_ITEM, &OSDDATA_ClearAllData, CLEARALLDATA_de , CLEARALLDATA_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osddata.h |
---|
0,0 → 1,38 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osddata.h |
//# |
//# 29.03.2014 OG |
//# - add: OSDDATA_ShowLastGPSPosition() |
//# |
//# 11.06.2013 OG |
//# - add: Menu_OSDData() - ehemals in menu.c jetzt hier |
//# - del: verschiedene andere exportierte Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _OSDDATA_H |
#define _OSDDATA_H |
void OSDDATA_ShowLastGPSPosition( void ); |
void Menu_OSDData( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osdold_messages.c |
---|
0,0 → 1,117 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#include <avr/pgmspace.h> |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
#include "../eeprom/eeprom.h" |
#include "osdold_messages.h" |
//--------------------------------------------------------------------------------- |
static const char OSD_ALTI_0_de[] PROGMEM = "Höhe aus "; |
static const char OSD_ALTI_0_en[] PROGMEM = "Alti. off "; |
static const char OSD_ALTI_1_de[] PROGMEM = "Höhe begr."; |
static const char OSD_ALTI_1_en[] PROGMEM = "Alti.Limit"; |
static const char OSD_VARIO_0_de[] PROGMEM = "Vario aus "; |
static const char OSD_VARIO_0_en[] PROGMEM = "Vario off "; |
static const char OSD_VARIO_1_de[] PROGMEM = "Vario Höhe"; |
static const char OSD_VARIO_1_en[] PROGMEM = "Vario Alt."; |
static const char OSD_CARE_FREE_0_de[] PROGMEM = " "; |
#define OSD_CARE_FREE_0_en OSD_CARE_FREE_0_de |
static const char OSD_CARE_FREE_1_de[] PROGMEM = "Care Free"; |
#define OSD_CARE_FREE_1_en OSD_CARE_FREE_1_de |
static const char OSD_NAVI_MODE_0_de[] PROGMEM = "Navi aus "; |
static const char OSD_NAVI_MODE_0_en[] PROGMEM = "Navi off "; |
static const char OSD_NAVI_MODE_1_de[] PROGMEM = "Pos. halten"; |
static const char OSD_NAVI_MODE_1_en[] PROGMEM = "Pos. Hold "; |
static const char OSD_NAVI_MODE_2_de[] PROGMEM = "Coming Home"; |
#define OSD_NAVI_MODE_2_en OSD_NAVI_MODE_2_de |
static const char OSD_FLAGS_0_de[] PROGMEM = " "; |
#define OSD_FLAGS_0_en OSD_FLAGS_0_de |
static const char OSD_FLAGS_1_de[] PROGMEM = "Justieren"; |
static const char OSD_FLAGS_1_en[] PROGMEM = "Calibrate"; |
static const char OSD_FLAGS_2_de[] PROGMEM = "Start "; |
#define OSD_FLAGS_2_en OSD_FLAGS_2_de |
static const char OSD_FLAGS_3_de[] PROGMEM = "Betrieb "; |
static const char OSD_FLAGS_3_en[] PROGMEM = "Run "; |
static const char OSD_FLAGS_4_de[] PROGMEM = "Fliegen "; |
static const char OSD_FLAGS_4_en[] PROGMEM = "Fly "; |
static const char OSD_FLAGS_5_de[] PROGMEM = "Landung "; |
static const char OSD_FLAGS_5_en[] PROGMEM = "Landing "; |
static const char OSD_FLAGS_6_de[] PROGMEM = "Akku leer"; |
static const char OSD_FLAGS_6_en[] PROGMEM = "Low Bat. "; |
static const char OSD_LED_Form_de[] PROGMEM = "Out1/2 Format"; |
static const char OSD_LED_Form_en[] PROGMEM = "Out1/2 format"; |
//------------------------------------------------------------------------------ |
const char * const strings_osdold[] PROGMEM= |
{ |
OSD_ALTI_0_de, OSD_ALTI_0_en, |
OSD_ALTI_1_de, OSD_ALTI_1_en, |
OSD_VARIO_0_de, OSD_VARIO_0_en, |
OSD_VARIO_1_de, OSD_VARIO_1_en, |
OSD_CARE_FREE_0_de, OSD_CARE_FREE_0_en, |
OSD_CARE_FREE_1_de, OSD_CARE_FREE_1_en, |
OSD_NAVI_MODE_0_de, OSD_NAVI_MODE_0_en, |
OSD_NAVI_MODE_1_de, OSD_NAVI_MODE_1_en, |
OSD_NAVI_MODE_2_de, OSD_NAVI_MODE_2_en, |
OSD_FLAGS_0_de, OSD_FLAGS_0_en, |
OSD_FLAGS_1_de, OSD_FLAGS_1_en, |
OSD_FLAGS_2_de, OSD_FLAGS_2_en, |
OSD_FLAGS_3_de, OSD_FLAGS_3_en, |
OSD_FLAGS_4_de, OSD_FLAGS_4_en, |
OSD_FLAGS_5_de, OSD_FLAGS_5_en, |
OSD_FLAGS_6_de, OSD_FLAGS_6_en, |
OSD_LED_Form_de, OSD_LED_Form_en, |
//****************************************************************** |
// hier stehen lassen, alle neuen Strings hier drüber einfügen |
//LAST_STR_de, LAST_STR_en, |
}; |
char const * strGetOSDOLD( int str_no ) |
{ |
if( Config.DisplayLanguage > 1 ) Config.DisplayLanguage = 1; |
if( Config.DisplayLanguage == 0 ) return (const char*) pgm_read_word( &strings_osdold[str_no*2] ); |
if( Config.DisplayLanguage == 1 ) return (const char*) pgm_read_word( &strings_osdold[(str_no*2)+1] ); |
return (const char*) pgm_read_word( &strings_osdold[0] ); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osdold_messages.h |
---|
0,0 → 1,40 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um messages.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_messages.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Texten aus messages.c speziell fuer die alten und nicht mehr |
//# unterstuetzten OSD-Screens |
//############################################################################ |
#ifndef OSDOLD_MESSAGES_H |
#define OSDOLD_MESSAGES_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
//--------------------------------------------------------------------------------------------------------------------- |
// Typdefinitionen für alle verwendeten Strings |
//LAST_STR muss am Ende stehen bleiben |
typedef enum |
{ |
OSD_ALTI_0, OSD_ALTI_1, OSD_VARIO_0, OSD_VARIO_1, OSD_CARE_FREE_0, OSD_CARE_FREE_1, |
OSD_NAVI_MODE_0, OSD_NAVI_MODE_1, OSD_NAVI_MODE_2, |
OSD_FLAGS_0, OSD_FLAGS_1, OSD_FLAGS_2, OSD_FLAGS_3, OSD_FLAGS_4, OSD_FLAGS_5, OSD_FLAGS_6, |
OSD_LED_Form |
} OSDOLDSTR; |
char const * strGetOSDOLD( int str_no ); |
#endif // USE_OSD_SCREEN_OLD |
#endif /* OSDOLD_MESSAGES_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osdold_screens.c |
---|
0,0 → 1,271 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.c |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../uart/usart.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../sound/pwmsine8bit.h" |
#include "../mk-data-structs.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../utils/xutils.h" |
#include "../mk/mkbase.h" |
#include "../osd/osdold_messages.h" |
#include "../osd/osdold_screens.h" |
#ifdef USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// OSD_Element_AltitudeControl( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ) |
{ |
//--------------------------------------------------------- |
// 10.03.2013 OG: |
// CFG2_HEIGHT_LIMIT im Augenblick nicht unterstuetzt |
// Siehe Anmerkungen in osd() |
//--------------------------------------------------------- |
/* |
if (Flags_ExtraConfig & CFG2_HEIGHT_LIMIT) |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGet(OSD_ALTI_1), 0); // Höhe begr. |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_ALTI_0), 0); // Höhe aus |
} |
else |
{ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
*/ |
if (naviData->FCStatusFlags2 & FC_STATUS2_ALTITUDE_CONTROL) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_1), 0); // Vario Höhe |
else |
lcd_printp_at (x, y, strGetOSDOLD(OSD_VARIO_0), 0); // Vario aus |
} |
//-------------------------------------------------------------- |
// OSD_Element_CareFree( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_CareFree( uint8_t x, uint8_t y ) |
{ |
lcd_printp_at (x, y, strGetOSDOLD( naviData->FCStatusFlags2 & FC_STATUS2_CAREFREE ? OSD_CARE_FREE_1 : OSD_CARE_FREE_0 ), 0); |
} |
//-------------------------------------------------------------- |
// OSD_Element_NaviMode( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ) |
{ |
if (naviData->NCFlags & NC_FLAG_FREE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_0), 0); // Navi aus |
else if (naviData->NCFlags & NC_FLAG_PH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_1), 0); // Pos. Hold |
else if (naviData->NCFlags & NC_FLAG_CH) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_NAVI_MODE_2), 0); // Coming Home |
} |
//-------------------------------------------------------------- |
// OSD_Element_StatusFlags( x, y) |
//-------------------------------------------------------------- |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ) |
{ |
// FC_StatusFlags 0.88 |
// #define FC_STATUS_MOTOR_RUN 0x01 |
// #define FC_STATUS_FLY 0x02 |
// #define FC_STATUS_CALIBRATE 0x04 |
// #define FC_STATUS_START 0x08 |
// #define FC_STATUS_EMERGENCY_LANDING 0x10 |
// #define FC_STATUS_LOWBAT 0x20 |
// #define FC_STATUS_VARIO_TRIM_UP 0x40 |
// #define FC_STATUS_VARIO_TRIM_DOWN 0x80 |
if (naviData->FCStatusFlags & FC_STATUS_CALIBRATE) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_1), 0); // Calibrate |
else if (naviData->FCStatusFlags & FC_STATUS_START) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_2), 0); // Start |
else if (naviData->FCStatusFlags & FC_STATUS_MOTOR_RUN) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_3), 0); // Run |
else if (naviData->FCStatusFlags & FC_STATUS_FLY) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_4), 0); // Fly |
else if (naviData->FCStatusFlags & FC_STATUS_EMERGENCY_LANDING) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_5), 0); // Landing |
else if (naviData->FCStatusFlags & FC_STATUS_LOWBAT) |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_6), 0); // LowBat |
else |
// lcd_printp_at (x, y, PSTR(" "), 0); // Clear |
lcd_printp_at (x, y, strGetOSDOLD(OSD_FLAGS_0), 0); // Clear |
} |
//############################################################## |
//# OLD OSD-Screens |
//############################################################## |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 0 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD0( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 3); |
OSD_Element_Altitude ( 11, 3, 0); |
OSD_Element_BatteryLevel ( 0, 7, 0); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 5); |
OSD_Element_CompassDegree ( 13, 0, 0); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 0, 1); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 16, 4, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 12, 4); |
OSD_Element_HomeDistance ( 10, 5, 0); |
OSD_Element_Target ( 10, 6, 0); |
//OSD_Element_TargetDegree ( x, y); |
OSD_Element_WayPoint ( 5, 6); |
OSD_Element_LED1Output ( 0, 6); |
OSD_Element_LED2Output ( 3, 6); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 4); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 12, 2); |
OSD_Element_SatsInUse ( 18, 2, 0); |
OSD_Element_StatusFlags ( 0, 2); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 1 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD1( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
//OSD_Element_AltitudeControl( x, y); |
OSD_Element_Altitude ( 1, 1, 1); |
OSD_Element_BatteryLevel ( 0, 7, 1); |
OSD_Element_Capacity ( 13, 7); |
OSD_Element_Current ( 8, 7); |
//OSD_Element_CareFree ( x, y); |
OSD_Element_CompassDegree ( 13, 0, 1); |
OSD_Element_CompassDirection( 18, 0, 0,0); |
OSD_Element_CompassRose ( 12, 1); |
OSD_Element_FlyingTime ( 7, 6); |
OSD_Element_GroundSpeed ( 0, 0); |
OSD_Element_HomeCircle ( 1, 3, 7, 0, 0,0); |
OSD_Element_HomeDegree ( 8, 3); |
OSD_Element_HomeDistance ( 7, 2, 1); |
//OSD_Element_Target ( x, y, 1); |
//OSD_Element_TargetDegree ( x, y); |
//OSD_Element_WayPoint ( x, y); |
//OSD_Element_LED1Output ( x, y); |
//OSD_Element_LED2Output ( x, y); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 8, 5); |
OSD_Element_RCIntensity ( 15, 6); |
OSD_Element_VarioWert ( 15, 2); |
OSD_Element_SatsInUse ( 8, 4, 1); |
//OSD_Element_StatusFlags ( x, y); |
OSD_Element_Variometer ( 9, 0); |
} |
//-------------------------------------------------------------- |
// ehemals: OSD_ALTITUDE,Config.OSD_ScreenMode == 2 |
//-------------------------------------------------------------- |
void OSD_Screen_OSD2( void ) |
{ |
//uint8_t xoffs; |
//----------------------------------------- |
// REDRAW static screen elements |
//----------------------------------------- |
if( OSDScreenRefresh == OSD_SCREEN_REDRAW ) |
{ |
// do things here for static screen elements like labels and so.... |
} |
OSD_Element_AltitudeControl ( 0, 1); |
OSD_Element_Altitude ( 0, 4, 2); |
OSD_Element_BatteryLevel ( 13, 7, 2); |
OSD_Element_Capacity ( 0, 7); |
OSD_Element_Current ( 8, 7); |
OSD_Element_CareFree ( 0, 3); |
OSD_Element_CompassDegree ( 12, 3, 2); |
//OSD_Element_CompassDirection( x, y, 0,0); |
//OSD_Element_CompassRose ( x, y); |
OSD_Element_FlyingTime ( 15, 5); |
//OSD_Element_GroundSpeed ( x, y); |
OSD_Element_HomeCircle ( 16, 0, 5, 0, 0,0); |
OSD_Element_HomeDegree ( 11, 5); |
OSD_Element_HomeDistance ( 0, 5, 2); |
OSD_Element_Target ( 0, 6, 2); |
OSD_Element_TargetDegree ( 11, 6); |
//OSD_Element_WayPoint ( x, y); |
OSD_Element_LED1Output ( 12, 2); |
OSD_Element_LED2Output ( 14, 2); |
//OSD_Element_Manuell ( x, y); |
OSD_Element_NaviMode ( 0, 2); |
//OSD_Element_RCIntensity ( x, y); |
OSD_Element_VarioWert ( 15, 4); |
OSD_Element_SatsInUse ( 10, 0, 2); |
OSD_Element_StatusFlags ( 0, 0); |
//OSD_Element_Variometer ( x, y); |
} |
#endif // ifdef: USE_OSD_SCREEN_OLD |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/osd/osdold_screens.h |
---|
0,0 → 1,35 |
/***************************************************************************** |
* hier wird Code von den alten OSD-Screens ausgelagert um osd.c zu entkernen |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY osdold_screens.h |
//# |
//# 01.06.2014 OG * NEU * |
//# Auslagerung von Funktionen fuer alte OSD-Screens aus osd.c |
//############################################################################ |
#ifndef _OSDOLD_SCREENS_H |
#define _OSDOLD_SCREENS_H |
#include "../main.h" |
#ifdef USE_OSD_SCREEN_OLD |
void OSD_Element_AltitudeControl( uint8_t x, uint8_t y ); |
void OSD_Element_CareFree( uint8_t x, uint8_t y ); |
void OSD_Element_NaviMode( uint8_t x, uint8_t y ); |
void OSD_Element_StatusFlags( uint8_t x, uint8_t y ); |
void OSD_Screen_OSD0( void ); |
void OSD_Screen_OSD1( void ); |
void OSD_Screen_OSD2( void ); |
#endif // USE_OSD_SCREEN_OLD |
#endif // _OSDOLD_SCREENS_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/pkt/pkt.c |
---|
0,0 → 1,1227 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.c |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_Ask(), PKT_Ask_P() auf PKT_AskX() angepasst |
//# - add: PKT_AskX() - ehemals _pkt_ask(), jetzt erweitert um zusaetzliche progmen Parameter und exportiert |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() (ehemals in setup.c) |
//# - chg: PKT_Update() auf PKT_Ask_P() umgestellt |
//# - add: PKT_Ask(), PKT_Ask_P(), _pkt_ask() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P(), _pkt_title() |
//# - chg: PKT_TitleFromMenuItem() umgestellt auf _pkt_title() |
//# - chg: PKT_TitlePKTVersion() umbenannt zu PKT_TitlePKT() |
//# - fix: PKT_TitleFromMenuItem() funktioniert jetzt auch wenn kein Menue vorhanden ist |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - fix: PKT_CtrlHook() bzgl. PKT_CheckUpdate() - seit Februar 2014 Probleme |
//# mit Update-Anforderung vom PKT-Updatetool -> PKT war zu schnell |
//# Es wurde jetzt ein Timer in PKT_CtrlHook() eingebaut der den Aufruf |
//# von PKT_CheckUpdate() etwas einbremst ohne das PKT auszubremsen. |
//# Ich hoffe, das es ist Loesung ist (bin guter Dinge nach Test's) |
//# - chg: PKT_CheckUpdate() - Code optimiert |
//# - add: PKT_Info() - Anzeige ob SV2-Patch vorhanden ist oder nicht (ganz oben) |
//# - add: PKT_Info() - Anzeige fuer USE_OSD_SCREEN_WAYPOINTS |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# - add: #include "../utils/menuctrl.h" |
//# |
//# 06.05.2014 OG |
//# - chg: PKT_CheckUpdate() - seit Februar funktioniert die Update-Anforderung |
//# vom PKT-Updatetool an das PKT nicht immer zuverlaessig - fgf. weil sich |
//# seitens des PKT's Timings etwas geaendert haben (durch weglassen von Code). |
//# Hier ein Versuch etwas daran wieder zu aendern... its zwar weiterhin |
//# nicht immer zuverlaessig aber vieleicht etwas besser. |
//# Wahrscheinlich kann man das nur richtig korrigieren in Verbindung mit |
//# Anpassungen am PKT-Updatetool - aber das ist ein anderes Thema... |
//# - chg: PKT_Update() - ein kurzer Beep wenn das PKT-Update aufgerufen wird |
//# |
//# 05.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P(), _pkt_popup() |
//# |
//# 11.04.2014 OG |
//# - chg: _pkt_message() ergaenzt um clear_key_all() |
//# |
//# 09.04.2014 OG |
//# - chg: PKT_Update() - umgestellt auf ShowTitle_P() |
//# |
//# 08.04.2014 OG |
//# - chg: PKT_Update() - Text bzgl. "Druecke Start..." zentriert |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# - add: _pkt_message() zeigt einen Titel mit PKT Version und Lipo an |
//# |
//# 03.04.2014 OG |
//# - chg: _pkt_message() abgesichert bzgl. zu langen Texten |
//# |
//# 27.03.2014 OG |
//# - chg: PKT_SwitchOff() Anzeige optimiert |
//# |
//# 21.02.2014 OG |
//# - fix: PKT_CheckUpdate() Zeile 0 flimmerte wegen einer nicht mehr |
//# benoetigten Textausgabe |
//# - chg: PKT_SwitchOff() auf PKT_TitlePKTVersion() angepasst |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P(), _pkt_message() |
//# - chg: MK_Info() auf USE_MKSETTINGS angepasst (ehemals MKPARAMETER) |
//# |
//# 13.02.2014 OG |
//# - chg: Screenlayout von PKT_Update() |
//# - chg: Screenlayout von PKT_SwitchOff() |
//# |
//# 11.02.2014 OG |
//# - chg: PKT_Info() Anzeige der FC-Version entfernt |
//# |
//# 04.02.2014 OG |
//# - fix: #include "../lipo/lipo.h" hinzugefuegt |
//# |
//# 03.02.2014 OG |
//# - chg: Layout von PKT_SwitchOff() und bessere Unterstuetzung von PKT_CtrlHook() |
//# - add: PKT_ShowTitleVersion() |
//# |
//# 30.01.2014 OG |
//# - chg: geaenderte Anzeige der Reihenfolgen unterstuetzter Module in PKT_Info() |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 24.01.2014 OG |
//# - chg: PKT_SwitchOff(): Layout; Code eliminiert; Beep; |
//# add PKT_Hook() (für PKT-Update) |
//# - chg: PKT_Update(): Layout |
//# |
//# 21.10.2013 CB |
//# - add: PKT_Info WyFly hinzugefügt |
//# |
//# 07.07.2013 OG |
//# - del: PKT_Info(): USE_OSD_SCREEN_ELECTRIC_N |
//# 26.06.2013 OG |
//# - del: PKT_Info(): USE_PKT_STARTINFO |
//# |
//# 13.06.2013 OG |
//# - add: PKT-Hardwareversion in der Titelanzeige von PKT_Info() |
//# - chg: Layout Titelanzeige von PKT_Info() |
//# - del: USE_PCFASTCONNECT in PKT_Info() (nicht mehr benoetigt) |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 20.05.2013 OG |
//# - chg: PKT_CtrlHook() bei PKT-Update um WriteParameter() ergaenzt |
//# |
//# 19.05.2013 OG |
//# - add: PKT_CtrlHook() ruft ggf. PKT_Update() auf und kann spaeter weitere |
//# PKT interne Sachen verarbeiten wie z.B. senden von Daten an den PC |
//# - chg: PKT_CheckUpdate() umgestellt auf timer_pktctrl um Konflikte mit |
//# mit z.B. osd zu vermeiden |
//# - chg: PKT_Info() bzgl. Codeverbrauch verringert |
//# - fix: PKT_Info() Anzeige von USE_OSD_SCREEN_NAVIGATION |
//# - add: PC_Fast_Connect() aus menu.c |
//# - add: PKT_* Funktionen aus main.c bzw. menu.c |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <util/atomic.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../mk-data-structs.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../utils/scrollbox.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
#include "pkt.h" |
//############################################################################################# |
//############################################################################################# |
uint8_t pkt_progress_act = 0; |
uint8_t pkt_progress_max = 0; |
int8_t pkt_progress_yoffs = 0; |
uint8_t pkt_progress_width = 0; |
uint8_t pkt_progress_height = 0; |
#define PROGRESS_YOFFS -8 |
#define PROGRESS_WIDTH 96 |
//############################################################################################# |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height) |
{ |
pkt_progress_act = 0; |
pkt_progress_max = max; |
pkt_progress_yoffs = yoffs; |
pkt_progress_width = 0; // Parameter width - aktuell nicht unterstuetzt |
pkt_progress_height = 0; // Parameter height - aktuell nicht unterstuetzt |
if( pkt_progress_width == 0 ) |
pkt_progress_width = PROGRESS_WIDTH; |
lcd_rect_round( 13, 32+pkt_progress_yoffs, 102, 6, 1, R1); // Rahmen fuer Progress |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_Progress_Next( void ) |
{ |
uint8_t width; |
pkt_progress_act++; // Progress: hochzaehlen |
width = (pkt_progress_act*pkt_progress_width) / pkt_progress_max; // Progress: Breite berechnen |
lcd_frect( 16, 34+pkt_progress_yoffs, width, 2, 1); // Progress: anzeigen |
return true; |
} |
//############################################################################################# |
//# Anzeigefunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
// PKT_KeylineUpDown( xUp, xDown, xoffs,yoffs) |
// |
// gibt in der Keyline (y=7) ein Up- und Down-Zeichen aus. |
// Das Up-Zeichen wird dabei um einen Pixel versetzt damit |
// es besser aussieht |
// |
// Parameter: |
// xUp : x-Position des Up-Zeichens in Zeile 7 |
// xDown: x-Position des Down-Zeichens in Zeile 7 |
// xoffs, yoffs: normalerweise 0,0 |
//-------------------------------------------------------------- |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs) |
{ |
lcdx_printp_at( xUp , 7, PSTR("\x1a") , MNORMAL, xoffs,yoffs+1); // Up |
lcdx_printp_at( xDown, 7, PSTR("\x1b") , MNORMAL, xoffs,yoffs); // Down |
} |
//############################################################################################# |
//# Titel Funktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void _pkt_title( uint8_t progmem, const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
const char *pMask; |
if( clearscreen ) lcd_cls(); |
lcd_frect( 0, 0, 127, 7, 1); // Titel: Invers |
if( progmem ) pMask = PSTR("%19S"); |
else pMask = PSTR("%19s"); |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, pMask, text ); |
if( lShowLipo ) show_Lipo(); |
} |
//-------------------------------------------------------------- |
// PKT_Title( text, lShowLipo, clearscreen ) |
// |
// text im RAM |
//-------------------------------------------------------------- |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( false, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Title_P( text, lShowLipo, clearscreen ) |
// |
// text im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ) |
{ |
_pkt_title( true, text, lShowLipo, clearscreen ); |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// optional: rechts die PKT Lipo-Spannung anzeigen |
//-------------------------------------------------------------- |
void PKT_TitlePKTlipo( uint8_t lShowLipo ) |
{ |
//PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), true, false ); // showlipo, kein clearscreen |
PKT_Title( buffered_sprintf_P(PSTR("PKT v%S"),PSTR(PKTSWVersion)), lShowLipo, true ); // showlipo und clearscreen |
} |
//-------------------------------------------------------------- |
// zeigt als Titel die PKT-Version (invers) |
// und rechts die PKT Lipo-Spannung |
//-------------------------------------------------------------- |
void PKT_TitlePKT( void ) |
{ |
PKT_TitlePKTlipo( true ); |
} |
//-------------------------------------------------------------- |
// PKT_TitleFromMenuItem( lShowLipo ) |
// |
// zeigt eine PKT-Titelzeile mit Text vom aufrufenden Menuepunkt |
// optional kann die PKT LiPo-Spannung angezeigt werden |
// |
// wenn kein Menue vorhandenist wird die PKT-Version angezeigt |
//-------------------------------------------------------------- |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ) |
{ |
const char *pStr; |
uint8_t isPGM; |
if( MenuCtrl_GetMenuIndex() < 0 ) |
{ |
PKT_TitlePKT(); // kein Menue vorhanden -> PKT-Version anzeigen |
return; |
} |
pStr = MenuCtrl_GetItemText(); |
isPGM = MenuCtrl_IsItemTextPGM(); |
_pkt_title( isPGM, pStr, lShowLipo, true ); // true = clearscreen |
} |
//############################################################################################# |
//# PKT Message |
//############################################################################################# |
//-------------------------------------------------------------- |
// _pkt_message( progmem, text, error, timeout, beep, clearscreen) |
// |
// INTERN |
// fuer: PKT_Message(), PKT_Message_P() |
// Parameter? -> siehe dort... |
//-------------------------------------------------------------- |
void _pkt_message( uint8_t progmem, const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
char *pStr; |
if( clearscreen ) |
lcd_cls(); |
PKT_TitlePKT(); |
if( error ) |
{ |
//lcd_frect_round( 0+36, (2*8)-2, 127-73, 8+2, 1, R1); // Fill: Schwarz |
lcd_frect_round( 0+36, (2*8)-1, 127-74, 8+2, 1, R1); // Fill: Schwarz |
lcdx_printp_center( 2, strGet(STR_ERROR), MINVERS, 0,1); // "FEHLER" |
} |
else |
lcdx_printp_center( 2, strGet(STR_PKT) , MNORMAL, 0,1); // "PKT" |
//----------------------- |
// Ausgabe der Nachricht |
//----------------------- |
pStr = buffered_sprintf_P( ( progmem ? PSTR("%20S") : PSTR("%20s")), text); // max. 20 Zeichen |
strrtrim( pStr ); // alle Leerzeichen rechts loeschen |
lcdx_print_center( 4, (uint8_t *)pStr, MNORMAL, 0,5); // Text zentriert; String im RAM |
lcd_rect_round( 0, 32, 127, 16, 1, R2); // Rahmen |
if( beep ) |
{ |
if( error ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
} |
if( timeout ) |
{ |
//lcd_printp_at( 19, 7, strGet(OK), MNORMAL); // Keyline... tja, welche Variante nehmen wir denn nun Final??? OK oder ENDE? |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
//if( get_key_short(1 << KEY_ENTER) ) break; |
if( get_key_short(1 << KEY_ESC) ) break; |
PKT_CtrlHook(); |
} |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Message( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im RAM) |
// |
// Parameter: |
// text : String im RAM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clearscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( false, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
// |
// zeigt eine Nachricht an ggf. mit Display-Timeout, Tasten, Beep (String im PROGMEM) |
// |
// Parameter: |
// text : String im PROGMEM |
// error : true/false |
// timeout : 0 = direktes Verlassen der Funktion ohne warten |
// 1..nnnn = max. Anzeigezeit (timer); Keyline wird |
// eingeblendet -> Benutzer kann auch eher beenden |
// beep : true/false - bei true haengt die Art des Beep's von 'error' ab |
// clrscreen: true/false - ggf. Bildschirm loeschen |
//-------------------------------------------------------------- |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
{ |
_pkt_message( true, text, error, timeout, beep, clearscreen ); |
} |
//-------------------------------------------------------------- |
// PKT_Message_Datenverlust( timeout, beep) |
//-------------------------------------------------------------- |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ) |
{ |
//_pkt_message( true, text, error, timeout, beep, clearscreen ); |
_pkt_message( true, strGet(ERROR_NODATA), true, timeout, beep, true ); |
} |
//############################################################################################# |
//# PKT Popup |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
// |
// PARAMETER: |
// progmem : true = Texte in PROGMEM |
// false = Texte im RAM (RAM geht noch nicht!) |
// timeout : Zeit bis zum automatischen beenden (z.B. 400 = 4 Sekunden) |
// wenn timeout == 0 dann direkt beenden ohne auf Tastendruck zu warten |
// text1..4: Text (die Hoehe des Popups berechnet sich aus der Anzahl der Texte) |
// -> ein nicht benutzer Text wird mit NULL oder Dezimal 0 uebergeben |
//-------------------------------------------------------------- |
void _pkt_popup( uint8_t progmem, uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4) |
{ |
uint8_t n = 0; |
uint8_t yoffs = 0; |
if( text1 ) { n = 1; yoffs = 1; } |
if( text2 ) { n = 2; yoffs = 2; } |
if( text3 ) { n = 3; yoffs = 2; } |
if( text4 ) { n = 4; yoffs = 3; } |
Popup_Draw(n+1); |
if( text1 ) lcdx_printp_center( 8-n, text1, MINVERS, 0,-5-yoffs); // Text zentriert; String im PROGMEM |
if( text2 ) lcdx_printp_center( 9-n, text2, MINVERS, 0,-4-yoffs); // Text zentriert; String im PROGMEM |
if( text3 ) lcdx_printp_center( 10-n, text3, MINVERS, 0,-3-yoffs); // Text zentriert; String im PROGMEM |
if( text4 ) lcdx_printp_center( 11-n, text4, MINVERS, 0,-2-yoffs); // Text zentriert; String im PROGMEM |
if( timeout ) |
{ |
clear_key_all(); |
timer2 = timeout; // max. Anzeigezeit (z.B. 800 = 8 Sekunden) |
while( timer2 > 0 ) |
{ |
if( get_key_short(1<<KEY_ESC) |
|| get_key_short(1<<KEY_ENTER) |
|| get_key_short(1<<KEY_PLUS) |
|| get_key_short(1<<KEY_MINUS) ) |
{ |
break; |
} |
} |
lcd_frect( 0, 58-((n+1)*8), 127, 5+((n+1)*8), 0); // Box clear - der Bereich des Popup's wird wieder geloescht (nur wenn timeout > 0!) |
} |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// PKT_Popup( timeout, text1, text2, text3, text4 ) |
// |
// Texte im RAM |
//-------------------------------------------------------------- |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( false, timeout, text1, text2, text3, text4 ); |
} |
//-------------------------------------------------------------- |
// PKT_Popup_P( timeout, text1, text2, text3, text4 ) |
// |
// Texte im PROGMEM |
//-------------------------------------------------------------- |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ) |
{ |
_pkt_popup( true, timeout, text1, text2, text3, text4 ); |
} |
//############################################################################################# |
//# PKT Ask |
//############################################################################################# |
//-------------------------------------------------------------- |
// wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
// |
// PARAMETER: |
// asktype : ASK_YES_NO, ASK_NO_YES, ASK_CANCEL_OK, ASK_END_OK, ASK_END_START |
// text1 : 1. Zeile Text |
// text2 : optionale 2. Zeile Text |
// text_progmem : text1 & text2 in PROGMEM oder RAM (PROGMEM = true) |
// headline : Ueberschrift |
// headline_progmem: -> PROGMEM oder RAM |
// title : ganz oben der Titel (Zeile 0, Invers) |
// title_progmem : -> PROGMEM oder RAM |
// |
// RUECKGABE: |
// true : Ja |
// false : Nein |
//-------------------------------------------------------------- |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ) |
{ |
uint8_t redraw = true; |
uint8_t keyenter = false; |
uint8_t retcode = false; |
const char *pMaskP; |
const char *pMaskR; |
const char *pMask; |
pMaskP = PSTR("%S"); |
pMaskR = PSTR("%s"); |
set_beep( 35, 0xffff, BeepNormal ); // kurzer Beep |
while( true ) |
{ |
//------------------------ |
// Screen zeichnen |
//------------------------ |
if( redraw ) |
{ |
if( title == NULL ) PKT_TitlePKT(); // Titel mit PKT-Version anzeigen (und clearscreen) |
else if( title_progmem ) PKT_Title_P( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (PROGMEM) |
else PKT_Title( title, true, true ); // uebergebenen Titel anzeigen (und clearscreen) (RAM) |
if( text2 == NULL ) |
{ |
// 1 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,2, pMask, headline); // headline |
lcd_rect_round( 0, 3*7+2+10, 127, 16+0, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,6, pMask, text1); // text1 |
} |
else |
{ |
// 2 zeiliger Text |
pMask = (headline_progmem ? pMaskP : pMaskR); // Formatmaske fuer headline je nach PROGMEN oder RAM |
lcdx_printf_center_P( 2, MNORMAL, 0,-1, pMask, headline); // headline |
lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
pMask = (text_progmem ? pMaskP : pMaskR); // Formatmaske fuer text1 und text2 je nach PROGMEN oder RAM |
lcdx_printf_center_P( 4, MNORMAL, 0,-1, pMask, text1); // text1 |
lcdx_printf_center_P( 5, MNORMAL, 0, 0, pMask, text2); // text2 |
} |
//------------------------ |
// Keyline |
//------------------------ |
switch( asktype ) |
{ |
case ASK_NO_YES: lcd_printp_at(12, 7, strGet(NOYES), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_YES_NO: lcd_printp_at(12, 7, strGet(YESNO), MNORMAL); // Keyline: "Nein Ja" |
break; |
case ASK_CANCEL_OK: lcd_printp_at(11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr. OK" |
lcd_printp_at(19, 7, PSTR("OK") , MNORMAL); |
break; |
case ASK_END_OK: lcd_printp_at(12, 7, strGet(KEYLINE4), MNORMAL); // Keyline: "Ende OK" |
break; |
case ASK_END_START: lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline: "Ende Start" |
break; |
} |
redraw = false; |
} |
//------------------------ |
// LiPo Spannung zeigen |
//------------------------ |
show_Lipo(); |
//------------------------ |
// ggf. auf PKT-Update reagieren |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Tasten abfragen |
//------------------------ |
if( get_key_short(1 << KEY_ESC) ) // 3. Taste |
{ |
break; |
} |
if( get_key_press(1 << KEY_ENTER) ) // 4. Taste |
{ |
keyenter = true; |
break; |
} |
} // end: while( true ) |
//------------------------- |
retcode = keyenter; |
if( asktype == ASK_YES_NO ) retcode = !retcode; |
clear_key_all(); |
return retcode; |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im RAM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, false, headline, true, title, true); |
//return _pkt_ask( false, asktype, text1, text2, headline, title); |
} |
//-------------------------------------------------------------- |
// wahl = PKT_Ask_P( asktype, text1, text2, headline, title) |
// |
// text1 und text2 im PROGMEM |
//-------------------------------------------------------------- |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ) |
{ |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
return PKT_AskX( asktype, text1, text2, true, headline, true, title, true); |
//return _pkt_ask( true, asktype, text1, text2, headline, title); |
} |
//############################################################################################# |
//# PKT Gauge |
//############################################################################################# |
uint8_t Gauge_px; |
uint8_t Gauge_py; |
uint8_t Gauge_rx; |
uint8_t Gauge_ry; |
uint8_t Gauge_step; |
volatile uint8_t Gauge_active; // wird in timer.c abgefragt |
#define GAUGE_DEFAULT_PX 64 // Default X (Pixel) |
#define GAUGE_DEFAULT_PY 40 // Default Y (Pixel) |
#define GAUGE_DEFAULT_RX 10 // Default Radius X Gauge-Kreis |
#define GAUGE_DEFAULT_RY 8 // Default Radius Y Gauge-Kreis |
#define GAUGE_TIMER 40 // alle n aktualisieren (Timer Intervall) |
#define GAUGE_DEGREE 45 // Winkel von Schritt zu Schritt |
#define GAUGE_LROFFS -2 // die Laenge der Gauge-Linie verringern/vergroessern |
//-------------------------------------------------------------- |
// PKT_Gauge_Begin( py ) |
//-------------------------------------------------------------- |
void PKT_Gauge_Begin( uint8_t py ) |
{ |
Gauge_px = GAUGE_DEFAULT_PX; |
Gauge_py = GAUGE_DEFAULT_PY; |
Gauge_rx = GAUGE_DEFAULT_RX; |
Gauge_ry = GAUGE_DEFAULT_RY; |
if( py>0 ) Gauge_py = py; |
Gauge_step = 0; |
timer_gauge = GAUGE_TIMER; |
Gauge_active = true; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (0), 1); |
lcd_ellipse( Gauge_px, Gauge_py, Gauge_rx, Gauge_ry, 1); |
} |
//-------------------------------------------------------------- |
// PKT_Gauge_End() |
//-------------------------------------------------------------- |
void PKT_Gauge_End( void ) |
{ |
Gauge_active = false; |
} |
// uint8_t old_step; |
//-------------------------------------------------------------- |
// PKT_Gauge_Next() |
// |
// INTERN fuer Timer ISR! |
//-------------------------------------------------------------- |
void PKT_Gauge_Next( void ) |
{ |
uint8_t old_step; |
old_step = Gauge_step; |
Gauge_step++; |
if( Gauge_step >= (360/GAUGE_DEGREE)) Gauge_step = 0; |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (old_step*GAUGE_DEGREE) , 0); |
lcd_ellipse_line( Gauge_px, Gauge_py, Gauge_rx+GAUGE_LROFFS, Gauge_ry+GAUGE_LROFFS, (Gauge_step*GAUGE_DEGREE), 1); |
timer_gauge = GAUGE_TIMER; |
} |
//############################################################################################# |
//# PKT Kontrollfunktionen |
//############################################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Reset_EEprom( void ) |
{ |
// Text1: "PKT-EEProm" (EEPROM1) |
// Text2: "wirklich löschen?" (EEPROM2) |
// Head : "* ACHTUNG '" (STR_ATTENTION) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_YES_NO, strGet(EEPROM1), strGet(EEPROM2), strGet(STR_ATTENTION), NULL) ) |
{ |
Delete_EEPROM(); |
clr_V_On(); |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_SwitchOff( void ) |
{ |
// Text1: "PKT ausschalten?" (SHUTDOWN) |
// Text2: NULL |
// Head : "PKT" (STR_PKT) |
// Titel: PKT-Version (NULL) |
if( PKT_Ask_P( ASK_NO_YES, strGet(SHUTDOWN), NULL, strGet(STR_PKT), NULL) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Ask_Restart( const char *title ) |
{ |
// Text1: "jetzt ausschalten?" (STR_PKT_SWITCHOFF_NOW) |
// Text2: NULL |
// Head : "PKT neu starten!" (STR_PKT_RESTART_NOW) |
// Titel: via Parameter |
if( PKT_Ask_P( ASK_NO_YES, strGet(STR_PKT_SWITCHOFF_NOW), NULL, strGet(STR_PKT_RESTART_NOW), title) ) |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_Update( void ) |
{ |
// Text1: "Drücke 'Start' für" (UPDATE2) |
// Text2: "min. 2 Sekunden" (UPDATE3) |
// Head : "Verbinde PKT mit PC!" (UPDATE1) |
// Titel: "PKT Update" |
if( PKT_Ask_P( ASK_END_START, strGet(UPDATE2), strGet(UPDATE3), strGet(UPDATE1), PSTR("PKT Update")) ) |
{ |
uart1_putc('S'); // Startzeichen zum Updatetool schicken |
wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
while(true); |
} |
} |
//void PKT_Update( void ) |
//{ |
// ShowTitle_P( PSTR("PKT Update"), true ); |
// lcdx_printp_center( 2, strGet(UPDATE1), MNORMAL, 0,-1); // "Verbinde PKT mit PC!" |
// lcd_rect_round(0, 4*8-5, 127, 16+7, 1, R2); // Rahmen |
// lcdx_printp_center( 4, strGet(UPDATE2), MNORMAL, 0,-1); // "Drücke 'Start' für" |
// lcdx_printp_center( 5, strGet(UPDATE3), MNORMAL, 0, 0); // "min. 2 Sekunden" |
// lcd_printp_at(11, 7, strGet(ENDSTART), MNORMAL); // Keyline |
// //PKTupdate = false; // Flag löschen |
// |
// set_beep( 80, 0x000f, BeepNormal); // kurzer Beep |
// |
// do |
// { |
// if( get_key_press (1 << KEY_ESC) ) |
// { |
// //get_key_press(KEY_ALL); |
// clear_key_all(); |
// return; |
// } |
// } |
// while( !(get_key_press (1 << KEY_ENTER)) ); |
// |
// uart1_putc('S'); // Startzeichen zum Updatetool schicken |
// wdt_enable( WDTO_250MS ); // start bootloader with Reset, Hold KEY_ENTER |
// while(true); |
//} |
//-------------------------------------------------------------- |
// prueft im Menue ob das Updatetool ein Update machen will |
//-------------------------------------------------------------- |
uint8_t PKT_CheckUpdate(void) |
{ |
unsigned int uart_data; |
uint8_t state = 0; |
uint8_t update = false; |
uint8_t error = false; |
uint8_t SendVersion = false; |
char s[7]; |
//---------------------------- |
// empfangen |
//---------------------------- |
if( uart1_available() > 0 ) // Zeichen von der USB-Schnittstelle vorhanden? |
{ |
timer_pktupdatecheck = 100; // Timeouttimer um die Funktion abzubrechen |
while( !timer_pktupdatecheck==0 && !update && !error && !SendVersion ) |
{ |
uart_data = uart1_getc(); |
if( !((uart_data & 0xFF00) == UART_NO_DATA) ) |
{ |
switch (state) |
{ |
case 0: if( uart_data == PKTESC ) |
{ |
state = 1; |
} |
else error = true; |
break; |
case 1: if( uart_data == PKTESC ) |
{ |
state = 2; |
} |
else error = true; |
break; |
case 2: if( uart_data == PKTUPDATE ) update = true; |
else if (uart_data == PKTVERSION) SendVersion = true; |
else error = true; |
break; |
} // end: switch |
} // end: if |
} // end: while |
//---------------------------- |
// Rueckmeldungen |
//---------------------------- |
if( error ) |
{ |
uart1_putc('E'); // Rueckmeldungen zum Updatetool |
} |
else if( update ) |
{ |
uart1_putc('U'); |
} |
else if( SendVersion ) |
{ |
uart1_putc('V'); |
itoa( EEpromVersion, s, 10 ); |
uart1_puts(s); |
} |
else if( timer_pktupdatecheck==0 ) |
{ |
uart1_putc('T'); |
} |
//uart1_flush(); |
} |
return update; |
} |
//-------------------------------------------------------------- |
// retcode = PKT_CtrlHook() |
// |
// ruft ggf. PKT_Update() auf und kann spaeter weitere PKT interne |
// Sachen verarbeiten wenn der Hook (=Nagel) in entsprechende |
// Routienen eingebaut ist (z.B. senden von Daten an den PC) |
// |
// Aktuell verwendet von: menuctrl, scrollbox, osd |
// |
// Rueckgabe: |
// retcode: 0 = nichts wurde gemacht (false) |
// 1 = es wurde etwas durchgefuehrt und die Aufruffunktion |
// muss gff. einen Screenrefresh durchfuehren |
// (z.B. wenn der PKT-Update Screen gezeigt wurde) |
//-------------------------------------------------------------- |
uint8_t PKT_CtrlHook( void ) |
{ |
uint8_t retcode = 0; |
// 31.05.2014 OG: PKT_CheckUpdate() darf nicht in 'Hoechstgeschwindigkeit' |
// aufgerufen werden da es sonst seitens des PKT-Updatetools ggf. zu einer |
// Fehlermeldung kommt - deswegen hier timer_pktupdatecheck der PKT_CheckUpdate |
// etwas einbremst sich aber nicht negativ auf die PKT-Performance auswirkt. |
if( !timer_pktupdatecheck ) |
{ |
if( PKT_CheckUpdate() ) // Update vom Updatetool angefordert? |
{ |
WriteParameter(); |
PKT_Update(); |
retcode = 1; |
} |
timer_pktupdatecheck = 10; |
} |
return retcode; |
} |
//-------------------------------------------------------------- |
// zeigt PKT-Version und Credits / Entwickler |
//-------------------------------------------------------------- |
void PKT_Info( void ) |
{ |
char *HWV_39m = "3.9m"; |
char *HWV_39x = "3.9x"; |
char *HWV = "????"; |
if( PCB_Version == PKT39m ) HWV = HWV_39m; |
if( PCB_Version == PKT39x ) HWV = HWV_39x; |
lcd_cls(); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MINVERS, PSTR(" PKT v%8S %s"), PSTR(PKTSWVersion), HWV ); // "3.6.8a..." und die PKT-Hardwareversion |
ScrollBox_Push_P( MINVERS, PSTR(" %S SV2-Patch"), (PCB_SV2RxTxPatch ? strGet(STR_WITH) : strGet(STR_WITHOUT)) ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
ScrollBox_Push_P( MNORMAL, PSTR("(C) GNU GPL License") ); |
ScrollBox_Push_P( MNORMAL, PSTR(" NO WARRANTY") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#ifdef USE_MODULEINFO |
const char *fmt = PSTR("%15S %4S"); |
//--- MODULE SUPPORT INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Modules installed") ); |
#ifdef USE_BLUETOOTH |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Bluetooth"), strGet(NOO) ); |
#endif |
#ifdef USE_WLAN |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("WLAN WyFly"), strGet(NOO) ); |
#endif |
#ifdef USE_MKSETTINGS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Settings"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDEBUGDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Debug Data"), strGet(NOO) ); |
#endif |
#ifdef USE_MKDISPLAY |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("MK Display"), strGet(NOO) ); |
#endif |
#ifdef USE_OSDDATA |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD-Data"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Old Screens"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_NAVIGATION |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Navigation"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Waypoints"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Electric"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD MKStatus"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD UserGPS"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD 3D-Lage"), strGet(NOO) ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("OSD Statistic"), strGet(NOO) ); |
#endif |
#ifdef USE_TRACKING |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Tracking"), strGet(NOO) ); |
#endif |
#ifdef USE_FOLLOWME |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Follow Me"), strGet(NOO) ); |
#endif |
#ifdef USE_JOYSTICK |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Joystick"), strGet(NOO) ); |
#endif |
#ifdef USE_I2CMOTORTEST |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("I2C Motortest"), strGet(NOO) ); |
#endif |
#ifdef USE_SOUND |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Sound"), strGet(NOO) ); |
#endif |
#ifdef USE_FONT_BIG |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(YES) ); |
#else |
ScrollBox_Push_P( MNORMAL, fmt, PSTR("Font Big"), strGet(NOO) ); |
#endif |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
#endif // USE_MODULEINFO |
//--- CREDITS INFO --- |
ScrollBox_Push_P( MINVERS, PSTR(" Credits") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2013 Oliver Gemesi") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Chr. Brandtner" ) ); |
ScrollBox_Push_P( MNORMAL, PSTR(" Harald Bongartz") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 Martin Runkel") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2012 gebad") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2010 Sebastian Boehm") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2009-2010 Peter Mack") ); |
ScrollBox_Push_P( MNORMAL, PSTR("2008 Thomas Kaiser") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/pkt/pkt.h |
---|
0,0 → 1,148 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2012 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2012 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY pkt.h |
//# |
//# 25.06.2014 OG |
//# - add: PKT_AskX() |
//# |
//# 24.06.2014 OG |
//# - add: PKT_Gauge_Begin(), PKT_Gauge_End(), PKT_Gauge_Next() |
//# |
//# 15.06.2014 OG |
//# - add: PKT_Progress_Init(), PKT_Progress_Next() |
//# |
//# 14.06.2014 OG |
//# - add: PKT_TitlePKTlipo() fuer optionale Anzeige der LiPo-Spannung |
//# |
//# 13.06.2014 OG |
//# - add: PKT_Ask_Restart() |
//# |
//# 12.06.2014 OG |
//# - add: PKT_Reset_EEprom() |
//# - add: PKT_Ask(), PKT_Ask_P() |
//# |
//# 11.06.2014 OG |
//# - add: PKT_Title(), PKT_Title_P() |
//# |
//# 04.06.2014 OG |
//# - add: PKT_Message_Datenverlust() |
//# |
//# 31.05.2014 OG |
//# - add: PKT_KeylineUpDown() |
//# |
//# 23.05.2014 OG |
//# - add: PKT_TitleFromMenuItem() |
//# |
//# 06.05.2014 OG |
//# - add: PKT_Popup(), PKT_Popup_P() |
//# |
//# 04.04.2014 OG |
//# - fix: define ESC umbenannt zu PKTESC da es einen Namenskonflikt mit enum |
//# STR in messages.h gab |
//# |
//# 21.02.2014 OG |
//# - chg: PKT_TitlePKTVersion() |
//# |
//# 17.02.2014 OG |
//# - add: PKT_Message(), PKT_Message_P() |
//# |
//# 13.02.2014 OG |
//# - add: PKT_print_PKT_center() |
//# |
//# 03.02.2014 OG |
//# - add: PKT_ShowTitlePKTVersion() |
//# |
//# 13.06.2013 OG |
//# - del: PC_Fast_Connect() - ersetzt durch Menu_PKTTools() in menu.c |
//# |
//# 19.05.2013 OG |
//# - Ausgliederungen aus verschiedenen anderen Sourcen |
//############################################################################ |
#ifndef _PKT_H |
#define _PKT_H |
#define PKTESC 27 // Parameter für PKT-Updatetool |
#define PKTUPDATE 'u' // starte Update |
#define PKTVERSION 'v' // sende PKT EEpromversion |
#define PKTSENDCONF 'e' // sende PKT Konfigdaten |
#define ASK_YES_NO 1 |
#define ASK_NO_YES 2 |
#define ASK_CANCEL_OK 3 |
#define ASK_END_OK 4 |
#define ASK_END_START 5 |
extern volatile uint8_t Gauge_active; |
void PKT_Title( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_Title_P( const char *text, uint8_t lShowLipo, uint8_t clearscreen ); |
void PKT_TitlePKT( void ); |
void PKT_TitlePKTlipo( uint8_t lShowLipo ); |
void PKT_TitleFromMenuItem( uint8_t lShowLipo ); |
void PKT_KeylineUpDown( uint8_t xUp, uint8_t xDown, uint8_t xoffs, uint8_t yoffs); |
void PKT_Progress_Init( uint8_t max, int8_t yoffs, uint8_t width, uint8_t height); |
uint8_t PKT_Progress_Next( void ); |
void PKT_Gauge_Begin( uint8_t py); |
void PKT_Gauge_End( void ); |
void PKT_Gauge_Next( void ); |
void PKT_Message( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ); |
void PKT_Message_Datenverlust( uint16_t timeout, uint8_t beep ); |
void PKT_Popup( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
void PKT_Popup_P( uint16_t timeout, const char *text1, const char *text2, const char *text3, const char *text4 ); |
uint8_t PKT_Ask( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_Ask_P( uint8_t asktype, const char *text1, const char *text2, const char *headline, const char *title ); |
uint8_t PKT_AskX( uint8_t asktype, const char *text1, const char *text2, uint8_t text_progmem, const char *headline, uint8_t headline_progmem, const char *title, uint8_t title_progmem ); |
uint8_t PKT_CtrlHook(void); |
uint8_t PKT_CheckUpdate(void); // prüft im Hauptmenü ob das Updatetool ein Update machen will |
void PKT_Reset_EEprom( void ); |
void PKT_Update(void); |
void PKT_SwitchOff(void); |
void PKT_Info(void); |
void PKT_Ask_Restart( const char *title ); |
#endif // end: #ifndef _PKT_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/setup/setup.c |
---|
0,0 → 1,2649 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.c |
//# |
//# |
//# 03.08.2015 CB |
//# - add: FollowMe Setup um Distance und Azimuth erweitert |
//# |
//# 27.06.2014 OG |
//# - chg: Setup_MAIN() - Reihenfolge von GPS-Maus/FollowMe/Tracking geaendert |
//# |
//# 25.06.2014 OG |
//# - chg: Text von GPS_SHOWDEV |
//# - chg: Setup_FollowMe() - deaktiviert/auskommentiert: FME_REFRESH |
//# - chg: Setup_GPSMouse() - deaktiviert/auskommentiert: GPS_TYPE, GPS_ACTIVE |
//# |
//# 24.06.2014 OG |
//# - chg: Setup_GPSMouse() angepasst auf geaendertes GPSMouse_ShowData() |
//# |
//# 22.06.2014 OG |
//# - del: verschiedene weitere Modul-Variablen entfernt weil nicht benoetigt |
//# - del: Variable CheckGPS |
//# - del: BT_ShowGPSData() - ist jetzt als GPSMouse_ShowData() in gps/gpsmouse.c |
//# - chg: BT_ShowGPSData() - Parameter und Rueckgabe |
//# - del: BT_ShowGPSData_OLD |
//# |
//# 21.06.2014 CB |
//# - chg: BT_SearchDevices - Timeoutanzeige bei Devicesuche |
//# |
//# 16.06.2014 OG |
//# - chg: BT_ShowGPSData() - neues Layout und Anzeige fuer "XCnt" (=RX-Count) |
//# hinzugefuegt; XCnt zeigt die Anzahl empfangener GPS-Datenpakete an |
//# und ist ein Indikator wie schnell die BT-GPS Maus Daten sendet |
//# |
//# 13.06.2014 OG |
//# - chg: Setup_PKTGeneric() - neue Einstellung "PKT aus nach" |
//# - del: ChangeWi_SV2() |
//# |
//# 12.06.2014 OG |
//# - del: Reset_EEprom() - ist jetzt als PKT_Reset_EEprom() in pkt.c |
//# |
//# 11.06.2014 OG |
//# - fix: Edit_generic() funktioniert jetzt auch, wenn kein vorgelagertes |
//# Menue vorhanden ist (ggf. muss das Label angepasst werden) |
//# |
//# 10.06.2014 OG |
//# - chg: verschiedene Funktionen umgestellt auf Wi232_Initalize() |
//# |
//# 08.06.2014 OG |
//# - add: Setup_MKConnection() - hier ist auch das setzen der Baudrate Wi232/BT |
//# hin verschoben worden (war vorher im Wi.232 Bereicch) |
//# |
//# 06.06.2014 OG |
//# - chg: Setup_WI232_MenuCreate()... Eintraege geloescht... |
//# |
//# 04.06.2014 OG |
//# - chg: Setup_PKTGeneric() eine Menue-Trennlinie hinter Sommerzeit eingefuegt |
//# |
//# 01.06.2014 OG |
//# - chg: Edit_LED_Form() Parameter 'Text' entfernt und auf strGetOSDOLD() |
//# umgestellt |
//# - add: include "../osd/osdold_messages.h" |
//# |
//# 31.05.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt; angepasst auf neue |
//# Titelzeile; PKT_CtrlHook integriert; PKT Lipo-Anzeige; |
//# auf 'redraw' umgestellt |
//# |
//# 30.05.2014 OG |
//# - chg: Edit_LipoOffset() - umgeschrieben auf neues Layout, Code optimiert, |
//# Increment jetzt +/-50 (statt vorher 20), Offset bis 15000 (statt 10000) |
//# - chg: Edit_LipoOffset() - keine Parameter mehr, keine Rueckgabe |
//# |
//# 29.05.2014 OG |
//# - chg: Setup_OSD() umgestellt auf Menuetexte statt strGet() mit Textaenderung |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# (Text enfernt da vom Menue geerbt; optionale Hilfstexte moeglich) |
//# |
//# 27.05.2014 OG |
//# - del: Edit_generic_int(), Edit_printout_int() - nicht mehr benoetigt da |
//# anhaengende Werte umgemappt auf Edit_generic() |
//# |
//# 26.05.2014 OG |
//# - chg: Menuetext TIME_ZONE ergaenzt um "UTC" |
//# - chg: Setup_MAIN() - neue Anordnung der Menueeintraege mit Trennlinien |
//# - chg: Setup_OSDScreens() - OSDSCREEN_WAYPOINTS hinzugefuegt |
//# - chg: Text "LCD aus nach..." nach "LCD aus nach" |
//# - chg: Text "PKT allgemein" geaendert auf "PKT Allgemein" |
//# - del: PKT_LCDINVERS inkl. Sprachen |
//# - chg: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) entfernt, |
//# nicht mehr unterstuetzt |
//# - chg: Edit_printout() - Hilfstexte CHANGENORMREV1 und 2 fuer NormRev entfernt |
//# |
//# 23.05.2014 OG |
//# - add: Edit_generic() - Anzeige Titel mit PKT_TitleFromMenuItem() |
//# - fix: Setup_PKTGeneric() - PKT_LCDINVERS (Config.LCD_DisplayMode) |
//# Edit-Range umgestellt von 0,4 auf 0,1 |
//# |
//# 11.05.2014 OG |
//# - chg: die Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# - chg: ein paar Menuetexte fuer Setup_MAIN() |
//# |
//# 18.04.2014 OG |
//# - chg: Setup_PKTGeneric() - PKT_BRIGHTNESS rausgenommen da es aktuell auch |
//# nicht mehr mit PKT 3.9m Hardware geht (spart ca. 650 Bytes) |
//# - chg: Text von "Tracking Servos" gekuerzt auf "Tracking" |
//# |
//# 09.04.2014 OG |
//# - chg: Edit_printout() Text von WlanMode 'Nein' auf 'Aus' geaendert |
//# |
//# 04.04.2014 OG |
//# - chg: Edit_String() auf ShowTitle_P() umgestellt |
//# - del: Edit_ShowTitle_P() |
//# - chg: in versch. Funktionen Titel-Strings fuer Edit_String() gekuerzt |
//# - chg: Edit_String() auf Edit_ShowTitle_P() umgestellt |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: Edit_printout(): WlanMode (Aus, AP-Mode, AdHoc) |
//# - chg: Edit_printout(): Code-Optimierung |
//# |
//# 01.04.2014 OG |
//# - add: Unterstuetzung fuer BLE (Bluetooth 4 Low Energy Zusatzmodul an SV2) |
//# - chg: PCB_WiFlyPatch umbenannt zu PCB_SV2RxTxPatch |
//# |
//# 30.03.2014 OG |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 27.03.2014 OG |
//# - chg: Setup_PKTGeneric(), Edit_printout() Sprache "Niederländisch" nicht mehr waehlbar |
//# |
//# 04.02.2014 OG |
//# - chg: kleine Layoutaenderung von 'PKT_Akku' in Edit_printout() |
//# - add: PKT_CtrlHook() in Edit_generic(), Edit_generic_int(), Edit_LipoOffset() eingebaut |
//# - add: kleiner Hilfetext in Edit_printout() bei Aenderung von Einstellung |
//# "LCD Normal/Invers" bzgl. PKT neu starten nach Aenderung |
//# - chg: Anzeige von Edit_LipoOffset() geaendert und fix vom springen der |
//# Eingabe wenn Wert geaendert wird |
//# |
//# 03.02.2014 OG |
//# - add: SHOWCELLU in Setup_OSD() ("zeige Zellensp.") |
//# |
//# 30.01.2014 OG |
//# - add: Unterstuetzung fuer USE_BLUETOOTH |
//# |
//# 29.07.2013 Cebra |
//# - chg: Fehler in Setup_WI232_MenuCreate, MenuCtrl_Push_P( WI_INSTALLED.. war auskommentiert, dadurch Fehler im Menu bei nicht |
//# aktivem Wi232 |
//# |
//# 18.07.2013 Cebra |
//# - chg: Edit_PKT_Accu entfernt, Edit_generic erweitert |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um Wlan Security erweitert |
//# |
//# 07.07.2013 OG |
//# - add: Setup_OSDScreens(), Mark_OSDScreen(), Edit_OSDScreen() |
//# |
//# 04.07.2013 Cebra |
//# - chg: Setupmenü um Wlan erweitert |
//# |
//# 02.07.2013 Cebra |
//# - chg: Edit_String, geändert für die Nutzung von SSID und Wlan Passwort |
//# |
//# 30.06.2013 OG |
//# - del: Setup_OSD(): OSDS_HOMEVIEW |
//# |
//# 26.06.2013 OG |
//# - del: USE_PKT_STARTINFO |
//# |
//# 24.06.2013 OG |
//# - chg: vor dem editieren des Bluetooth Namens bt_fixname() eingefuegt |
//# |
//# 24.06.2013 OG |
//# - fix: Edit_String(): Fehlermeldung bei EDIT_BT_NAME wenn erstes Zeichen = Space |
//# - chg: Edit_String(): Code-Layout und diverses |
//# - chg: Setup_BTM222(): Bearbeitung von BT_NAME geaendert |
//# - chg: Setup_BTM222(): Menuetexte geaendert |
//# - chg: Setup_MAIN(): Text im Setup-Hauptmenue von "BTM-222" auf "Bluetooth" geaendert |
//# |
//# 21.06.2013 OG |
//# - fix: Menuetext GPSMouse "Zeige Geräteliste" (Anzeige "ä" korrigiert) |
//# |
//# 15.06.2013 OG |
//# - add: "zeige MK-Setting" in Setup_OSD() hinzugefuegt |
//# |
//# 15.06.2013 OG |
//# - chg: Umstrukturierungen am OSD-Setup |
//# - chg: OSD Empfangsausfall von Setup_PKTGeneric() nach Setup_OSD() verschoben |
//# |
//# 13.06.2013 OG |
//# - del: Setup_Time() - jetzt in Setup_PKTGeneric() |
//# - del: Setup_PKTAccu() - jetzt in Setup_PKTGeneric() |
//# - del: SETUP_PKTDEBUG in Setup_MAIN() entfernt (auskommentiert, wenn notwendig ggf. mit einem Debug define regeln) |
//# - chg: diverse Aenderungen in Menutexten |
//# - fix: keine LCD-Helligkeitseinstellung bei HW 3.9x mehr durch Abfrage von PCB_Version |
//# |
//# 31.05.2013 cebra |
//# - chg: Umlaute, soweit gefunden, korrigiert |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: etliche Aenderungen rund um Bluetooth/BT-GPS Maus fuer bessere |
//# Usebility, Code-Einsparung und Code-Lesbarkeit |
//# - add: Wait4KeyOK() |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: BT_SelectDevice() |
//# |
//# 22.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Setup_Time(), Setup_FollowMe(), Setup_OSD() |
//# - chg: weitere Speicheroptimierungen fuer USE_* |
//# - chg: Menu_Setup() umbenannt zu Setup_MAIN() |
//# |
//# 20.05.2013 OG |
//# - chg: umgestellt auf menuctrl: Menu_Setup(), Setup_PKTGeneric(), Setup_WI232() |
//# Setup_BTM222(), Setup_PKTAccu(), Setup_GPSMouse() |
//# - add: GPSMouse_ShowData() |
//# |
//# 19.05.2013 OG |
//# - chg: Aufruf von Update_PKT() umgestellt auf PKT_Update() |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.05.2013 Cebra |
//# - chg: Fehler bei On/Off Settings |
//# |
//# 05.05.2013 Cebra |
//# - add: PKT Zeitsetup |
//# |
//# 19.04.2013 Cebra |
//# - chg: Fehlerbehandlung im GPS Menü für den Fall das kein BTM22 eingebaut oder GPS-Device konfiguriert |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 12.04.2013 Cebra |
//# - chg: edit_generic um Sprungfaktor erweitert, erm�glicht bei hohen Werten einen schnelleren Wertewechsel |
//# |
//# 03.04.2013 Cebra |
//# - chg: find/replace Fehler bei lcd_puts_at beseitigt, GPS und BT-Maus Bereich |
//# |
//# 02.04.2013 Cebra |
//# - chg: Textfehler bei der Einstellung "Verbindung zum MK" beseitigt |
//# |
//# 04.03.2013 Cebra |
//# - del: OSD-Sreenmode ,is no longer necessary, last OSD-Screen is saved at shutdown |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include "../main.h" |
#include "../setup/setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../wi232/Wi232.h" |
#include "../bluetooth/bluetooth.h" |
#include "../connect.h" |
#include "../lipo/lipo.h" |
#include "../messages.h" |
#include "../eeprom/eeprom.h" |
#include "../tracking/tracking.h" |
#include "../uart/uart1.h" |
#include "../sound/pwmsine8bit.h" |
#include "../tracking/servo_setup.h" |
#include "../bluetooth/error.h" |
#include "../stick/stick_setup.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../pkt/pkt.h" |
#include "../osd/osd.h" |
#include "../wifly/wifly_setup.h" |
#include "../gps/gpsmouse.h" |
#include "../utils/scrollbox.h" |
#include "../osd/osdold_messages.h" |
uint8_t edit = 0; |
uint16_t Pre; |
char EditString[21]; |
uint8_t length_tmp; |
uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
//############################################################################ |
//############################################################################ |
//----------------------------- |
// Setup_Main() |
//----------------------------- |
#define SETUP_MKCONNECTION 1 |
#define SETUP_PKTCONFIG 2 |
#define SETUP_WI232 3 |
#define SETUP_BTM222 4 |
#define SETUP_SERVOCONFIG 5 |
#define SETUP_OSDVIEW 6 |
#define SETUP_TIME 7 |
#define SETUP_GPSMAUS 8 |
#define SETUP_FOLLOWME 9 |
#define SETUP_JOYSTICK 10 |
#define SETUP_PKTAKKU 11 |
#define SETUP_PKTUPDATE 12 |
#define SETUP_PKTDEBUG 13 |
#define SETUP_EEPROMRESET 14 |
#define SETUP_WIFLY 15 |
#define SETUP_OSDSCREENS 16 |
#define SETUP_BLE 17 |
static const char SETUP_MKCONNECTION_de[] PROGMEM = "Verbindung MK"; |
static const char SETUP_MKCONNECTION_en[] PROGMEM = "connection MK"; |
static const char SETUP_PKTCONFIG_de[] PROGMEM = "PKT Allgemein"; |
static const char SETUP_PKTCONFIG_en[] PROGMEM = "PKT general"; |
static const char SETUP_WI232_de[] PROGMEM = "Wi.232"; |
#define SETUP_WI232_en SETUP_WI232_de |
static const char SETUP_BTM222_de[] PROGMEM = "Bluetooth"; |
#define SETUP_BTM222_en SETUP_BTM222_de |
static const char SETUP_BLE_de[] PROGMEM = "Bluetooth BLE"; |
#define SETUP_BLE_en SETUP_BLE_de |
static const char SETUP_WIFLY_de[] PROGMEM = "WLAN WiFly"; |
#define SETUP_WIFLY_en SETUP_WIFLY_de |
static const char SETUP_SERVOCONFIG_de[] PROGMEM = "Tracking"; |
#define SETUP_SERVOCONFIG_en SETUP_SERVOCONFIG_de |
static const char SETUP_OSDVIEW_de[] PROGMEM = "OSD Anzeige"; |
static const char SETUP_OSDVIEW_en[] PROGMEM = "OSD display"; |
static const char SETUP_OSDSCREENS_de[] PROGMEM = "OSD Screens"; |
static const char SETUP_OSDSCREENS_en[] PROGMEM = "OSD screens"; |
static const char SETUP_GPSMAUS_de[] PROGMEM = "GPS Maus"; |
static const char SETUP_GPSMAUS_en[] PROGMEM = "GPS mouse"; |
static const char SETUP_FOLLOWME_de[] PROGMEM = "Follow Me"; |
#define SETUP_FOLLOWME_en SETUP_FOLLOWME_de |
static const char SETUP_JOYSTICK_de[] PROGMEM = "Joystick"; |
#define SETUP_JOYSTICK_en SETUP_JOYSTICK_de |
static const char SETUP_PKTUPDATE_de[] PROGMEM = "PKT Update"; |
#define SETUP_PKTUPDATE_en SETUP_PKTUPDATE_de |
static const char SETUP_PKTDEBUG_de[] PROGMEM = "Debug PKT"; |
#define SETUP_PKTDEBUG_en SETUP_PKTDEBUG_de |
static const char SETUP_EEPROMRESET_de[] PROGMEM = "PKT Reset"; |
#define SETUP_EEPROMRESET_en SETUP_EEPROMRESET_de |
//############################################################################ |
//----------------------------- |
// Setup_PKTGeneric() ("PKT allgemein") |
//----------------------------- |
#define PKT_LANGUAGE 3 |
#define PKT_LIGHTOFF 4 |
#define PKT_BRIGHTNESS 5 |
#define PKT_CONTRAST 6 |
#define PKT_SOUNDMODUL 9 |
#define PKT_BEEPER 10 |
#define PKT_VOLUME 11 |
#define PKT_ACCUTYPE 12 |
#define PKT_ACCUMEASURE 13 |
#define TIME_ZONE 14 |
#define TIME_SUMMER 15 |
#define PKT_PKTOFF 16 |
static const char LANGUAGE_de[] PROGMEM = "Sprache"; |
static const char LANGUAGE_en[] PROGMEM = "language"; |
static const char TIME_ZONE_de[] PROGMEM = "Zeitzone UTC"; |
static const char TIME_ZONE_en[] PROGMEM = "time zone UTC"; |
static const char TIME_SUMMER_de[] PROGMEM = "Sommerzeit"; |
static const char TIME_SUMMER_en[] PROGMEM = "summer time"; |
static const char PKT_PKTOFF_de[] PROGMEM = "PKT aus nach"; |
static const char PKT_PKTOFF_en[] PROGMEM = "PKT off after"; |
static const char LIGHTOFF_de[] PROGMEM = "LCD aus nach"; |
static const char LIGHTOFF_en[] PROGMEM = "LCD off after"; |
//static const char BRIGHTNESS_de[] PROGMEM = "LCD Helligkeit"; |
//static const char BRIGHTNESS_en[] PROGMEM = "LCD brightness"; |
static const char CONTRAST_de[] PROGMEM = "LCD Kontrast"; |
static const char CONTRAST_en[] PROGMEM = "LCD contrast"; |
static const char BEEPER_de[] PROGMEM = "Hardware Beeper"; |
#define BEEPER_en BEEPER_de |
#ifdef USE_SOUND |
static const char SOUNDMODUL_de[] PROGMEM = "Soundmodul"; |
static const char SOUNDMODUL_en[] PROGMEM = "Soundmodule"; |
static const char VOLUME_de[] PROGMEM = "Lautstärke Sound"; |
static const char VOLUME_en[] PROGMEM = "Volume Sound"; |
#endif |
static const char ACCUTYPE_de[] PROGMEM = "Akku Typ"; |
static const char ACCUTYPE_en[] PROGMEM = "Accu type"; |
static const char ACCUMEASURE_de[] PROGMEM = "Akku Messung"; |
static const char ACCUMEASURE_en[] PROGMEM = "Accu measure"; |
//############################################################################ |
//----------------------------- |
// Setup_MKConnection() |
//----------------------------- |
#define MKCONN_CONNECTION 1 |
#define MKCONN_BAUDRATE 2 |
#define MKCONN_CONNECTION_de SETUP_MKCONNECTION_de |
#define MKCONN_CONNECTION_en SETUP_MKCONNECTION_en |
static const char MKCONN_BAUDRATE_de[] PROGMEM = "Baudrate Wi232/BT"; |
#define MKCONN_BAUDRATE_en MKCONN_BAUDRATE_de |
//############################################################################ |
//----------------------------- |
// Setup_WI232() |
//----------------------------- |
#define WI_INSTALLED 1 |
#define WI_TXRX 2 |
#define WI_GROUP 3 |
#define WI_MODE 4 |
#define WI_TIMEOUT 5 |
#define WI_MTU 6 |
#define WI_INIT 7 |
#define WI_PCCONFIG 8 |
static const char WI_INSTALLED_de[] PROGMEM = "Modul eingebaut"; |
static const char WI_INSTALLED_en[] PROGMEM = "module built in"; |
static const char WI_TXRX_de[] PROGMEM = "TX/RX Kanal"; |
static const char WI_TXRX_en[] PROGMEM = "TX/RX Channel"; |
static const char WI_GROUP_de[] PROGMEM = "NetW. Gruppe"; |
static const char WI_GROUP_en[] PROGMEM = "NetW. Group"; |
static const char WI_MODE_de[] PROGMEM = "NetW. Mode"; |
#define WI_MODE_en WI_MODE_de |
static const char WI_TIMEOUT_de[] PROGMEM = "TX Timeout"; |
#define WI_TIMEOUT_en WI_TIMEOUT_de |
static const char WI_MTU_de[] PROGMEM = "TX MTU"; |
#define WI_MTU_en WI_MTU_de |
static const char WI_INIT_de[] PROGMEM = "Initialisieren"; |
static const char WI_INIT_en[] PROGMEM = "initialize"; |
static const char WI_PCCONFIG_de[] PROGMEM = "Konfig. mit PC"; |
static const char WI_PCCONFIG_en[] PROGMEM = "config. with PC"; |
//############################################################################ |
//----------------------------- |
// Setup_BTM222() (Bluetooth) |
//----------------------------- |
#define BT_INSTALLED 1 |
#define BT_NAME 2 |
#define BT_PIN 3 |
#define BT_INIT 4 |
#define BT_MAC 5 |
#define BT_REID 6 |
#define BT_PCCONFIG 7 |
#define BT_SHOWCONFIG 8 |
#define BT_INSTALLED_de WI_INSTALLED_de |
#define BT_INSTALLED_en WI_INSTALLED_en |
static const char BT_NAME_de[] PROGMEM = "BT Name"; |
static const char BT_NAME_en[] PROGMEM = "BT name"; |
static const char BT_PIN_de[] PROGMEM = "BT Pin"; |
static const char BT_PIN_en[] PROGMEM = "BT pin"; |
#define BT_INIT_de WI_INIT_de |
#define BT_INIT_en WI_INIT_en |
static const char BT_MAC_de[] PROGMEM = "zeige BT MAC"; |
static const char BT_MAC_en[] PROGMEM = "show BT MAC"; |
static const char BT_REID_de[] PROGMEM = "RE-ID Notiz"; |
#define BT_REID_en BT_REID_de |
#define BT_PCCONFIG_de WI_PCCONFIG_de |
#define BT_PCCONFIG_en WI_PCCONFIG_en |
static const char BT_SHOWCONFIG_de[] PROGMEM = "zeige BT Konfig"; |
static const char BT_SHOWCONFIG_en[] PROGMEM = "show BT config"; |
//############################################################################ |
//----------------------------- |
// Setup_BLE() (Bluetooth 4 Low Energy) |
//----------------------------- |
#define BLE_INSTALLED 1 |
// Text ist in messages.c: MODULE_EXIST |
//############################################################################ |
//----------------------------- |
// Setup_GPSMouse() |
//----------------------------- |
#define GPS_DEVICES 1 |
#define GPS_SEARCH 2 |
#define GPS_TYPE 3 |
#define GPS_ACTIVE 4 |
#define GPS_SHOWDEV 5 |
#define GPS_DATA 6 |
static const char GPS_SEARCH_de[] PROGMEM = "suche GPS-Maus"; |
static const char GPS_SEARCH_en[] PROGMEM = "search GPS-mouse"; |
static const char GPS_DEVICES_de[] PROGMEM = "Geräteliste"; |
static const char GPS_DEVICES_en[] PROGMEM = "devicelist"; |
static const char GPS_SHOWDEV_de[] PROGMEM = "aktuelle Maus"; |
static const char GPS_SHOWDEV_en[] PROGMEM = "act. GPS-mouse"; |
static const char GPS_DATA_de[] PROGMEM = "GPS-Maus Daten"; |
static const char GPS_DATA_en[] PROGMEM = "GPS-mouse data"; |
static const char GPS_TYPE_de[] PROGMEM = "GPS-Maus Typ"; // aktuell nicht verwendet |
static const char GPS_TYPE_en[] PROGMEM = "GPS-mouse typ"; |
static const char GPS_ACTIVE_de[] PROGMEM = "GPS-Maus aktiv"; // aktuell nicht verwendet |
static const char GPS_ACTIVE_en[] PROGMEM = "GPS-mouse activ"; |
//############################################################################ |
//----------------------------- |
// Setup_FollowMe() |
//----------------------------- |
#define FME_DISTANCE 1 |
#define FME_SPEED 2 |
#define FME_RADIUS 3 |
#define FME_AZIMUTH 4 |
static const char FME_DISTANCE_de[] PROGMEM = "FollowMe Abstand"; |
static const char FME_DISTANCE_en[] PROGMEM = "followMe distance"; |
static const char FME_AZIMUTH_de[] PROGMEM = "FollowMe Winkel"; |
static const char FME_AZIMUTH_en[] PROGMEM = "followMe angle"; |
static const char FME_SPEED_de[] PROGMEM = "FollowMe Speed"; |
#define FME_SPEED_en FME_SPEED_de |
static const char FME_RADIUS_de[] PROGMEM = "Toleranz Radius"; |
static const char FME_RADIUS_en[] PROGMEM = "tolerance radius"; |
//############################################################################ |
//----------------------------- |
// Setup_OSD() |
//----------------------------- |
#define OSDS_SHOWCELLU 1 |
#define OSDS_LOWBAT 2 |
#define OSDS_RCERROR 3 |
#define OSDS_MKSETTING 4 |
#define OSDS_MAH 5 |
#define OSDS_DATASV2 6 |
#define OSDS_OUTFORMAT 7 |
#define OSDS_OUTPOLARITY 8 |
#define OSDS_FALLSPEED 9 |
#define OSDS_VARIOBEEP 10 |
#define OSDS_VOLTBAR 11 |
static const char OSDS_LOWBAT_de[] PROGMEM = "Warn Unterspannung"; |
static const char OSDS_LOWBAT_en[] PROGMEM = "Warn LowBat"; |
static const char OSDS_MAH_de[] PROGMEM = "Warn mAh"; |
static const char OSDS_MAH_en[] PROGMEM = "Warn mAh"; |
static const char OSDS_RCERROR_de[] PROGMEM = "Warn RC-Fehler"; |
static const char OSDS_RCERROR_en[] PROGMEM = "Warn RC-Error"; |
static const char OSDS_SHOWCELLU_de[] PROGMEM = "zeige Zellspannung"; |
static const char OSDS_SHOWCELLU_en[] PROGMEM = "show cell voltage"; |
static const char OSDS_MKSETTING_de[] PROGMEM = "zeige MK Setting"; |
static const char OSDS_MKSETTING_en[] PROGMEM = "show mk setting"; |
static const char OSDS_DATASV2_de[] PROGMEM = "Navi Daten an SV2"; |
static const char OSDS_DATASV2_en[] PROGMEM = "Navi data to SV2"; |
#ifdef USE_OSD_SCREEN_OLD |
static const char OSDS_OUTFORMAT_de[] PROGMEM = "OUT1/2 Format"; |
static const char OSDS_OUTFORMAT_en[] PROGMEM = "OUT1/2 format"; |
static const char OSDS_FALLSPEED_de[] PROGMEM = "Max. Sinkrate m/s"; |
static const char OSDS_FALLSPEED_en[] PROGMEM = "max fallspeed m/s"; |
static const char OSDS_VARIOBEEP_de[] PROGMEM = "Variometer Beep"; |
static const char OSDS_VARIOBEEP_en[] PROGMEM = "Variometer beep"; |
static const char OSDS_OUTPOLARITY_de[] PROGMEM = "OUT1/2 Polarität"; |
static const char OSDS_OUTPOLARITY_en[] PROGMEM = "OUT1/2 polarity"; |
static const char OSDS_VOLTBAR_de[] PROGMEM = "Volt Balken"; |
static const char OSDS_VOLTBAR_en[] PROGMEM = "volt bargraph"; |
#endif |
//############################################################################ |
//############################################################################ |
//# Allgemeines |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyOK( void ) |
{ |
set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(19, 7, strGet(OK) , MNORMAL); |
while( !get_key_press(1 << KEY_ENTER) ) PKT_CtrlHook(); // PKT-Update usw... |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Wait4KeyENDE( uint8_t beep ) |
{ |
if( beep ) set_beep( 60, 0xffff, BeepNormal); |
lcd_printp_at(12, 7, strGet(ENDE) , MNORMAL); |
while( !get_key_press(1 << KEY_ESC) ) |
{ |
show_Lipo(); |
PKT_CtrlHook(); // PKT-Update usw... |
} |
clear_key_all(); |
} |
//############################################################################ |
//# BT-Funktionen (GPS-Maus) |
//############################################################################ |
#ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// BT_CheckBTM222() |
// |
// prueft ob das Bluetooth Modul im PKT installiert ist |
//-------------------------------------------------------------- |
uint8_t BT_CheckBTM222( void ) |
{ |
if( !Config.UseBT ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return false; // -> kein BTM222 eingebaut |
} |
return true; |
} |
//-------------------------------------------------------------- |
// BT_SelectDevice() |
// |
// Auswahl eines BT-Device aus einer Liste von BT-Devices |
//-------------------------------------------------------------- |
void BT_SelectDevice( void ) |
{ |
uint8_t itemid; |
uint8_t event; |
uint8_t i; |
//------------------------------- |
// BTM222 Modul vorhanden? |
//------------------------------- |
if( !BT_CheckBTM222() ) return; |
//------------------------------- |
// BT Device-Suche durchgefuert? |
//------------------------------- |
if( bt_devicecount == 0 ) |
{ |
// PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Suche durchführen!"), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
return; |
} |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( strGet(STR_BT_DEVICES) ); // "BT Geräte" |
//--------------- |
// Menuitems |
//--------------- |
for( i = 0; i < bt_devicecount; i++ ) |
{ |
MenuCtrl_Push( i, MENU_ITEM, NOFUNC, device_list[i].DevName ); |
if( strncmp(device_list[i].mac,Config.gps_UsedMac,14) == 0 ) |
MenuCtrl_ItemMark( i, true ); // aktuelles BT-Device markieren |
} |
//--------------- |
// Control |
//--------------- |
event = MenuCtrl_Control( MENUCTRL_EVENT ); |
if( event == MENUEVENT_ITEM ) |
{ |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
strncpy( Config.gps_UsedMac , device_list[itemid].mac , 14); |
strncpy( Config.gps_UsedDevName, device_list[itemid].DevName, 20); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// BT_SearchDevices() |
// |
// sucht BT-Devices |
//-------------------------------------------------------------- |
void BT_SearchDevices( void ) |
{ |
uint8_t wahl; |
char *title; |
//------------------- |
// 1. Frage: suchen? |
//------------------- |
if( Config.gps_UsedDevName[0] != 0 ) // ist eine BT GPS-Maus bereits vorhanden? |
title = Config.gps_UsedDevName; // dann zeige im Titel den Namen der GPS-Maus an |
else |
title = "BT Devices"; |
set_beep( 50, 0xffff, BeepNormal); |
//wahl = PKT_AskX( asktype, text1, text2, text_progmem, headline, headline_progmem, title, title_progmem) |
wahl = PKT_AskX( ASK_END_START, strGet(STR_SEARCH_BT_ASK), strGet(STR_BT_SEARCHTIME), true, MenuCtrl_GetItemText(), true, title, false); // "BT Geräte suchen?" |
//------------------- |
// 2. BT suchen |
//------------------- |
if( wahl ) |
{ |
if( BT_CheckBTM222() ) |
{ |
//PKT_Title_P( text, lShowLipo, clearscreen ) |
PKT_Title_P( PSTR("BT Suche"), true, true ); |
lcdx_printp_center( 2, strGet(STR_SEARCH_BT), MNORMAL, 0,1); // "suche BT Geräte" |
//PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
set_BTOn(); |
bt_downlink_init(); |
lcd_printp_at( 11, 7, strGet(KEYCANCEL), MNORMAL); // Keyline: "Abbr." bzw "Cancel" |
bt_searchDevice(); |
set_BTOff(); |
//PKT_Gauge_End(); // Gauge: Ende |
if( bt_devicecount==0 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( strGet(STR_NO_BT_FOUND), true, 2000, true, true); |
} |
else |
{ |
set_beep( 50, 0xffff, BeepNormal); |
BT_SelectDevice(); |
} |
} |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//############################################################################ |
//# Edit-Funktionen |
//############################################################################ |
//-------------------------------------------------------------- |
// Ausgabe von Werten fuer Edit_generic() |
//-------------------------------------------------------------- |
void Edit_generic_printout( int16_t Value, int16_t min, int16_t max, uint8_t what, int8_t yoffs) |
{ |
const char *pStr = 0; // PGM |
const char *pstr = 0; // RAM |
//uint8_t x = 0; |
//uint8_t y = 3; |
switch( what ) |
{ |
case Show_uint3: pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Show_uint5: pstr = buffered_sprintf_P( PSTR("%5d"), Value ); |
break; |
case Show_uint10th: pstr = buffered_sprintf_P( PSTR("%2.1d"), Value ); |
break; |
case MK_Connection: switch( Value ) |
{ |
case 0: pStr = PSTR("Wi.232"); break; |
case 1: pStr = strGet(KABEL); break; |
} |
if( (Value == 0) && (Config.UseWi == false) ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strGet(STR_WI232_ACTIVATE), MNORMAL, 0,-7); // "aktiviere Wi.232!" |
} |
else |
{ |
lcd_frect( 0, 37, 127, 21-6, 0); // Hilfebereich loeschen |
} |
break; |
case GPSMOUSE: switch( Value ) |
{ |
case GPS_Bluetoothmouse1: pStr = PSTR("BT-Mouse"); break; |
case GPS_Mikrokopter: pStr = PSTR("Mikrokopter"); break; |
default: pStr = PSTR("unknown"); |
} |
break; |
case Wi_Netmode: switch( Value ) |
{ |
case false: pStr = strGet(SLAVE); break; |
case true : pStr = strGet(NORMAL); break; |
} |
break; |
case OnOff: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = strGet(ON); break; |
} |
break; |
case YesNo: switch( Value ) |
{ |
case 0: pStr = strGet(NOO); break; |
case 1: pStr = strGet(YES); break; |
} |
break; |
case WlanMode: switch( Value ) |
{ |
case 0: pStr = strGet(OFF); break; |
case 1: pStr = PSTR("AP-Mode"); break; |
case 2: pStr = PSTR("AdHoc"); break; |
} |
break; |
case NormRev: switch( Value ) // wird noch von stick/stick_setup.c verwendet |
{ |
case 0 : pStr = strGet(NORMAL); break; |
case 1 : pStr = strGet(REVERSE); break; |
} |
break; |
case Kontrast: if( Value >= max ) |
{ |
Value = max; |
set_beep( 200, 0x0080, BeepNormal); |
} |
if( Value == min ) |
{ |
Value = min; |
set_beep( 200, 0x0080, BeepNormal); |
} |
lcd_set_contrast( Value ); |
pstr = buffered_sprintf_P( PSTR("%3d"), Value ); |
break; |
case Baudrate: pstr = buffered_sprintf_P( PSTR("%ld"), Baud_to_uint32(Value) ); |
break; |
case Language: switch( Value ) |
{ |
case 0: pStr = strGet(DEUTSCH); break; |
case 1: pStr = strGet(ENGLISCH); break; |
} |
break; |
case Sticktype: switch( Value ) |
{ |
case false: pStr = strGet(POTI); break; |
case true : pStr = strGet(TASTER); break; |
} |
break; |
/* |
case WlanSecurity: // Anmerkung 02.04.2014 OG: wird aktuell nicht benoetigt weil anders geloest |
switch (Value) |
{ |
case 0x0 : lcd_printp_at(15, 2, PSTR("Adhoc"), 0); |
break; |
case 0x1 : lcd_printp_at(15, 2, PSTR("WPA "), 0); |
break; |
} |
break; |
*/ |
case PKT_Akku: switch( Value ) |
{ |
case true : pStr = PSTR("Lipo"); break; |
case false: pStr = PSTR("LiIon"); break; |
} |
break; |
} // end: switch( what ) |
//lcdx_printf_at_P( uint8_t x, uint8_t y, uint8_t mode, int8_t xoffs, int8_t yoffs, const char *format, ... ) |
if( pStr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16S"), pStr ); // PGM |
if( pstr ) lcdx_printf_at_P( 0, 3, MNORMAL, 0,yoffs, PSTR("> %16s"), pstr ); // RAM |
} |
//-------------------------------------------------------------- |
// int16_t Edit_generic( Value, min, max, what, Addfactor, strHelp1, strHelp2) |
// |
// generische Funktion zum aendern von Setup-Werten |
// |
// PARAMETER: |
// Value: Ausgangswert der zu aendern ist |
// min : minimaler Wert |
// max : maximaler Wert |
// what : Typ des Wertes, abhaengig davon wird der Wert dargestellt, ENUM dafuer in setup.h |
// Addfactor : Sprungwert der beim erhöhen und erniedrigen addiert wird |
// strHelp1 : Hilefetext 1 (PGM) |
// strHelp2 : Hilefetext 2 (PGM) |
//S |
// RUECKGABE: |
// int16_t Return : Ergebnis der Veraenderung des Wertes |
//-------------------------------------------------------------- |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ) |
{ |
uint8_t redraw = true; |
uint8_t noCancel = false; |
int8_t yoffs = 0; // yoffs fuer das Edit-Label |
int8_t yoffsV = 4; // yoffs fuer den Eingabebereich |
const char *pStr; |
Pre = Value; |
do |
{ |
//------------------------ |
// Screen neu zeichnen |
//------------------------ |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // Titel mit PKT-LiPo Anzeige und clearcsreen |
//------------------------ |
// Platz reservieren fuer |
// spezielle Typen |
//------------------------ |
if( what == MK_Connection ) |
{ |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 1 zeiliger Hilfstext |
//------------------------ |
if( strHelp1 != NULL && strHelp2 == NULL ) |
{ |
lcd_rect_round( 0, 37, 127, 21-6, 1, R2); // Rahmen |
lcdx_printp_center( 6, strHelp1, MNORMAL, 0,-7); // Hilfe-Text 1 |
yoffs = -2; // yoffs fuer das Edit-Label |
yoffsV = 0; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// 2 zeiliger Hilfstext |
//------------------------ |
if( strHelp2 != NULL ) |
{ |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_center( 5, strHelp1, MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_center( 6, strHelp2, MNORMAL, 0,-5); // Hilfe-Text 2 |
yoffs = -4; // yoffs fuer das Edit-Label |
yoffsV = -2; // yoffs fuer den Eingabebereich |
} |
//------------------------ |
// Label |
//------------------------ |
pStr = PSTR("???"); |
if( MenuCtrl_GetMenuIndex() >= 0 ) pStr = MenuCtrl_GetItemText(); // das Label wird vom Menueeintrag uebernommen (Menuetext muss im PGM sein!) |
else |
{ |
if( what == Language ) // Ausnahme: wird ggf. beim Start vom PKT von HAL_HW3_9.c aufgerufen |
{ |
pStr = strGetLanguage( LANGUAGE_de, LANGUAGE_en); |
noCancel = true; |
} |
} |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,yoffs, PSTR("%S:"), pStr ); // Ausgabe des Labels |
lcd_printp_at( 0, 7, strGet(KEYLINE5) , MNORMAL); // Keyline: <- -> Abbr. OK |
if( noCancel ) lcd_printp_at(11, 7, PSTR(" "), MNORMAL); // Keyline: "Abbr." loeschen wenn noCancel |
Edit_generic_printout( Value, min, max, what, yoffsV); // aktuellen Eingabewert anzeigen |
redraw = false; |
} |
//------------------------ |
// PKT CtrlHook |
//------------------------ |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------ |
// Key: PLUS |
//------------------------ |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS), 2)) ) |
{ |
if( Value <= (max-Addfactor) ) Value = Value + Addfactor; |
else Value = min; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: MINUS |
//------------------------ |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS), 2)) ) |
{ |
if( Value >= (min + Addfactor) ) Value = Value - Addfactor; |
else Value = max; |
Edit_generic_printout( Value, min, max, what, yoffsV); |
} |
//------------------------ |
// Key: ENTER |
//------------------------ |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
return Value; |
} |
//------------------------ |
// Key: ESC |
//------------------------ |
if( !noCancel && get_key_press(1 << KEY_ESC) ) // Ende ohne speichern |
{ |
break; |
} |
} |
while( true ); |
get_key_press(KEY_ALL); |
return Pre; |
} |
//-------------------------------------------------------------- |
// bei HW 3.9x geht das nicht mehr wegen dem Platinenlayout |
// bei HW 3.9m geht es theoretisch noch - aber durch einen Bug scheinbar |
// nicht mehr. Kann nur von jemanden mit HW 3.9m repariert werden. |
// ALSO -> raus und Speicher sparen |
//-------------------------------------------------------------- |
/* |
uint8_t Edit_DisplayHelligkeit(uint8_t Value, uint8_t min, uint8_t max) |
{ |
float ValCorr = 2.55; // (Value * ValCorr) maximal 255 |
Pre = Value; |
OCR2A = Value * ValCorr; |
lcd_cls(); |
// lcd_printp_at (0, 2, Text, 0); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
write_ndigit_number_u (16, 2, Value, 3, 0,0); |
lcd_printp_at (17, 2, PSTR("%"), 0); |
// lcd_printp_at (0, 7, PSTR(KEY_LINE_2), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
write_ndigit_number_u (16, 2,Value, 3, 0,0); |
lcd_frect ((8*0), (8*4), (Value * (16*8) / 100), 6, 1); |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (Value < max)) |
{ |
Value++; |
if (Value >= max) |
{ |
Value = max; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (Value > min)) |
{ |
lcd_frect (((Value - 1) * (16*8) / 100), (8*4), (16*8), 6, 0); |
Value--; |
if (Value == min) |
{ |
Value = min; |
set_beep ( 200, 0x0080, BeepNormal); |
} |
else |
OCR2A = Value * ValCorr; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
OCR2A = Value * ValCorr; |
Config.LCD_Helligkeit = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
OCR2A = Pre * ValCorr; |
Config.LCD_Helligkeit = Pre; |
// WriteParameter(); |
return Pre; |
} |
} |
*/ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type) |
{ |
uint8_t redraw = true; |
uint8_t x = 1; |
uint8_t I = 0; |
uint8_t lOk = false; |
uint8_t i; |
for( i = 0; i < length; i++) |
{ |
EditString[i] = data[i]; |
} |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
for( i = 0; i < length; i++) |
{ |
lcd_putc ( (i*2)+1, 3, EditString[i], MNORMAL); |
lcd_printp_at( (i*2)+2, 3, PSTR(" ") , MNORMAL); |
} |
lcd_rect( (x*6)-3, (8*3)-2, 10, 10, 1); |
lcd_printp_at(13, 6, PSTR("C"), MNORMAL); |
lcd_printp_at( 0, 7, PSTR(" \x17 \x16 \x19 OK"), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// PKT-LiPo anzeigen |
//------------------------- |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
if( (type == EDIT_BT_NAME) || (type == EDIT_SSID) || (type == EDIT_WL_PASSWORD)) // Name |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 2)) && EditString[I] < 'z') |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
if (EditString[I] > '9' && EditString[I] < 'A') |
EditString[I] = 'A'; |
if (EditString[I] > 'Z' && EditString[I] < 'a') |
EditString[I] = 'a'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 2)) && EditString[I] > ' ') |
{ |
EditString[I]--; |
if (EditString[I] < 'a' && EditString[I] > 'Z') |
EditString[I] = 'Z'; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
if (EditString[I] < '0' && EditString[I] > ' ') |
EditString[I] = ' '; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
else if( type == EDIT_BT_PIN ) // PIN |
{ |
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (EditString[I] < '9')) |
{ |
EditString[I]++; |
if (EditString[I] >= 0x00 && EditString[I] < ' ') |
EditString[I] = ' '; |
if (EditString[I] > ' ' && EditString[I] < '0') |
EditString[I] = '0'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (EditString[I] > '0')) |
{ |
EditString[I]--; |
if (EditString[I] < 'A' && EditString[I] > '9') |
EditString[I] = '9'; |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
} |
if( get_key_long (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME|| type == EDIT_SSID || type == EDIT_WL_PASSWORD) EditString[I] = ' '; // Zeichen loeschen |
if( type == EDIT_BT_PIN) EditString[I] = '0'; // Zeichen setzen |
lcd_putc (x, 3, EditString[I], 0); |
edit = 1; |
} |
if( get_key_short (1 << KEY_ESC)) |
{ |
if( type == EDIT_BT_NAME ) length_tmp = bt_name_length; |
if( type == EDIT_BT_PIN ) length_tmp = bt_pin_length; |
if (type == EDIT_SSID ) length_tmp = wlan_ssid_length; |
if (type == EDIT_WL_PASSWORD ) length_tmp = wlan_password_length; |
if ((x / 2) + 2 > length_tmp) |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
} |
else |
{ |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); |
x++; |
x++; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I++; //Zeiger auf Zeichen |
} |
} |
if( get_key_press (1 << KEY_ENTER) ) |
{ |
lOk = true; |
if( type == EDIT_BT_NAME && EditString[0] == ' ' ) // BT-Name: 1. Zeichen darf nicht Space sein |
{ |
lcdx_printp_at( 0, 5, PSTR(" FEHLER! 1. Zeichen! "), MNORMAL, 0,-4); |
set_beep ( 300, 0xf00f, BeepNormal); |
_delay_ms(2000); |
lcd_frect( 0, 5*8-4, 127, 7, 0); // loesche Fehlertext |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 0); // setze Cursor auf Position 1 |
x = 1; |
lcd_rect ((x*6)-3, (8*3)-2, 10, 10, 1); |
I = 0; |
get_key_press(KEY_ALL); |
lOk = false; |
} |
} |
} while( !lOk ); |
return 1; |
} |
//-------------------------------------------------------------- |
// min,max sind in Setup_PKTGeneric() festgelegt |
//-------------------------------------------------------------- |
void Edit_LipoOffset( void ) |
{ |
uint8_t redraw = true; |
uint8_t inc = 50; // in diesen Schritten hoch/runter zaehlen |
uint16_t min = 0; // min. Offset Wert |
uint16_t max = 15000; // max. Offset Wert |
uint16_t oldValue; |
if( Config.Lipo_UOffset % inc ) // ggf. wurde frueher ein anderer Wert fuer das Increment 'inc' verwendet |
Config.Lipo_UOffset = (uint16_t)(Config.Lipo_UOffset/inc)*inc; // hier wird der Wert auf das aktuelle 'inc' angepasst |
oldValue = Config.Lipo_UOffset; |
do |
{ |
//------------------------- |
// Screen zeichnen |
//------------------------- |
if( redraw ) |
{ |
PKT_TitleFromMenuItem( true ); // true = showlipo (mit clearscreen) |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,-4, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_rect_round( 0, 32, 127, 21, 1, R2); // Rahmen |
lcdx_printp_at( 1, 5, strGet(STR_HELP_LIPOOFFSET1), MNORMAL, 0,-5); // Hilfe-Text 1 |
lcdx_printp_at( 1, 6, strGet(STR_HELP_LIPOOFFSET2), MNORMAL, 0,-5); // Hilfe-Text 2 |
lcd_printp_at( 0, 7, strGet(KEYLINE5), MNORMAL); // Keyline |
redraw = false; |
} |
//------------------------- |
// Wert ausgeben |
//------------------------- |
lcdx_printf_at_P( 0, 3, MNORMAL, 0,-2, PSTR("> %5d => %1.2dv"), Config.Lipo_UOffset, volt_avg ); |
show_Lipo(); |
//------------------------- |
// PKT Update-Anforderung? |
//------------------------- |
if( PKT_CtrlHook() ) |
{ |
redraw = true; |
} |
//------------------------- |
// Tasten |
//------------------------- |
if( (get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2)) && (Config.Lipo_UOffset < max) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset + inc; |
} |
if( (get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2)) && (Config.Lipo_UOffset > min) ) |
{ |
Config.Lipo_UOffset = Config.Lipo_UOffset - inc; |
} |
if( get_key_press(1 << KEY_ENTER) ) |
{ |
clear_key_all(); |
return; |
} |
} while( !get_key_press(1 << KEY_ESC) ); |
clear_key_all(); |
Config.Lipo_UOffset = oldValue; // Abbruch, orginalen Wert wieder herstellen |
return; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_OSD_SCREEN_OLD |
uint8_t Edit_LED_Form (uint8_t Value, uint8_t min, uint8_t max ) |
{ |
Pre = Value; |
lcd_cls(); |
lcd_printp_at(0, 2, strGetOSDOLD(OSD_LED_Form), MNORMAL); |
switch (Value) |
{ |
case 0x1: |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
break; |
case 0x3 : |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
break; |
break; |
} |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
do |
{ |
if ((get_key_press (1 << KEY_PLUS)) && (Value == 1)) |
{ |
Value = 3; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // schwarz |
} |
if ((get_key_press (1 << KEY_MINUS)) && (Value == 3)) |
{ |
Value = 1; |
lcd_circle (14 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 3, 0); // l�schen |
lcd_circle (16 * 6 + 5, 2 * 8 + 3, 3, 1); // kreis |
lcd_fcircle (16 * 6 + 5, 2 * 8 + 3, 1, 1); // plus |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
edit = 1; |
Config.OSD_LEDform = Value; |
// WriteParameter(); |
edit = 0; |
return Value; |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
{ |
get_key_press(KEY_ALL); |
edit = 0; |
Config.OSD_LEDform = Pre; |
return Pre; |
} |
} |
#endif // USE_OSD_SCREEN_OLD |
//-------------------------------------------------------------- |
// Show_MAC( progmem, headline, mac) |
//-------------------------------------------------------------- |
void Show_MAC( uint8_t progmem, const char *headline, const char *mac ) |
{ |
uint8_t i; |
uint8_t z; |
lcd_cls(); |
PKT_TitleFromMenuItem( true ); |
if( progmem ) |
lcdx_printp_center( 2, headline, MNORMAL, 0,2); |
else |
lcdx_print_center( 2, (uint8_t *)headline, MNORMAL, 0,2); |
z = 0; |
for( i = 0; i < 13; i++) |
{ |
lcdx_putc( z+2, 5, mac[i], MNORMAL, 0,-1); |
if( (i%2==1) && (i<11) ) |
{ |
z++; |
lcdx_printp_at( z+2, 5, PSTR(":"), MNORMAL, 0,-1); |
} |
z++; |
} |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
Wait4KeyENDE( false ); |
} |
//-------------------------------------------------------------- |
// Setup_OSD() |
//-------------------------------------------------------------- |
void Setup_OSD( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Anzeige" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( OSDS_LOWBAT , MENU_ITEM, NOFUNC , OSDS_LOWBAT_de , OSDS_LOWBAT_en ); |
//MenuCtrl_PushML2_P( OSDS_MAH , MENU_ITEM, NOFUNC , OSDS_MAH_de , OSDS_MAH_en ); |
MenuCtrl_PushML2_P( OSDS_RCERROR , MENU_ITEM, NOFUNC , OSDS_RCERROR_de , OSDS_RCERROR_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_SHOWCELLU , MENU_ITEM, NOFUNC , OSDS_SHOWCELLU_de , OSDS_SHOWCELLU_en ); |
MenuCtrl_PushML2_P( OSDS_MKSETTING , MENU_ITEM, NOFUNC , OSDS_MKSETTING_de , OSDS_MKSETTING_en ); |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_FALLSPEED , MENU_ITEM, NOFUNC , OSDS_FALLSPEED_de , OSDS_FALLSPEED_en ); |
MenuCtrl_PushML2_P( OSDS_VARIOBEEP , MENU_ITEM, NOFUNC , OSDS_VARIOBEEP_de , OSDS_VARIOBEEP_en ); |
MenuCtrl_PushML2_P( OSDS_VOLTBAR , MENU_ITEM, NOFUNC , OSDS_VOLTBAR_de , OSDS_VOLTBAR_en ); |
MenuCtrl_PushML2_P( OSDS_OUTFORMAT , MENU_ITEM, NOFUNC , OSDS_OUTFORMAT_de , OSDS_OUTFORMAT_en ); |
MenuCtrl_PushML2_P( OSDS_OUTPOLARITY , MENU_ITEM, NOFUNC , OSDS_OUTPOLARITY_de , OSDS_OUTPOLARITY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( OSDS_DATASV2 , MENU_ITEM, NOFUNC , OSDS_DATASV2_de , OSDS_DATASV2_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == OSDS_LOWBAT ) { Config.MK_LowBat = Edit_generic( Config.MK_LowBat ,32,255, Show_uint10th,1 ,strGet(STR_HELP_LOWBAT1),strGet(STR_HELP_LOWBAT2)); } |
if( itemid == OSDS_SHOWCELLU ) { Config.OSD_ShowCellU = Edit_generic( Config.OSD_ShowCellU , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_MAH ) { Config.OSD_mAh_Warning = Edit_generic( Config.OSD_mAh_Warning , 0,30000, Show_uint5,100 ,NULL,NULL); } |
if( itemid == OSDS_RCERROR ) { Config.OSD_RCErrorbeep = Edit_generic( Config.OSD_RCErrorbeep , 0, 1, OnOff,1 ,NULL,NULL); } |
if( itemid == OSDS_MKSETTING ) { Config.OSD_ShowMKSetting = Edit_generic( Config.OSD_ShowMKSetting, 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_DATASV2 ) { Config.OSD_SendOSD = Edit_generic( Config.OSD_SendOSD , 0, 1, YesNo,1 ,NULL,NULL); } |
#ifdef USE_OSD_SCREEN_OLD |
if( itemid == OSDS_FALLSPEED ) { Config.OSD_Fallspeed = Edit_generic( Config.OSD_Fallspeed , 0,247, Show_uint10th,1 ,NULL,NULL); } |
if( itemid == OSDS_VARIOBEEP ) { Config.OSD_VarioBeep = Edit_generic( Config.OSD_VarioBeep , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_OUTFORMAT ) { Config.OSD_LEDform = Edit_LED_Form(Config.OSD_LEDform , 1, 3 ); } |
if( itemid == OSDS_OUTPOLARITY ) { Config.OSD_InvertOut = Edit_generic( Config.OSD_InvertOut , 0, 1, YesNo,1 ,NULL,NULL); } |
if( itemid == OSDS_VOLTBAR ) { Config.OSD_LipoBar = Edit_generic( Config.OSD_LipoBar , 0, 1, YesNo,1 ,NULL,NULL); } |
#endif |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Mark_OSDScreen( uint8_t osdscreenid ) |
{ |
MenuCtrl_ItemMarkR( osdscreenid, (Config.OSD_UseScreen & (1 << osdscreenid)) != 0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Edit_OSDScreen( uint8_t osdscreenid ) |
{ |
if( (Config.OSD_UseScreen & (1 << osdscreenid)) == 0 ) |
Config.OSD_UseScreen = (Config.OSD_UseScreen | (1 << osdscreenid)); |
else |
Config.OSD_UseScreen = (Config.OSD_UseScreen ^ (1 << osdscreenid)); |
Mark_OSDScreen( osdscreenid ); |
} |
//-------------------------------------------------------------- |
// Setup_OSDScreens() |
//-------------------------------------------------------------- |
void Setup_OSDScreens( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "OSD Screens" |
//--------------- |
// Menuitems |
//--------------- |
#ifdef USE_OSD_SCREEN_NAVIGATION |
MenuCtrl_Push_P( OSDSCREEN_NAVIGATION, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_NAVIGATION) ); Mark_OSDScreen( OSDSCREEN_NAVIGATION ); |
#endif |
#ifdef USE_OSD_SCREEN_WAYPOINTS |
MenuCtrl_Push_P( OSDSCREEN_WAYPOINTS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_WAYPOINTS) ); Mark_OSDScreen( OSDSCREEN_WAYPOINTS ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_ELECTRIC_N |
MenuCtrl_Push_P( OSDSCREEN_ELECTRIC , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_ELECTRIC) ); Mark_OSDScreen( OSDSCREEN_ELECTRIC ); |
#endif |
#ifdef USE_OSD_SCREEN_MKSTATUS |
MenuCtrl_Push_P( OSDSCREEN_MKSTATUS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_MKSTATUS) ); Mark_OSDScreen( OSDSCREEN_MKSTATUS ); |
#endif |
#ifdef USE_OSD_SCREEN_USERGPS |
MenuCtrl_Push_P( OSDSCREEN_USERGPS , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_USERGPS) ); Mark_OSDScreen( OSDSCREEN_USERGPS ); |
#endif |
#ifdef USE_OSD_SCREEN_3DLAGE |
MenuCtrl_Push_P( OSDSCREEN_3DLAGE , MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_3DLAGE) ); Mark_OSDScreen( OSDSCREEN_3DLAGE ); |
#endif |
#ifdef USE_OSD_SCREEN_STATISTIC |
MenuCtrl_Push_P( OSDSCREEN_STATISTICS, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_STATISTIC) ); Mark_OSDScreen( OSDSCREEN_STATISTICS ); |
#endif |
#ifdef USE_OSD_SCREEN_OLD |
MenuCtrl_Push_P( OSDSCREEN_OSD0, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD0) ); Mark_OSDScreen( OSDSCREEN_OSD0 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD1, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD1) ); Mark_OSDScreen( OSDSCREEN_OSD1 ); |
MenuCtrl_Push_P( OSDSCREEN_OSD2, MENU_ITEM, NOFUNC, strGet(STR_OSDSCREEN_OSD2) ); Mark_OSDScreen( OSDSCREEN_OSD2 ); |
#endif |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
Edit_OSDScreen( itemid ); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_FollowMe() |
//-------------------------------------------------------------- |
#ifdef USE_FOLLOWME |
void Setup_FollowMe( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitle_P( PSTR("FollowMe Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nicht verwendet. FollowMe wird aktuell ueber die Daten-Updaterate |
// der BT GPS-Maus getriggert! |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( FME_REFRESH , MENU_ITEM, NOFUNC , FME_REFRESH_de , FME_REFRESH_en ); |
#ifdef USE_FOLLOWME_STEP2 |
MenuCtrl_PushML2_P( FME_DISTANCE, MENU_ITEM, NOFUNC, FME_DISTANCE_de, FME_DISTANCE_en ); |
MenuCtrl_PushML2_P( FME_AZIMUTH , MENU_ITEM, NOFUNC, FME_AZIMUTH_de , FME_AZIMUTH_en ); |
#endif |
MenuCtrl_PushML2_P( FME_SPEED , MENU_ITEM, NOFUNC, FME_SPEED_de , FME_SPEED_en ); |
MenuCtrl_PushML2_P( FME_RADIUS , MENU_ITEM, NOFUNC, FME_RADIUS_de , FME_RADIUS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC, GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//if( itemid == FME_REFRESH ) { Config.FM_Refresh = Edit_generic( Config.FM_Refresh ,250,60000, Show_uint3,1 ,NULL,NULL); } |
if( itemid == FME_DISTANCE) { Config.FM_Distance= Edit_generic( Config.FM_Distance , 0, 100, Show_uint3,1 ,strGet(STR_METERS),NULL); } |
if( itemid == FME_AZIMUTH ) { Config.FM_Azimuth = Edit_generic( Config.FM_Azimuth , 0, 360, Show_uint3,1 ,PSTR("Grad"),NULL); } |
if( itemid == FME_SPEED ) { Config.FM_Speed = Edit_generic( Config.FM_Speed , 0, 100, Show_uint3,1 ,PSTR("0.1 m/s") ,NULL); } |
if( itemid == FME_RADIUS ) { Config.FM_Radius = Edit_generic( Config.FM_Radius , 1, 20, Show_uint3,1 ,strGet(STR_METERS),NULL); } |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
#endif // USE_FOLLOWME |
//-------------------------------------------------------------- |
// Setup_GPSMouse() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_GPSMouse( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "GPS Maus" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( GPS_SEARCH , MENU_ITEM, &BT_SearchDevices , GPS_SEARCH_de , GPS_SEARCH_en ); |
MenuCtrl_PushML2_P( GPS_DEVICES , MENU_ITEM, &BT_SelectDevice , GPS_DEVICES_de , GPS_DEVICES_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil erstmal nicht weiter benoetigt. Wurde zwar verwendet in tracking.c |
// aber auch dort wurde betroffener Code deaktiviert - siehe Anmerkung tracking.c / PKT_tracking() |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_TYPE , MENU_ITEM, NOFUNC , GPS_TYPE_de , GPS_TYPE_en ); |
//-------------------------------------------------------------------------------------------------------- |
// 25.06.2014 OG - auskommentiert weil nirgendwo verwendet |
//-------------------------------------------------------------------------------------------------------- |
//MenuCtrl_PushML2_P( GPS_ACTIVE , MENU_ITEM, NOFUNC , GPS_ACTIVE_de , GPS_ACTIVE_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( GPS_SHOWDEV , MENU_ITEM, NOFUNC , GPS_SHOWDEV_de , GPS_SHOWDEV_en ); |
MenuCtrl_PushML2_P( GPS_DATA , MENU_ITEM, NOFUNC , GPS_DATA_de , GPS_DATA_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// GPS_TYPE |
//-------------------- |
/* |
if( itemid == GPS_TYPE ) |
{ |
Config.gps_UsedGPSMouse = Edit_generic( Config.gps_UsedGPSMouse, 0,1,GPSMOUSE,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_ACTIVE |
//-------------------- |
/* |
if( itemid == GPS_ACTIVE ) |
{ |
Config.gps_UseGPS = Edit_generic( Config.gps_UseGPS, 0,1,YesNo,1 ,NULL,NULL); |
} |
*/ |
//-------------------- |
// GPS_SHOWDEV |
//-------------------- |
if( itemid == GPS_SHOWDEV ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( false, Config.gps_UsedDevName, Config.gps_UsedMac); |
} |
//-------------------- |
// GPS_DATA |
//-------------------- |
if( itemid == GPS_DATA ) |
{ |
GPSMouse_ShowData( GPSMOUSE_SHOW_TIME, 0 ); // 0 = beenden mit Verbindungstrennung |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// zeigt die Konfig des BTM-222 |
//-------------------------------------------------------------- |
void BTM222_Info( void ) |
{ |
lcd_cls(); |
set_Modul_On( Bluetooth ); |
if( !ScrollBox_Create(55) ) // max. 55 Zeilen |
return; // kein RAM mehr |
ScrollBox_Push_P( MNORMAL, PSTR("BTM-222 Config") ); |
ScrollBox_Push_P( MNORMAL, PSTR("") ); |
bt_showsettings(); |
ScrollBox_Push_P( MNORMAL, PSTR("End of Config") ); |
ScrollBox_Show(); |
ScrollBox_Destroy(); // free memory |
set_Modul_On( USB ); |
} |
//-------------------------------------------------------------- |
// Setup_BTM222_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob BTM222 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_BTM222_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( BT_INSTALLED , MENU_ITEM, NOFUNC , BT_INSTALLED_de , BT_INSTALLED_en ); |
if( Config.UseBT ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_NAME , MENU_ITEM, NOFUNC , BT_NAME_de , BT_NAME_en ); |
MenuCtrl_PushML2_P( BT_PIN , MENU_ITEM, NOFUNC , BT_PIN_de , BT_PIN_en ); |
MenuCtrl_PushML2_P( BT_REID , MENU_ITEM, NOFUNC , BT_REID_de , BT_REID_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_INIT , MENU_ITEM, NOFUNC , BT_INIT_de , BT_INIT_en ); |
MenuCtrl_PushML2_P( BT_PCCONFIG , MENU_ITEM, &Port_FC2CFG_BT , BT_PCCONFIG_de , BT_PCCONFIG_en ); |
MenuCtrl_PushML2_P( BT_SHOWCONFIG, MENU_ITEM, &BTM222_Info , BT_SHOWCONFIG_de, BT_SHOWCONFIG_en); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( BT_MAC , MENU_ITEM, NOFUNC , BT_MAC_de , BT_MAC_en ); |
} |
} |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BTM222() |
//-------------------------------------------------------------- |
#ifdef USE_BLUETOOTH |
void Setup_BTM222( void ) |
{ |
uint8_t itemid; |
uint8_t UseBT; |
uint8_t i; |
char string[11]; |
Setup_BTM222_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
UseBT = Config.UseBT; |
Config.UseBT = Edit_generic( Config.UseBT ,0,1, YesNo,1 ,NULL,NULL); |
if( UseBT != Config.UseBT ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseBT ) // BTM222 wurde aktiviert - initialisieren |
{ |
BTM222_Initalize(); |
//if( bt_init() ) |
// Config.BTIsSet = true; |
//else |
// Config.BTIsSet = false; |
} |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_BTM222_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// BT_NAME |
//-------------------- |
if( itemid == BT_NAME ) |
{ |
bt_fixname(); |
strncpyfill( string, Config.bt_name, bt_name_length+1 ); // bt_name_length=10 + 1 Terminierende 0 |
Edit_String( string, bt_name_length, EDIT_BT_NAME ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
//lcd_print_at( 0, 5, EditString, 0); // DEBUG |
//lcdx_printf_at( 17, 5, MNORMAL, 0,0, "%3d", strlen(EditString) ); |
//_delay_ms(8000); |
strncpy( Config.bt_name, EditString, bt_name_length+1 ); |
} |
} |
//-------------------- |
// BT_PIN |
//-------------------- |
if( itemid == BT_PIN ) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
string[i] = Config.bt_pin[i]; |
} |
string[bt_pin_length] = 0; |
Edit_String( string, bt_pin_length, EDIT_BT_PIN ); |
if (edit == 1) |
{ |
for( i = 0; i < bt_pin_length; i++) |
{ |
Config.bt_pin[i] = EditString[i]; |
} |
} |
} |
//-------------------- |
// BT_INIT |
//-------------------- |
if( itemid == BT_INIT ) |
{ |
//lcd_cls (); |
//Old_Baudrate = Config.PKT_Baudrate; |
BTM222_Initalize(); |
/* |
if( bt_init() == true ) |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init ok"), MNORMAL); |
WriteBTInitFlag(); |
} |
else |
{ |
lcd_printp_at( 0, 3, PSTR("BT Init Error"), MNORMAL); |
Config.BTIsSet = false; |
set_beep( 1000, 0x0040, BeepNormal); |
} |
_delay_ms(2500); // 2.5 Sekunden anzeigen fuer Benutzer |
*/ |
} |
//-------------------- |
// BT_MAC |
//-------------------- |
if( itemid == BT_MAC ) |
{ |
//Show_MAC( progmem, headline, mac) |
Show_MAC( true, PSTR("BTM-222 MAC"), Config.bt_Mac); |
} |
//-------------------- |
// BT_REID |
//-------------------- |
if( itemid == BT_REID ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
string[i] = Config.RE_ID[i]; |
} |
string[RE_ID_length] = 0; |
Edit_String( string, RE_ID_length, EDIT_BT_PIN ); |
if( edit == 1 ) |
{ |
for( i = 0; i < RE_ID_length; i++) |
{ |
Config.RE_ID[i] = EditString[i]; |
} |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
#endif // end: #ifdef USE_BLUETOOTH |
//-------------------------------------------------------------- |
// Setup_BLE() |
//-------------------------------------------------------------- |
void Setup_BLE( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( BLE_INSTALLED, MENU_ITEM, NOFUNC, strGet(MODULE_EXIST) ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// BT_INSTALLED |
//-------------------- |
if( itemid == BT_INSTALLED ) |
{ |
Config.UseBLE = Edit_generic( Config.UseBLE, 0, 1, YesNo, 1 ,strGet(STR_EXTSV2MODULE),NULL); |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_MKConnection() |
//-------------------------------------------------------------- |
void Setup_MKConnection( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Bluetooth BLE" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MKCONN_CONNECTION , MENU_ITEM, NOFUNC , MKCONN_CONNECTION_de , MKCONN_CONNECTION_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( MKCONN_BAUDRATE , MENU_ITEM, NOFUNC , MKCONN_BAUDRATE_de , MKCONN_BAUDRATE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// MKCONN_CONNECTION |
//-------------------- |
if( itemid == MKCONN_CONNECTION ) |
{ |
old = Config.U02SV2; |
Config.U02SV2 = Edit_generic( Config.U02SV2, 0, 1, MK_Connection,1, NULL,NULL); |
if( Config.U02SV2 != old ) // Verbindung geaendert? |
{ |
if( Config.U02SV2 == 1 ) Change_Output( Uart02FC ); // 1 = Kabel |
else Change_Output( Uart02Wi ); // 0 = Wi.232 |
} |
if( (Config.U02SV2 == 0) && (Config.UseWi == false) ) // FEHLER: Wi.232 nicht aktiviert (Config.U02SV2 = 0 -> entspricht Wi.232) |
{ |
//PKT_Message_P( text , error, timeout, beep, clear) |
PKT_Message_P( strGet(STR_WI232_ACTIVATE), true , 3000 , true, true); |
} |
} |
//-------------------- |
// MKCONN_BAUDRATE |
//-------------------- |
if( itemid == MKCONN_BAUDRATE ) |
{ |
old = Config.PKT_Baudrate; |
Config.PKT_Baudrate = Edit_generic( Config.PKT_Baudrate ,1,6,Baudrate,1 ,PSTR("Standard: ! 57600 !"),NULL); |
if( Config.PKT_Baudrate != old ) |
{ |
// Wi.232 umkonfigurieren |
if( Config.UseWi ) |
{ |
Wi232_Initalize(); |
} |
// BTM222 umkonfigurieren |
#ifdef USE_BLUETOOTH |
if( Config.UseBT ) |
{ |
//set_BTOn(); |
BTM222_Initalize(); |
//set_BTOff(); |
} |
#endif // end: #ifdef USE_BLUETOOTH |
} |
} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_BTM222() |
//-------------------------------------------------------------- |
// Setup_WI232_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob Wi.232 ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WI232_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Wi.232" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_Push_P( WI_INSTALLED , MENU_ITEM, NOFUNC , strGet(CONNECT13) ); |
if( Config.UseWi ) |
{ |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_TXRX , MENU_ITEM, NOFUNC , WI_TXRX_de , WI_TXRX_en ); |
MenuCtrl_PushML2_P( WI_GROUP , MENU_ITEM, NOFUNC , WI_GROUP_de , WI_GROUP_en ); |
// 06.06.2014 OG |
//MenuCtrl_PushML2_P( WI_MODE , MENU_ITEM, NOFUNC , WI_MODE_de , WI_MODE_en ); |
//MenuCtrl_PushML2_P( WI_TIMEOUT , MENU_ITEM, NOFUNC , WI_TIMEOUT_de , WI_TIMEOUT_en ); |
//MenuCtrl_PushML2_P( WI_MTU , MENU_ITEM, NOFUNC , WI_MTU_de , WI_MTU_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( WI_INIT , MENU_ITEM, NOFUNC , WI_INIT_de , WI_INIT_en ); |
//MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Port_USB2CFG_Wi , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
MenuCtrl_PushML2_P( WI_PCCONFIG , MENU_ITEM, &Wi232_ConfigPC , WI_PCCONFIG_de , WI_PCCONFIG_en ); |
} |
} |
//-------------------------------------------------------------- |
// Setup_WI232() |
//-------------------------------------------------------------- |
void Setup_WI232( void ) |
{ |
uint8_t itemid; |
uint8_t UseWi; |
Setup_WI232_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
if( itemid == WI_TXRX ) { Config.WiTXRXChannel = Edit_generic( Config.WiTXRXChannel , 0, 5, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_GROUP ) { Config.WiNetworkGroup = Edit_generic( Config.WiNetworkGroup , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MODE ) { Config.WiNetworkMode = Edit_generic( Config.WiNetworkMode , 0, 1, Wi_Netmode,1 ,NULL,NULL); } |
//if( itemid == WI_TIMEOUT ) { Config.WiTXTO = Edit_generic( Config.WiTXTO , 0, 127, Show_uint3,1 ,NULL,NULL); } |
//if( itemid == WI_MTU ) { Config.WiUartMTU = Edit_generic( Config.WiUartMTU , 0, 127, Show_uint3,1 ,NULL,NULL); } |
if( itemid == WI_INIT ) { Wi232_Initalize(); } |
//-------------------- |
// WI_INSTALLED |
//-------------------- |
if( itemid == WI_INSTALLED ) |
{ |
UseWi = Config.UseWi; |
Config.UseWi = Edit_generic( Config.UseWi, 0,1, YesNo,1, NULL,NULL); |
if( UseWi != Config.UseWi ) // hat Benutzer Einstellung geaendert? |
{ |
if( Config.UseWi ) |
Wi232_Initalize(); // Wi232 wurde aktiviert: init |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wi232 vorhanden / nicht vorhanden |
Setup_WI232_MenuCreate(); |
continue; |
} |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_PKTGeneric() |
// |
// entspricht "PKT allgemein" |
//-------------------------------------------------------------- |
void Setup_PKTGeneric( void ) |
{ |
uint8_t itemid; |
uint8_t old; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT allgemein" |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( PKT_LANGUAGE , MENU_ITEM, NOFUNC , LANGUAGE_de , LANGUAGE_en ); |
MenuCtrl_PushML2_P( TIME_ZONE , MENU_ITEM, NOFUNC , TIME_ZONE_de , TIME_ZONE_en ); |
MenuCtrl_PushML2_P( TIME_SUMMER , MENU_ITEM, NOFUNC , TIME_SUMMER_de , TIME_SUMMER_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( PKT_PKTOFF , MENU_ITEM, NOFUNC , PKT_PKTOFF_de , PKT_PKTOFF_en ); |
MenuCtrl_PushML2_P( PKT_LIGHTOFF , MENU_ITEM, NOFUNC , LIGHTOFF_de , LIGHTOFF_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
// 18.04.2014 OG: rausgenommen da es aktuell auch nicht mehr mit PKT 3.9m Hardware geht |
//if( PCB_Version == PKT39m ) // Helligkeit nur bei PKT39m - bei der PKT39x geht das nicht mehr |
// MenuCtrl_PushML2_P( PKT_BRIGHTNESS , MENU_ITEM, NOFUNC , BRIGHTNESS_de , BRIGHTNESS_en ); |
MenuCtrl_PushML2_P( PKT_CONTRAST , MENU_ITEM, NOFUNC , CONTRAST_de , CONTRAST_en ); |
MenuCtrl_PushML2_P( PKT_BEEPER , MENU_ITEM, NOFUNC , BEEPER_de , BEEPER_en ); |
#ifdef USE_SOUND |
MenuCtrl_PushML2_P( PKT_SOUNDMODUL , MENU_ITEM, NOFUNC , SOUNDMODUL_de , SOUNDMODUL_en ); |
MenuCtrl_PushML2_P( PKT_VOLUME , MENU_ITEM, NOFUNC , VOLUME_de , VOLUME_en ); |
#endif |
MenuCtrl_PushML2_P( PKT_ACCUTYPE , MENU_ITEM, NOFUNC , ACCUTYPE_de , ACCUTYPE_en ); |
MenuCtrl_PushML2_P( PKT_ACCUMEASURE , MENU_ITEM, NOFUNC , ACCUMEASURE_de , ACCUMEASURE_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == PKT_LANGUAGE ) |
{ |
old = Config.DisplayLanguage; |
Config.DisplayLanguage = Edit_generic( Config.DisplayLanguage , 0, 1, Language,1, NULL,NULL); |
if( old != Config.DisplayLanguage ) |
{ |
PKT_Ask_Restart( strGetLanguage(LANGUAGE_de,LANGUAGE_en) ); |
} |
} |
if( itemid == TIME_ZONE ) Config.timezone = Edit_generic( Config.timezone ,-12, 12, Show_uint3,1, PSTR("+1 = Berlin "),PSTR("-5 = New York")); |
if( itemid == TIME_SUMMER ) Config.summertime = Edit_generic( Config.summertime , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_LIGHTOFF ) Config.DisplayTimeout = Edit_generic( Config.DisplayTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_PKTOFF ) Config.PKTOffTimeout = Edit_generic( Config.PKTOffTimeout , 0,254, Show_uint3,1, strGet(STR_HELP_PKTOFFTIME1),NULL); |
if( itemid == PKT_BEEPER ) Config.HWBeeper = Edit_generic( Config.HWBeeper , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_SOUNDMODUL ) Config.HWSound = Edit_generic( Config.HWSound , 0, 1, YesNo,1, NULL,NULL); |
if( itemid == PKT_VOLUME ) Config.Volume = Edit_generic( Config.Volume , 0, 50, Show_uint3,1, NULL,NULL); |
if( itemid == PKT_CONTRAST ) { Config.LCD_Kontrast = Edit_generic( Config.LCD_Kontrast , 10, 40, Kontrast,1, NULL,NULL); |
lcd_set_contrast( Config.LCD_Kontrast ); |
} |
//if( itemid == PKT_BRIGHTNESS ) Config.LCD_Helligkeit = Edit_DisplayHelligkeit(Config.LCD_Helligkeit,0,100); |
if( itemid == PKT_ACCUTYPE ) Config.PKT_Accutyp = Edit_generic( Config.PKT_Accutyp , 0, 1, PKT_Akku,1, NULL,NULL); |
if( itemid == PKT_ACCUMEASURE ) Edit_LipoOffset(); |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Main() |
// |
// Das Hauptmenue fuer das gesamte Setup |
//-------------------------------------------------------------- |
void Setup_MAIN( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
//--------------- |
// Einstellungen |
//--------------- |
MenuCtrl_SetTitleFromParentItem(); // "PKT Setup" |
//MenuCtrl_SetTitle_P( PSTR("PKT Setup") ); |
//MenuCtrl_SetCycle( false ); |
//MenuCtrl_SetShowBatt( false ); |
//MenuCtrl_SetBeep( true ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( SETUP_PKTCONFIG , MENU_SUB , &Setup_PKTGeneric , SETUP_PKTCONFIG_de , SETUP_PKTCONFIG_en ); |
MenuCtrl_PushML2_P( SETUP_OSDVIEW , MENU_SUB , &Setup_OSD , SETUP_OSDVIEW_de , SETUP_OSDVIEW_en ); |
MenuCtrl_PushML2_P( SETUP_OSDSCREENS , MENU_SUB , &Setup_OSDScreens , SETUP_OSDSCREENS_de , SETUP_OSDSCREENS_en ); |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_MKCONNECTION , MENU_SUB , &Setup_MKConnection , SETUP_MKCONNECTION_de , SETUP_MKCONNECTION_en ); |
MenuCtrl_PushML2_P( SETUP_WI232 , MENU_SUB , &Setup_WI232 , SETUP_WI232_de , SETUP_WI232_en ); |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_BTM222 , MENU_SUB , &Setup_BTM222 , SETUP_BTM222_de , SETUP_BTM222_en ); |
#endif |
#if (defined(USE_SV2MODULE_BLE) || defined(USE_WLAN)) |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#endif |
#ifdef USE_SV2MODULE_BLE |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_BLE , MENU_SUB , &Setup_BLE , SETUP_BLE_de , SETUP_BLE_en ); |
#endif |
#ifdef USE_WLAN |
if( PCB_SV2RxTxPatch ) // nur sichtbar mit SV2 Patch! |
MenuCtrl_PushML2_P( SETUP_WIFLY , MENU_SUB , &Setup_WiFly , SETUP_WIFLY_de , SETUP_WIFLY_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
#ifdef USE_BLUETOOTH |
MenuCtrl_PushML2_P( SETUP_GPSMAUS , MENU_SUB , &Setup_GPSMouse , SETUP_GPSMAUS_de , SETUP_GPSMAUS_en ); |
#endif |
#ifdef USE_FOLLOWME |
MenuCtrl_PushML2_P( SETUP_FOLLOWME , MENU_SUB , &Setup_FollowMe , SETUP_FOLLOWME_de , SETUP_FOLLOWME_en ); |
#endif |
#ifdef USE_TRACKING |
MenuCtrl_PushML2_P( SETUP_SERVOCONFIG , MENU_SUB , &Setup_ServoTracking, SETUP_SERVOCONFIG_de , SETUP_SERVOCONFIG_en ); // tracking/servo_setup.c |
#endif |
#ifdef USE_JOYSTICK |
MenuCtrl_PushML2_P( SETUP_JOYSTICK , MENU_SUB , &Setup_Joysticks , SETUP_JOYSTICK_de , SETUP_JOYSTICK_en ); |
#endif |
MenuCtrl_PushSeparator(); // MENUE TRENNLINIE |
MenuCtrl_PushML2_P( SETUP_PKTUPDATE , MENU_ITEM, &PKT_Update , SETUP_PKTUPDATE_de , SETUP_PKTUPDATE_en ); |
//MenuCtrl_PushML2_P( SETUP_PKTDEBUG , MENU_ITEM, NOFUNC , SETUP_PKTDEBUG_de , SETUP_PKTDEBUG_en ); |
MenuCtrl_PushML2_P( SETUP_EEPROMRESET , MENU_ITEM, &PKT_Reset_EEprom , SETUP_EEPROMRESET_de , SETUP_EEPROMRESET_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
//-------------------- |
// SETUP_PKTDEBUG |
//-------------------- |
if( itemid == SETUP_PKTDEBUG ) |
{ |
Config.Debug = Edit_generic( Config.Debug, 0,1,Show_uint3,1 ,NULL,NULL); |
} |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/setup/setup.h |
---|
0,0 → 1,111 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY setup.h |
//# |
//# 26.06.2014 OG |
//# - add: Setup_FollowMe() |
//# |
//# 22.06.2014 OG |
//# - del: Variable CheckGPS |
//# |
//# 13.06.2014 OG |
//# - del: ChangeWi_SV2() |
//# |
//# 11.06.2014 OG |
//# - del: Edit_Language() |
//# |
//# 31.04.2014 OG |
//# - chg: Edit_String() - Parameter 'Text' entfernt |
//# |
//# 28.05.2014 OG |
//# - chg: Edit_generic() - geaenderte Aufrufparameter |
//# - Parameter 'Text' enfernt da vom Menue geerbt |
//# - Parameter fuer Hilfetexte |
//# |
//# 04.04.2014 OG |
//# - del: Edit_ShowTitle_P() |
//# |
//# 02.04.2014 OG |
//# - add: Edit_ShowTitle_P() |
//# - add: WlanMode |
//# |
//# 15.07.2013 Cebra |
//# - add: Edit_Printout um WlanSecurity erweitert |
//# |
//# 22.05.2013 OG |
//# - del: verschiedene Cleanups |
//############################################################################ |
#ifndef _setup_H |
#define _setup_H |
#define EDIT_BT_NAME 1 |
#define EDIT_BT_PIN 2 |
#define EDIT_SSID 3 |
#define EDIT_WL_PASSWORD 4 |
typedef enum // Varianten fuer die Anzeige der Werte im Setupmenüe |
{ |
Show_uint3, Show_int3, Show_uint5, Show_uint10th, GPSMOUSE, Wi_Netmode, |
OnOff, YesNo, Orientation, NormRev, Kontrast, Baudrate, Language, Sticktype, WlanMode, WlanSecurity, PKT_Akku, MK_Connection |
} EditMode; |
void Setup_MAIN(void); |
void Setup_FollowMe( void ); |
void Display_Setup (void); |
void Wi_Use (void); |
void BT_Use (void); |
void BT_SelectDevice (void); |
uint8_t BT_CheckBTM222( void ); |
void Wait4KeyOK( void ); |
int16_t Edit_generic( int16_t Value, int16_t min, int16_t max, uint8_t what, uint8_t Addfactor, const char *strHelp1, const char *strHelp2 ); |
uint8_t Edit_String( const char *data, const uint8_t length, uint8_t type); |
uint8_t BT_ShowGPSData( uint8_t modus ); |
void BTM222_Info( void ); |
extern uint8_t bt_name_len; |
extern uint8_t length_tmp; |
extern uint8_t Old_Baudrate; //Merkzelle für alte Baudrate |
extern uint8_t edit; |
extern char EditString[21]; |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/setup |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/trunk/sound/pwmsine8bit.c |
---|
0,0 → 1,280 |
//file pwmsine8bit.c |
//generate 8 bit sinewave using wavetable lookup |
//12khz sample rate, 62500 hz pwm on pd5 oc1a |
//for ere co embmega32 16Mhz 38400 |
//May 3 07 Bob G initial edit 8 bit |
//May 5 07 Bob G add traffic tones |
//Jan 18 08 Bob G compile with 7.15 |
//#include <stdio.h> |
//#include <stdlib.h> |
#include <ctype.h> |
#include <math.h> //sin |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <string.h> |
#include <stdint.h> |
#include "../timer/timer.h" |
#include "../main.h" |
#ifdef USE_SOUND |
#define INTR_OFF() asm("CLI") |
#define INTR_ON() asm("SEI") |
//------vars in bss------- |
volatile unsigned char sindat[256]; //8 bit sine table |
//unsigned int addat[8]; //internal 10 bit |
unsigned int idesfreq; |
unsigned char iattenfac; //0-255 |
unsigned char dispon; |
unsigned char freqincipart,freqincfpart; |
unsigned char waveptipart,waveptfpart; |
unsigned char tics; |
unsigned char generate; |
float basefreq; |
float freqratio; |
float sampfreq; //12khz |
float samppd; //83.33usec |
float fdesfreq; |
float attendb; //0-50 |
float attenfac; //.999-0 |
const float pi=3.141592654; |
#define MSB(w) ((char*) &w)[1] |
#define LSB(w) ((char*) &w)[0] |
#define TONE_BUFFER_SIZE 64 |
#define TONE_BUFFER_MASK ( TONE_BUFFER_SIZE - 1) |
static volatile unsigned char ToneBuf[TONE_BUFFER_SIZE]; |
static volatile unsigned char ToneBufHead; |
static volatile unsigned char ToneBufTail; |
//------------------------------- |
void cvtfreq2frac(void){ |
//calc freq ratio, separate into integer and fractional parts |
fdesfreq=idesfreq; |
freqratio=fdesfreq/basefreq; //calc freq ratio from basefreq |
freqincipart=(int)freqratio; //calc integer part |
freqincfpart=(freqratio-freqincipart)*256; //calc fractional part |
// cprintf("ipart %02x fpart %02x\n",freqincipart,freqincfpart); |
} |
//------------------------------- |
void cvtdb2fac(void){ |
//cvt db to attenuation factor |
attenfac=pow(10.0,-attendb/20.0); //10^0=1 |
iattenfac=(unsigned char)(attenfac*255.0); |
// cprintf("attenfac %#7.5f\n",attenfac); |
// cprintf("iattenfac %02x\n",iattenfac); |
} |
//------------------------- |
void initsindat(void){ |
//init 8 bit signed sin table 0-ff in ram 36dB s/n 46Hz basefreq |
unsigned int n; |
for(n=0; n < 256; n++){ |
sindat[n]=(signed char)127.0*sin(2.0*pi*n/256.0); //256 sin points |
} |
} |
//------------------------- |
void initvars(void){ |
//init vars |
// sampfreq=12000.0; //hz |
sampfreq=19530.0; //hz |
// printf("samp freq %#7.1f hz\n\r",sampfreq); |
// printf("samp freq %#7.1f hz\n\r",1234567.1); |
samppd=1.0/sampfreq; //83.33usec |
// printf("samppd %#7.1f usec \n\r",samppd*1e6); |
basefreq=sampfreq/256.0; //46.875hz |
// printf("basefreq %#7.5f hz\n\r",basefreq); |
idesfreq=400; |
cvtfreq2frac(); |
cvtdb2fac(); //0db =.9999= 0xff |
} |
//--------------------- |
void tone1(int hz) |
//tone at hz for ms |
{ |
uint8_t volume = 0; |
// TCNT2=0; |
// sound_timer = ms/5; |
// soundpause_timer = soundpause/5; |
idesfreq=hz; |
cvtfreq2frac(); |
attendb=volume; |
cvtdb2fac(); |
if (hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
} |
//------------------------------- |
void tone_handler(void) // wird aus Timerinterrupt aufgerufen |
{ |
uint16_t f_Hz=0; |
uint16_t dur_ms=0; |
char volume=0; |
char tmp_high = 0; |
char tmp_low = 0; |
unsigned char tmptail; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* f_Hz low */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_high = ToneBuf[tmptail]; /* f_Hz high */ |
}else{ |
return; |
} |
f_Hz = (((uint16_t) tmp_high) << 8) | tmp_low; |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
volume = ToneBuf[tmptail]; /* volume */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer */ |
tmp_low = ToneBuf[tmptail]; /* low Duratuion */ |
}else{ |
return; |
} |
if ( ToneBufHead != ToneBufTail) { |
/* calculate and store new buffer index */ |
tmptail = (ToneBufTail + 1) & TONE_BUFFER_MASK; |
ToneBufTail = tmptail; |
/* get one byte from buffer*/ |
tmp_high = ToneBuf[tmptail]; /* high Duration */ |
}else{ |
return; |
} |
dur_ms = (((uint16_t) tmp_high) << 8) | tmp_low; |
sound_timer = dur_ms/10; |
// TCNT2=0; |
idesfreq= f_Hz; |
cvtfreq2frac(); |
attendb= volume; |
cvtdb2fac(); |
if (f_Hz ==0) generate=0; |
else { generate = 1; |
// TIMSK2 |= _BV(TOIE2); // Interrupt freigeben |
} |
// printf("tone_handler f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
} |
void tone_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (ToneBufHead + 1) & TONE_BUFFER_MASK; |
while ( tmphead == ToneBufTail ){ |
;/* wait for free space in buffer */ |
} |
ToneBuf[tmphead] = data; |
ToneBufHead = tmphead; |
}/* tone_putc */ |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume) |
{ |
// printf("Playtone f_Hz: %i dur_ms: %i volume: %i\n\r", f_Hz,dur_ms,volume); |
tone_putc(LSB(f_Hz)); |
tone_putc( MSB(f_Hz)); |
tone_putc( volume); |
tone_putc(LSB(dur_ms)); |
tone_putc(MSB(dur_ms)); |
} |
//----------------- |
void InitSound(void){//main program |
ToneBufHead = 0; |
ToneBufTail = 0; |
initvars(); |
initsindat(); |
Timer2_Init(); |
Timer3_Init(); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/sound/pwmsine8bit.h |
---|
0,0 → 1,22 |
/* |
* pwmsine8bit.h |
* |
* Created on: 01.09.2012 |
* Author: cebra |
*/ |
#ifndef PWMSINE8BIT_H_ |
#define PWMSINE8BIT_H_ |
extern unsigned char freqincipart,freqincfpart; |
extern unsigned char waveptipart,waveptfpart; |
extern unsigned char iattenfac; //0-255 |
extern unsigned char generate; |
extern volatile unsigned char sindat[256]; //8 bit sine table |
void InitSound(void); |
void tone(int hz,uint8_t volume, uint16_t ms, uint16_t soundpause); |
void tone1(int hz); |
void playTone(uint16_t f_Hz, uint16_t dur_ms, uint8_t volume); |
void tone_handler(void); |
#endif /* PWMSINE8BIT_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/stick/stick.c |
---|
0,0 → 1,507 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* Copyright (C) 2012 Martin Runkel * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
//#include "../tracking/servo.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "stick.h" |
#include "stick_setup.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#define SERVO_CORRECT 3.125 |
#include <util/delay.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
int16_t Pos_Stick[12]; // 1,5mS |
int16_t Pos_alt[5]; // |
uint8_t BalkenPos = 0; |
uint8_t Stick_Display = 0; |
//uint8_t serialChannelRichtung = 0; |
//uint8_t serialChannelNeutral = 0; |
//uint8_t serialChannelConfig = 2; |
//-------------------------------------------------------------- |
// |
void joystick (void) |
{ |
// uint8_t chg = 0; |
// uint8_t Pos_Stick = 150; // 1,5mS |
// uint8_t Pos_alt = 150; // |
//int16_t Pos_Stick[12]; // 1,5mS |
uint8_t chg = 0; |
//uint8_t BalkenPos = 0; |
uint8_t Stick_Nr = 0; |
//uint8_t Stick_Display = 0; |
uint8_t i = 0; |
memset (Pos_Stick, 150, 3); // füllt 3+1 Byte vom Pos_Stick[12] mit 150 |
//int16_t Pos_alt[5]; // |
int16_t Poti_Summe[5]; // |
memset (Poti_Summe, 0, 5); // füllt 3+1 Byte mit 0 |
int16_t Poti_Neutral[5]; // |
// ADC- init |
//if (Config.Lipomessung == true) |
//{ |
//} |
//for (uint8_t i = 0; i < 4; i++) |
if (Config.stick_typ[0] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA0); |
DDRA &= ~(1<<DDA0); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA0); |
DDRA |= (1<<DDA0); |
} |
if (Config.Lipomessung == false) |
{ |
if (Config.stick_typ[1] == 0) // Poti |
{ |
PORTA &= ~(1<<PORTA1); |
DDRA &= ~(1<<DDA1); |
} |
else // Taster |
{ |
PORTA |= (1<<PORTA1); |
DDRA |= (1<<DDA1); |
} |
} |
if (Config.stick_typ[2] == 0) |
{ |
PORTA &= ~(1<<PORTA2); |
DDRA &= ~(1<<DDA2); |
} |
else |
{ |
PORTA |= (1<<PORTA2); |
DDRA |= (1<<DDA2); |
} |
if (Config.stick_typ[3] == 0) |
{ |
PORTA &= ~(1<<PORTA3); |
DDRA &= ~(1<<DDA3); |
} |
else |
{ |
PORTA |= (1<<PORTA3); |
DDRA |= (1<<DDA3); |
} |
//if (Config.stick_dir[i] == 0) serialChannelRichtung &= ~(1<<i); else serialChannelRichtung |= (1<<i); |
//if (Config.stick_neutral[i] == 0) serialChannelNeutral &= ~(1<<i); else serialChannelNeutral |= (1<<i); |
//if (Config.Lipomessung == true) |
//serialChannelConfig |
// Ermittlung der Neutralposition |
for (uint8_t i = 0; i < 4; i++) |
{ |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[i] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[i],108,148); |
} |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
ADCSRA |= (1<<ADSC); // ADC Start |
/* |
Stick_Nr = 0; |
ADMUX = (1<<REFS0)|(0<<MUX0); // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1 |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[Stick_Nr] verwenden |
timer = 50; |
while (timer > 0); |
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128 |
// Stick-Neutralposition bestimmen |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 2 ; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
while (ADCSRA & (1<<ADSC)); // wenn ADC fertig |
Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff); |
LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148); |
Stick_Nr = 0; |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
*/ |
//OCR1A = 150 * SERVO_CORRECT; // Servomitte |
lcd_cls (); |
// Kopfzeile und Rahmen zeichnen |
lcd_printp (PSTR(" serielle Potis 1-5 "), 2); |
//lcd_printp_at (7, 5, PSTR("%"), 0); |
//lcd_printp_at (16, 5, PSTR("mS"), 0); |
lcd_printp_at(0, 7, strGet(KEYLINE3), 0); |
lcd_printp_at (18, 7, PSTR("\x19O\x18"), 0); |
for (i=0 ; i< 5 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_rect(3,BalkenPos, 120, 6, 1); // +-150% Rahmen |
lcd_line(23,BalkenPos,23,(BalkenPos+6),1); // -100% |
lcd_line(43,BalkenPos,43,(BalkenPos+6),1); // -50% |
lcd_frect(62,BalkenPos, 2, 6, 1); // 0% |
lcd_line(83,BalkenPos,83,(BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
} |
// Reset auf Mittelstellung |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
chg = 255; |
do |
{ |
if (!(ADCSRA & (1<<ADSC))) // wenn ADC fertig |
{ |
//Pos_Stick[Stick_Nr] = 150 + 128 - ((ADCW>>2)&0xff); |
//if (serialChannelRichtung & (1<<Stick_Nr)) |
if (Config.stick_dir[Stick_Nr] > 0) |
Pos_Stick[Stick_Nr] = Poti_Neutral[Stick_Nr] - ((ADCW>>2)&0xff); |
else |
Pos_Stick[Stick_Nr] = ((ADCW>>2)&0xff) - Poti_Neutral[Stick_Nr]; |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],-120,120); |
//if ((Stick_Nr==0) || (Stick_Nr==2)) // nur die Potis 1,2 sind nicht neutralisierend |
if (Config.stick_neutral[Stick_Nr]==0) // nicht neutralisierend |
{ |
Poti_Summe[Stick_Nr] += (Pos_Stick[Stick_Nr]/8) * abs(Pos_Stick[Stick_Nr]/8); |
LIMIT_MIN_MAX (Poti_Summe[Stick_Nr],-(120*128),(120*128)); |
Pos_Stick[Stick_Nr]= Poti_Summe[Stick_Nr] / 128; // nicht neutralisierend |
} |
Pos_Stick[Stick_Nr] += 150; |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],Config.stick_min[Stick_Nr],Config.stick_max[Stick_Nr]); // war 30, 270 |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) // nur bei Änderung |
{ |
chg |= (1<<Stick_Nr) ; |
//Pos_alt=Pos_Stick ; // verschoben |
} |
Stick_Nr ++ ; |
//if (Stick_Nr==1) Stick_Nr=2; // Lipo überspringen |
if (Stick_Nr==3) // Taster |
{ |
if (Config.stick_dir[Stick_Nr] > 0) |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
} |
else |
{ |
if (PINA & (1 << KEY_EXT)) Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr]; |
else Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr]; |
} |
if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) |
{ |
chg |= (1<<Stick_Nr) ; |
} |
Stick_Nr=0; |
} |
/* |
#ifndef ohne_Lipo // MartinR |
Stick_Nr = 1; // MartinR AD-Kanal 1 überspringen wegen Lipo Überwachung |
#endif |
*/ |
ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden |
// Stick_Nr 1,2,3 = Potis, Stick_Nr 0= Lipo |
ADCSRA |= (1<<ADSC); // ADC Start |
//serialPotis (); |
} |
if ((get_key_press (1 << KEY_PLUS) | get_key_long_rpt_sp ((1 << KEY_PLUS), 3))) |
//if (get_key_press (1 << KEY_PLUS)) |
//if (PINA & (1 << KEY_PLUS)) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] ++ ; else Pos_Stick[4] -- ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_max[4] ; else Pos_Stick[4] = Config.stick_min[4] ; |
} |
chg |= (1<<4) ; |
} |
else if ((get_key_press (1 << KEY_MINUS) | get_key_long_rpt_sp ((1 << KEY_MINUS), 3))) |
{ |
if (Config.stick_neutral[4]==0) // nicht neutralisierend |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] -- ; else Pos_Stick[4] ++ ; |
LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270 |
//LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225 |
} |
else |
{ |
if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_min[4] ; else Pos_Stick[4] = Config.stick_max[4] ; |
} |
chg |= (1<<4) ; |
} |
if (Config.stick_neutral[4]!= 0 && Pos_Stick[4]!= 150 && (PINA &(1 << KEY_PLUS)) && (PINA &(1 << KEY_MINUS))) |
{ |
Pos_Stick[4] = 150 ; |
chg |= (1<<4) ; |
} |
if (get_key_press (1 << KEY_ENTER)) |
{ |
/* |
for (i=0 ; i< 4 ; i++) |
{ |
BalkenPos = 12 + (i*8) ; |
lcd_frect (4, (BalkenPos+1), 118, 4, 0); // Balken löschen |
lcd_frect(62, BalkenPos, 2, 6, 1); // 0% |
} |
*/ |
Pos_Stick[0] = 150; |
Poti_Summe[0] = 0; |
Pos_Stick[2] = 150; |
Poti_Summe[2] = 0; |
Pos_Stick[4] = 150; |
Poti_Summe[4] = 0; |
BeepTime = 200; |
BeepMuster = 0x0080; |
chg = 255; |
} |
if (chg) |
{ |
if (chg & (1<<0)); // Stick 1 |
{ |
BalkenPos = 12 + (0*8) ; |
Stick_Display = 0; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
// Stick 2 = Lipo |
if (chg & (1<<1)); // Stick 2 |
{ |
BalkenPos = 12 + (1*8) ; |
Stick_Display = 1; |
//if (serialChannelConfig & (0<<1)) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
if (Config.Lipomessung == false) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<2)); // Stick 3 |
{ |
BalkenPos = 12 + (2*8) ; |
Stick_Display = 2; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<3)); // Stick 4 = Taster |
{ |
BalkenPos = 12 + (3*8) ; |
Stick_Display = 3; |
Balken_Zeichnen () ; |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
if (chg & (1<<4)); // Stick 5 = Taster vom PKT |
{ |
BalkenPos = 12 + (4*8) ; |
Stick_Display = 4; |
Balken_Zeichnen () ; |
//OCR1A = (((Pos_Stick[Stick_Display]-150)/1.6)+150) * SERVO_CORRECT; // Servostellung , 1.6=0.8*0.5 |
Pos_alt[Stick_Display]=Pos_Stick[Stick_Display]; |
} |
chg = 0; |
serialPotis (); |
} |
} |
while (!get_key_press (1 << KEY_ESC)); |
get_key_press(KEY_ALL); |
//#ifdef HWVERSION3_9 |
//#ifndef ohne_Lipo // MartinR |
if (Config.Lipomessung == true) // MartinR: geändert |
{ |
//DDRA &= ~(1<<DDA1); // Eingang: PKT Lipo Messung |
//PORTA &= ~(1<<PORTA1); |
ADC_Init(); // ADC für Lipomessung wieder aktivieren |
} |
//ADC_Init(); // ADC für Lipomessung wieder aktivieren |
//#endif |
//#endif |
} |
//-------------------------------------------------------------- |
// |
void serialPotis (void) |
{ |
uint8_t i = 0; |
memset (buffer, 0, 12); // füllt die 12+1 Byte vom buffer mit 0 |
for (i=0 ; i< 5 ; i++) |
{ |
buffer[i] = Pos_Stick[i]-150 ; |
} |
SendOutData('y', ADDRESS_FC, 1, buffer, 12); |
} |
//-------------------------------------------------------------- |
// |
void Balken_Zeichnen (void) |
{ |
// Balken löschen |
if ((Pos_Stick[Stick_Display] > Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] < 150)) // Balken links löschen |
lcd_frect ((63-((150 -Pos_alt[Stick_Display]) * 0.5)), (BalkenPos+1), (63-((150- Pos_Stick[Stick_Display]) * 0.5)), 4, 0); |
if ((Pos_Stick[Stick_Display] < Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] > 150)) // Balken rechts löschen |
lcd_frect ((63+((Pos_Stick[Stick_Display] - 150) * 0.5)), (BalkenPos+1), (63+((Pos_alt[Stick_Display] - 150) * 0.5)), 4, 0); |
// Balken zeichnen |
if (Pos_Stick[Stick_Display] >= 150) |
{ |
lcd_frect (63, (BalkenPos+1), ((Pos_Stick[Stick_Display] - 150) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((Pos_Stick[Stick_Display] - 150) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
else |
{ |
lcd_frect (63 - ((150 - Pos_Stick[Stick_Display]) * 0.5), (BalkenPos+1), ((150 - Pos_Stick[Stick_Display]) * 0.5), 4, 1); |
//write_ndigit_number_u (4, 5, ((150 - Pos_Stick[Stick_Display]) * 1.25), 3, 0, 0); // Pulse width in % |
lcd_frect(62, (BalkenPos), 2, 6, 1); // 0% |
} |
// Raster zeichnen |
lcd_line(3, BalkenPos,3, (BalkenPos+6),1); // -150% |
lcd_line(23, BalkenPos,23, (BalkenPos+6),1); // -100% |
lcd_line(43, BalkenPos,43, (BalkenPos+6),1); // -50% |
lcd_line(83, BalkenPos,83, (BalkenPos+6),1); // +50% |
lcd_line(103,BalkenPos,103,(BalkenPos+6),1); // +100% |
lcd_line(123,BalkenPos,123,(BalkenPos+6),1); // +150% |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/stick/stick.h |
---|
0,0 → 1,42 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
#ifndef _STICK_H |
#define _STICK_H |
void joystick (void); |
void serialPotis (void); |
void Balken_Zeichnen (void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/stick/stick_setup.c |
---|
0,0 → 1,279 |
/****************************************************************/ |
/* Setup für die Sticks */ |
/* 2013 Cebra */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY stick_setup.c |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 31.05.2013 OG |
//# - chg: einige redundante Menuetitel auf define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Menüsteuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "stick_setup.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
#ifdef USE_JOYSTICK |
//############################################################################ |
//----------------------------- |
// Setup_Joysticks() |
//----------------------------- |
#define JSTICKS_OVERVIEW 1 |
#define JSTICKS_STICK1 2 |
#define JSTICKS_STICK2 3 |
#define JSTICKS_STICK3 4 |
#define JSTICKS_STICK4 5 |
#define JSTICKS_STICK5 6 |
#define JSTICKS_LIPO 7 |
static const char JSTICKS_OVERVIEW_de[] PROGMEM = "Übersicht"; |
static const char JSTICKS_OVERVIEW_en[] PROGMEM = "summary"; |
static const char JSTICKS_STICK1_de[] PROGMEM = "Stick 1"; |
#define JSTICKS_STICK1_en JSTICKS_STICK1_de |
static const char JSTICKS_STICK2_de[] PROGMEM = "Stick 2"; |
#define JSTICKS_STICK2_en JSTICKS_STICK2_de |
static const char JSTICKS_STICK3_de[] PROGMEM = "Stick 3"; |
#define JSTICKS_STICK3_en JSTICKS_STICK3_de |
static const char JSTICKS_STICK4_de[] PROGMEM = "Stick 4"; |
#define JSTICKS_STICK4_en JSTICKS_STICK4_de |
static const char JSTICKS_STICK5_de[] PROGMEM = "Stick 5"; |
#define JSTICKS_STICK5_en JSTICKS_STICK5_de |
static const char JSTICKS_LIPO_de[] PROGMEM = "PKT Lipomessung"; |
static const char JSTICKS_LIPO_en[] PROGMEM = "PKT Lipo measure."; |
//############################################################################ |
//----------------------------- |
// Setup_Stick() |
//----------------------------- |
#define MSTICK_MIN 1 |
#define MSTICK_MAX 2 |
#define MSTICK_TYPE 3 |
#define MSTICK_DIR 4 |
#define MSTICK_NEUT 5 |
static const char MSTICK_MIN_de[] PROGMEM = "Minimal Wert"; |
static const char MSTICK_MIN_en[] PROGMEM = "minimal value"; |
static const char MSTICK_MAX_de[] PROGMEM = "Maximal Wert"; |
static const char MSTICK_MAX_en[] PROGMEM = "maximal value"; |
static const char MSTICK_TYPE_de[] PROGMEM = "Type"; |
static const char MSTICK_TYPE_en[] PROGMEM = "type"; |
static const char MSTICK_DIR_de[] PROGMEM = "Richtung"; |
static const char MSTICK_DIR_en[] PROGMEM = "direction"; |
static const char MSTICK_NEUT_de[] PROGMEM = "Neutralisiered"; |
static const char MSTICK_NEUT_en[] PROGMEM = "neutralizing"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Joysticks_Uebersicht(void) |
{ |
lcd_cls (); |
lcd_printpns_at(0, 0, PSTR(" Joystick Setup "), 2); |
lcd_printpns_at(0, 1, PSTR("S Min Max Typ Dir N"), 0); |
for (uint8_t i = 0; i < 5; i++) { |
write_ndigit_number_u (0, 2+i,i+1, 1, 0,0); |
write_ndigit_number_u (2, 2+i,Config.stick_min[i], 3, 0,0); |
write_ndigit_number_u (6, 2+i,Config.stick_max[i], 3, 0,0); |
if (Config.stick_typ[i] == 0) lcd_printpns_at(10, 2+i, PSTR("Poti"), 0); else lcd_printpns_at(10, 2+i, PSTR("Tast"), 0); |
//if (Config.stick_typ[i] == 0) // MartinR: geändert |
//{ |
if (Config.stick_dir[i] == 0) lcd_printpns_at(15, 2+i, PSTR("Norm"), 0); else lcd_printpns_at(15, 2+i, PSTR("Rev"), 0); |
if (Config.stick_neutral[i] == 0) lcd_printpns_at(20, 2+i, PSTR("N"), 0); else lcd_printpns_at(20, 2+i, PSTR("Y"), 0); |
//} |
if (i == 1) if (Config.Lipomessung == true) lcd_printpns_at(3, 2+i, PSTR("PKT Lipomessung "), 0); |
} |
lcd_printp_at (18, 7, PSTR("OK"), 0); |
do{} |
while (!(get_key_press (1 << KEY_ENTER))); |
} |
//-------------------------------------------------------------- |
// Setup_Stick() |
//-------------------------------------------------------------- |
void Setup_Stick( uint8_t stick ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Stick n" |
//MenuCtrl_SetTitle_P( PSTR("Joystick") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSTICK_MIN , MENU_ITEM, NOFUNC , MSTICK_MIN_de , MSTICK_MIN_en ); |
MenuCtrl_PushML2_P( MSTICK_MAX , MENU_ITEM, NOFUNC , MSTICK_MAX_de , MSTICK_MAX_en ); |
MenuCtrl_PushML2_P( MSTICK_TYPE , MENU_ITEM, NOFUNC , MSTICK_TYPE_de , MSTICK_TYPE_en ); |
MenuCtrl_PushML2_P( MSTICK_DIR , MENU_ITEM, NOFUNC , MSTICK_DIR_de , MSTICK_DIR_en ); |
MenuCtrl_PushML2_P( MSTICK_NEUT , MENU_ITEM, NOFUNC , MSTICK_NEUT_de , MSTICK_NEUT_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSTICK_MIN ) { Config.stick_min[stick] = Edit_generic( Config.stick_min[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_MAX ) { Config.stick_max[stick] = Edit_generic( Config.stick_max[stick] ,0,300,Show_uint3,1 ,NULL,NULL); } |
if( itemid == MSTICK_TYPE ) { Config.stick_typ[stick] = Edit_generic( Config.stick_typ[stick] ,0, 1,Sticktype,1 ,NULL,NULL); } |
if( itemid == MSTICK_DIR ) { Config.stick_dir[stick] = Edit_generic( Config.stick_dir[stick] ,0, 1,NormRev,1 ,NULL,NULL); } |
if( itemid == MSTICK_NEUT ) { Config.stick_neutral[stick]= Edit_generic( Config.stick_neutral[stick] ,0, 1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_Joysticks() |
//-------------------------------------------------------------- |
void Setup_Joysticks( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Joystick" |
//MenuCtrl_SetTitle_P( PSTR("Joystick Setup") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( JSTICKS_OVERVIEW , MENU_ITEM, NOFUNC , JSTICKS_OVERVIEW_de , JSTICKS_OVERVIEW_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK1 , MENU_SUB , NOFUNC , JSTICKS_STICK1_de , JSTICKS_STICK1_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK2 , MENU_SUB , NOFUNC , JSTICKS_STICK2_de , JSTICKS_STICK2_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK3 , MENU_SUB , NOFUNC , JSTICKS_STICK3_de , JSTICKS_STICK3_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK4 , MENU_SUB , NOFUNC , JSTICKS_STICK4_de , JSTICKS_STICK4_en ); |
MenuCtrl_PushML2_P( JSTICKS_STICK5 , MENU_SUB , NOFUNC , JSTICKS_STICK5_de , JSTICKS_STICK5_en ); |
MenuCtrl_PushML2_P( JSTICKS_LIPO , MENU_ITEM, NOFUNC , JSTICKS_LIPO_de , JSTICKS_LIPO_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == JSTICKS_OVERVIEW ) { Joysticks_Uebersicht(); } |
if( itemid == JSTICKS_STICK1 ) { Setup_Stick(0); } |
if( itemid == JSTICKS_STICK2 ) { if (Config.Lipomessung == true) |
{ |
lcd_cls (); |
lcd_printp_at(0, 3, PSTR("LiPo measure!"), MNORMAL); |
_delay_ms(1000); |
} |
else Setup_Stick(1); //Stick 2 |
} |
if( itemid == JSTICKS_STICK3 ) { Setup_Stick(2); } |
if( itemid == JSTICKS_STICK4 ) { Setup_Stick(3); } |
if( itemid == JSTICKS_STICK5 ) { Setup_Stick(4); } |
if( itemid == JSTICKS_LIPO ) { Config.Lipomessung = Edit_generic( Config.Lipomessung ,0,1,YesNo,1 ,NULL,NULL); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//############################################################################ |
#endif // USE_JOYSTICK |
//############################################################################ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/stick/stick_setup.h |
---|
0,0 → 1,13 |
//############################################################################ |
//# HISTORY stick_setup.h |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl |
//############################################################################ |
#ifndef _STICK_SETUP_H_ |
#define _STICK_SETUP_ |
void Setup_Joysticks(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/stick |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
/Transportables_Koptertool/PKT/trunk/timer/timer.c |
---|
0,0 → 1,856 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.c |
//# |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 29.06.2014 OG |
//# - chg: ISR( TIMER0_COMPA_vect ) - LipoCheck() wieder hinzugefuegt |
//# - chg: LipoCheck() - leichte optische Anpassung und lcd-Ausgabe an ISR angepasst |
//# - add: #include "../lipo/lipo.h" |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# - chg: ISR(TIMER0_COMPA_vect) - "PKT aus nach" (via timer_pkt_off) |
//# - chg: ISR(TIMER0_COMPA_vect) - "LCD aus nach" umgestellt von Sekunden |
//# auf Minuten (via timer_lcd_off) |
//# - chg: Code-Formattierung |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() - loescht ALLE Tasten egal ob short, long, repeat... |
//# |
//# 13.06.2013 cebra |
//# - chg: Abfrage nach Hardwaresound bei Displaybeleuchtung entfällt |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//# |
//# 24.03.2013 OG |
//# - add: UTCTime - inkrementiert wird UTCTime.seconds auf der Basis |
//# von timer_pkt_uptime (modulo 100) aktuell initialisert von |
//# der MK-NC Time durch OSD_MK_UTCTime() |
//# - add: timer_get_tidata |
//# |
//# 21.03.2013 OG |
//# - add: timer_pkt_uptime (auch in timer.h) |
//# - add: timer_osd_refresh (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_bldata (auch in timer.h) -> verwendet in osd.c |
//# - add: timer_get_erdata (auch in timer.h) -> verwendet in osd.c |
//# |
//# 09.03.2013 OG |
//# - add: timer2 (auch in timer.h) |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../lcd/lcd.h" |
#include "../uart/uart1.h" |
#include "../bluetooth/bluetooth.h" |
#include "../setup/setup.h" |
#include"../tracking/tracking.h" |
#include "../sound/pwmsine8bit.h" |
#include "../lipo/lipo.h" |
#include "../pkt/pkt.h" |
// |
//#if defined HWVERSION1_2W || defined HWVERSION1_2 |
//#include "HAL_HW1_2.h" |
//#endif |
// |
//#if defined HWVERSION1_3W || defined HWVERSION1_3 |
//#include "HAL_HW1_3.h" |
//#endif |
//#ifdef HWVERSION3_9 |
#include "../HAL_HW3_9.h" |
//#endif |
//---------------------- |
// generische Timer |
//---------------------- |
volatile uint16_t timer; |
volatile uint16_t timer1; |
volatile uint16_t timer2; |
volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV!) |
volatile uint16_t timer_osd_refresh; // verwendet in osd.c |
volatile uint16_t timer_get_bldata; // verwendet in osd.c |
volatile uint16_t timer_get_tidata; // verwendet in osd.c |
volatile uint16_t timer_get_displaydata; // verwendet in osd.c |
volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
volatile uint16_t abo_timer; |
uint32_t timer_lcd_off = 0; // LCD aus nach... (der Zaehler muss 32Bit sein) |
uint32_t timer_pkt_off = 0; // PKT aus nach... (der Zaehler muss 32Bit sein) |
volatile uint16_t sound_timer; |
volatile uint16_t soundpause_timer; |
volatile static unsigned int tim_main; |
volatile PKTdatetime_t UTCTime; |
volatile uint32_t timer_pkt_uptime = 0; // OG: ja, 32 bit muss so sein! |
uint8_t key_state = 0; // debounced and inverted key state: |
// bit = 1: key pressed |
uint8_t key_press = 0; // key press detect |
uint8_t key_long = 0; // key long press |
uint8_t key_rpt = 0; // key repeat |
uint8_t key_lrpt = 0; // key long press and repeat |
uint8_t key_rpts = 0; // key long press and speed repeat |
uint8_t repeat_speed = 0; |
volatile uint8_t Display_on; // LCD Flag Display on/off |
volatile uint16_t WarnCount = 0; // Zähler der LIPO Warnzeit |
volatile uint16_t WarnToggle = 0; // Togglezähler zum blinken |
volatile uint16_t WarnTime = 10; // Laenge der LIPO Warnzeit 10 Sek. |
volatile uint16_t PoffTime = 15; // Laenge der Wartezeit vor abschalten 15 Sek. |
//volatile uint16_t PoffTime = 30; // Laenge der Wartezeit vor abschalten 30 Sek. |
unsigned int BeepTime = 0; |
unsigned int BeepMuster = 0xffff; |
unsigned int BeepPrio = 0; |
volatile unsigned int CountMilliseconds = 0; |
//-------------------------------------------------------------- |
// System (100Hz) |
//-------------------------------------------------------------- |
void Timer0_Init( void ) |
{ |
timer = 0; |
TCCR0A = (1 << WGM01); |
TCCR0B = (1 << CS02) | (1 << CS00); |
OCR0A = (F_CPU / (100L * 1024L)) ; |
TIMSK0 |= (1 << OCIE0A); // enable interrupt for OCR |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void Timer1_Init (void) // Timer 1-A |
//{ |
// // löschen |
// TCCR1A = 0; |
// TCCR1B = 0; |
// TIMSK1 = 0; |
// |
// // setzen |
// TCCR1A |= (1 << COM1A1) | (1 << WGM11); |
// TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << WGM13) | (1 << WGM12); |
// |
// ICR1 = (F_CPU / 64) * 20 / 1000; |
// |
// OCR1A = 470; // ca. Servomitte |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPA_vect ) |
{ |
PORTD |= (1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
ISR( TIMER2_COMPB_vect ) |
{ |
PORTD &= ~(1 << PD7); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Timer2_Init( void ) |
{ |
if( Config.HWSound == 1 ) |
{ // Sound PWM |
TCCR2A = 0x00; //stop |
ASSR = 0x00; //set async mode |
TCNT2 = 0x00; //setup |
OCR2A = 0xff; |
//Fast PWM 0xFF BOTTOM MAX |
//Set OC2A on Compare Match |
//clkT2S/8 (From prescaler) |
TCCR2A |= (1 << WGM20) | (1 << WGM21) |(1 << COM2A1) | (1 << COM2A0); |
TCCR2B |= (1 << CS20); |
} |
else |
{ // Displayhelligkeit |
DDRD |= (1 << DDD7); // PD7 output |
TCCR2A |= (1 << WGM21) | (1 << WGM20) | (1 << COM2A1); // non invers |
TCCR2B |= (1 << CS20); // Prescaler 1/1 |
TIMSK2 |= (1 << OCIE2A) | (1 << OCIE2B); |
OCR2A = 255; |
} |
} |
//-------------------------------------------------------------- |
// Sound, Timer CTC |
//-------------------------------------------------------------- |
void Timer3_Init( void ) |
{ |
TCCR3A = 0x00; // stop |
TCNT3H = 0xF8; // set count |
TCNT3L = 0x00; // set count |
OCR3AH = 0x07; // set compare |
OCR3AL = 0xFF; // set compare |
TCCR3A |= (1 << WGM31); |
TCCR3B |= (1 << CS30); |
TIMSK3 |= (1 << OCIE3A); // timer interrupt sources 2=t0 compare |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
#ifdef USE_SOUND |
ISR(TIMER3_COMPA_vect) // Sound |
{ |
//void timer0_comp_isr(void){ |
//was 8 KHz 125usec sampling rate |
//now 12 KHz 83usec sampling rate |
unsigned char oldfpart; |
signed char fullsamp; |
signed int tmp; |
TCNT3 = 0; |
if( generate ) |
{ |
DDRD |= (1<<DDD7); // Port aus Ausgang |
oldfpart=waveptfpart; // remember fractional part |
waveptfpart+=freqincfpart; // add frac part of freq inc to wave pointer |
if( waveptfpart < oldfpart ) // did it walk off the end? |
{ |
waveptipart++; // yes, bump integer part |
} |
waveptipart+=freqincipart; // add int part of freq increment to wave pointer |
fullsamp=sindat[waveptipart]; // get 8 bit sin sample from table (signed) |
tmp=fullsamp*iattenfac; // cvt 7 bit x 8 = 15 bit |
OCR2A=(tmp >> 8)+128; // cvt 15 bit signed to 8 bit unsigned |
} |
else |
{ |
DDRD &= ~(1 << DDD7); // Port auf Eingang, sperrt das Rauschen |
} |
} |
#endif |
//-------------------------------------------------------------- |
// Timer-Interrupt (100 Hz) |
//-------------------------------------------------------------- |
ISR( TIMER0_COMPA_vect ) |
{ |
static uint8_t ct0 = 0; |
static uint8_t ct1 = 0; |
static uint8_t k_time_l = 0; |
static uint8_t k_time_r = 0; |
static uint8_t k_time_lr = 0; |
static uint8_t k_time_rs = 0; |
uint8_t i; |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char beeper_ein = 0; |
i = key_state ^ ~KEY_PIN; // key changed ? |
ct0 = ~(ct0 & i); // reset or count ct0 |
ct1 = ct0 ^ (ct1 & i); // reset or count ct1 |
i &= (ct0 & ct1); // count until roll over ? |
key_state ^= i; // then toggle debounced state |
key_press |= (key_state & i); // 0->1: key press detect |
if( !cnt-- ) |
{ |
cnt = 9; |
CountMilliseconds++; |
cnt_1ms++; |
} |
//-------------------------------- |
// Key pressed -> Timer Reset |
//-------------------------------- |
if( i!=0 ) // eine Taste wurde gedrueckt! -> Timer Rest und weiteres... |
{ |
if( Display_on == 0 ) // ggf. Displaylicht einschalten |
set_D_LIGHT(); |
Display_on = 1; // Flag Display on |
timer_lcd_off = 0; // Timer Reset |
timer_pkt_off = 0; // Timer Reset |
} |
//-------------------------------- |
// LCD off Timeout |
// LCD ausschalten nach n Minuten |
//-------------------------------- |
if( (Config.DisplayTimeout > 0) && (Display_on == 1) ) |
{ |
timer_lcd_off++; |
if( (timer_lcd_off/(100*60)) == Config.DisplayTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
clr_D_LIGHT(); // Displaylicht ausschalten |
Display_on = 0; // Flag Display off |
} |
} |
//-------------------------------- |
// PKT off Timeout |
// PKT ausschalten nach n Minuten |
//-------------------------------- |
if( Config.PKTOffTimeout > 0 ) |
{ |
timer_pkt_off++; |
if( (timer_pkt_off/(100*60)) == Config.PKTOffTimeout ) // ISR laeuft mit 100Hz; umgerechnet auf Minuten |
{ |
WriteParameter(); // am Ende alle Parameter sichern |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------- |
// PKT bei Unterspannung abschalten |
//-------------------------------- |
LipoCheck(); |
//-------------------------------- |
// Beeper |
//-------------------------------- |
if( Config.HWBeeper==1 ) |
{ |
if( BeepTime ) |
{ |
if( BeepTime > 10 ) BeepTime -= 10; |
else BeepTime = 0; |
if( BeepTime & BeepMuster ) beeper_ein = 1; |
else beeper_ein = 0; |
} |
else |
{ |
beeper_ein = 0; |
BeepMuster = 0xffff; |
BeepPrio = BeepNormal; |
} |
if( beeper_ein == 1 ) set_BEEP(); |
else clr_BEEP(); |
} |
//-------------------------------- |
// Sound |
//-------------------------------- |
#ifdef USE_SOUND |
if (sound_timer > 0) // Ton spielen |
{ |
sound_timer--; |
} |
else |
{ |
//TIMSK2 &= ~_BV(TOIE2); // Interrupt sperren, verhindert Störgeräusche |
//TCCR2A = 0x00; //stop |
generate = 0; // Sound aus |
tone_handler(); |
if (soundpause_timer > 0) |
{ |
soundpause_timer --; // Ton pause |
} |
} |
#endif |
//-------------------------------- |
// Tasten |
//-------------------------------- |
if ((key_state & LONG_MASK) == 0) // check long key function |
k_time_l = REPEAT_START; // start delay |
if (--k_time_l == 0) // long countdown |
key_long |= (key_state & LONG_MASK); |
//------ |
if ((key_state & REPEAT_MASK) == 0) // check repeat function |
k_time_r = 1; // kein delay |
if (--k_time_r == 0) |
{ |
k_time_r = REPEAT_NEXT; // repeat delay |
key_rpt |= (key_state & REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_MASK) == 0) // check repeat function |
k_time_lr = REPEAT_START; // start delay |
if (--k_time_lr == 0) |
{ |
k_time_lr = REPEAT_NEXT; // repeat delay |
key_lrpt |= (key_state & LONG_REPEAT_MASK); |
} |
//------ |
if ((key_state & LONG_REPEAT_SP_MASK) == 0) // check repeatX function |
k_time_rs = REPEAT_START; // start delay |
if( --k_time_rs == 0 ) // repeat countdown |
{ |
if( repeat_speed == 1 ) |
{ |
k_time_rs = REPEAT_SPEED_1; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 2 ) |
{ |
k_time_rs = REPEAT_SPEED_2; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
else if( repeat_speed == 3 ) |
{ |
k_time_rs = REPEAT_SPEED_3; |
key_rpts |= (key_state & LONG_REPEAT_SP_MASK); |
} |
} |
//-------------------------------- |
// generische Timer |
//-------------------------------- |
if( timer > 0 ) timer --; |
if( timer1 > 0 ) timer1 --; |
if( timer2 > 0 ) timer2 --; |
if( timer3 > 0 ) timer3 --; |
//-------------------------------- |
// spezielle Timer |
//-------------------------------- |
if( timer_osd_refresh > 0 ) timer_osd_refresh--; // Timer fuer OSD-Screenrefresh (verwendet von osd.c) |
if( timer_get_bldata > 0 ) timer_get_bldata--; // Timer um BL-Daten zu holen (verwendet von osd.c) |
if( timer_get_tidata > 0 ) timer_get_tidata--; // Timer um Datum/Zeit vom MK zu holen (verwendet von osd.c) |
if( timer_get_displaydata > 0 ) timer_get_displaydata--; // Timer um Display vom MK zu holen (verwendet von osd.c) |
if( timer_mk_timeout > 0 ) timer_mk_timeout--; // verwendet u.a. von osd.c |
if( abo_timer > 0 ) abo_timer --; // Timer zum anfordern neuer Abo-Datenpakete wie OSD oder BL-Daten (verwendet u.a. von osd.c) |
if( timer_pktupdatecheck > 0 ) timer_pktupdatecheck--; // Timer fuer pkt.c (PKT-Update-Check) - * FUER NICHTS ANDERES! * |
if( timer_nmea_timeout > 0 ) timer_nmea_timeout--; // verwendet u.a. von osd.c |
//-------------------------------- |
//-------------------------------- |
if( Gauge_active ) // Gauge_active -> pkt.c |
{ |
if( timer_gauge > 0 ) timer_gauge--; |
if( timer_gauge==0 ) PKT_Gauge_Next(); |
} |
//-------------------------------- |
// PKT Uptime Timer |
//-------------------------------- |
timer_pkt_uptime++; |
if( timer_pkt_uptime % 100 == 0 ) // theoretisch muesste noch die Tagesgrenze (0 Uhr) implementiert werden... |
UTCTime.seconds++; |
} // end: ISR(TIMER0_COMPA_vect) |
//-------------------------------------------------------------- |
// Lowbatpin des Spannungswandlers pruefen |
// LBO des LT1308 wechselt zum Ende der Batterielaufzeit haeufig seinen Zustand in der Uebergangsphase zum LowBat |
// Die Akkuspannung schwankt auch abhaengig vom momentanen Stromverbrauch |
//-------------------------------------------------------------- |
void LipoCheck( void ) |
{ |
uint8_t lcd_xpos_save; |
uint8_t lcd_ypos_save; |
if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
{ |
set_beep( 1000, 0x0020, BeepNormal); |
lcd_xpos_save = lcd_xpos; // innerhalb einer ISR -> LCD Screenpos muss gesichert werden! |
lcd_ypos_save = lcd_ypos; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 0, MINVERS ,0 ); // Zeile 0 komplett invers |
lcd_printp_at( 0, 0, PSTR(" ** PKT LiPo! ** "), MINVERS); // und Warn-Text ausgeben |
lcd_xpos = lcd_xpos_save; // lcd Screenpos wieder herstellen |
lcd_ypos = lcd_ypos_save; |
} |
if( WarnToggle == WarnTime * 100 ) |
WarnToggle = 0; // erstmal bis hier warnen |
if( WarnToggle > 0 ) |
WarnToggle++; // weiter hochzaehlen |
if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
{ |
WarnCount = 0; |
//if (WarnCount > 0) |
// WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
} |
if( !(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
{ |
WarnCount++; // solange LBO low ist Zähler hochzählen |
if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
WarnToggle = 1; // Warnhinweis starten |
} |
if( WarnCount == (PoffTime * 100) ) |
{ |
set_beep( 50, 0xffff, BeepNormal ); // ein Mini-Beep zum Abschied (laenger geht nicht, wahrscheinlich wegen der ISR) |
WriteParameter(); // am Ende alle Parameter sichern |
clr_V_On(); // Spannung abschalten |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
//void LipoCheck_OLD( void ) |
//{ |
// if( WarnToggle == 1 ) // Beim ersten Auftreten Warnung ausgeben, Rythmus 5/10 Sekunden |
// { |
// set_beep ( 1000, 0x0020, BeepNormal); |
// lcd_printp_at (0, 0, PSTR(" LIPO !!Warnung!! "), 2); |
// } |
// |
// if( WarnToggle == WarnTime * 100 ) |
// WarnToggle = 0; // erstmal bis hier warnen |
// |
// if( WarnToggle > 0 ) |
// WarnToggle++; // weiter hochzaehlen |
// |
// if( PINC & (1 << LowBat) ) // Kurzzeitige Unterspannung bearbeiten und Warnung ausgeben |
// { |
// WarnCount = 0; |
// //if (WarnCount > 0) |
// // WarnCount--; // Bei LIPO OK erstmal runterzaehlen, LT1308 ueberlegt sich noch genauer ob nun ok oder nicht |
// } |
// |
// if (!(PINC & (1 << LowBat)) ) // LT1308 hat Unterspannung erkannt |
// { |
// WarnCount++; // solange LBO low ist Zähler hochzählen |
// if( (WarnCount == 10) && (WarnToggle == 0) ) // mit "10" etwas unempfindlicher gegen kurze Impulse machen |
// WarnToggle = 1; // Warnhinweis starten |
// } |
// |
// if( WarnCount == (PoffTime * 100) ) |
// { |
// clr_V_On(); // Spannung abschalten |
// } |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
unsigned int SetDelay( unsigned int t ) |
{ |
return( CountMilliseconds + t + 1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
char CheckDelay( unsigned int t ) |
{ |
return( ((t - CountMilliseconds) & 0x8000) >> 9 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Delay_ms( unsigned int w ) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while( !CheckDelay(akt) ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_press( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_press; // read key(s) |
key_press ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_short( uint8_t key_mask ) |
{ |
uint8_t ret; |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
ret = get_key_press (~key_state & key_mask); |
SREG = sreg; // restore status register |
return ret; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_long; // read key(s) |
key_long ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_press (get_key_rpt (key_mask)); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpt; // read key(s) |
key_rpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt( uint8_t key_mask ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_lrpt; // read key(s) |
key_lrpt ^= key_mask; // clear key(s) |
SREG = sreg; // restore status register |
return get_key_rpt (~key_press^key_mask); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t get_key_long_rpt_sp( uint8_t key_mask, uint8_t key_speed ) |
{ |
uint8_t sreg = SREG; |
cli(); // disable all interrupts |
key_mask &= key_rpts; // read key(s) |
key_rpts ^= key_mask; // clear key(s) |
repeat_speed = key_speed; |
SREG = sreg; // restore status register |
return key_mask; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void clear_key_all( void ) |
{ |
key_rpt = 0; // clear key(s) |
key_rpts = 0; // clear key(s) |
key_lrpt = 0; // clear key(s) |
key_long = 0; // clear key(s) |
key_press = 0; // clear key(s) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void set_beep( uint16_t Time, uint16_t Muster, uint8_t Prio) |
{ |
if( Config.HWBeeper == 1 ) |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // BEEPER: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) // BEEPER: hohe Prio setzen |
{ |
BeepPrio = BeepSevere; |
BeepTime = Time; |
BeepMuster = Muster; |
} |
if( Prio == BeepOff ) |
{ |
BeepPrio = BeepNormal; // BEEPER: Beep hohe Prio aus |
BeepTime = 0; |
BeepMuster = 0; |
} |
} // end: if( Config.HWBeeper==1 ) |
#ifdef USE_SOUND |
else |
{ |
if( (Prio == BeepNormal) && (BeepPrio == BeepNormal) ) // SOUND: nur setzen wenn keine hohe Prio schon aktiv ist |
{ |
playTone(900,Time/10,0); |
} |
if( (Prio == BeepSevere) && (!BeepPrio == BeepSevere) ) |
{ |
playTone(1200,Time/10,0); |
playTone(1100,Time/10,0); |
} |
if( Prio == BeepOff ) |
{ |
playTone(0,0,0); |
} |
} |
#endif // end: #ifdef USE_SOUND |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/timer/timer.h |
---|
0,0 → 1,170 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* based on the key handling by Peter Dannegger * |
* see www.mikrocontroller.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY timer.h |
//# |
//# 08.08.2015 CB |
//# - add: timer_nmea_timeout |
//# |
//# 24.06.2014 OG |
//# - add: timer_gauge |
//# |
//# 13.06.2014 OG |
//# - del: IdleTimer entfernt da nicht verwendet |
//# |
//# 31.05.2014 OG |
//# - chg: 'timer_pktctrl' umbenannt zu 'timer_pktupdatecheck' (exklusiv fuer pkt.c) |
//# |
//# 25.04.2014 OG |
//# - add: timer_get_displaydata |
//# |
//# 24.04.2014 OG |
//# - add: timer1 (generischer Timer) |
//# - add: timer3 (generischer Timer) |
//# - add: timer_mk_timeout |
//# - del: timer_get_erdata |
//# |
//# 29.03.2014 OG |
//# - add: clear_key_all() |
//# |
//# 07.07.2013 OG |
//# - chg: ABO_TIMEOUT verschoben nach main.h |
//# |
//# 19.05.2013 OG |
//# - add: timer_pktctrl - exclusive Verwendung von pkt.c |
//############################################################################ |
#ifndef _TIMER_H |
#define _TIMER_H |
#include "../cpu.h" |
#include "../main.h" |
typedef struct |
{ |
uint32_t seconds; // seconds since nnew day |
uint8_t day; |
uint8_t month; |
uint16_t year; |
} PKTdatetime_t; |
#define KEY_ALL ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define LONG_REPEAT_SP_MASK ((1 << KEY_PLUS) | (1 << KEY_MINUS) | (1 << KEY_ENTER) | (1 << KEY_ESC)) |
#define REPEAT_START 70 // after 700ms |
#define REPEAT_NEXT 15 // every 150ms |
#define REPEAT_SPEED_1 20 // every 200ms |
#define REPEAT_SPEED_2 8 // every 80ms |
#define REPEAT_SPEED_3 1 // every 10ms |
#define BeepNormal 0 // Normal Beep |
#define BeepSevere 1 // schwerer Fehler, Beep nicht unterbrechbar |
#define BeepOff 2 // Beep aus |
extern volatile uint8_t Display_on; |
extern volatile PKTdatetime_t UTCTime; |
//---------------------- |
// generische Timer |
//---------------------- |
extern volatile uint16_t timer; |
extern volatile uint16_t timer1; |
extern volatile uint16_t timer2; |
extern volatile uint16_t timer3; |
//---------------------- |
// spezielle Timer |
//---------------------- |
extern volatile uint32_t timer_pkt_uptime; |
extern volatile uint16_t timer_pktupdatecheck; // verwendet von pkt.c (!EXKLUSIV! FUER NICHTS ANDERES!) |
extern volatile uint16_t timer_osd_refresh; |
extern volatile uint16_t timer_get_bldata; |
extern volatile uint16_t timer_get_tidata; |
extern volatile uint16_t timer_get_displaydata; |
extern volatile uint16_t timer_mk_timeout; // verwendet u.a. in osd.c |
extern volatile uint16_t timer_gauge; // verwendet in pkt.c fuer den rotierenden Wartekreisel |
extern volatile uint16_t abo_timer; |
extern volatile uint16_t timer_nmea_timeout; // verwendet in nmea.c |
extern volatile uint16_t sound_timer; |
extern volatile uint16_t soundpause_timer; |
//extern volatile uint16_t WarnCount; |
//extern volatile unsigned int BeepTime; |
extern unsigned int BeepTime; |
extern unsigned int BeepMuster; |
/** |
* Get the value of the system timer counter. |
* |
* @return The value of the system timer counter. |
*/ |
uint16_t timer_get_system_count(void); |
void Timer0_Init (void); // Systeminterrupt |
void Timer1_Init (void); // Servotester |
void Timer2_Init (void); // Displayhelligkeit |
void Timer3_Init (void); // Überwachung |
uint8_t get_key_press (uint8_t key_mask); // sofort beim drücken |
uint8_t get_key_short (uint8_t key_mask); // erst beim loslassen |
uint8_t get_key_long (uint8_t key_mask); // verzögert |
uint8_t get_key_rpt (uint8_t key_mask); // mit verzögerung |
uint8_t get_key_long_rpt (uint8_t key_mask); // |
uint8_t get_key_long_rpt_sp (uint8_t key_mask, uint8_t key_speed); // mit verzögerung und 3 versch. geschw. |
void clear_key_all( void ); |
void set_beep ( uint16_t Time, uint16_t Muster, uint8_t Prio); |
extern volatile unsigned int CountMilliseconds; |
void Delay_ms(unsigned int); |
unsigned int SetDelay (unsigned int t); |
char CheckDelay (unsigned int t); |
void LipoCheck (void); // Lowbatpin des Spannungswandlers prüfen |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/ng_servo.c |
---|
0,0 → 1,236 |
/********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library.*/ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 2 */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/********************************************************************/ |
//############################################################################ |
//# HISTORY ng_servo.c |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 24.04.2013 OG |
//# - chg: umbenannt servo_test(void) nach _servo_test(void) |
//# das verursachte in meinem PKT-Projekt Errors weil die Funktion |
//# servo_test() ebenfalls ein servo.c vorhanden ist |
//############################################################################ |
#include <avr/interrupt.h> |
#include <avr/io.h> |
#include "../tracking/ng_servo.h" |
#include "../tracking/tracking.h" |
#include "../HAL_HW3_9.h" |
#include "../eeprom/eeprom.h" |
//#include "config.h" |
#ifdef USE_TRACKING |
// Global variables |
uint16_t ServoPosTics; |
uint16_t ServoPeriodTics; |
// servo channel registers |
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS]; |
const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER}, |
{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}}; |
// Servo limits (depend on hardware) |
const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX}, |
{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX}, |
{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}}, |
{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX}, |
{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX}, |
{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}}; |
// Servopositionen normiert für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
const steps_pw_t steps_pw[2] = {{(uint64_t)950*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*F_CPU*1e-6/SERVO_PRESCALER + 0.5,(uint64_t)1500*F_CPU*1e-6/SERVO_PRESCALER + 0.5}, |
{(uint64_t)950*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5}}; |
// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 }; |
// functions |
void _servo_test(void) |
{ |
//Dummy |
} |
//! initializes software PWM system 16-bit Timer |
// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt |
// Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!! |
void servoInit(uint8_t servo_period) |
{ uint16_t OCValue; // set initial interrupt time |
// disble Timer/Counter1, Output Compare A Match Interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
// set the prescaler for timer1 |
if (Config.sIdxSteps == STEPS_255) { |
TCCR1B &= ~((1<<CS11) | (1<<CS10)); |
TCCR1B |= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel |
} |
else { |
TCCR1B &= ~(1<<CS12); |
TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel |
} |
// attach the software PWM service routine to timer1 output compare A |
// timerAttach(TIMER1OUTCOMPAREA_INT, servoService); |
// enable channels |
for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) { |
// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5 |
ServoChannels[channel].pin = (1 << (SERVO2 + channel)); |
} |
ServoPosTics = 0; // set PosTics |
// set PeriodTics |
ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3; |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH << 8); // read high byte of OCR1A |
OCValue += ServoPeriodTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
TIMSK1 |= (1<<OCIE1A); // enable the timer1 output compare A interrupt |
coldstart = 1; |
} |
void servoSetDefaultPos(void) |
{ |
servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN |
servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT |
} |
uint16_t pw_us(uint16_t Steps) |
{ |
return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in µs |
} |
uint16_t ServoSteps(void) |
{ |
return(ServoConst[Config.sIdxSteps].steps); |
} |
void servoSet_rev(uint8_t channel, uint8_t val) |
{ |
ServoChannels[channel].rev = val & 0x01; |
} |
void servoSet_min(uint8_t channel, uint16_t min) |
{ |
ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min; |
} |
void servoSet_max(uint8_t channel, uint16_t max) |
{ |
ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max; |
} |
void servoSet_mid(uint8_t channel, uint16_t mid) |
{ |
ServoChannels[channel].mid = mid; |
// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen |
ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \ |
(16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min; |
} |
//! turns off software PWM system |
void servoOff(void) |
{ |
// disable the timer1 output compare A interrupt |
TIMSK1 &= ~(1<<OCIE1A); |
} |
//! set servo position on channel (raw unscaled format) |
void servoSetPositionRaw(uint8_t channel, uint16_t position) |
{ |
// bind to limits |
if (position < ServoChannels[channel].min) position = ServoChannels[channel].min; |
if (position > ServoChannels[channel].max) position = ServoChannels[channel].max; |
// set position |
ServoChannels[channel].duty = position; |
} |
//! set servo position on channel |
// input should be between 0 and ServoSteps() (entspricht Maximalausschlag) |
void servoSetPosition(uint8_t channel, uint16_t position) |
{ uint16_t pos_scaled; |
// calculate scaled position |
if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position; |
if (position < ServoConst[Config.sIdxSteps].steps/2) |
//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2) |
pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \ |
ServoConst[Config.sIdxSteps].steps; |
else |
//bei Position >= Servomittelstellung |
pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \ |
* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps; |
// set position |
servoSetPositionRaw(channel, pos_scaled); |
} |
// Umrechnung Winkel in Servoschritte |
void servoSetAngle(uint8_t servo_nr, int16_t angle) |
{ |
servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180)); |
} |
//Interruptroutine |
ISR(TIMER1_COMPA_vect) |
{ static uint8_t ServoChannel; |
uint16_t nextTics; |
uint16_t OCValue; // schedule next interrupt |
if(ServoChannel < SERVO_NUM_CHANNELS) { |
PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel |
} |
ServoChannel++; // next channel |
if(ServoChannel != SERVO_NUM_CHANNELS) { |
// loop to channel 0 if needed |
if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
// turn on new channel |
PORTD |= ServoChannels[ServoChannel].pin; |
// schedule turn off time |
nextTics = ServoChannels[ServoChannel].duty; |
} |
else { |
// ***we could save time by precalculating this |
// schedule end-of-period |
nextTics = ServoPeriodTics-ServoPosTics; |
} |
// read in current value of output compare register OCR1A |
OCValue = OCR1AL; // read low byte of OCR1A |
OCValue += (OCR1AH <<8); // read high byte of OCR1A |
OCValue += nextTics; // increment OCR1A value by nextTics |
// set future output compare time to this new value |
OCR1AH = OCValue >> 8; // write high byte |
OCR1AL = OCValue & 0x00FF; // write low byte |
ServoPosTics += nextTics; // set our new tic position |
if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/ng_servo.h |
---|
0,0 → 1,190 |
/*********************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/* using */ |
/*! \file servo.c \brief Interrupt-driven RC Servo function library. */ |
/* */ |
/*File Name : 'servo.c' */ |
/*Title : Interrupt-driven RC Servo function library */ |
/*Author : Pascal Stang - Copyright (C) 2002 */ |
/*Created : 7/31/2002 */ |
/*Revised : 8/02/2002 */ |
/*Version : 1.0 */ |
/*Target MCU : Atmel AVR Series */ |
/*Editor Tabs : 4 */ |
/* */ |
/*ingroup driver_sw */ |
/*defgroup servo Interrupt-driven RC Servo Function Library (servo.c)*/ |
/*code #include "servo.h" \endcode */ |
/*par Overview */ |
/* */ |
/*This code is distributed under the GNU Public License */ |
/*which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/*********************************************************************/ |
#ifndef SERVO_H |
#define SERVO_H |
/* Servo */ |
#define SERVO_PAN 0 |
#define SERVO_TILT 1 |
#define SERVO_NUM_CHANNELS 2 // Anzahl der angeschlossen Servos max. 2!!! |
/* Servokalibrierungen derzeit zu SERVO_STEPS = 255 skaliert */ |
//prescaler 256 |
#define SERVO_I0_RIGHT 45 // default Wert, ca. 0,9ms |
#define SERVO_I0_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I0_RIGHT_MAX 100 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I0_LEFT 45 // default Wert, ca. 2,1ms |
#define SERVO_I0_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I0_LEFT_MAX 100 // SERVO_MAX - SERVO_LEFT |
#define SERVO_I0_MIDDLE SERVO_STEPS/2 |
#define SERVO_I0_MIDDLE_MIN SERVO_STEPS/2 - 25 |
#define SERVO_I0_MIDDLE_MAX SERVO_STEPS/2 + 25 |
//prescaler 64 |
#define SERVO_I1_RIGHT 180 // default Wert, ca. 0,9ms |
#define SERVO_I1_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position |
#define SERVO_I1_RIGHT_MAX 400 // SERVO_MIN + SERVO_RIGHT |
#define SERVO_I1_LEFT 180 // default Wert, ca. 2,1ms |
#define SERVO_I1_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position |
#define SERVO_I1_LEFT_MAX 400 // SERVO_MAX - SERVO_LEFT |
//#define SERVO_I1_MIDDLE ((SERVO_STEPS + 1) * 4 - 1)/2 |
#define SERVO_I1_MIDDLE_MIN ((SERVO_STEPS + 1) * 4 - 1)/2 - 100 |
#define SERVO_I1_MIDDLE_MAX ((SERVO_STEPS + 1) * 4 - 1)/2 + 100 |
#define SERVO_REV 0 // kein Reverse |
/* Test Servo */ |
#define SERVO_PERIODE 20 // default Angabe in ms |
#define SERVO_PERIODE_MIN 10 // 10ms |
#define SERVO_PERIODE_MAX 30 // 30ms |
#define SINGLE_STEP 0 // Einzelschritt aus |
#define SINGLE_STEP_MIN 0 |
#define SINGLE_STEP_MAX 20 // bei prescaler 256, sonst * 4 (von links nach rechts in 9 Abschnitte) |
// zwischen den Schritten muss Pause > der Servoperiode sein, sonst keine Aktualisierung |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// The numbers below good for parallax servos at an F_CPU of 20.000MHz. |
// Da einige Servo's auch eien Winkel von 180 grd zulassen, Wertebereich |
// entgegen den sonst üblichen. Einschränkung mit default Kalibrierung |
// auf 0,9ms (45) bis 2,1ms(45) gesetzt. Je nach Servo, entspricht einen |
// Winkel von etwa 180grd |
// Periode default 20ms |
#define SERVO_MAX 211 // 2,7 ms bei prescaler 256, bei prescaler 64 SERVO_MAX * 4 |
#define SERVO_MIN 26 // 0,33ms bei prescaler 256, bei prescaler 64 SERVO_MIN * 4 |
#define SERVO_STEPS 255 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define SERVO_PRESCALER 256 // bei prescaler 256, bei prescaler 64 SERVO_PRESCALER / 4 |
#define STEPS_255 0 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
#define STEPS_1023 1 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle |
typedef struct //Servo-Konstante je nach Prescaler |
{ |
uint16_t max; |
uint16_t min; |
uint16_t steps; |
uint16_t prescaler; |
}ServoConst_t; |
typedef struct //struct_ServoChannel |
{ |
uint8_t pin; // hardware I/O port and pin for this channel |
uint16_t duty; // PWM duty setting which corresponds to servo position |
uint8_t rev; // Parameter, wie on/off; reverse; range |
uint16_t min; // SERVO_MIN + Parameter min |
uint16_t max; // SERVO_MAX - Parameter max |
uint16_t mid_scaled; // skalierte Servomitte |
int16_t mid; // Servomitte = SERVO_STEPS/2 +/- x Schritte; bei Pescaler 256 wird nur uint8_t benötigt aber bei 64 |
}ServoChannelType; |
//uint8_t sIdxSteps; // 0 für 255 und 1 für 1023 Schritte; Prescaler 256 oder 64 |
// Define servo limits (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
}servo_limit_t; |
extern const servo_limit_t servo_limit[2][3]; |
// Define servo positions (depend on hardware) |
typedef struct { |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
}steps_pw_t; |
typedef struct { |
uint8_t rev; |
uint16_t min; |
uint16_t max; |
uint16_t mid; |
} servo_t; |
// Servopositionen für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
extern const steps_pw_t steps_pw[2]; |
#define RIGHT 0 // Servopostionen, welche vom Einbau abhängig sind |
#define LEFT 1 |
#define MIDDLE 2 |
#define POSIDX_MAX 4 |
extern const uint8_t PosIdx[POSIDX_MAX]; // anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
// functions |
void servoSetDefaultPos(void); |
// initializes servo system |
// You must run this to begin servo control |
void servoInit(uint8_t servo_period); |
// turns off servo system |
// This stops controlling the servos and |
// returns control of the SERVOPORT to your code |
void servoOff(void); |
// set servo position on a given channel |
// servoSetPosition() commands the servo on <channel> to the position you |
// desire. The position input must lie between 0 and POSITION_MAX and |
// will be automatically scaled to raw positions between SERVO_MIN and |
// SERVO_MAX |
void servoSetPosition(uint8_t channel, uint16_t position); |
// set raw servo position on a given channel |
// Works like non-raw commands but position is not scaled. Position must |
// be between SERVO_MIN and SERVO_MAX |
void servoSetPositionRaw(uint8_t channel, uint16_t position); |
// set servo to a specific angle |
void servoSetAngle(uint8_t servo_nr, int16_t angle); |
// vor servoInit(), oder vor sei() ServoWerte mit servoSet...() initialisieren, einschließlich servoSetPosition(...)! |
void servoSet_rev(uint8_t channel, uint8_t val); |
void servoSet_min(uint8_t channel, uint16_t min); |
void servoSet_max(uint8_t channel, uint16_t max); |
void servoSet_mid(uint8_t channel, uint16_t mid); |
uint16_t pw_us(uint16_t Steps); // gibt Zeit in µs bei x Servoschritte |
uint16_t ServoSteps(void); // gibt "Konstante" derzeitiger Servoschritte zürück |
#endif /* SERVO_H */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/servo_setup.c |
---|
0,0 → 1,1577 |
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
//############################################################################ |
//# HISTORY servo_setup.c |
//# |
//# 27.06.2014 OG |
//# - del: #include "tools.h" |
//# - del: #include "mymath.h" |
//# |
//# 30.05.2014 OG |
//# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben; |
//# einige Optimierungen an Aufruffunktionen (unnoetige Menue- |
//# Interfacefunktionen entfernt) |
//# |
//# 13.05.2014 OG |
//# - chg: Menu_Test_Start() - Variable 'range' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt |
//# wegen Warning: variable set but not used |
//# |
//# 11.05.2014 OG |
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
//# |
//# 30.03.2014 OG |
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING |
//# |
//# 30.05.2013 cebra |
//# - chg: doppelte Texte auf #define umgestellt |
//# |
//# 24.05.2013 OG |
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
//# |
//# 22.05.2013 OG |
//# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(), |
//# Setup_ServoTestCont(), Setup_ServoAdjust() |
//# - del: include utils/menuold.h |
//# |
//# 17.05.2013 OG |
//# - chg: umgestellt auf utils/menuold.h |
//# |
//# 16.04.2013 Cebra |
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
//# |
//# 04.04.2013 Cebra |
//# - chg: Texttuning |
//# |
//# 03.04.2013 Cebra |
//# - chg: Holländische Menütexte |
//# |
//# 27.03.2013 Cebra |
//# - chg: Fehler bei der Men�steuerung behoben |
//############################################################################ |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "servo_setup.h" |
#include "tracking.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "../menu.h" |
#include "../messages.h" |
#include "../mk-data-structs.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../utils/menuctrl.h" |
#include "../pkt/pkt.h" |
#ifdef USE_TRACKING |
//#include "ng_usart.h" |
//#include "ng_config.h" |
//#include "servo.h" |
//#include "tools.h" |
//#include "mk.h" |
//#include "keys.h" |
//#include "mymath.h" |
// |
//GPS_Pos_t last5pos[7]; |
//uint8_t error1 = 0; |
//NaviData_t *naviData; |
//HomePos_t MK_pos; // Home position of station |
//GPS_Pos_t currentPos; // Current position of flying object |
//int8_t satsInUse; // Number of satelites currently in use |
//uint8_t tracking = TRACKING_MIN; |
//uint8_t track_hyst = TRACKING_HYSTERESE; |
//uint8_t track_tx = 0; |
//geo_t geo; |
//int16_t anglePan, angleTilt; // Servo Winkel |
//uint8_t coldstart = 1; |
uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden |
//uint8_t FCStatusFlags; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTracking() |
//----------------------------- |
#define MSERVO_STEPS 1 |
#define MSERVO_SERVO1 2 |
#define MSERVO_SERVO2 3 |
#define MSERVO_SERVOTEST 4 |
static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte"; |
static const char MSERVO_STEPS_en[] PROGMEM = "servo steps"; |
static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1"; |
#define MSERVO_SERVO1_en MSERVO_SERVO1_de |
static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2"; |
#define MSERVO_SERVO2_en MSERVO_SERVO2_de |
static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest"; |
#define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de |
//############################################################################ |
//----------------------------- |
// Setup_ServoTest() |
//----------------------------- |
#define MSERVOT_PULS 1 |
#define MSERVOT_CONT 2 |
#define MSERVOT_SERVO 3 |
#define MSERVOT_FRAME 4 |
static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulslänge"; |
static const char MSERVOT_PULS_en[] PROGMEM = "test puls width"; |
static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd."; |
static const char MSERVOT_CONT_en[] PROGMEM = "test cont."; |
static const char MSERVOT_SERVO_de[] PROGMEM = "Servo"; |
#define MSERVOT_SERVO_en MSERVOT_SERVO_de |
static const char MSERVOT_FRAME_de[] PROGMEM = "Periode"; |
static const char MSERVOT_FRAME_en[] PROGMEM = "frame"; |
//############################################################################ |
//----------------------------- |
// Setup_ServoTestCont() |
//----------------------------- |
#define MSERVOTC_START 1 |
#define MSERVOTC_SINGLE 2 |
#define MSERVOTC_COUNT 3 |
#define MSERVOTC_PAUSEEND 4 |
#define MSERVOTC_PAUSEINC 5 |
static const char MSERVOTC_START_de[] PROGMEM = "Start Test"; |
static const char MSERVOTC_START_en[] PROGMEM = "start test"; |
static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt"; |
static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step"; |
static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test"; |
static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test"; |
static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos."; |
static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos"; |
static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc."; |
static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc."; |
//############################################################################ |
//----------------------------- |
// Setup_ServoAdjust() |
//----------------------------- |
#define MSERVOADJ_REV 1 |
#define MSERVOADJ_LEFT 2 |
#define MSERVOADJ_RIGHT 3 |
#define MSERVOADJ_MID 4 |
static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse"; |
static const char MSERVOADJ_REV_en[] PROGMEM = "reverse"; |
static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links"; |
static const char MSERVOADJ_LEFT_en[] PROGMEM = "left"; |
static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts"; |
static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right"; |
static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte"; |
static const char MSERVOADJ_MID_en[] PROGMEM = "middle"; |
//############################################################################ |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void draw_input_screen( void ) |
{ |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); |
} |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */ |
///* :1 springt nach max_val auf min_val und umgedreht */ |
///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */ |
///* uint16_t Change_Value_plmi(...) :R�ckgabe ge�nderter Wert */ |
///* */ |
///************************************************************************************/ |
void Servo_tmp_Original(uint8_t track) |
{ |
servoSetDefaultPos(); |
// tracking = track; // urspr�nglicher Wert Tracking aus, RSSI oder GPS |
// NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet |
// Jump_Menu(pmenu); |
} |
uint8_t Servo_tmp_on(uint8_t servo_period) |
{ |
// uint8_t tmp_tracking = tracking; |
// tracking = 0; // Servopositionierung durch tracking abschalten |
// if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten |
servoInit(servo_period); |
// lcdGotoXY(0, 0); // lcd Cursor vorpositionieren |
// return(tmp_tracking); |
return (0); |
} |
void Displ_Off_On(uint16_t val) |
{ |
if (val == 0) lcd_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_at(17, 2, strGet(ON), 0); |
} |
uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct) |
{ |
// >> Menueauswahl nach oben |
if (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) |
{ |
if (val < max_val) { |
edit = 1; |
val++; |
} |
else |
{ |
val = min_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
// >> Menueauswahl nach unten |
if (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) |
{ |
if (val > min_val) { |
val--; |
edit = 1; |
} |
else |
{ |
val = max_val; |
} |
Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
} |
return(val); |
} |
// |
///************************************************************************************/ |
///* */ |
///* �ndern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */ |
///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
///* */ |
///* Parameter: */ |
///* uint16_t *val :zu �ndernter Wert */ |
///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */ |
///* Bit 6 zyklische Werte�nderung aus/ein */ |
///* Bit 4-5 z.Z. ohne Funktion */ |
///* Bit 0-3 Wert xPos auf LCD */ |
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
///* uint8_t Change_Value(...) :R�ckgabe ge�ndert ergibt TRUE */ |
///* */ |
///************************************************************************************/ |
//// Bei Bedarf k�nnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) �bergeben werden |
uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct) |
{ |
uint16_t tmp_val; |
tmp_val = *val; |
Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Men�punkt |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) |
*val = Change_Value_plmi(*val, min_val, max_val, 16,2, Displ_Fnct); |
if (*val == tmp_val) { |
edit = 0; |
// lcd_printp_at (0, 5, PSTR("Edit=0"), 0); |
// _delay_ms(500); |
//// return (*val); |
} |
// |
else |
{ |
edit = 1; |
// lcd_printp_at (0, 5, PSTR("Edit=1"), 0); |
// _delay_ms(500); |
} |
return (tmp_val != *val); |
} |
uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid) |
{ uint16_t range; |
if (PosProzent < 0) { |
range = mid - min; |
// if (chrxs == CHRRS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
// chrxs = CHRLS; |
//// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// } |
} |
else { |
range = max - mid; |
// if (chrxs == CHRLS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
//// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRRS; |
// } |
} |
return(range); |
} |
/************************************************************************************/ |
/* zeigt einen max. 3-stelligen Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Format_Int( uint16_t val ) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Displ_PulseWidth( uint16_t val ) |
{ |
int16_t PosProzent, range; |
servoSetPositionRaw( servo_nr, val); |
PosProzent = val - steps_pw[Config.sIdxSteps].mid; |
range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid); |
PosProzent = (int32_t)1000 * PosProzent / range; |
lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent ); |
} |
/************************************************************************************/ |
/* zeigt Pausenl�nge der Links-, Mittel- und Rechtsposition auf Display an */ |
/* Parameter: */ |
/* uint16_t val : Zeit in 1ms * 100 */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause(uint16_t val) |
{ |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val ); |
//lcdPuts("s "); |
} |
/************************************************************************************/ |
/* zeigt aus oder Integerwert auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Off_Format_Int( uint16_t val ) |
{ |
if( val == 0 ) |
{ |
lcd_printp_at( 17, 2, strGet(OFF), MNORMAL); |
} |
else |
{ |
Displ_Format_Int( val ); |
} |
} |
/************************************************************************************/ |
/* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */ |
/* Parameter: */ |
/* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
/* */ |
/************************************************************************************/ |
void Displ_Pause_Step( uint16_t val ) |
{ |
Displ_Off_Format_Int( val ); |
} |
/************************************************************************************/ |
/* zeigt zu testende Servonummer zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_ServoNr( uint16_t val ) |
{ |
Displ_Format_Int( val+1 ); |
} |
/**************************/ |
/* */ |
/* Servos-Tests */ |
/* */ |
/**************************/ |
//void Menu_Servo_Test(void) |
//{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME}; |
// |
// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global |
// servo_nr = eeprom_read_byte(&ep_servo_nr); |
// Jump_Menu(pmenu); |
//} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Frame( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.servo_frame; |
if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) ) |
{ |
Config.servo_frame = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_ServoNr( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = servo_nr; |
if( Change_Value( &tmp_val, 0, 1, Displ_ServoNr) ) |
{ // pmenu global; es gibt nur 0=Servo1, 1=Servo2 |
servo_nr = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
// Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: f�r Modelleinstellungen ohne Empf�nger |
//-------------------------------------------------------------- |
void Menu_Test_PulseWidth( void ) |
{ |
uint8_t tmp_tracking; |
uint16_t tmp_val; |
draw_input_screen(); |
// 30.05.2014 OG: macht dieser Code Sinn? |
// Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen? |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
tmp_val = steps_pw[Config.sIdxSteps].mid; |
Change_Value( &tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, Displ_PulseWidth); // pmenu global |
cli(); |
servoInit( SERVO_PERIODE ); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_SingleStep(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.single_step; |
if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) ) |
{ // pmenu global |
Config.single_step = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Repeat( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.repeat; |
if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) ) |
{ |
Config.repeat = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause; |
if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) ) |
{ |
Config.pause = tmp_val; |
} |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Test_Pause_Step( void ) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.pause_step; |
if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) ) |
{ |
Config.pause_step = tmp_val; |
} |
} |
int8_t calc_dir(uint8_t idx, int16_t *Position) |
{ uint8_t nextIdx; |
int8_t nextDir = 1; |
nextIdx = idx; |
if ((idx + 1) < POSIDX_MAX) |
nextIdx++; |
else |
nextIdx = 0; |
if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1; |
return(nextDir); |
} |
void Displ_LoopCounter( uint8_t val ) |
{ |
lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val ); |
//lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0); |
} |
// Test �ber Scalierung der Servos mit Anschlagkalibrierung |
void Menu_Test_Start(void) |
{ |
uint8_t tmp_tracking, idx, rep; |
int8_t dir; |
int16_t sPos; |
int16_t Position[3]; |
//int16_t range; |
tmp_tracking = Servo_tmp_on(Config.servo_frame); |
// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph |
// chrxs = CHRLS; // Flag, welche K�stchensymbole geladen |
// Displ_Title(MSG_CONTINOUS); |
lcd_cls (); |
PKT_TitleFromMenuItem( true ); |
lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
Displ_LoopCounter(Config.repeat); |
Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl m�glich |
Position[1] = ServoSteps()/2; |
Position[2] = ServoSteps(); |
// init Einzelschritt |
idx = 0; |
dir = calc_dir(idx, Position); |
sPos = Position[PosIdx[idx]]; |
idx++; |
rep = Config.repeat; |
// Test bis Ende der Wiederholungen oder irgendein Enter |
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) { |
calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
//range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
// draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile |
servoSetPosition(servo_nr, sPos); |
if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht? |
sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren |
if (((dir < 0) && (sPos < Position[PosIdx[idx]])) || ((dir > 0) && (sPos > Position[PosIdx[idx]])) || !(Config.single_step)) |
sPos = Position[PosIdx[idx]]; // �berlauf bei variabler Schrittweite ber�cksichtigen oder Einzelschritt |
Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 w�rden welche �bersprungen, zus�tzlich pro Servoschritt verz�gert |
} |
else { |
dir = calc_dir(idx, Position); // Richtungs�nderung |
if (idx < (POSIDX_MAX - 1)) { |
if (idx == 0) { |
rep--; // bei jeden vollen Durchlauf Wiederholz�hler verringern |
Displ_LoopCounter(rep); |
} |
idx++; // Index f�r n�chsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge |
} |
else |
idx = 0; |
_delay_ms( Config.pause*100 ); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf) |
} |
} |
// lcdClear(); |
// if (pmenu[0] == '\0') |
// Displ_Main_Disp(); |
// else |
// return_m_pkt(strlen(pmenu)); // um bei R�cksprung auf urspr�nglichen Men�punkt zeigen oder Displ_Main_Disp() |
// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char f�r Bargraph zur�ckschreiben |
cli(); |
servoInit(SERVO_PERIODE); |
sei(); |
Servo_tmp_Original(tmp_tracking); |
} |
/********************************************************************************/ |
/* zeigt Servo-Anschlagposition links auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/********************************************************************************/ |
void Displ_Servo_Min(uint16_t val) |
{ |
uint16_t steps = 0; |
write_ndigit_number_s( 15, 2, val, 5, 0,0); |
servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = ServoSteps(); |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition rechts auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Max(uint16_t val) |
{ |
uint16_t steps = ServoSteps(); |
write_ndigit_number_u( 15, 2, val, 5, 0,0); |
servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die |
if (Config.servo[servo_nr].rev) steps = 0; |
servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
} |
/************************************************************************************/ |
/* zeigt Servo-Anschlagposition Mitte auf Display an */ |
/* mit sofortiger Wirkung auf Servo */ |
/* Parameter: */ |
/* uint16_t val :anzuzeigender Wert, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Mid( uint16_t val ) |
{ |
int16_t mid_val; |
mid_val = val - ServoSteps()/2; |
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val ); |
servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die |
servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_rev( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
tmp_val = Config.servo[servo_nr].rev; |
if( Change_Value(&tmp_val , 0, 1, Displ_Off_On) ) |
{ //reverse gibt es nur 0=off, 1=on |
Config.servo[servo_nr].rev = tmp_val ; |
servoSet_rev(servo_nr, tmp_val ); |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_left( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].max; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) ) |
{ |
Config.servo[servo_nr].max = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_right( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].min; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) ) |
{ |
Config.servo[servo_nr].min = tmp_val; |
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
} |
Servo_tmp_Original(tmp_tracking); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Servo_middle( void ) |
{ |
uint16_t tmp_val; |
uint8_t tmp_tracking; |
draw_input_screen(); |
tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen |
tmp_val = Config.servo[servo_nr].mid; |
if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) ) |
{ |
Config.servo[servo_nr].mid = tmp_val; |
} |
Servo_tmp_Original(tmp_tracking); |
} |
void Servo_NewValues(uint8_t idx_presc) |
{ |
for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) { |
if (idx_presc == STEPS_255) { // Werte umrechnen f�r Prescaler = 256 |
Config.servo[i].min /= 4; |
Config.servo[i].max /= 4; |
Config.servo[i].mid /= 4; |
} |
else { // Werte umrechnen f�r Prescaler = 64 |
Config.servo[i].min *= 4; |
Config.servo[i].max *= 4; |
Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1; |
} |
servoSet_min(i, Config.servo[i].min); |
servoSet_max(i, Config.servo[i].max); |
servoSet_mid(i, Config.servo[i].mid); |
// eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t)); |
} |
// Vorberechnung von ServoChannels[channel].duty |
servoSetDefaultPos(); // Ausgangsstellung beider Servos |
} |
/************************************************************************************/ |
/* zeigt Servoschritte zur Auswahl auf Display an */ |
/* Parameter: */ |
/* uint16_t val :0 = 255 oder 1 = 1023, */ |
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
/* */ |
/************************************************************************************/ |
void Displ_Servo_Steps(uint16_t val) |
{ |
if (val==0) |
lcd_puts_at(16,2,INTERNAT_STEPS_255,0 ); |
else |
lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void Menu_Servo_Steps(void) |
{ |
uint16_t tmp_val; |
draw_input_screen(); |
tmp_val = Config.sIdxSteps; |
if( Change_Value( &tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps) ) |
{ |
cli(); |
Config.sIdxSteps = tmp_val; |
Servo_NewValues( Config.sIdxSteps ); // hier ist der neue Index anzugeben! |
servoInit(SERVO_PERIODE); |
sei(); |
} |
} |
//-------------------------------------------------------------- |
// Setup_ServoAdjust() |
//-------------------------------------------------------------- |
void Setup_ServoAdjust( uint8_t servo ) |
{ |
servo_nr = servo; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1" |
//MenuCtrl_SetTitle_P( PSTR("Servo Adjust") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en ); |
MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTestCont() |
//-------------------------------------------------------------- |
void Setup_ServoTestCont( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd." |
//MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") ); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en ); |
MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en ); |
MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en ); |
MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTest() |
//-------------------------------------------------------------- |
void Setup_ServoTest( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Servotest" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en ); |
MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en ); |
MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en ); |
MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en ); |
//--------------- |
// Control |
//--------------- |
MenuCtrl_Control( MENUCTRL_EVENT ); |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
// Setup_ServoTracking() |
//-------------------------------------------------------------- |
void Setup_ServoTracking( void ) |
{ |
uint8_t itemid; |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
MenuCtrl_SetTitleFromParentItem(); // "Tracking" |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en ); |
MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en ); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); } |
if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); } |
} |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} |
//-------------------------------------------------------------- |
//void start_tracking(void) |
//{ |
// #define TIMEOUT 200 // 2 sec |
// |
// uint16_t old_anglePan = 0; |
// uint16_t old_angleTilt = 0; |
// |
// //uint16_t old_hh = 0; |
// uint8_t flag; |
// uint8_t tmp_dat; |
// |
// lcd_cls (); |
// //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2); |
// |
// //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0); |
// |
// lcd_ecircle(22, 35, 16, 1); |
// lcd_ecircle(88, 35, 16, 1); |
// lcd_putc (10, 1, 0x1e, 0); // degree symbol |
// lcd_putc (20, 1, 0x1e, 0); // degree symbol |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// SwitchToNC(); |
// |
// mode = 'O'; |
// |
// // disable debug... |
// // RS232_request_mk_data (0, 'd', 0); |
// tmp_dat = 0; |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// if(error1 == 1) |
// lcd_cls(); |
// } |
// |
// GPS_Pos_t currpos; |
// currpos.Latitude = naviData->CurrentPosition.Latitude; |
// currpos.Longitude = naviData->CurrentPosition.Longitude; |
// |
// flag = 0; |
// timer = TIMEOUT; |
// abo_timer = ABO_TIMEOUT; |
// |
// coldstart = 1; |
// |
// do |
// { |
// if (rxd_buffer_locked) |
// { |
// timer = TIMEOUT; |
// Decode64 (); |
// naviData = (NaviData_t *) pRxData; |
// |
// |
////CB uint8_t FCStatusFlag = naviData->FCFlags; |
// uint8_t FCStatusFlag = naviData->FCStatusFlags; |
// //write_ndigit_number_u (0, 0, FCStatusFlag); |
// |
// Tracking_GPS(); |
// |
// //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
// |
// // alte Linien l�schen |
// //lcd_ecirc_line (22, 35, 15, old_hh, 0); |
// //old_hh = heading_home; |
// lcd_ecirc_line (22, 35, 15, old_anglePan, 0); |
// old_anglePan = anglePan; |
// lcd_ecirc_line (88, 35, 15, old_angleTilt, 0); |
// old_angleTilt = angleTilt; |
// |
// lcd_ecirc_line (22, 35, 15, anglePan, 1); |
// write_ndigit_number_u (7, 1, anglePan, 3, 0,0); |
// lcd_ecirc_line (88, 35, 15, angleTilt, 1); |
// write_ndigit_number_u (17, 1, angleTilt, 3, 0,0); |
// |
// rxd_buffer_locked = FALSE; |
// |
// if (!abo_timer) |
// { // renew abo every 3 sec |
// // request OSD Data from NC every 100ms |
// // RS232_request_mk_data (1, 'o', 100); |
// tmp_dat = 10; |
// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
// |
// abo_timer = ABO_TIMEOUT; |
// } |
// } |
// |
// if (!timer) |
// { |
// OSD_Timeout(flag); |
// flag = 0; |
// } |
// } |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// //lcd_cls(); |
// //return; |
//} |
//-------------------------------------------------------------- |
// |
//void conect2at_unit(void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_ser (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
// |
////-------------------------------------------------------------- |
// |
//void conect2gps_bt (void) |
//{ |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2); |
// |
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
// |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(!get_key_press (1 << KEY_ESC)); |
// |
// lcd_cls(); |
// return; |
//} |
//-------------------------------------------------------------- |
//void conect2gps_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_CONECT_GPS; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// conect2gps_ser(); |
// if((val+Offset) == 2 ) |
// conect2gps_bt(); |
// } |
//} |
//-------------------------------------------------------------- |
//void tracking_menu(void) |
//{ |
// uint8_t ii = 0; |
// uint8_t Offset = 0; |
// uint8_t size = ITEMS_AT; |
// uint8_t dmode = 0; |
// uint8_t target_pos = 1; |
// uint8_t val = 0; |
// |
// while(1) |
// { |
// lcd_cls (); |
// lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2); |
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
// while(2) |
// { |
// ii = 0; |
// if(Offset > 0) |
// { |
// lcd_printp_at(1,1, PSTR("\x12"), 0); |
// } |
// for(ii = 0;ii < 6 ; ii++) |
// { |
// if((ii+Offset) < size) |
// { |
// lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0); |
// } |
// if((ii == 5)&&(ii+Offset < (size-1))) |
// { |
// lcd_printp_at(1,6, PSTR("\x13"), 0); |
// } |
// } |
// if(dmode == 0) |
// { |
// if(Offset == 0) |
// { |
// if(size > 6) |
// { |
// val = menu_choose2 (1, 5, target_pos,0,1); |
// } |
// else |
// { |
// val = menu_choose2 (1, size, target_pos,0,0); |
// } |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(dmode == 1) |
// { |
// if(Offset+7 > size) |
// { |
// val = menu_choose2 (2, 6, target_pos,1,0); |
// } |
// else |
// { |
// val = menu_choose2 (2, 5, target_pos,1,1); |
// } |
// } |
// if(val == 254) |
// { |
// Offset++; |
// dmode = 1; |
// target_pos = 5; |
// } |
// else if(val == 253) |
// { |
// Offset--; |
// dmode = 0; |
// target_pos = 2; |
// } |
// else if(val == 255) |
// { |
// return; |
// } |
// else |
// { |
// break; |
// } |
// } |
// target_pos = val; |
// |
// if((val+Offset) == 1 ) |
// test_servo_menu(); |
// if((val+Offset) == 2 ) |
// adjust_servo_menu(); |
// if((val+Offset) == 3 ) |
// show_angle(); |
// if((val+Offset) == 4 ) |
////TODO: start_tracking(); |
// if((val+Offset) == 5 ) |
// conect2at_unit(); |
// if((val+Offset) == 6 ) |
// conect2gps_menu(); |
// } |
//} |
//-------------------------------------------------------------- |
// kapeschi Ant.Treking Funktionen |
//-------------------------------------------------------------- |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ int32_t lat1, lon1, lat2, lon2; |
// int32_t d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = pos->Longitude; |
// lat2 = pos->Latitude; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator |
// // es wird jedoch in dm Meter weiter gerechnet |
// // (tlon1 - tlon2)/10) sonst uint32_t-�berlauf bei cos(0) gleich 1 |
// d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
// dlat = 1113 * (lat1 - lat2) / 10000; |
// geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
// geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
//void do_tracking(void) |
//{ //static uint8_t hysteresis = 0; |
// // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
// // (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
// |
// geo = calc_geo(&MK_pos, ¤tPos); |
// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
// //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
// //geo.bearing = MK_pos.direction; |
// //angleTilt = 0; |
// //hysteresis = 1; |
// //} |
// //else { |
// //hysteresis = 0; |
// //} |
//// |
// //// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen |
// //// 360 grd negative Winkelwerte als positive |
// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
// |
// //if (angleTilt < 0) angleTilt = 0; |
// //if (angleTilt > 180) angleTilt = 180; |
//// |
// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
// //anglePan = anglePan - 180; |
// //angleTilt = 180 - angleTilt; |
// // |
// //} |
////angleTilt = 180; |
////angleTilt = 180; |
// |
//// servoSetAngle(0, anglePan); |
//// servoSetAngle(1, angleTilt); |
//} |
/****************************************************************/ |
/* */ |
/* MK GPS Tracking */ |
/* */ |
/****************************************************************/ |
// MK OSD-Daten lesen und verifizieren |
//uint8_t OSD_Data_valid(NaviData_t **navi_data) |
//{ uint8_t ret = 0; |
//char *tx_osd = {"#co?]==EH\r"}; |
//// char interval[2] = {10, '\0'}; |
// |
//if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren |
////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
//currentPos = (*navi_data)->CurrentPosition; |
//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
//ret = 1; |
//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
//// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//satsInUse = (*navi_data)->SatsInUse; |
//} |
//} |
//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval |
//return ret; |
//} |
// |
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
// Hauptprogramm von GPS Antennen-Nachf�hrung |
//void Tracking_GPS(void) |
//{ //NaviData_t *navidata; |
// static uint8_t track_running = 0; |
// |
// if (!track_running) |
// { |
// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
// //if (OSD_Data_valid(&naviData)) { |
// if (coldstart) |
// { |
// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen |
// //if (naviData->FCFlags & FC_FLAG_MOTOR_START) |
// //{ |
// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0; |
// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0; |
// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude; |
// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude; |
// MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
// MK_pos.direction = naviData->CompassHeading; |
// coldstart = 0; |
// //} |
// //} |
// //else { |
// //do_tracking(); |
// } |
// //} |
// track_running = 0; |
// } |
// do_tracking(); |
//} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/servo_setup.h |
---|
0,0 → 1,45 |
#ifndef _TRACKING_H_ |
#define _TRACKING_H_ |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define DLEFT 0 |
#define DRIGHT 1 |
#define AltFaktor 22.5 |
#define PAN_SERVO_CORRECT 3.125 |
#define TILT_SERVO_CORRECT 3.125 |
typedef void (*Displ_Fnct_t)( uint16_t ); |
//typedef struct { |
// int32_t distance; |
// int16_t bearing; |
//}geo_t; |
//typedef struct { |
// double Home_Lon; // in degrees |
// double Home_Lat; // in degrees |
// int32_t Home_Lon7; // in 1E-7 degrees |
// int32_t Home_Lat7; // in 1E-7 degrees |
// int32_t Home_Alt; // in mm |
// // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
// int16_t direction; |
//}__attribute__((packed)) HomePos_t; |
#define INTERNAT_STEPS_255 "255 " |
#define INTERNAT_STEPS_1023 "1023" |
// kapeschi |
void Setup_ServoTracking( void ); |
void Tracking_GPS(void); |
void Tracking_NMEA(void); |
void Tracking_RSSI(void); |
void setNMEAdir(void); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/tracking.c |
---|
0,0 → 1,648 |
/* |
* tracking.c |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.c |
//# |
//# 27.06.2014 OG |
//# - umgestellt auf gps/gps_nmea.c |
//# - Codeformattierung |
//# - chg: auf #include "../gps/mymath.h" angepasst |
//# - del: #include "tools.h" |
//# |
//# 25.06.2014 OG |
//# - chg: PKT_tracking() - auskommentierungen um Zugriffe auf Config.gps_UsedGPSMouse |
//# zu unterbinden. Anmerkung dazu siehe dort. |
//# |
//# 22.06.2014 OG |
//# - chg: Tracking_NMEA() - do_tracking() auskommentiert (spaeter fuer |
//# Tracking wieder geradeziehen) |
//# - Variable 'CheckGPS' hinzugefuegt weil aus anderen Sourcen (setup.c) entfernt |
//# |
//# 18.06.2014 OG |
//# - chg: Code-Formattierung |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# |
//# 01.06.2014 OG |
//# - chg: PKT_trackingMK() - verschiedene Anzeigefunktionen auskommentiert und |
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
//# |
//# 30.05.2014 OG |
//# - chg: calc_geo()- my_itoa() ersetzt durch writex_gpspos() und lcdx_printf_at_P() |
//# weil my_itoa() nicht mehr vorhanden |
//# |
//# 13.05.2014 OG |
//# - chg: PKT_trackingBT() - Variable 'BT_WhasOn' auskommentiert |
//# wegen Warning: variable set but not used |
//# - chg: PKT_trackingBT() - Variable 'flag' auskommentiert |
//# wegen Warning: variable set but not used |
//# |
//# 28.04.2014 OG |
//# - chg: PKT_trackingBT(), PKT_trackingMK() - OSD_Timeout() entfernt (weil es |
//# das nicht mehr gibt) und erstmal durch ein PKT_Message_P() ersetzt |
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, tracking hat niedrige Prio) |
//# - add: include "../pkt/pkt.h" |
//# |
//# 12.02.2014 OG |
//# - del: mk_param_struct entfernt |
//# |
//# 29.08.2013 Cebra |
//# - chg: falsche Parameterübergabe bei getLatitude/getLongitude |
//# |
//# 25.08.2013 Cebra |
//# - add: #ifdef USE_TRACKING . NMEA Routinen werden nur noch für BT_NMEA genutzt |
//# |
//# 26.06.2013 Cebra |
//# - chg: Modulumschaltung für WiFlypatch geändert |
//# |
//############################################################################ |
#include "../cpu.h" |
#include <string.h> |
#include <util/delay.h> |
#include <avr/interrupt.h> |
#include <stdlib.h> |
#include <math.h> |
#include "../main.h" |
//++++++++++++++++++++++++++++++++++ |
#ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
#include "../tracking/tracking.h" |
#include "../tracking/ng_servo.h" |
#include <avr/pgmspace.h> |
#include "../bluetooth/fifo.h" |
#include "../bluetooth/bluetooth.h" |
#include "../lcd/lcd.h" |
#include "../mk-data-structs.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../timer/timer.h" |
#include "../uart/uart1.h" |
#include "../uart/usart.h" |
#include "../osd/osd.h" |
#include "../gps/mymath.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
//#include "../gps/gps_nmea.h" |
#include "../avr-nmea-gps-library/nmea.h" |
uint8_t CheckGPS = true; // Patch 22.06.2014 OG: hier integriert weil ais anseren Sourcen entfernt (u.a. setup.c) |
char data_decode[RXD_BUFFER_SIZE]; |
#define DLEFT 0 |
#define DRIGHT 1 |
#define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
#define AltFaktor 22.5 |
#define TIMEOUT 200 // 2 sec |
NaviData_t *naviData; |
HomePos_t MK_pos; // Home position of station |
GPS_Pos_t currentPos; // Current position of flying object |
uint8_t tracking = TRACKING_MIN; |
uint8_t track_hyst = TRACKING_HYSTERESE; |
uint8_t track_tx =0; |
uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
geo_t geo; |
int16_t anglePan, angleTilt; |
//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
//// zur aktuellen Position(nach Motorstart) |
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
//{ double lat1, lon1, lat2, lon2, d1, dlat; |
// geo_t geo; |
// |
// lon1 = MK_pos.Home_Lon; |
// lat1 = MK_pos.Home_Lat; |
// lon2 = (double)pos->Longitude / 10000000.0; |
// lat2 = (double)pos->Latitude / 10000000.0; |
// |
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
// // es wird jedoch in Meter weiter gerechnet |
// d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
// dlat = 111300 * (double)(lat1 - lat2); |
// // returns a value in metres http://www.kompf.de/gps/distcalc.html |
// geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
// if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
// geo.distance = sqrt(d1 * d1 + dlat * dlat); |
// return(geo); |
//} |
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
// zur aktuellen Position(nach Motorstart) |
//-------------------------------------------------------------- |
// bei Gelegenheit den Code auf gps/gps.c/gps_Deviation() aendern |
//-------------------------------------------------------------- |
geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos ) |
{ |
int32_t lat1, lon1, lat2, lon2; |
int32_t d1, dlat; |
geo_t geo; |
lon1 = home->Home_Lon; |
lat1 = home->Home_Lat; |
lon2 = pos->Longitude; |
lat2 = pos->Latitude; |
if( !CheckGPS ) |
{ |
writex_gpspos( 0, 3, home->Home_Lat , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 3, home->Home_Lon , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
writex_gpspos( 0, 4, pos->Latitude , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude) |
writex_gpspos( 11, 4, pos->Longitude , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude) |
//lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
//lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
} |
// Formel verwendet von http://www.kompf.de/gps/distcalc.html |
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator |
// es wird jedoch in dm Meter weiter gerechnet |
// (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1 |
d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
dlat = 1113 * (lat1 - lat2) / 10000; |
geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
if( !CheckGPS ) |
{ |
lcd_printp_at (0, 5, PSTR("Bear:"), 0); |
lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing ); |
//lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance ); |
//lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr |
lcd_printp_at (20, 5, PSTR("m"), 0); |
} |
return(geo); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void do_tracking( void ) |
{ |
static uint8_t hysteresis = 0; |
geo = calc_geo(&MK_pos, ¤tPos); |
angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance); |
if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) |
{ // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
geo.bearing = MK_pos.direction; |
if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf |
hysteresis = 1; |
} |
else |
{ |
hysteresis = 0; |
} |
// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
// 360 grd negative Winkelwerte als positive |
anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
if (angleTilt < 0) |
angleTilt = 0; |
if (angleTilt > 180) |
angleTilt = 180; |
if (anglePan >= 180) |
{ // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
anglePan = anglePan - 180; |
angleTilt = 180 - angleTilt; |
} |
servoSetAngle(0, anglePan); |
servoSetAngle(1, angleTilt); |
if (!CheckGPS) |
{ |
lcd_printp_at (0, 6, PSTR("Pan :"), 0); |
write_ndigit_number_u (6, 6, anglePan, 3, 1,0); |
lcd_printp_at (11, 6, PSTR("Tilt:"), 0); |
write_ndigit_number_u (17, 6, angleTilt, 3, 1,0); |
} |
// write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0); |
// |
// write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0); |
//// lcd_printp_at (4, 4, PSTR("."), 0); |
// write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0); |
// write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingBT( void ) // Tracking mit NMEA-Daten von BT-Maus |
{ |
//uint8_t BT_WhasOn = 0; |
uint8_t BT_status; |
//uint8_t flag; |
uint8_t tmp_dat; |
coldstart =1; |
//lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0); |
lcd_puts_at (0, 1,Config.gps_UsedDevName, 0); |
//set_BTOn(); |
set_Modul_On(Bluetooth); |
//BT_WhasOn = true; |
if( Config.BTIsSlave==true ) |
{ |
bt_downlink_init(); |
} |
lcd_printp_at (18, 1, PSTR(" ?? "), 0); |
BT_status = bt_connect(Config.gps_UsedMac); |
if( BT_status==true ) |
{ |
lcd_printp_at (18, 1, PSTR(" OK "), 0); |
receiveNMEA = true; |
} |
else |
{ |
lcd_printp_at (17, 1, PSTR("FAIL"), 2); |
} |
if( receiveNMEA==true ) |
{ |
lcd_cls_line( 0, 1, 20); |
lcd_printp_at( 0, 2, PSTR(" Latitude Longitude"), 2); |
lcd_printp_at( 0, 3, PSTR("H"), 0); |
lcd_printp_at( 0, 4, PSTR("M"), 0); |
bt_rx_ready = 0; |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData( 'd', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
//OSD_active = true; // benoetigt für Navidata Ausgabe an SV2 |
//flag = 0; |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
do |
{ |
//bt_rx_ready = 0; |
// if( !NMEA_receiveBT() ) |
// break; |
if (!NMEA_isdataready()) |
break; |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
currentPos = naviData->CurrentPosition; |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//----------------- |
// MK: Lat / Long |
//----------------- |
writex_gpspos( 2, 4, currentPos.Latitude , MNORMAL, 0,0 ); // MK: Latitude |
writex_gpspos(12, 4, currentPos.Longitude, MNORMAL, 0,0 ); // MK: Longitude |
// NMEA_GetNewData(); // neue NMEA GPGGA Daten von der BT GPA-Maus holen |
//----------------- |
// GPS-Maus: Lat / Long |
//----------------- |
writex_gpspos( 2, 3, NMEA.Latitude , MNORMAL, 0,0 ); // GPS-Maus: Latitude |
writex_gpspos(12, 3, NMEA.Longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
//do_tracking(); // das geht so noch nicht mit einer BT GPS-Maus! do_tracking(); ueberarbeiten! |
if( !CheckGPS ) |
{ |
//lcd_printp_at( 0, 2, PSTR("GPS Time: "), 0); |
lcd_puts_at( 13, 0, NMEA.Time , 2); |
lcd_printp_at( 16, 1, PSTR("Sat:"), 0); |
write_ndigit_number_u( 19, 1, NMEA.SatsInUse, 2, 1,0); |
lcd_printp_at( 0, 1, PSTR("Fix:"), 0); |
write_ndigit_number_u( 4, 1, NMEA.SatFix, 1, 1,0); |
lcd_printp_at( 6, 1, PSTR("HDOP:"), 0); |
write_ndigit_number_u_10th( 11, 1, NMEA.HDOP, 3, 0,0); |
} |
rxd_buffer_locked = FALSE; |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if (rxd_buffer_locked) |
if( !timer ) |
{ |
//OSD_Timeout(flag); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
//flag = 0; |
error = 1; |
} |
} while( !get_key_press (1 << KEY_ESC) || !receiveNMEA==true || error ==1); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if( !receiveNMEA ) |
lcd_printp_at (0, 2, PSTR("lost BT data"), 0); |
lcd_printp_at (0, 3, PSTR("GPS trennen"), 0); |
} // end: if( receiveNMEA==true ) |
else |
{ |
lcd_printp_at (0, 4, PSTR("Error at connecting"), 0); |
lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0); |
while( !get_key_press (1 << KEY_ENTER) ); |
} |
receiveNMEA = false; |
if( !bt_disconnect() ) |
lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0); |
//set_BTOff(); |
set_Modul_On(USB); |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t PKT_trackingMK( void ) // Tracking mit GPS-Daten vom Mikrokopter |
{ |
//uint8_t BT_WhasOn = 0; |
//uint8_t BT_status; |
uint8_t GPSfix=0; |
uint8_t tmp_dat; |
uint8_t toggletimer=0; |
coldstart = true; |
lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
lcd_cls_line (0,1,20); |
//lcd_printp_at (0, 3, PSTR("H"), 0); |
//lcd_printp_at (0, 4, PSTR("M"), 0); |
SwitchToNC(); |
mode = 'O'; |
// disable debug... |
// RS232_request_mk_data (0, 'd', 0); |
tmp_dat = 0; |
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
timer = TIMEOUT; |
abo_timer = ABO_TIMEOUT; |
error = 0; |
do |
{ |
if( rxd_buffer_locked ) |
{ |
timer = TIMEOUT; |
Decode64 (); |
naviData = (NaviData_t *) pRxData; |
//OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1); |
//if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//OSD_Element_SatsInUse( 18, 1, 1); |
//--- |
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!) |
//--- |
//if (GPSfix == true) OSD_Element_StatusFlags( 0, 1); |
if (!(naviData->NCFlags & NC_FLAG_GPS_OK)) |
{ |
toggletimer++; |
if (toggletimer == 50) toggletimer = 0; |
if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2); |
if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0); |
rxd_buffer_locked = false; |
GPSfix = false; |
} |
else GPSfix = true; |
if( GPSfix ) |
{ |
if( coldstart ) |
{ |
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) |
{ |
MK_pos.Home_Lon = naviData->HomePosition.Longitude; |
MK_pos.Home_Lat = naviData->HomePosition.Latitude; |
MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
MK_pos.direction = naviData->CompassHeading; |
coldstart = false; |
rxd_buffer_locked = false; |
lcd_printp_at(0,1, PSTR(" "), 0); |
} |
else |
{ |
lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0); |
rxd_buffer_locked = false; |
} |
} // end: if( coldstart ) |
else |
{ |
//run |
currentPos = naviData->CurrentPosition; |
currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
do_tracking(); |
//lcd_puts_at (13, 0, NMEAtime, 2); |
//lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
//write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
//lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
//write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
//lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
//write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
rxd_buffer_locked = FALSE; |
} // run |
} |
if( !abo_timer ) |
{ |
// renew abo every 3 sec |
// request OSD Data from NC every 100ms |
// RS232_request_mk_data (1, 'o', 100); |
tmp_dat = 10; |
SendOutData( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
abo_timer = ABO_TIMEOUT; |
} |
} // end: if( rxd_buffer_locked ) |
if( !timer ) |
{ |
//OSD_Timeout(1); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet) |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen |
error = 1; |
} |
} while( (!get_key_press (1 << KEY_ENTER)) && (error ==0) ); |
lcd_cls_line(0,1,21); |
lcd_cls_line(0,2,21); |
lcd_cls_line(0,3,21); |
lcd_cls_line(0,4,21); |
lcd_cls_line(0,5,21); |
lcd_cls_line(0,6,21); |
if (error ==1) |
{ |
lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
_delay_ms(2000); |
} |
return true; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void PKT_tracking(void) |
{ |
clear_key_all(); |
lcd_cls (); |
// 25.06.2014 OG: auskommentiert weil erstmal kein Config.gps_UsedGPSMouse mehr unterstuetzt |
// wird. Falls tracking.c mal richtig implementiert wird sollte man sich auf eine BT-GPS-Maus |
// konzentrieren bis Tracking einwandfrei laeuft - wenn dann noch immer Bedarf nach MK-GPS ist |
// kann man das ja wieder einbauen |
// |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2); |
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT(); |
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK(); |
lcd_printp_at( 0,0, PSTR("Tracking Bluetooth "), MINVERS); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline |
PKT_trackingBT(); |
clear_key_all(); |
} |
//++++++++++++++++++++++++++++++++++ |
#endif // #ifdef USE_TRACKING |
//++++++++++++++++++++++++++++++++++ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking/tracking.h |
---|
0,0 → 1,77 |
/* |
* tracking.h |
* |
* Created on: 13.02.2012 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY tracking.h |
//# |
//# 27.06.2014 OG |
//# - Entkernt: verschiedene Variablen und Funktionen geloescht |
//# |
//# 16.06.2014 OG |
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete) |
//# - add: Source-Historie |
//############################################################################ |
#ifndef TRACKING_H_ |
#define TRACKING_H_ |
#define REPEAT 1 |
#define REPEAT_MIN 1 |
#define REPEAT_MAX 100 |
#define PAUSE 10 |
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss |
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms |
#define PAUSE_STEP 0 |
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms |
#define PAUSE_STEP_MAX 200 |
// Antennen-Nachführung |
#define TRACKING_MIN 0 // aus, TRACKING_RSSI, TRACKING_GPS, TRACKING_MKCOCKPIT, TRACKING_NMEA |
#define TRACKING_MAX 4 |
// Antennen-Nachführung per RSSI |
#define TRACKING_HYSTERESE 40 // Hysterese bevor Tracking bei Richtungswechsel anspricht |
#define TRACKING_HYST_MIN 0 |
#define TRACKING_HYST_MAX 100 |
#define FC_FLAG_MOTOR_RUN 0x01 |
#define FC_FLAG_FLY 0x02 |
#define FC_FLAG_CALIBRATE 0x04 |
#define FC_FLAG_MOTOR_START 0x08 |
typedef struct { |
int32_t distance; |
int16_t bearing; |
} geo_t; |
typedef struct { |
int32_t Home_Lon; // in 1E-7 degrees |
int32_t Home_Lat; // in 1E-7 degrees |
int32_t Home_Alt; // in mm |
int16_t direction; // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading |
} __attribute__((packed)) HomePos_t; |
extern uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
#ifdef USE_TRACKING |
void PKT_tracking(void); |
#endif // #ifdef USE_TRACKING |
#endif // TRACKING_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/tracking |
---|
Property changes: |
Added: svn:ignore |
+_doc |
/Transportables_Koptertool/PKT/trunk/uart/uart1.c |
---|
0,0 → 1,638 |
/************************************************************************* |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.c,v 1.6.2.2 2009/11/29 08:56:12 Peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4.6 or higher |
Hardware: any AVR with built-in UART, |
License: GNU General Public License |
DESCRIPTION: |
An interrupt is generated when the UART has finished transmitting or |
receiving a byte. The interrupt handling routines use circular buffers |
for buffering received and transmitted data. |
The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define |
the buffer size in bytes. Note that these variables must be a |
power of 2. |
USAGE: |
Refere to the header file uart.h for a description of the routines. |
See also example test_uart.c. |
NOTES: |
Based on Atmel Application Note AVR306 |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
*************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.c |
//# |
//# 08.08.2015 CB |
//# - chg: Umbau ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - add: Erweiterung ISR(USART1_RX_vect) um Behandlung der NMEA Datensätze |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 04.07.2013 Cebra |
//# - add: neue Funktion uart1_peek |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <string.h> |
#include <stdbool.h> |
#include "uart1.h" |
#include "usart.h" |
#include "../main.h" |
#include "../bluetooth/bluetooth.h" |
#include "../tracking/tracking.h" |
#include "../avr-nmea-gps-library/nmea.h" |
// |
// constants and macros |
// |
// size of RX/TX buffers |
#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1) |
#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1) |
#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK ) |
#error RX buffer size is not a power of 2 |
#endif |
#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK ) |
#error TX buffer size is not a power of 2 |
#endif |
// ATmega with two USART |
#define ATMEGA_USART1 |
#define UART1_STATUS UCSR1A |
#define UART1_CONTROL UCSR1B |
#define UART1_DATA UDR1 |
#define UART1_UDRIE UDRIE1 |
// |
// module global variables |
// |
#if defined( ATMEGA_USART1 ) |
static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE]; |
static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE]; |
static volatile unsigned char UART1_TxHead; |
static volatile unsigned char UART1_TxTail; |
static volatile unsigned char UART1_RxHead; |
static volatile unsigned char UART1_RxTmpHead; |
static volatile unsigned char UART1_RxTail; |
static volatile unsigned char UART1_LastRxError; |
static volatile unsigned char UART1_RxTmpHead;; |
volatile uint16_t UART1_RxError; |
#endif |
volatile uint8_t UART1_receiveNMEA = false; |
void SetBaudUart1(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: uart1_init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: uart1_init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
// case Baud_9600: uart1_init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_9600: uart1_init( 129); break; |
case Baud_19200: uart1_init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: uart1_init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_57600: uart1_init( 21); break; |
// case Baud_115200: uart1_init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
case Baud_115200: uart1_init( 10 ); break; |
} |
} |
// |
// these functions are only for ATmegas with two USART |
// |
#if defined( ATMEGA_USART1 ) |
#if 0 |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
// read UART status register and UART data register |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
UART1_LastRxError = lastRxError; |
} |
#else |
#warning Test neue UART ISR für NMEA |
//-------------------------------------------------------------- |
// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
// Purpose: called when the UART1 has received a character |
//-------------------------------------------------------------- |
ISR(USART1_RX_vect) |
{ |
unsigned char tmphead; |
unsigned char data; |
unsigned char usr; |
unsigned char lastRxError; |
usr = UART1_STATUS; |
data = UART1_DATA; |
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// calculate buffer index |
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
if ( tmphead == UART1_RxTail ) |
{ |
// error: receive buffer overflow |
lastRxError = UART_BUFFER_OVERFLOW >> 8; |
UART1_RxError++; |
} |
else |
{ |
if (UART1_receiveNMEA) |
{ |
fusedata(data); // NMEA Daten bearbeiten |
} |
else // if (UART1_receiveNMEA) |
{ |
// store new index |
UART1_RxHead = tmphead; |
// store received data in buffer |
UART1_RxBuf[tmphead] = data; |
} |
} |
UART1_LastRxError = lastRxError; |
} |
////-------------------------------------------------------------- |
//// Function: UART1 Receive Complete interrupt, decode NMEA GPGGA |
//// Purpose: called when the UART1 has received a character |
////-------------------------------------------------------------- |
//ISR(USART1_RX_vect) |
//{ |
// unsigned char tmphead; |
// unsigned char data; |
// unsigned char usr; |
// unsigned char lastRxError; |
// |
// static volatile uint8_t GGA; |
// static char buffer[6]; |
// static volatile uint8_t bufferindex; |
// static volatile uint8_t NMEAstart,NMEAend = false; |
// |
// // read UART status register and UART data register |
// usr = UART1_STATUS; |
// data = UART1_DATA; |
// |
// lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) ); |
// |
// // calculate buffer index |
// |
// tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK; |
// |
// if ( tmphead == UART1_RxTail ) |
// { |
// // error: receive buffer overflow |
// lastRxError = UART_BUFFER_OVERFLOW >> 8; |
// UART1_RxError++; |
// } |
// else |
// { |
// if (UART1_receiveNMEA) |
// { |
// cli(); |
// switch(data) |
// { |
// case '$': //start of a packet |
// |
// if (!NMEAstart && GGA) // falls mitten im Datensatz eine neuer kommt, warum auch immer |
// { |
// // restore index, war kein GPGGA Datensatz |
// UART1_RxHead = UART1_RxTmpHead; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// } |
// NMEAstart = true; |
// NMEAend = false; |
// GGA=false; |
// bufferindex=0; //wipe all these so they can be reused |
// |
// UART1_RxTmpHead = tmphead-1; // erstmal sichern, Start des NMEA Datensatzes |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// sei(); |
// break; |
// |
// default: //we have some of the CSV data |
// if(bufferindex<6) //dont mess up ! Dont overflow |
// { |
// buffer[bufferindex]=data; //stick the character in our buffer |
// } |
// |
// if(GGA) |
// { |
// // store new index, erstam in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// switch ( data) |
// { |
// case '\r' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// |
// case '\n' : |
// GGA = false; |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
// NMEAend = true; |
// UART1_GPGGA++; |
// memset(buffer,0,6); |
// break; // Ende mit NMEA Datensatz |
// } |
// |
// } |
// |
// else if(NMEAstart) //the header, erstes Komma noch nicht gekommen |
// |
// { |
// if(bufferindex<4) |
// |
// { |
// bufferindex++; //increase the position in the buffer |
// |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
// |
// |
// } |
// else |
// |
// { |
//// cli(); |
// if(!strcmp(buffer,"GPGGA")) //the last character will be a 0(end of string) |
// |
// { |
// GGA=true; |
// NMEAstart = false; |
// // store new index, erstmal in den Buffer schreiben |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
//// USART_putc(data); |
//// sei(); |
// } |
// |
// else |
// |
// { |
// // restore index, war kein GPGGA Datensatz |
// |
// UART1_RxHead = UART1_RxTmpHead; |
//// commacount=0; |
// memset(buffer,0,6); |
// bufferindex=0; //wipe all these so they can be reused |
// NMEAstart = false; |
//// sei(); |
// } |
// } |
// } |
// else if (!NMEAend && !GGA) UART1_RxHead = UART1_RxTmpHead; // Zeichen verwerfen |
// |
// } // end switch (data) |
// } |
// |
// else // if (UART1_receiveNMEA) |
// |
// { |
// // store new index |
// UART1_RxHead = tmphead; |
// // store received data in buffer |
// UART1_RxBuf[tmphead] = data; |
// } |
// } |
// UART1_LastRxError = lastRxError; |
//} |
#endif |
//-------------------------------------------------------------- |
// Function: UART1 Data Register Empty interrupt |
// Purpose: called when the UART1 is ready to transmit the next byte |
//-------------------------------------------------------------- |
ISR(USART1_UDRE_vect) |
{ |
unsigned char tmptail; |
if ( UART1_TxHead != UART1_TxTail) |
{ |
// calculate and store new buffer index |
tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK; |
UART1_TxTail = tmptail; |
// get one byte from buffer and write it to UART |
UART1_DATA = UART1_TxBuf[tmptail]; // start transmission |
} |
else |
{ |
// tx buffer empty, disable UDRE interrupt |
UART1_CONTROL &= ~_BV(UART1_UDRIE); |
} |
} |
//-------------------------------------------------------------- |
// Function: uart1_init() |
// Purpose: initialize UART1 and set baudrate |
// Input: baudrate using macro UART_BAUD_SELECT() |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_init(unsigned int baudrate) |
{ |
UART1_TxHead = 0; |
UART1_TxTail = 0; |
UART1_RxHead = 0; |
UART1_RxTail = 0; |
UART1_RxError = 0; |
UART1_receiveNMEA = false; //Empfang von NMEA Daten aus |
// Set baud rate |
if ( baudrate & 0x8000 ) |
{ |
UART1_STATUS = (1<<U2X1); //Enable 2x speed |
baudrate &= ~0x8000; |
} |
UBRR1H = (unsigned char)(baudrate>>8); |
UBRR1L = (unsigned char) baudrate; |
// Enable USART receiver and transmitter and receive complete interrupt |
UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1); |
// Set frame format: asynchronous, 8data, no parity, 1stop bit |
#ifdef URSEL1 |
UCSR1C = (1<<URSEL1)|(3<<UCSZ10); |
#else |
UCSR1C = (3<<UCSZ10); |
// UCSR1C = (1<<UCSZ11) | (1<<UCSZ10); // 8data Bit |
#endif |
} |
//-------------------------------------------------------------- |
// Function: uart1_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
unsigned int uart1_getc(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return UART_NO_DATA; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
UART1_RxTail = tmptail; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//-------------------------------------------------------------- |
// Function: uart1_peek() |
// Purpose: Returns the next byte (character) of incoming serial data without removing it from the internal serial buffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//-------------------------------------------------------------- |
int uart1_peek(void) |
{ |
unsigned char tmptail; |
unsigned char data; |
if ( UART1_RxHead == UART1_RxTail ) |
{ |
return -1; // no data available |
} |
// calculate /store buffer index |
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK; |
// get data from receive buffer |
data = UART1_RxBuf[tmptail]; |
return (UART1_LastRxError << 8) + data; |
}/* uart1_getc */ |
//int HardwareSerial::peek(void) |
//{ |
// if (_rx_buffer->head == _rx_buffer->tail) { |
// return -1; |
// } else { |
// return _rx_buffer->buffer[_rx_buffer->tail]; |
// } |
//} |
//-------------------------------------------------------------- |
// Function: uart1_putc() |
// Purpose: write byte to ringbuffer for transmitting via UART |
// Input: byte to be transmitted |
// Returns: 1 on succes, 0 if remote not ready |
//-------------------------------------------------------------- |
int uart1_putc(unsigned char data) |
{ |
unsigned char tmphead; |
tmphead = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK; |
while ( tmphead == UART1_TxTail ) |
{;} // wait for free space in buffer |
UART1_TxBuf[tmphead] = data; |
UART1_TxHead = tmphead; |
// enable UDRE interrupt |
UART1_CONTROL |= _BV(UART1_UDRIE); |
return (UART1_LastRxError << 8) + data; |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts() |
// Purpose: transmit string to UART1 |
// Input: string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts(const char *s ) |
{ |
while (*s) |
uart1_putc(*s++); |
} |
//-------------------------------------------------------------- |
// Function: uart1_puts_p() |
// Purpose: transmit string from program memory to UART1 |
// Input: program memory string to be transmitted |
// Returns: none |
//-------------------------------------------------------------- |
void uart1_puts_p(const char *progmem_s ) |
{ |
register char c; |
while ( (c = pgm_read_byte(progmem_s++)) ) |
uart1_putc(c); |
} |
//-------------------------------------------------------------- |
// Function: uart1_available() |
// Purpose: Determine the number of bytes waiting in the receive buffer |
// Input: None |
// Returns: Integer number of bytes in the receive buffer |
//-------------------------------------------------------------- |
uint16_t uart1_available(void) |
{ |
return (UART_RX_BUFFER_MASK + UART1_RxHead - UART1_RxTail) % UART_RX_BUFFER_MASK; |
} |
//-------------------------------------------------------------- |
// Function: uart1_flush() |
// Purpose: Flush bytes waiting the receive buffer. Acutally ignores them. |
// Input: None |
// Returns: None |
//-------------------------------------------------------------- |
void uart1_flush(void) |
{ |
UART1_RxHead = UART1_RxTail; |
} |
/************************************************************************* |
Function: utoa() |
Purpose: convert unsigned integer to ascii |
Input: string_buffer, string_buffer_size, unsigned integer value |
Returns: first ascii character |
**************************************************************************/ |
char *utoa1(char* buffer, const unsigned int size, unsigned int value) |
{ |
char *p; |
// p points to last char |
p = buffer+size-1; |
// Set terminating Zero |
*p='\0'; |
do |
{ |
*--p = value%10 + '0'; |
value = value/10; |
} while (value!=0 && p>buffer); |
return p; |
}/* utoa */ |
#endif |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/uart/uart1.h |
---|
0,0 → 1,194 |
/************************************************************************ |
Title: Interrupt UART library with receive/transmit circular buffers |
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
File: $Id: uart.h,v 1.8.2.1 2007/07/01 11:14:38 peter Exp $ |
Software: AVR-GCC 4.1, AVR Libc 1.4 |
Hardware: any AVR with built-in UART, tested on AT90S8515 & ATmega8 at 4 Mhz |
License: GNU General Public License |
Usage: see Doxygen manual |
LICENSE: |
Copyright (C) 2006 Peter Fleury |
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
any later version. |
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
************************************************************************/ |
//############################################################################ |
//# HISTORY uart1.h |
//# |
//# 06.08.2015 CB |
//# - add: uint8_t receiveNMEA // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
//# - chg: UART_RX_BUFFER_SIZE war 1024, darf aber nur max 256 sein |
//# |
//# 26.06.2014 OG |
//# - del: receiveNMEA |
//# |
//# 30.03.2013 CB |
//# - add: uart1_flush(void); in Headerdatei aufgenommen |
//########################################################################### |
// |
// @defgroup pfleury_uart UART Library |
// @code #include <uart.h> @endcode |
// |
// @brief Interrupt UART library using the built-in UART with transmit and receive circular buffers. |
// |
// This library can be used to transmit and receive data through the built in UART. |
// |
// An interrupt is generated when the UART has finished transmitting or |
// receiving a byte. The interrupt handling routines use circular buffers |
// for buffering received and transmitted data. |
// |
// The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE constants define |
// the size of the circular buffers in bytes. Note that these constants must be a power of 2. |
// You may need to adapt this constants to your target and your application by adding |
// CDEFS += -DUART_RX_BUFFER_SIZE=nn -DUART_RX_BUFFER_SIZE=nn to your Makefile. |
// |
// @note Based on Atmel Application Note AVR306 |
// @author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
// |
#ifndef UART_H |
#define UART_H |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
// constants and macros |
// @brief UART Baudrate Expression |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu)/((baudRate)*16l)-1) |
// @brief UART Baudrate Expression for ATmega double speed mode |
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz |
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600 |
// |
#define UART_BAUD_SELECT_DOUBLE_SPEED(baudRate,xtalCpu) (((xtalCpu)/((baudRate)*8l)-1)|0x8000) |
// Size of the circular receive buffer, must be power of 2 |
#ifndef UART_RX_BUFFER_SIZE |
#define UART_RX_BUFFER_SIZE 256 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// Size of the circular transmit buffer, must be power of 2 |
#ifndef UART_TX_BUFFER_SIZE |
#define UART_TX_BUFFER_SIZE 64 /* 2,4,8,16,32,64,128 or 256 bytes */ |
#endif |
// test if the size of the circular buffers fits into SRAM |
#if ( (UART_RX_BUFFER_SIZE+UART_TX_BUFFER_SIZE) >= (RAMEND-0x60 ) ) |
#error "size of UART_RX_BUFFER_SIZE + UART_TX_BUFFER_SIZE larger than size of SRAM" |
#endif |
//global variable |
extern volatile uint16_t UART1_RxError; |
extern volatile uint8_t UART1_receiveNMEA; // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// high byte error return code of uart_getc() |
#define UART_FRAME_ERROR 0x0800 // Framing Error by UART |
#define UART_OVERRUN_ERROR 0x0400 // Overrun condition by UART |
#define UART_BUFFER_OVERFLOW 0x0200 // receive ringbuffer overflow |
#define UART_NO_DATA 0x0100 // no receive data available |
#define TRACKING_RSSI 1 |
#define TRACKING_GPS 2 |
#define TRACKING_MKCOCKPIT 3 |
#define TRACKING_NMEA 4 |
#define NMEA_receive 5 // Flag zur Empfangssteuerung in ISR(USART1_RX_vect) |
// |
// function prototypes |
// |
// |
// @brief Initialize UART and set baudrate |
// @param baudrate Specify baudrate using macro UART_BAUD_SELECT() |
// @return none |
// |
extern void uart_init(unsigned int baudrate); |
// |
// @brief Get received byte from ringbuffer |
// |
// Returns in the lower byte the received character and in the |
// higher byte the last receive error. |
// UART_NO_DATA is returned when no data is available. |
// |
// @param void |
// @return lower byte: received byte from ringbuffer |
// @return higher byte: last receive status |
// - \b 0 successfully received data from UART |
// - \b UART_NO_DATA |
// <br>no receive data available |
// - \b UART_BUFFER_OVERFLOW |
// <br>Receive ringbuffer overflow. |
// We are not reading the receive buffer fast enough, |
// one or more received character have been dropped |
// - \b UART_OVERRUN_ERROR |
// <br>Overrun condition by UART. |
// A character already present in the UART UDR register was |
// not read by the interrupt handler before the next character arrived, |
// one or more received characters have been dropped. |
// - \b UART_FRAME_ERROR |
// <br>Framing Error by UART |
// |
extern unsigned int uart_getc(void); |
// |
// @brief Put byte to ringbuffer for transmitting via UART |
// @param data byte to be transmitted |
// @return none |
// |
// @brief Set Baudrate USART1 |
extern void SetBaudUart1(uint8_t Baudrate); |
// @brief Initialize USART1 (only available on selected ATmegas) @see uart_init |
extern void uart1_init(unsigned int baudrate); |
// @brief Get received byte of USART1 from ringbuffer. (only available on selected ATmega) @see uart_getc |
extern unsigned int uart1_getc(void); |
// @brief Put byte to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_putc |
//extern void uart1_putc(unsigned char data); |
extern int uart1_putc(unsigned char data); |
// @brief Put string to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts |
extern void uart1_puts(const char *s ); |
// @brief Put string from program memory to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts_p |
extern void uart1_puts_p(const char *s ); |
// @brief Macro to automatically put a string constant into program memory |
#define uart1_puts_P(__s) uart1_puts_p(PSTR(__s)) |
extern char *utoa1(char* buffer, const unsigned int size, unsigned int value); |
extern uint16_t uart1_available(void); |
extern int uart1_peek(void); |
// @brief Flush bytes waiting the receive buffer. Acutally ignores them. |
extern void uart1_flush(void); |
#endif // UART_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/uart/usart.c |
---|
0,0 → 1,732 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.c |
//# |
//# 3.04.2015 Cebra |
//# - chg: 2 weitere /r am Ende des gesendeten Strings angefügt, damit Funktion gleich wie im MK-Tool |
//# |
//# 24.01.2014 OG |
//# - add: Debug-Code fuer die ISR-RX Funktion |
//# schaltbar via define DEBUG_FC_COMMUNICATION in main.h - der Code |
//# kann somit drin bleiben |
//# - add: Source-History |
//############################################################################ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include "../cpu.h" |
#include <util/delay.h> |
#include <stdarg.h> |
#include "../main.h" |
#include "usart.h" |
#include "../lcd/lcd.h" |
#include "../timer/timer.h" |
#include "uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../osd/osd.h" |
uint8_t buffer[30]; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t ReceivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t RxDataLen = 0; |
volatile uint16_t stat_crc_error = 0; |
volatile uint16_t stat_overflow_error = 0; |
volatile uint8_t rx_byte; |
volatile uint8_t rxFlag = 0; |
#define UART_RXBUFSIZE 64 |
#define UART_NO_DATA 0x0100 /* no receive data available */ |
volatile static uint8_t rxbuf[UART_RXBUFSIZE]; |
volatile static uint8_t *volatile rxhead, *volatile rxtail; |
#ifdef DEBUG_FC_COMMUNICATION |
char *tmp = (char *)" "; |
#endif |
/* |
//----------------------------------------------------------------------------- |
// USART1 transmitter ISR |
ISR (USART1_TX_vect) |
{ |
static uint16_t ptr_txd1_buffer = 0; |
uint8_t tmp_tx1; |
if(!txd1_complete) // transmission not completed |
{ |
ptr_txd1_buffer++; // [0] was already sent |
tmp_tx1 = txd1_buffer[ptr_txd1_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx1 == '\r') || (ptr_txd1_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd1_buffer = 0; // reset txd pointer |
txd1_complete = TRUE; // stop transmission |
} |
UDR1 = tmp_tx1; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd1_buffer = 0; |
} |
*/ |
#ifdef USART_INT |
//----------------------------------------------------------------------------- |
// USART0 transmitter ISR |
ISR (USART_TX_vect) |
{ |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if(!txd_complete) // transmission not completed |
{ |
ptr_txd_buffer++; // [0] was already sent |
tmp_tx = txd_buffer[ptr_txd_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd_buffer = 0; // reset txd pointer |
txd_complete = TRUE; // stop transmission |
} |
UDR = tmp_tx; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd_buffer = 0; |
} |
#endif |
void SetBaudUart0(uint8_t Baudrate) |
{ |
switch (Baudrate) |
{ |
case Baud_2400: USART_Init( UART_BAUD_SELECT(2400,F_CPU) ); break; |
case Baud_4800: USART_Init( UART_BAUD_SELECT(4800,F_CPU) ); break; |
case Baud_9600: USART_Init( UART_BAUD_SELECT(9600,F_CPU) ); break; |
case Baud_19200: USART_Init( UART_BAUD_SELECT(19200,F_CPU) ); break; |
case Baud_38400: USART_Init( UART_BAUD_SELECT(38400,F_CPU) ); break; |
case Baud_57600: USART_Init( UART_BAUD_SELECT(57600,F_CPU) ); break; |
// case Baud_115200: USART_Init( UART_BAUD_SELECT(115200,F_CPU) ); break; |
// Macro erechnet falschen Wert (9,85 = 9) für 115200 Baud mit 20Mhz Quarz, zu grosse Abweichung |
//#warning "Baurate prüfen wenn kein 20 Mhz Quarz verwendet wird" |
case Baud_115200: USART_Init( 10 ); break; |
} |
} |
//----------------------------------------------------------------------------- |
// |
// |
//uint8_t uart_getc_nb(uint8_t *c) |
//{ |
// if (rxhead==rxtail) return 0; |
// *c = *rxtail; |
// if (++rxtail == (rxbuf + UART_RXBUFSIZE)) rxtail = rxbuf; |
// return 1; |
//} |
ISR (USART0_RX_vect) |
{ |
static uint16_t crc; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t c; |
// IdleTimer = 0; |
if (current_hardware == Wi232) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
// USART_putc (c); |
return; |
} |
if (current_hardware == MKGPS) |
{ |
// rx_byte = c; |
// rxFlag = 1; |
int diff; |
uint8_t c; |
c=UDR; |
diff = rxhead - rxtail; |
if (diff < 0) diff += UART_RXBUFSIZE; |
if (diff < UART_RXBUFSIZE -1) |
{ |
*rxhead = c; |
++rxhead; |
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf; |
}; |
return; |
} |
c = UDR; // catch the received byte |
if (OSD_active && Config.OSD_SendOSD) // Daten an SV2 senden |
uart1_putc(c); |
if (rxd_buffer_locked) |
return; // if rxd buffer is locked immediately return |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
{ |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
lcd_print( "#", MNORMAL ); // OG DEBUG |
#endif |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
} |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
{ |
if(c != '\r') // no termination character |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
else // termination character was received |
{ |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer-2]; |
crc -= rxd_buffer[ptr_rxd_buffer-1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
// compare checksum to transmitted checksum bytes |
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
{ // checksum valid |
// OG DEBUG: DEBUG_FC_COMMUNICATION (einstellbar in main.h) |
#ifdef DEBUG_FC_COMMUNICATION |
*tmp = rxd_buffer[2]; |
lcd_print( tmp, MNORMAL ); |
lcd_putc( 0,0, '[', MNORMAL ); |
lcd_putc( 1,0, rxd_buffer[2], MNORMAL ); |
lcd_putc( 2,0, ']', MNORMAL ); x |
if( rxd_buffer[2] != 'k' ) set_beep( 13, 0x0001, BeepNormal); |
if( rxd_buffer[2] == 'Q' ) set_beep( 150, 0x0080, BeepNormal); |
#endif |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
if (mode == rxd_buffer[2]) |
{ |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
} |
} |
else |
{ // checksum invalid |
stat_crc_error++; |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
ptr_rxd_buffer = 0; // reset rxd buffer pointer |
} |
} |
else // rxd buffer overrun |
{ |
stat_overflow_error++; |
ptr_rxd_buffer = 0; // reset rxd buffer |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
} |
//----------------------------------------------------------------------------- |
// Function: uart0_getc() |
// Purpose: return byte from ringbuffer |
// Returns: lower byte: received byte from ringbuffer |
// higher byte: last receive error |
//----------------------------------------------------------------------------- |
char USART_getc(void) |
{ |
char val; |
// while(rxhead==rxtail) ; |
if (rxhead==rxtail) |
return val=0; |
// IdleTimer = 0; |
val = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return val; |
} |
uint8_t uart_getc_nb(uint8_t *c) |
{ |
if (rxhead==rxtail) |
return 0; |
// IdleTimer = 0; |
*c = *rxtail; |
if (++rxtail == (rxbuf + UART_RXBUFSIZE)) |
rxtail = rxbuf; |
return 1; |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void USART_Init (unsigned int baudrate) |
{ |
// set clock divider |
// #undef BAUD |
// #define BAUD baudrate |
// #include <util/setbaud.h> |
// UBRRH = UBRRH_VALUE; |
// UBRRL = UBRRL_VALUE; |
//#ifndef F_CPU |
///* In neueren Version der WinAVR/Mfile Makefile-Vorlage kann |
// F_CPU im Makefile definiert werden, eine nochmalige Definition |
// hier wuerde zu einer Compilerwarnung fuehren. Daher "Schutz" durch |
// #ifndef/#endif |
// |
// Dieser "Schutz" kann zu Debugsessions führen, wenn AVRStudio |
// verwendet wird und dort eine andere, nicht zur Hardware passende |
// Taktrate eingestellt ist: Dann wird die folgende Definition |
// nicht verwendet, sondern stattdessen der Defaultwert (8 MHz?) |
// von AVRStudio - daher Ausgabe einer Warnung falls F_CPU |
// noch nicht definiert: */ |
//#warning "F_CPU war noch nicht definiert, wird nun nachgeholt mit 4000000" |
//#define F_CPU 18432000UL // Systemtakt in Hz - Definition als unsigned long beachten |
// Ohne ergeben sich unten Fehler in der Berechnung |
//#endif |
//#define BAUD 115200UL // Baudrate |
// |
//// Berechnungen |
//#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) // clever runden |
//#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) // Reale Baudrate |
//#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) // Fehler in Promille, 1000 = kein Fehler. |
// |
// |
//#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010)) |
// #error "Systematischer Fehler der Baudrate grösser 1% und damit zu hoch!" |
//#endif |
UBRRH = (unsigned char)(baudrate>>8); |
UBRRL = (unsigned char) baudrate; |
// UBRRH = (unsigned char)(BAUD_REAL>>8); |
// UBRRL = (unsigned char) BAUD_REAL; |
#if USE_2X |
UCSRA |= (1 << U2X); // enable double speed operation |
#else |
UCSRA &= ~(1 << U2X); // disable double speed operation |
#endif |
// set 8N1 |
#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega32__) |
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0); |
#else |
UCSRC = (1 << UCSZ1) | (1 << UCSZ0); |
#endif |
UCSRB &= ~(1 << UCSZ2); |
// flush receive buffer |
while ( UCSRA & (1 << RXC) ) UDR; |
UCSRB |= (1 << RXEN) | (1 << TXEN); |
#ifdef USART_INT |
UCSRB |= (1 << RXCIE) | (1 << TXCIE); |
#else |
UCSRB |= (1 << RXCIE); |
#endif |
rxhead = rxtail = rxbuf; |
} |
//----------------------------------------------------------------------------- |
// disable the txd pin of usart |
void USART_DisableTXD (void) |
{ |
#ifdef USART_INT |
UCSRB &= ~(1 << TXCIE); // disable TX-Interrupt |
#endif |
UCSRB &= ~(1 << TXEN); // disable TX in USART |
DDRB &= ~(1 << DDB3); // set TXD pin as input |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
} |
//----------------------------------------------------------------------------- |
// enable the txd pin of usart |
void USART_EnableTXD (void) |
{ |
DDRB |= (1 << DDB3); // set TXD pin as output |
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin |
UCSRB |= (1 << TXEN); // enable TX in USART |
#ifdef USART_INT |
UCSRB |= (1 << TXCIE); // enable TX-Interrupt |
#endif |
} |
//----------------------------------------------------------------------------- |
// short script to directly send a request thorugh usart including en- and disabling it |
// where <address> is the address of the receipient, <label> is which data set to request |
// and <ms> represents the milliseconds delay between data |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms) |
{ |
USART_EnableTXD (); // re-enable TXD pin |
unsigned char mstenth = ms/10; |
SendOutData(cmd, addr, 1, &mstenth, 1); |
// wait until command transmitted |
while (txd_complete == FALSE); |
USART_DisableTXD (); // disable TXD pin again |
} |
//----------------------------------------------------------------------------- |
// |
void USART_putc (char c) |
{ |
#ifdef USART_INT |
#else |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = c; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts (char *s) |
{ |
#ifdef USART_INT |
#else |
while (*s) |
{ |
USART_putc (*s); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void USART_puts_p (const char *s) |
{ |
#ifdef USART_INT |
#else |
while (pgm_read_byte(s)) |
{ |
USART_putc (pgm_read_byte(s)); |
s++; |
} |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
{ |
va_list ap; |
uint16_t pt = 0; |
uint8_t a,b,c; |
uint8_t ptr = 0; |
uint16_t tmpCRC = 0; |
uint8_t *pdata = 0; |
int len = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
va_start(ap, numofbuffers); |
if(numofbuffers) |
{ |
pdata = va_arg (ap, uint8_t*); |
len = va_arg (ap, int); |
ptr = 0; |
numofbuffers--; |
} |
while(len) |
{ |
if(len) |
{ |
a = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
a = 0; |
if(len) |
{ |
b = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
b = 0; |
if(len) |
{ |
c = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else |
c = 0; |
txd_buffer[pt++] = '=' + (a >> 2); |
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
txd_buffer[pt++] = '=' + ( c & 0x3f); |
} |
va_end(ap); |
for(a = 0; a < pt; a++) |
{ |
tmpCRC += txd_buffer[a]; |
} |
tmpCRC %= 4096; |
txd_buffer[pt++] = '=' + tmpCRC / 64; |
txd_buffer[pt++] = '=' + tmpCRC % 64; |
txd_buffer[pt++] = '\r'; |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_buffer[pt++] = '\r'; // Add 3.4.2015 Cebra |
txd_complete = FALSE; |
#ifdef USART_INT |
UDR = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
#else |
for(a = 0; a < pt; a++) |
{ |
loop_until_bit_is_set(UCSRA, UDRE); |
UDR = txd_buffer[a]; |
} |
txd_complete = TRUE; |
#endif |
} |
//----------------------------------------------------------------------------- |
// |
void Decode64 (void) |
{ |
uint8_t a,b,c,d; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = ReceivedBytes - 6; |
while (len) |
{ |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
//if(ptrIn > ReceivedBytes - 3) break; |
if (len--) |
rxd_buffer[ptrOut++] = (a << 2) | (b >> 4); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2); |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = ((c & 0x03) << 6) | d; |
else |
break; |
} |
pRxData = &rxd_buffer[3]; |
RxDataLen = ptrOut - 3; |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToNC (void) |
{ |
if(hardware == NC) |
{ |
// switch to NC |
USART_putc (0x1b); |
USART_putc (0x1b); |
USART_putc (0x55); |
USART_putc (0xaa); |
USART_putc (0x00); |
current_hardware = NC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
//----------------------------------------------------------------------------- |
// |
void SwitchToWi232 (void) |
{ |
// if(hardware == NC) |
{ |
// switch to Wi232 |
current_hardware = Wi232; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToFC (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to FC |
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = FC; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToMAG (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MK3MAG |
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MK3MAG; |
_delay_ms (50); |
} |
} |
//----------------------------------------------------------------------------- |
// |
void SwitchToGPS (void) |
{ |
uint8_t cmd; |
if (current_hardware == NC) |
{ |
// switch to MKGPS |
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS |
SendOutData('u', ADDRESS_NC, 1, &cmd, 1); |
current_hardware = MKGPS; |
_delay_ms (50); |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/uart/usart.h |
---|
0,0 → 1,162 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY usart.h |
//# |
//# 24.01.2014 OG |
//# - fix: RXD_BUFFER_LEN und TXD_BUFFER_LEN vergroessert 180 auf 230 wegen |
//# neuer FC-Version (FC v2.03) |
//# - add: Source-History |
//############################################################################ |
#ifndef _USART_H |
#define _USART_H |
//-------------------------------------------------------------- |
// |
#ifndef FALSE |
#define FALSE 0 |
#endif |
#ifndef TRUE |
#define TRUE 1 |
#endif |
// addresses |
#define ADDRESS_ANY 0 |
#define ADDRESS_FC 1 |
#define ADDRESS_NC 2 |
#define ADDRESS_MAG 3 |
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
//#define TXD_BUFFER_LEN 180 |
//#define RXD_BUFFER_LEN 180 |
#define TXD_BUFFER_LEN 230 |
#define RXD_BUFFER_LEN 230 |
// Baud rate of the USART |
#define USART_BAUD 57600 |
//#define USART_BAUD 125000 |
//-------------------------------------------------------------- |
// |
extern uint8_t buffer[30]; |
extern volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd_complete; |
extern volatile uint8_t txd1_buffer[TXD_BUFFER_LEN]; |
extern volatile uint8_t txd1_complete; |
extern volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
extern volatile uint8_t rxd_buffer_locked; |
extern volatile uint8_t ReceivedBytes; |
extern volatile uint8_t *pRxData; |
extern volatile uint8_t RxDataLen; |
extern volatile uint16_t stat_crc_error; |
extern volatile uint16_t stat_overflow_error; |
extern volatile uint8_t rxFlag; |
extern volatile uint8_t rx_byte; |
//-------------------------------------------------------------- |
// |
void SetBaudUart0(uint8_t Baudrate); |
void USART_Init (unsigned int baudrate); |
void USART_DisableTXD (void); |
void USART_EnableTXD (void); |
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms); |
void USART_putc (char c); |
void USART_puts (char *s); |
void USART_puts_p (const char *s); |
extern char USART_getc(void); |
void SendOutData (uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...); // uint8_t *pdata, uint8_t len, ... |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, uint8_t *pdata, uint8_t len); // uint8_t *pdata, uint8_t len, ... |
void Decode64 (void); |
void SwitchToNC (void); |
void SwitchToFC (void); |
void SwitchToMAG (void); |
void SwitchToGPS (void); |
void SwitchToWi232 (void); |
void debug1(void); |
uint8_t uart_getc_nb(uint8_t*); |
//-------------------------------------------------------------- |
//Anpassen der seriellen Schnittstellen Register |
#define USART_RXC_vect USART0_RX_vect |
//-------------------------------------------------------------- |
#define UCSRA UCSR0A |
#define UCSRB UCSR0B |
#define UCSRC UCSR0C |
#define UDR UDR0 |
#define UBRRL UBRR0L |
#define UBRRH UBRR0H |
// UCSRA |
#define RXC RXC0 |
#define TXC TXC0 |
#define UDRE UDRE0 |
#define FE FE0 |
#define UPE UPE0 |
#define U2X U2X0 |
#define MPCM MPCM0 |
// UCSRB |
#define RXCIE RXCIE0 |
#define TXCIE TXCIE0 |
#define UDRIE UDRIE0 |
#define TXEN TXEN0 |
#define RXEN RXEN0 |
#define UCSZ2 UCSZ02 |
#define RXB8 RXB80 |
#define TXB8 TXB80 |
// UCSRC |
#define UMSEL1 UMSEL01 |
#define UMSEL0 UMSEL00 |
#define UPM1 UPM01 |
#define UPM0 UPM00 |
#define USBS USBS0 |
#define UCSZ1 UCSZ01 |
#define UCSZ0 UCSZ00 |
#define UCPOL UCPOL0 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/i2cmaster.h |
---|
0,0 → 1,178 |
#ifndef _I2CMASTER_H |
#define _I2CMASTER_H 1 |
/************************************************************************* |
* Title: C include file for the I2C master interface |
* (i2cmaster.S or twimaster.c) |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: i2cmaster.h,v 1.10 2005/03/06 22:39:57 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device |
* Usage: see Doxygen manual |
**************************************************************************/ |
#ifdef DOXYGEN |
/** |
@defgroup pfleury_ic2master I2C Master library |
@code #include <i2cmaster.h> @endcode |
@brief I2C (TWI) Master Software Library |
Basic routines for communicating with I2C slave devices. This single master |
implementation is limited to one bus master on the I2C bus. |
This I2c library is implemented as a compact assembler software implementation of the I2C protocol |
which runs on any AVR (i2cmaster.S) and as a TWI hardware interface for all AVR with built-in TWI hardware (twimaster.c). |
Since the API for these two implementations is exactly the same, an application can be linked either against the |
software I2C implementation or the hardware I2C implementation. |
Use 4.7k pull-up resistor on the SDA and SCL pin. |
Adapt the SCL and SDA port and pin definitions and eventually the delay routine in the module |
i2cmaster.S to your target when using the software I2C implementation ! |
Adjust the CPU clock frequence F_CPU in twimaster.c or in the Makfile when using the TWI hardware implementaion. |
@note |
The module i2cmaster.S is based on the Atmel Application Note AVR300, corrected and adapted |
to GNU assembler and AVR-GCC C call interface. |
Replaced the incorrect quarter period delays found in AVR300 with |
half period delays. |
@author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
@par API Usage Example |
The following code shows typical usage of this library, see example test_i2cmaster.c |
@code |
#include <i2cmaster.h> |
#define Dev24C02 0xA2 // device address of EEPROM 24C02, see datasheet |
int main(void) |
{ |
unsigned char ret; |
i2c_init(); // initialize I2C library |
// write 0x75 to EEPROM address 5 (Byte Write) |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_write(0x75); // write value 0x75 to EEPROM |
i2c_stop(); // set stop conditon = release bus |
// read previously written value back from EEPROM address 5 |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_rep_start(Dev24C02+I2C_READ); // set device address and read mode |
ret = i2c_readNak(); // read one byte from EEPROM |
i2c_stop(); |
for(;;); |
} |
@endcode |
*/ |
#endif /* DOXYGEN */ |
/**@{*/ |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
#include <avr/io.h> |
/** defines the data direction (reading from I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_READ 1 |
/** defines the data direction (writing to I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_WRITE 0 |
/** |
@brief initialize the I2C master interace. Need to be called only once |
@param void |
@return none |
*/ |
extern void i2c_init(void); |
/** |
@brief Terminates the data transfer and releases the I2C bus |
@param void |
@return none |
*/ |
extern void i2c_stop(void); |
/** |
@brief Issues a start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_start(unsigned char addr); |
/** |
@brief Issues a repeated start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_rep_start(unsigned char addr); |
/** |
@brief Issues a start condition and sends address and transfer direction |
If device is busy, use ack polling to wait until device ready |
@param addr address and transfer direction of I2C device |
@return none |
*/ |
extern void i2c_start_wait(unsigned char addr); |
/** |
@brief Send one byte to I2C device |
@param data byte to be transfered |
@retval 0 write successful |
@retval 1 write failed |
*/ |
extern unsigned char i2c_write(unsigned char data); |
/** |
@brief read one byte from the I2C device, request more data from device |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readAck(void); |
/** |
@brief read one byte from the I2C device, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readNak(void); |
/** |
@brief read one byte from the I2C device |
Implemented as a macro, which calls either i2c_readAck or i2c_readNak |
@param ack 1 send ack, request more data from device<br> |
0 send nak, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_read(unsigned char ack); |
#define i2c_read(ack) (ack) ? i2c_readAck() : i2c_readNak(); |
/**@}*/ |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/menuctrl.c |
---|
0,0 → 1,1501 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.c |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 31.05.2014 OG |
//# - chg: MenuCtrl_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 14.05.2014 OG |
//# - chg: include "../mk/paramset.h" geaendert auf "../mksettings/paramset.h" |
//# - chg: include "../mk/mkparameters.h" geaendert auf "../mksettings/mkparameters.h" |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# - chg: _MenuCtrl_Error() - auf PKT_Popup_P() umgestellt, Layout geaendert und |
//# einen neuen Fehler (NOPARENTMENU) hinzugefuegt |
//# |
//# 09.05.2014 OG |
//# - fix: MenuCtrl_Refresh() Anzeige von deaktivierten MK-Parametern (Favoriten) |
//# Wenn MK-Paramketer-Menueeintraege deaktiviert sind werden sie vom |
//# aktuellen MK-Parameterset nicht unterstuetzt - dennoch wurde der |
//# aktuelle Wert des MK-Parameters im Menuepunkt (ganz rechts) angezeigt. |
//# Das ist jetzt behoben - es wird kein Wert angezeigt (stattdessen "***") |
//# und auch keine entsprechende editParam-Funktionen aufgerufen. |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - chg: MenuCtrl_Refresh() Keyline leicht angepasst |
//# - chg: 'not possible' Anzeige (bei deaktiviertem Item) umgestellt auf |
//# PKT_Popup_P() |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - chg: MenuCtrl_Control() |
//# -> Unterstuetzung fuer: MenuCtrl_SetMove() - Menueeintraege verschieben |
//# - chg: MenuCtrl_Refresh() - Anpassung Slider fuer MenuCtrl_SetDelete() |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# - add: _MenuCtrl_SwapItem() |
//# - add: die Datenstruktur vom Menue wurde erweitert um die Eigenschaften |
//# canMove und canDelete |
//# |
//# 17.04.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 22 auf 28 erhoeht |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 29.03.2014 OG |
//# - chg: default MENUDEFAULT_SHOWBATT auf true umgestellt (=PKT-Batterie anzeigen) |
//# - chg: MenuCtrl_Control() umgestellt auf clear_key_all() |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() - fuegt eine Trennlinie im Menue ein |
//# - chg: MenuCtrl_Refresh() Codeoptimierung bzgl. topspace/Zeile loeschen/Slider |
//# - fix: MenuCtrl_Control() es wurde zuoft ein Screen-Refresh durchgefuehrt |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - chg: MenuCtrl_Refresh() Unterstuetzung fuer MENU_PARAMEDIT |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: paramset.h und mkparameters.h |
//# |
//# 04.03.2014 OG |
//# - fix: MenuCtrl_Refresh() bei einem blauen Display (das ist wohl schneller |
//# in der Anzeige als das S/W-Display) flackerte der Menue-Slider |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# - chg: MenuCtrl_Refresh() angepasst auf MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 01.02.2014 OG |
//# - chg: MENUCTRL_MAXITEMS von 16 auf 22 fuer MK_parameters |
//# - fix: _MenuCtrl_Error() Anzeigezeit von Menu-Errors auf 8 Sekunden verlaengert |
//# und Anzeige von "menuctrl.c" |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() - Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts) |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - chg: MenuCtrl_Refresh() Anpassungen und Optimierung |
//# - add: MenuCtrl_ItemMark() |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - fix: MenuCtrl_Control() Eventcode Rueckgabe bei KEY_ENTER |
//# |
//# 22.05.2013 OG |
//# - fix: nach Aufruf einer Menuefunktion in MenuCtrl_Control() jetzt |
//# get_key_press(KEY_ALL) |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 20.05.2013 OG |
//# - chg: MenuCtrl_Control() wieder mit get_key_press(KEY_ALL) ergaenzt |
//# wenn aufgerufen mit MENUCTRL_EVENT oder erster Aufruf mit RETURN |
//# |
//# 20.05.2013 OG |
//# - chg: Space am Anfang der Titelanzeige |
//# |
//# 19.05.2013 OG |
//# - add: define MENU_SHOWLEVEL mit dem der aktuelle Menuelevel in der |
//# Titelzeile angezeigt werden kann |
//# - fix: Redraw-Logik bei MENUCTRL_RETURN |
//# - chg: MenuCtrl_Control() erweitert um PKT_Update() |
//# |
//# 18.05.2013 OG - NEU |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include <avr/wdt.h> |
#include <util/delay.h> |
#include <string.h> // memset |
#include <stdarg.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../lipo/lipo.h" |
#include "../menu.h" |
#include "../pkt/pkt.h" |
#include "../mksettings/paramset.h" |
#include "../mksettings/mkparameters.h" |
#include "menuctrl.h" |
//############################################################################################# |
//# internal defines & structs |
//############################################################################################# |
#define MENU_SHOWLEVEL false // Anzeige der Menue-Verschachtelungstiefe in der Titelzeile |
// zeigt dem Benutzer wie weit er wieder zurueckspringen muss |
// um in das Hauptmenue zu kommen |
#define MENUDEFAULT_CYCLE false // cycle: wenn Menue am Ende dann wieder zum Anfang |
#define MENUDEFAULT_SHOWBATT true // showbatt: zeigt PKT-Batterie im Menuetitel ganz rechts |
#define MENUDEFAULT_BEEP true // beep: wenn Cursor ueber Ende/Anfang |
#define MENUCTRL_MAXMENUES 6 // Anzahl max. verschachlteter Menues |
//#define MENUCTRL_MAXITEMS 22 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
//#define MENUCTRL_MAXITEMS 28 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define MENUCTRL_MAXITEMS 34 // Anzahl max. Menu-Items pro Menue (verbraucht Speicher) |
#define ERROR_NOMENU 1 // Errorcode |
#define ERROR_MAXMENU 2 // Errorcode |
#define ERROR_MAXITEM 3 // Errorcode |
#define ERROR_NOITEM 4 // Errorcode |
#define ERROR_NOPARENTMENU 5 // Errorcode |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
const char *text; // Tastentext |
void (*func)(void); // ggf. Funktions-Pointer (0=keine Funktion) |
} menue_key_t; |
//--------------------------- |
// typedef: einzelnes Menueitem |
//--------------------------- |
typedef struct |
{ |
uint8_t id; |
uint8_t type; // MENU_ITEM, MENU_SUB, MENU_PARAMEDIT |
uint8_t disabled; // true / false (deaktiviert einen Menuepunkt) |
uint8_t mark; // true / false (markiert einen Menuepunkt Links '*') |
uint8_t markR; // true / false (markiert einen Menuepunkt Rechts) |
uint8_t textPGM; // true / false (true = text in PROGMEN; false = text im RAM) |
uint8_t textcount; // 1 .. NUM_LANG |
const char *text[NUM_LANG]; // de, en, nl (NUM_LANG aus messages.h) |
void (*func)(void); |
} menue_item_t; |
//--------------------------- |
// typedef: Menueitem Liste |
//--------------------------- |
typedef struct |
{ |
uint8_t active; // aktives item |
uint8_t itemcount; // Anzahl Items |
uint8_t scroll_pos; // aktuelle obere Anzeigezeile |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
uint8_t topspace; // obere Leerzeilen - default=0 (Vorsicht! ggf. nur bedingt einsetzbar bei Menues die ohne scrollen auskommen!) |
uint8_t firstredraw; // true / false (flag fuer ein erstes redraw bei MENUCTRL_RETURN) |
uint8_t cycle; // true / false |
uint8_t showbatt; // true / false |
uint8_t beep; // true / false |
uint8_t canMove; // true / false |
uint8_t canDelete; // false/0 = kein entfernen; true/1 = entfernen mit Nachfrage; 2 = entfernen ohne Nachfrage |
uint8_t titlePGM; // true / false |
const char *title; // Menuetitel (oberste Zeile) (ggf. Multisprache machen?) |
uint8_t lastkey; // verwendet von GetKey() |
menue_key_t key_enter; |
menue_key_t key_enter_long; |
menue_key_t key_esc; |
menue_key_t key_esc_long; |
menue_item_t item[MENUCTRL_MAXITEMS]; |
} menue_t; |
uint8_t showlevel = MENU_SHOWLEVEL; // Anzeige Menulevel in der Titelanzeige |
int8_t midx = -1; // aktives Menue; -1 = kein Menue |
menue_t menu[MENUCTRL_MAXMENUES]; // Stack fuer n geschachltete Menues |
//############################################################################################# |
//# PRIVAT funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Error( uint8_t error ) |
{ |
const char *pStr = NULL; |
switch( error ) |
{ |
case ERROR_NOMENU: pStr = PSTR("NOMENU"); break; |
case ERROR_MAXMENU: pStr = PSTR("MAXMENU"); break; |
case ERROR_MAXITEM: pStr = PSTR("MAXITEM"); break; |
case ERROR_NOITEM: pStr = PSTR("NOITEM"); break; |
case ERROR_NOPARENTMENU: pStr = PSTR("NOPARENTMENU"); break; |
} |
set_beep( 1000, 0x000f, BeepNormal); // Beep Error |
PKT_Popup_P( 10000, PSTR("menuctrl.c"), 0, PSTR("* ERROR *"), pStr ); // 10000 = max. 100 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
int8_t _MenuCtrl_FindItem( uint8_t itemid ) |
{ |
int8_t i; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return -1; } |
for( i=0; i<menu[midx].itemcount; i++) |
{ |
if( itemid == menu[midx].item[i].id ) |
{ |
return i; // found: return index |
} |
} |
return -1; // not found |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_CalcDisplayPos( void ) |
{ |
int8_t i; |
i = menu[midx].active - menu[midx].display_pos; |
if( i < 0 ) |
menu[midx].display_pos = menu[midx].active; |
else if( i > 5 ) |
menu[midx].display_pos = (menu[midx].active - 5); |
} |
//-------------------------------------------------------------- |
// INTERN |
//-------------------------------------------------------------- |
void _MenuCtrl_Beep( void ) |
{ |
if( menu[midx].beep ) |
set_beep( 25, 0xffff, BeepNormal ); |
} |
//-------------------------------------------------------------- |
// INTERN |
// Dreieckstausch von zwei Menuepunkten. |
// Wird verwendet um Menueitems zu verschieben. |
//-------------------------------------------------------------- |
void _MenuCtrl_SwapItem( uint8_t itemindex_1, uint8_t itemindex_2 ) |
{ |
menue_item_t tmpItem; |
memcpy( &tmpItem , &menu[midx].item[itemindex_1], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_1] , &menu[midx].item[itemindex_2], sizeof(menue_item_t) ); |
memcpy( &menu[midx].item[itemindex_2] , &tmpItem, sizeof(menue_item_t) ); |
} |
//############################################################################################# |
//# PUBLIC funcs |
//############################################################################################# |
//-------------------------------------------------------------- |
// MenuCtrl_Create() |
//-------------------------------------------------------------- |
void MenuCtrl_Create( void ) |
{ |
if( midx >= MENUCTRL_MAXMENUES) { _MenuCtrl_Error( ERROR_MAXMENU ); return; } |
midx++; |
memset( &menu[midx], 0, sizeof(menue_t) ); |
menu[midx].cycle = MENUDEFAULT_CYCLE; |
menu[midx].showbatt = MENUDEFAULT_SHOWBATT; |
menu[midx].beep = MENUDEFAULT_BEEP; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Destroy() |
//-------------------------------------------------------------- |
void MenuCtrl_Destroy( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
midx--; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ShowLevel( value ) |
// |
// Diese Einstellung ist Global und wirkt sich direkt auf alle |
// Menues aus! |
// Man kann das also nicht fuer einzelne Menues ein-/ausschalten. |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ShowLevel( uint8_t value ) |
{ |
showlevel = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle_P( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle_P( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = true; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitle( *title ) |
// |
// Parameter: |
// title: Menuetitel; Zeiger auf String im RAM |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitle( const char *title ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].title = title; |
menu[midx].titlePGM = false; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTitleFromParentItem() |
// |
// setzte anhand des uebergeordneten Menuepunktes (dem aufrufenden |
// Menuepunkt) den Titel des neuen Menues |
// |
// wird u.a. verwendet in setup.c |
//-------------------------------------------------------------- |
void MenuCtrl_SetTitleFromParentItem( void ) |
{ |
uint8_t lang; |
const char *pStr; |
uint8_t mparent; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( midx < 1) { _MenuCtrl_Error( ERROR_NOPARENTMENU ); return; } |
mparent = midx-1; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[mparent].item[ menu[mparent].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Text vom uebergeordneten Menuepunkt |
//-------------------------- |
pStr = menu[mparent].item[ menu[mparent].active ].text[lang]; |
//-------------------------- |
// Titel setzen |
//-------------------------- |
if( menu[mparent].item[ menu[mparent].active ].textPGM ) |
{ |
MenuCtrl_SetTitle_P( pStr ); |
} |
else |
{ |
MenuCtrl_SetTitle( pStr ); |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetCycle( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetCycle( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].cycle = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetBeep( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetBeep( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].beep = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetMove( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege nach |
// oben oder nach unten zu verschieben. |
// Zum verschieden eines Menueeintrages muessen die PLUS/MINUS Tasten |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetMove( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canMove = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetDelete( value ) |
// |
// Schaltet in einem Menue die Moeglichkeit ein Menueeintraege zu loeschen. |
// Zum loeschen eines Menueeintrages muss dann die ganz rechte 'OK' Taste |
// lange gedrueckt werden. |
// |
// Parameter: |
// value: true / false (Default: false) |
//-------------------------------------------------------------- |
void MenuCtrl_SetDelete( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].canDelete = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetShowBatt( value ) |
// |
// Parameter: |
// value: true / false |
//-------------------------------------------------------------- |
void MenuCtrl_SetShowBatt( uint8_t value ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].showbatt = value; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetTopSpace( n ) |
// |
// fuegt oben im Menue n Leerzeichen ein um Abstand vom Menuetitel |
// zu erhalten |
// |
// ACHTUNG! Das funktioniert nur mit Menues die nicht Scrollen! |
// -> also weniger als 6 Eintraege haben (je nach Anzahl von TopSpace) |
// |
// ACHTUNG! Keine Absicherung bzgl. obigem Warnpunkt! Das kann man |
// zwar machen - ist aber aktuell nicht so! |
// |
// Parameter: |
// value: 0..n Anzahl der Leerzeilen ueber dem Menue |
//-------------------------------------------------------------- |
void MenuCtrl_SetTopSpace( uint8_t n ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
menu[midx].topspace = n; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_SetKey( key, *keytext, *keyfunc ) |
// |
// Parameter: |
// key : KEY_ENTER, KEY_ENTER_LONG, KEY_ESC, KEY_ESC_LONG |
// keytext: Pointer auf Text PROGMEM oder NOTEXT (=0) |
// keyfunc: Pointer auf Funktion oder NOFUNC (=0) |
//-------------------------------------------------------------- |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein aktives Menue |
if( key == KEY_ENTER ) |
{ |
menu[midx].key_enter.active = true; |
menu[midx].key_enter.text = keytext; |
menu[midx].key_enter.func = keyfunc; |
} |
if( key == KEY_ENTER_LONG ) |
{ |
menu[midx].key_enter_long.active = true; |
menu[midx].key_enter_long.text = keytext; |
menu[midx].key_enter_long.func = keyfunc; |
} |
if( key == KEY_ESC ) |
{ |
menu[midx].key_esc.active = true; |
menu[midx].key_esc.text = keytext; |
menu[midx].key_esc.func = keyfunc; |
} |
if( key == KEY_ESC_LONG ) |
{ |
menu[midx].key_esc_long.active = true; |
menu[midx].key_esc_long.text = keytext; |
menu[midx].key_esc_long.func = keyfunc; |
} |
} |
//-------------------------------------------------------------- |
// key = MenuCtrl_GetKey() |
// |
// Rueckgabe: |
// key: z.B. KEY_ENTER oder KEY_ENTER_LONG |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetKey( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].lastkey; |
} |
//-------------------------------------------------------------- |
// menuindex = MenuCtrl_GetMenuIndex() |
// |
// gibt den aktuellen Menuindex zurueck |
// |
// Rueckgabe: |
// menuindex: <0 = keine Menue (-1) |
// =0 = Hauptmenue (Toplevel) |
// >0 = Untermenue Level |
//-------------------------------------------------------------- |
int8_t MenuCtrl_GetMenuIndex( void ) |
{ |
return midx; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemId() |
// |
// gibt die itemID des aktuell angewaehlten Menuepunktes zurueck |
// |
// Rueckgabe: |
// itemid: |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemId( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].id; |
} |
//-------------------------------------------------------------- |
// itemid = MenuCtrl_GetItemIdByIndex( index ) |
// |
// damit koennen die aktuell vorhanden itemID's der Menuepunkte |
// ermittelt wenn im Menue via SetMove oder SetDelete Menuepunkte |
// durch den Benutzer verschoben oder geloescht wurden. |
// |
// Parameter: |
// index: von 0..itemcount-1 |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
if( (menu[midx].itemcount == 0) || (index > menu[midx].itemcount-1) ) return 0; // index ausserhalb aktiver Menuepunkte |
return menu[midx].item[ index ].id; |
} |
//-------------------------------------------------------------- |
// num = MenuCtrl_GetItemCount() |
// |
// gibt die Anzahl der Menuepunkte zurueck |
// |
// Rueckgabe: |
// num: Anzahl der Menuepunkte |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_GetItemCount( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].itemcount; |
} |
//-------------------------------------------------------------- |
// textPtr = MenuCtrl_GetItemText() |
// |
// gibt einen Pointer auf den Text des aktuellen Menuepunktes zurueck |
// |
// ACHTUNG! Der Pointer kann auf RAM oder auch auf PGM zeigen! |
// |
// Um herauszufinden auf welchen Bereich der Pointer zeigt kann |
// MenuCtrl_IsItemTextPGM() verwendet werden. |
// |
// Rueckgabe: |
// textPtr: in RAM oder PGM |
//-------------------------------------------------------------- |
const char * MenuCtrl_GetItemText( void ) |
{ |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[ menu[midx].active ].textcount == 1) ? 0 : Config.DisplayLanguage; |
return menu[midx].item[ menu[midx].active ].text[lang]; |
} |
//-------------------------------------------------------------- |
// isPGM = MenuCtrl_IsItemTextPGM() |
// |
// gibt true/false zurueck je nachdem der Text von MenuCtrl_GetItemText() |
// im Progmem (=true) oder im RAM (=false) ist |
// |
// Rueckgabe: |
// isPGM: true / false (true = text in PROGMEN; false = text im RAM) |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_IsItemTextPGM( void ) |
{ |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein aktives Menue |
return menu[midx].item[ menu[midx].active ].textPGM; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemSelect( itemid ) |
// |
// aktiviert einen Menuepunkt / setzt den Menue-Cursor darauf |
// |
// die Funktion ist nicht durchgetestet, funktioniert aber |
// in MKSettings_Menu() / mkparameters.c |
// |
// es kann sein das es zu Problemen kommt wenn das Menue |
// scrollt (mehr als 6 Menueeintraege) |
// |
// Parameter: |
// itemid: ID des Menuitems |
//-------------------------------------------------------------- |
void MenuCtrl_ItemSelect( uint8_t itemid ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].active = i; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemActive( itemid, value ) |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].disabled = !value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMark( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem '*' am Anfang (Links). |
// Wird u.a. von BT_SelectDevice() (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].mark = value; |
} |
} |
//-------------------------------------------------------------- |
// MenuCtrl_ItemMarkR( itemid, value ) |
// |
// Markiert einen Menueeintrag mit einem Haken am ENDE (Rechts). |
// Wird u.a. von ... (setup.c) verwendet |
// |
// Parameter: |
// itemid: ID des Menuitems |
// value : true / false |
//-------------------------------------------------------------- |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ) |
{ |
int8_t i; |
i = _MenuCtrl_FindItem( itemid ); |
if( i >= 0 ) |
{ |
menu[midx].item[i].markR = value; |
} |
} |
//-------------------------------------------------------------- |
// INTERN |
// |
// fuer: MenuCtrl_PushML_P(), MenuCtrl_Push() |
//-------------------------------------------------------------- |
void _MenuCtrl_Push( uint8_t useprogmem, uint8_t numlang, uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), ... ) |
{ |
va_list ap; |
uint8_t idx; |
uint8_t i; |
if( midx < 0 ) { _MenuCtrl_Error( ERROR_NOMENU ); return; } |
if( menu[midx].itemcount >= MENUCTRL_MAXITEMS ) { _MenuCtrl_Error( ERROR_MAXITEM ); return; } |
//if( numlang > NUM_LANG ) { _MenuCtrl_Error( ERROR_MAXLANG ); return; } |
idx = menu[midx].itemcount; |
menu[midx].item[idx].id = itemid; |
menu[midx].item[idx].type = itemtype; // MENU_ITEM, MENU_SUB |
menu[midx].item[idx].mark = false; |
menu[midx].item[idx].func = menufunc; |
menu[midx].item[idx].textPGM = useprogmem; |
menu[midx].item[idx].textcount= numlang; |
va_start(ap, menufunc); |
for( i=0; i<numlang; i++) |
{ |
menu[midx].item[idx].text[i] = va_arg( ap, const char *); |
} |
va_end(ap); |
menu[midx].itemcount++; |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2_P( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// PROGMEN Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf PROGMEM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushML2( itemid, itemtype, *menufunc, *text_de, *text_en, *text_nl ) |
// |
// MultiLanguage Texte fuer 2 Sprachen (DE, EN) |
// RAM Version |
// |
// Parameter: |
// itemid : ID des Menueitems |
// itemtype: MENU_ITEM oder MENU_SUB (bei MENU_SUB wird noch ein ">" angezeigt) |
// menufunc: Pointer auf Funktion oder NOFUNC (=0) wenn keine Funktion |
// texte : alles Zeiger auf RAM |
//-------------------------------------------------------------- |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, NUM_LANG, itemid, itemtype, menufunc, text_de, text_en ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( false, 1, itemid, itemtype, menufunc, text ); |
} |
//-------------------------------------------------------------- |
// Spezialfunktion fuer mkparameters.c ! |
// |
// -> siehe: mkparameters.c / Menu_EditCategory() |
// |
// ACHTUNG! |
// Die externe Variable paramEditItem muss in mkparameters.c |
// vor Aufruf dieser Funktion richtig initialisiert sein! |
// Ich habe darauf verzichtet hier nochmal ein find_paramEditItem() |
// aufzurufen um Speicherplatz zu sparen. |
//-------------------------------------------------------------- |
void MenuCtrl_PushParamEdit( uint8_t paramID ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 2, paramID, MENU_PARAMEDIT, NOFUNC, paramEditItem.title_de, paramEditItem.title_en ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparator() |
// |
// fuegt eine Trennlinie im Menue ein |
// (die itemID ist dabei 0) |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparator( void ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, 0, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_PushSeparatorID( itemId ) |
// |
// fuegt eine Trennlinie im Menue ein mit einer eigenen itemID |
//-------------------------------------------------------------- |
void MenuCtrl_PushSeparatorID( uint8_t itemid ) |
{ |
//_MenuCtrl_Push( useprogmem, numlang, itemid, itemtype, void (*menufunc)(void), ... ) |
_MenuCtrl_Push( true, 0, itemid, MENU_SEPARATOR, NOFUNC ); |
} |
//-------------------------------------------------------------- |
// MenuCtrl_Refresh( mode ) |
// |
// Parameter: |
// mode: MENU_REDRAW || MENU_REFRESH |
//-------------------------------------------------------------- |
void MenuCtrl_Refresh( uint8_t mode ) |
{ |
uint8_t y; |
uint8_t item; |
uint8_t sh; |
uint8_t sy; |
uint8_t actchar; |
uint8_t markchar; |
uint8_t markcharR; |
uint8_t lang; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return; } // kein Menue vorhanden |
if( mode == MENU_REDRAW ) |
{ |
//-------------------------- |
// Clear |
//-------------------------- |
lcd_frect( 0, 8, 6, 47, 0); // Clear: ganz linke Spalte des Sliders |
lcd_frect( 127-2, 0, 2, 63, 0); // Clear: ganz rechts 2 Pixel |
//-------------------------- |
// Titel |
//-------------------------- |
if( menu[midx].title != 0 ) |
{ |
if( showlevel ) |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %19s"), menu[midx].title ); |
writex_ndigit_number_u( 0, 0, midx, 1, 0, MINVERS, 1,0); // Menuelevel zeigen |
} |
else |
{ |
if( menu[midx].titlePGM ) |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20S"), menu[midx].title ); |
else |
lcdx_printf_at_P( 0, 0, MINVERS, 0,0 , PSTR(" %20s"), menu[midx].title ); |
} |
} |
else |
lcd_frect( 0, 0, 128, 7, 1); // Titel: Leerzeile |
//-------------------------- |
// Keyline Beschriftung |
//-------------------------- |
lcdx_cls_row( 7, MNORMAL, 0); |
PKT_KeylineUpDown( 1, 7, 0,0); |
lcd_printp_at( 12, 7, strGet(KEYLINE4), MNORMAL); |
// Taste: KEY_ENTER |
if( menu[midx].key_enter.active && menu[midx].key_enter.text != 0 ) |
{ |
lcdx_printf_at_P( 17, 7, MNORMAL, 0,0 , PSTR("%4S"), menu[midx].key_enter.text ); |
} |
// Taste: KEY_ESC |
if( menu[midx].key_esc.active && menu[midx].key_esc.text != 0 ) |
{ |
lcdx_printf_at_P( 12, 7, MNORMAL, 0,0 , PSTR("%5S"), menu[midx].key_esc.text ); |
} |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Zeilen |
//-------------------------- |
for( y=0; y<6; y++) |
{ |
item = y + menu[midx].display_pos; |
if( item < menu[midx].itemcount ) |
{ |
//-------------------------- |
// char: aktuelle Auswahl |
//-------------------------- |
actchar = ' '; |
if( item == menu[midx].active ) |
{ |
if( menu[midx].item[item].disabled ) actchar = 0x15; // aktuelle Auswahl: Pfeil disabled |
else actchar = 0x1d; // aktuelle Auswahl: Pfeil |
} |
//-------------------------- |
// char: markiert |
//-------------------------- |
markchar = ' '; |
if( menu[midx].item[item].mark ) markchar = '*'; |
//-------------------------- |
// char: markiert RECHTS |
//-------------------------- |
markcharR = ' '; |
if( menu[midx].item[item].type == MENU_SUB ) markcharR = 0x1d; |
if( menu[midx].item[item].markR ) markcharR = SYMBOL_CHECK; |
//-------------------------- |
// Sprache: Multilanguage oder 0 wenn wenn nur ein Text |
//-------------------------- |
lang = (menu[midx].item[item].textcount == 1) ? 0 : Config.DisplayLanguage; |
//-------------------------- |
// Ausgabe: RAM oder PROGMEM |
//-------------------------- |
if( MENU_SEPARATOR == menu[midx].item[item].type ) |
{ |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%18S") , actchar, markchar, PSTR("") ); |
lcd_frect( 6*3, ((y+1)*8)+3, 124-(6*3), 0, 1); // Trennlinie |
} |
else if( MENU_PARAMEDIT == menu[midx].item[item].type ) |
{ |
// SPEZIELL fuer mkparameters.c - zeigt die paramID-Bezeichnung inkl. dem aktuellen WERT an! |
strcpy_P( mkparam_strValueBuffer, PSTR("***")); // Vorbelegung von mkparam_strValueBuffer |
if( !menu[midx].item[item].disabled ) // nur wenn Menuepunkt nicht deaktiviert (disabled = paramID nicht vorhanden) |
{ // -> ermittle den aktuellen Wert der paramID fuer Anzeige |
find_paramEditItem( menu[midx].item[item].id ); // paramID suchen (Ergebnis in Variable 'paramEditItem') |
paramEditItem.editfunc( paramEditItem.paramID, MKPARAM_SHORTVALUE ); // Wert von paramID in mkparam_strValueBuffer darstellen |
} |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%14S %3s") , actchar, markchar, menu[midx].item[item].text[lang], mkparam_strValueBuffer ); |
} |
else |
{ |
// Anmerkung zu menu[midx].topspace: NUR fuer Menues OHNE scrollen!! |
if( menu[midx].item[item].textPGM ) |
{ |
// String im PGM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16S%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17S ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
else |
{ |
// String im RAM |
if( menu[midx].item[item].type == MENU_SUB || menu[midx].item[item].markR ) |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%16s%c "), actchar, markchar, menu[midx].item[item].text[lang], markcharR ); |
else |
lcdx_printf_at_P( 1, y+1+menu[midx].topspace, MNORMAL, 0,0 , PSTR("%c%c%17s ") , actchar, markchar, menu[midx].item[item].text[lang] ); |
} |
} |
} // end: if( item < menu[midx].itemcount ) |
//-------------------------- |
// evtl. Leerzeile loeschen |
//-------------------------- |
if( (y < menu[midx].topspace) || (item >= menu[midx].itemcount+menu[midx].topspace) ) |
{ |
lcd_frect( 6, (y+1)*8, 128-6, 7, 0); // Zeile loeschen |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 45 // Finetuning der Slider-Hoehe |
lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear |
if( menu[midx].itemcount > 6 ) // Slider nur bei mehr als 6 Menueitems |
{ |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 6) / menu[midx].itemcount; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = 8+(menu[midx].display_pos * (SLIDERH-sh)) / (menu[midx].itemcount-6); // Slider: Position |
//lcd_frect( 0, 8+1, 1, SLIDERH, 0); // Slider: full Clear (flackert auf blauen Display!) |
//lcd_frect( 0, 8+1, 1, sy-(8), 0); // Slider: Clear / oben (nicht mehr notwendig da Aenderung am Refresh) |
//lcd_frect( 0, sy+1+sh+1, 1, SLIDERH-(sh-sy+1), 0); // Slider: Clear / unten (nicht mehr notwendig da Aenderung am Refresh) |
lcd_frect( 0, sy+1, 1, sh, 1); // Slider: Draw |
} |
} |
//-------------------------------------------------------------- |
// event = MenuCtrl_Control( ctrlmode ) |
// |
// Parameter: |
// ctrlmode: MENUCTRL_EVENT || MENUCTRL_RETURN |
// |
// Rueckgabe: |
// event: MENUEVENT_NONE || MENUEVENT_ITEM || MENUEVENT_KEY |
//-------------------------------------------------------------- |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ) |
{ |
uint8_t menu_event; |
uint8_t refreshmode; |
uint8_t item; |
uint8_t wahl; |
uint8_t i; |
uint8_t goUp = false; |
uint8_t goDown = false; |
if( midx < 0) { _MenuCtrl_Error( ERROR_NOMENU ); return 0; } // kein Menue vorhanden |
if( menu[midx].itemcount == 0) { _MenuCtrl_Error( ERROR_NOITEM ); return 0; } // kein Menueitem vorhanden |
// bei MENUCTRL_RETURN muss man ggf. selber fuer ein Redraw sorgen |
// (beim ersten Aufruf wird es jedoch dennoch erzwungen) |
if( ctrlmode == MENUCTRL_EVENT || !menu[midx].firstredraw ) |
{ |
MenuCtrl_Refresh( MENU_REDRAW ); |
menu[midx].firstredraw = true; |
} |
clear_key_all(); |
do |
{ |
menu_event = MENUEVENT_NONE; |
menu[midx].lastkey = KEY_NONE; |
refreshmode = false; |
//-------------------------- |
// Pruefe PKT Update oder |
// andere PKT-Aktion |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
refreshmode = MENU_REDRAW; |
} |
//-------------------------- |
// PKT Batterie Anzeige |
//-------------------------- |
if( menu[midx].showbatt ) |
{ |
show_Lipo(); |
} |
//-------------------------- |
// Cursor: nach oben (UP) |
//-------------------------- |
//+++++ |
// UP - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_MINUS) ) |
{ |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active-1 ); |
goUp = true; |
} |
else _MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
if( get_key_short(1 << KEY_MINUS) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus AKTIV |
//+++++ |
// UP - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp((1 << KEY_MINUS),2) ) // up |
{ |
goUp = true; |
} |
} // end: UP - Verschiebemodus NICHT aktiv |
//+++++ |
// UP - goUp |
//+++++ |
if( goUp ) // up |
{ |
refreshmode = MENU_REFRESH; |
if( (menu[midx].active > 0) && (menu[midx].itemcount > 0) ) |
{ |
menu[midx].active--; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = menu[midx].itemcount-1; |
} |
else |
{ |
_MenuCtrl_Beep(); // am oberen Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
// Cursor: nach unten (DOWN) |
//-------------------------- |
//+++++ |
// DOWN - Verschiebemodus AKTIV |
//+++++ |
if( menu[midx].canMove ) |
{ |
if( get_key_long(1 << KEY_PLUS) ) |
{ |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
_MenuCtrl_SwapItem( menu[midx].active, menu[midx].active+1 ); |
goDown = true; |
} |
else _MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
if( get_key_short(1 << KEY_PLUS) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus AKTIV |
//+++++ |
// DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
if( !menu[midx].canMove ) // up |
{ |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp((1 << KEY_PLUS),2) ) // up |
{ |
goDown = true; |
} |
} // end: DOWN - Verschiebemodus NICHT aktiv |
//+++++ |
// DOWN - goDown |
//+++++ |
if( goDown ) // down |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].active < menu[midx].itemcount-1 ) |
{ |
menu[midx].active++; |
} |
else if( menu[midx].cycle ) |
{ |
menu[midx].active = 0; |
} |
else |
{ |
_MenuCtrl_Beep(); // am unteren Ende angelangt |
} |
_MenuCtrl_CalcDisplayPos(); |
} |
//-------------------------- |
//-------------------------- |
item = menu[midx].active; |
goUp = false; |
goDown = false; |
//-------------------------- |
// KEY_ENTER |
//-------------------------- |
if( get_key_short(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
// todo: keyfunc |
if( menu[midx].item[item].type == MENU_SEPARATOR ) |
{ |
// do nothing |
} |
else if( menu[midx].item[item].disabled ) |
{ |
set_beep( 200, 0x00ff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_NOTPOSSIBLE),0,0,0); // "nicht möglich!" |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].item[item].func != NOFUNC ) |
{ |
menu[midx].item[item].func(); |
refreshmode = MENU_REDRAW; |
} |
else if( menu[midx].key_enter.func != NOFUNC ) |
{ |
menu[midx].key_enter.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu[midx].lastkey = KEY_ENTER; |
menu_event = MENUEVENT_ITEM; |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (Deletemodus AKTIV) |
// |
// loescht einen Menueeintrag |
//-------------------------- |
if( menu[midx].canDelete && get_key_long(1 << KEY_ENTER) && (menu[midx].itemcount > 0) ) |
{ |
wahl = 1; |
if( menu[midx].canDelete == 1 ) // ==1 -> mit Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 0, strGet(STR_MENUCTRL_DELASK),PSTR(""),0,0); // "Eintrag entfernen?" |
lcd_printp_at( 12, 7, strGet(YESNO), MINVERS); |
wahl = 0; |
while( !wahl ) |
{ |
if( get_key_short(1<<KEY_ESC) ) wahl = 1; // "Ja" |
if( get_key_short(1<<KEY_ENTER) ) wahl = 2; // "Nein" |
} |
refreshmode = MENU_REDRAW; |
} |
if( wahl==1 ) |
{ |
for( i=menu[midx].active; i<menu[midx].itemcount-1; i++) |
{ |
_MenuCtrl_SwapItem( i, i+1 ); |
} |
menu[midx].itemcount--; |
if( (menu[midx].display_pos > 0) && (menu[midx].display_pos+6 > menu[midx].itemcount) ) |
{ |
menu[midx].display_pos--; |
menu[midx].active--; |
} |
if( menu[midx].active >= menu[midx].itemcount ) |
menu[midx].active = menu[midx].itemcount-1; |
_MenuCtrl_CalcDisplayPos(); |
refreshmode = MENU_REDRAW; |
if( menu[midx].canDelete == 2 ) // ==2 -> direkt loeschen, ohne Nachfrage |
{ |
set_beep( 200, 0xffff, BeepNormal); // Beep |
PKT_Popup_P( 200, strGet(STR_MENUCTRL_DELITEM),0,0,0); // "Eintrag entfernt!" |
} |
} |
} |
//-------------------------- |
// KEY_ENTER_LONG (NICHT Deletemodus) |
//-------------------------- |
if( !menu[midx].canDelete && menu[midx].key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_enter_long.func != NOFUNC ) |
{ |
menu[midx].key_enter_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ENTER_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC_LONG |
//-------------------------- |
if( menu[midx].key_esc_long.active && get_key_long(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc_long.func != NOFUNC ) |
{ |
menu[midx].key_esc_long.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC_LONG; |
} |
} |
//-------------------------- |
// KEY_ESC |
//-------------------------- |
if( get_key_short(1 << KEY_ESC) ) |
{ |
refreshmode = MENU_REFRESH; |
if( menu[midx].key_esc.func != NOFUNC ) |
{ |
menu[midx].key_esc.func(); |
refreshmode = MENU_REDRAW; |
} |
else |
{ |
menu_event = MENUEVENT_KEY; |
menu[midx].lastkey = KEY_ESC; |
} |
} |
if( refreshmode ) |
{ |
MenuCtrl_Refresh( refreshmode ); |
clear_key_all(); |
} |
} |
while ( menu_event == MENUEVENT_NONE && ctrlmode == MENUCTRL_EVENT ); |
return menu_event; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/menuctrl.h |
---|
0,0 → 1,138 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY menuctrl.h |
//# |
//# 11.06.2014 OG |
//# - add: MenuCtrl_GetMenuIndex() |
//# |
//# 11.05.2014 OG |
//# - add: MenuCtrl_SetTitleFromParentItem() |
//# - add: MenuCtrl_GetItemText(), MenuCtrl_IsItemTextPGM() |
//# |
//# 07.05.2014 OG |
//# - add: MenuCtrl_PushSeparatorID() |
//# |
//# 06.05.2014 OG |
//# - add: MenuCtrl_GetItemIdByIndex(), MenuCtrl_GetItemCount() |
//# |
//# 05.05.2014 OG |
//# - add: MenuCtrl_SetDelete(), MenuCtrl_SetMove() |
//# |
//# 30.03.2014 OG |
//# - del: MenuCtrl_PushML_P(), MenuCtrl_PushML() (Umstellung Sprachen abgeschlossen) |
//# - add: MenuCtrl_PushML2(), MenuCtrl_PushML2_P() fuer Umstellung von 3 auf 2 Sprachen |
//# |
//# 24.03.2014 OG |
//# - add: MenuCtrl_PushSeparator() |
//# - add: MENU_SEPARATOR |
//# - chg: Wert von MENU_REFRESH und MENU_REDRAW |
//# |
//# 23.03.2014 OG |
//# - add: Unterstuetzung fuer MK-Parameter-Edit (mkparameters.c) |
//# - add: MenuCtrl_PushParamEdit() |
//# - add: define MENU_PARAMEDIT |
//# |
//# 17.02.2014 OG |
//# - add: MenuCtrl_SetTopSpace() |
//# |
//# 15.02.2014 OG |
//# - add: MenuCtrl_ItemSelect() |
//# |
//# 07.07.2013 OG |
//# - add: MenuCtrl_ItemMarkR() |
//# |
//# 24.05.2013 OG |
//# - chg: MenuCtrl_Push... Funktionen vereinheitlicht mit internen Aenderungen |
//# betrifft externe Funktionsaufrufe die geändert wurden |
//# - del: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# - add: MenuCtrl_ItemMark() |
//# |
//# 23.05.2013 OG |
//# - add: MenuCtrl_PushSimple_P(), MenuCtrl_PushSimple() |
//# |
//# 21.05.2013 OG |
//# - add: MenuCtrl_ShowLevel() |
//# |
//# 17.05.2013 OG - NEU |
//############################################################################ |
#ifndef MENUCTRL_H |
#define MENUCTRL_H |
#define MENU_ITEM 1 // Typ des Menueitems: normaler Eintrag ohne ">" am Ende |
#define MENU_SUB 2 // Typ des Menueitems: Submenue - es wird noch ein ">" angezeigt |
#define MENU_SEPARATOR 3 // nur fuer MenuCtrl_PushSeparator() fuer (verwendet von mkparameters.c) |
#define MENU_PARAMEDIT 4 // nur fuer MenuCtrl_PushParamEdit() fuer (verwendet von mkparameters.c) |
#define NOFUNC 0 // keine Funktion uebergeben |
#define NOTEXT 0 // kein Text uebergeben |
#define MENUEVENT_NONE 0 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_ITEM 1 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUEVENT_KEY 2 // wird von MenuCtrl_Ctrl() zurueckgegeben |
#define MENUCTRL_EVENT 0 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung nur bei einem Event |
#define MENUCTRL_RETURN 1 // Parameter fuer MenuCtrl_Ctrl() - Ruecksprung immer |
#define MENU_REFRESH 1 // MenuCtrl_Refresh() |
#define MENU_REDRAW 2 // MenuCtrl_Refresh() |
void MenuCtrl_Create( void ); |
void MenuCtrl_Destroy( void ); |
void MenuCtrl_ShowLevel( uint8_t value ); |
void MenuCtrl_SetTitle( const char *title ); |
void MenuCtrl_SetTitle_P( const char *title ); |
void MenuCtrl_SetTitleFromParentItem( void ); |
void MenuCtrl_SetCycle( uint8_t value ); |
void MenuCtrl_SetShowBatt( uint8_t value ); |
void MenuCtrl_SetBeep( uint8_t value ); |
void MenuCtrl_SetKey( uint8_t key, const char *keytext, void (*keyfunc)(void) ); |
void MenuCtrl_SetTopSpace( uint8_t value ); |
void MenuCtrl_SetMove( uint8_t value ); |
void MenuCtrl_SetDelete( uint8_t value ); |
int8_t MenuCtrl_GetMenuIndex( void ); |
uint8_t MenuCtrl_GetItemId( void ); |
uint8_t MenuCtrl_GetKey( void ); |
uint8_t MenuCtrl_GetItemIdByIndex( uint8_t index ); |
uint8_t MenuCtrl_GetItemCount( void ); |
const char * MenuCtrl_GetItemText( void ); |
uint8_t MenuCtrl_IsItemTextPGM( void ); |
void MenuCtrl_ItemSelect( uint8_t itemid ); |
void MenuCtrl_ItemActive( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMark( uint8_t itemid, uint8_t value ); |
void MenuCtrl_ItemMarkR( uint8_t itemid, uint8_t value ); |
void MenuCtrl_PushML2_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_PushML2( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text_de, const char *text_en ); |
void MenuCtrl_Push_P( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_Push( uint8_t itemid, uint8_t itemtype, void (*menufunc)(void), const char *text ); |
void MenuCtrl_PushParamEdit( uint8_t itemid ); |
void MenuCtrl_PushSeparator( void ); |
void MenuCtrl_PushSeparatorID( uint8_t itemid ); |
void MenuCtrl_Refresh( uint8_t mode ); |
uint8_t MenuCtrl_Control( uint8_t ctrlmode ); |
#endif // MENUCTRL_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/scrollbox.c |
---|
0,0 → 1,570 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.c |
//# |
//# 31.05.2014 OG |
//# - chg: ScrollBox_Refresh() - umgestellt auf PKT_KeylineUpDown() |
//# |
//# 29.03.2014 OG |
//# - chg: ScrollBox_Show() umgestellt auf clear_key_all() |
//# |
//# 22.05.2013 OG |
//# - fix: include pkt/pkt.h |
//# |
//# 19.05.2013 OG |
//# - chg: ScrollBox_Show() erweitert um PKT_CtrlHook() um u.a. PKT-Updates |
//# zu ermoeglichen |
//# |
//# 04.05.2013 OG |
//# - chg: angepasst auf xutils.c |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() - Variante fuer 'format' im RAM |
//# - chg: ScrollBox_Push_P() -> keine Rueckgabe mehr (void) |
//# - chg: auf xprintf umgestellt (siehe utils/xstring.c) |
//# - add: verschiedene Beschreibungen ueber Funktionen |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
//#define USE_SCROLLBOX_DEBUG // Debug-Funktionen einkompilieren/aktivieren (gesendet wird an uart1) |
#include "../cpu.h" |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdarg.h> |
#include <string.h> |
#include <stdlib.h> |
#include "../main.h" |
#include "../lcd/lcd.h" |
#include "../eeprom/eeprom.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" // xprintf |
#include "scrollbox.h" |
// Debug |
#ifdef USE_SCROLLBOX_DEBUG |
#include "../uart/uart1.h" |
#include <util/delay.h> |
#endif |
#define SCROLLBOX_ALLOC_MEM // dynamische RAM-Belegen (malloc) verwenden? (Standardeinstellung: ja) |
#define SCROLLBOX_LINESIZE 20+1 // Ausgabezeile der ScrollBox (20 Chars + 0x00) |
#define SCROLLBOX_KEYSIZE 4+1 // Textlabel einer Taste (4 Chars + 0x00) |
#define SCROLLBOX_W 20 // Textbreite |
#define SCROLLBOX_H 7 // Textzeilen |
#define MSBLINE 10 // drawmode-code fuer: Linie |
//----------------------------------------------------------- |
// statischer Speicher wenn nicht SCROLLBOX_ALLOC_MEM |
// verwendet wird |
//----------------------------------------------------------- |
#ifndef SCROLLBOX_ALLOC_MEM |
#define SCROLLBOX_BUFFER_SIZE 2048 // 2 KByte - ram-size in bytes |
char scrollbox_buffer[SCROLLBOX_BUFFER_SIZE]; |
#endif |
//----------------------------------------------------------- |
// typedef: scrollbox_key_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t active; // Taste aktiv? (true/false) |
char text[SCROLLBOX_KEYSIZE]; // Tastentext |
} scrollbox_key_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_line_t |
//----------------------------------------------------------- |
typedef struct |
{ |
uint8_t mode; // drawmode: MNORMAL, MINVERSE oder MSBLINE |
char line[SCROLLBOX_LINESIZE]; // malloc: lines * 21 bytes (20 chars + 0) |
} scrollbox_line_t; |
//----------------------------------------------------------- |
// typedef: scrollbox_t |
//----------------------------------------------------------- |
typedef struct |
{ |
scrollbox_line_t *buffer; // malloc: lines * 22 bytes (21 chars + 0) |
uint8_t maxlines; // max. reservierte Lines (malloc buffer) |
uint8_t lines; // Anzahl gepushte lines |
uint8_t display_pos; // aktuelle obere Anzeigezeile |
scrollbox_key_t key_enter; |
scrollbox_key_t key_enter_long; |
} scrollbox_t; |
//----------------------------------------------------------- |
// Buffer & Co. |
//----------------------------------------------------------- |
scrollbox_t scrollbox; |
char buffer_sbline[SCROLLBOX_LINESIZE]; |
//############################################################################################# |
//############################################################################################# |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#ifdef USE_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// nur wenn define: USE_SCROLLBOX_DEBUG |
//-------------------------------------------------------------- |
void ScrollBox_Debug( void ) |
{ |
char s[33]; |
uint8_t i,j; |
char *p; |
scrollbox_line_t *sboxp; |
uint32_t h; |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: BEGIN\r\n\r\n") ); |
ltoa( scrollbox.buffer, s, 16); |
uart1_puts_p( PSTR("address: buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
ltoa( scrollbox_buffer, s, 16); |
uart1_puts_p( PSTR("address: scrollbox_buffer = 0x") ); |
uart1_puts( s ); |
ltoa( scrollbox.buffer, s, 10); |
uart1_puts( " (" ); |
uart1_puts( s ); |
uart1_puts( ")\r\n" ); |
itoa( sizeof(scrollbox_line_t), s, 10); |
uart1_puts_p( PSTR("sizeof(scrollbox_line_t) = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.lines, s, 10); |
uart1_puts_p( PSTR("scrollbox.lines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.maxlines, s, 10); |
uart1_puts_p( PSTR("scrollbox.maxlines = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
itoa( scrollbox.display_pos, s, 10); |
uart1_puts_p( PSTR("scrollbox.display_pos = ") ); |
uart1_puts( s ); |
uart1_puts( "\r\n" ); |
uart1_puts( "\r\n" ); |
for(i=0; i<scrollbox.lines; i++) |
{ |
itoa( i, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
//sboxp = scrollbox.buffer + sizeof(scrollbox_line_t)*i; |
sboxp = scrollbox.buffer + i; |
ltoa( sboxp, s, 16); // hex |
uart1_puts( "[0x" ); |
uart1_puts( s ); |
uart1_puts( "]" ); |
ltoa( sboxp, s, 10); // dec |
uart1_puts( "[" ); |
uart1_puts( s ); |
uart1_puts( "] " ); |
itoa( sboxp->mode, s, 10); |
if( strlen(s)<2 ) uart1_puts( " " ); |
uart1_puts( s ); |
uart1_puts( ": " ); |
p = sboxp->line; |
if( sboxp->mode != 10 ) |
{ |
uart1_puts( p ); |
} |
uart1_puts( "|\r\n" ); |
} |
//-------------------------------------------------------------- |
// Hier wird fuer eine gewisse Zeit (insgesamt 2500 mal) permanent die Zeile 23 ausgegeben. |
// Wenn ich das richtig sehe schreibe ich hierbei nicht ;-) |
// Aber was sagt das Empfangsterminal.... |
//-------------------------------------------------------------- |
sboxp = scrollbox.buffer + 23; |
for( h=0; h<10; h++) |
{ |
itoa( h, s, 10); |
uart1_puts( s ); |
uart1_puts( "# " ); |
uart1_puts( sboxp->line ); |
uart1_puts( "|\r\n" ); |
_delay_ms(10); |
} |
//-------------------------------------------------------------- |
uart1_puts_p( PSTR("\r\nScrollBox_Debug: END\r\n") ); |
uart1_puts( "##########################################\r\n\r\n" ); |
} |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
#endif // USER_SCROLLBOX_DEBUG |
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
//-------------------------------------------------------------- |
// ok = ScrollBox_Create( uint8_t maxlines ) |
// |
// Rueckgabe: |
// true = ok |
// false = kein Speicher |
// |
// - reserviert speicher fuer max. maxlines |
// - initialisiert scrollbox-Datenstruktur |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Create( uint8_t maxlines ) |
{ |
scrollbox.buffer = 0; |
#ifdef SCROLLBOX_ALLOC_MEM |
scrollbox.buffer = malloc( maxlines * sizeof(scrollbox_line_t)); |
//scrollbox.buffer = calloc( maxlines, sizeof(scrollbox_line_t) ); |
#else |
scrollbox.buffer = (scrollbox_line_t *) scrollbox_buffer; |
#endif |
if( !scrollbox.buffer ) // kein RAM mehr |
{ |
lcd_cls(); |
lcd_printp_at( 1, 3, PSTR("Error: ScrollBox"), MNORMAL); |
lcd_printp_at( 1, 4, PSTR("No more RAM"), MNORMAL); |
set_beep ( 500, 0x3333, BeepNormal); |
while (!get_key_short (1 << KEY_ESC)); |
return false; |
} |
scrollbox.maxlines = maxlines; |
scrollbox.lines = 0; |
scrollbox.display_pos = 0; |
scrollbox.key_enter.active = false; |
scrollbox.key_enter_long.active = false; |
return true; |
} |
//-------------------------------------------------------------- |
// ScrollBox_Destroy() |
// |
// gibt reservierten Speicher wieder frei |
// Wichtig! Da malloc verwendet wird! |
//-------------------------------------------------------------- |
void ScrollBox_Destroy( void ) |
{ |
if( scrollbox.buffer ) |
{ |
#ifdef SCROLLBOX_ALLOC_MEM |
free( scrollbox.buffer ); |
#endif |
} |
scrollbox.buffer = 0; |
} |
//-------------------------------------------------------------- |
// ScrollBox_PushLine() |
// |
// fuegt eine Trennlinie hinzu |
//-------------------------------------------------------------- |
void ScrollBox_PushLine( void ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
sboxp->mode = MSBLINE; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// _scrollbox_push(...) |
// |
// intern fuer: ScrollBox_Push_P(), ScrollBox_Push() |
// Beschreibung siehe dort |
//-------------------------------------------------------------- |
void _scrollbox_push( uint8_t useprogmem, uint8_t mode, const char *format, va_list ap ) |
{ |
scrollbox_line_t *sboxp; |
if( scrollbox.lines < scrollbox.maxlines ) |
{ |
sboxp = scrollbox.buffer + scrollbox.lines; |
_xvsnprintf( useprogmem, buffer_sbline, SCROLLBOX_LINESIZE, format, ap); |
strncpyfill( sboxp->line, buffer_sbline, SCROLLBOX_LINESIZE); // copy: buffer_sbline zur scrollbox |
sboxp->mode = mode; |
scrollbox.lines++; |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push_P( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im PROGMEM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( true, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// ScrollBox_Push( mode, format, ...) |
// |
// Textzeile hinzufuegen - 'format' ist im RAM. |
// |
// Parameter: |
// mode : MNORMAL, MINVERS |
// format : siehe xprint-Doku in utils/xstring.c |
// ... : Parameterliste fuer 'format' |
//-------------------------------------------------------------- |
void ScrollBox_Push( uint8_t mode, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_scrollbox_push( false, mode, format, ap); |
va_end(ap); |
} |
//-------------------------------------------------------------- |
// key: KEY_ENTER, KEY_ENTER_LONG (keine weiteren bisher!) |
//-------------------------------------------------------------- |
void ScrollBox_SetKey( uint8_t key, const char *keytext ) |
{ |
if( key == KEY_ENTER ) |
{ |
scrollbox.key_enter.active = true; |
strncpyfill( scrollbox.key_enter.text, keytext, SCROLLBOX_KEYSIZE); |
} |
if( key == KEY_ENTER_LONG ) |
{ |
scrollbox.key_enter_long.active = true; |
strncpyfill( scrollbox.key_enter_long.text, keytext, SCROLLBOX_KEYSIZE); |
} |
} |
//-------------------------------------------------------------- |
// ScrollBox_Refresh() |
// |
// zeigt die ScrollBox - wird normalerweise von ScrollBox_Show() |
// automatisch durchgefuehrt |
//-------------------------------------------------------------- |
void ScrollBox_Refresh( void ) |
{ |
uint8_t y; |
uint8_t sh; |
uint8_t sy; |
scrollbox_line_t *sboxp; |
//-------------------------- |
// Text |
//-------------------------- |
for( y=0; y<7; y++) |
{ |
sboxp = scrollbox.buffer + (scrollbox.display_pos + y); |
if( y + scrollbox.display_pos < scrollbox.lines ) |
{ |
if( sboxp->mode == MSBLINE ) |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
lcd_line ( (21-SCROLLBOX_W)*6, (y*8)+3, 125, (y*8)+3, 1); // line |
} |
else |
{ |
lcd_puts_at( 21-SCROLLBOX_W, y, sboxp->line, sboxp->mode); |
} |
//p += sizeof(scrollbox_line_t); |
} |
else // clear |
{ |
lcd_frect( (21-SCROLLBOX_W)*6, (y*8), (SCROLLBOX_W*6), 7, 0); // clear |
} |
} |
//-------------------------- |
// Slider |
//-------------------------- |
#define SLIDERH 55 // Finetuning der Slider-Hoehe |
// Slider: 7 zeilen * 8 pixel = 56 pixel |
sh = (SLIDERH * 7) / scrollbox.lines; // Slider: Hoehe |
sh = (sh > SLIDERH) ? SLIDERH : sh; |
sy = (scrollbox.display_pos * (SLIDERH-sh)) / (scrollbox.lines-7); // Slider: Position |
lcd_frect( 0, 0, 1, SLIDERH, 0); // Slider: Clear |
lcd_frect( 0, sy, 1, sh, 1); // Slider: draw // Slider: Draw |
//-------------------------- |
// Keyline |
//-------------------------- |
PKT_KeylineUpDown( 1, 7, 0,0); // Keyline: Up / Down |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline: Ende |
if( scrollbox.key_enter.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter.text , MNORMAL); |
else if ( scrollbox.key_enter_long.active ) lcd_print_at (17, 7, (uint8_t *)scrollbox.key_enter_long.text, MNORMAL); |
else lcd_printp_at(17, 7, PSTR(" ") , MNORMAL); |
//ScrollBox_Debug(); |
} |
//-------------------------------------------------------------- |
// key = ScrollBox_Show() |
// |
//-------------------------------------------------------------- |
uint8_t ScrollBox_Show( void ) |
{ |
uint8_t keyexit = 0; |
clear_key_all(); |
if( scrollbox.buffer ) |
{ |
ScrollBox_Refresh(); |
do |
{ |
//-------------------------- |
// Pruefe auf PKT-Update und |
// andere interne PKT-Aktionen |
//-------------------------- |
if( PKT_CtrlHook() ) // Update vom Updatetool angefordert? |
{ |
lcd_cls(); |
ScrollBox_Refresh(); |
} |
//-------------------------- |
// scrollen: nach unten |
//-------------------------- |
if( get_key_press(1 << KEY_PLUS) || get_key_long_rpt_sp( (1 << KEY_PLUS),2 )) // down |
{ |
if( scrollbox.display_pos < ( scrollbox.lines - SCROLLBOX_H) ) |
{ |
scrollbox.display_pos++; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am unteren Ende angelangt |
} |
} |
//-------------------------- |
// scrollen: nach oben |
//-------------------------- |
if( get_key_press(1 << KEY_MINUS) || get_key_long_rpt_sp( (1 << KEY_MINUS),2 )) // up |
{ |
if( scrollbox.display_pos > 0 ) |
{ |
scrollbox.display_pos--; |
ScrollBox_Refresh(); |
} |
else |
{ |
set_beep ( 25, 0xffff, BeepNormal); // am oberen Ende angelangt |
} |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter.active && get_key_short(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER; |
} |
//-------------------------- |
//-------------------------- |
if( scrollbox.key_enter_long.active && get_key_long(1 << KEY_ENTER) ) |
{ |
keyexit = KEY_ENTER_LONG; |
} |
} while( !get_key_press(1 << KEY_ESC) && (keyexit == 0) ); |
} |
//get_key_press(KEY_ALL); // ersetzt durch obiges clear_key_all() |
return keyexit; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/scrollbox.h |
---|
0,0 → 1,49 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY scrollbox.h |
//# |
//# 17.05.2013 OG |
//# - del: defines KEY_xyz_LONG verschoben nach HAL_HW3_9.h |
//# |
//# 28.04.2013 OG |
//# - add: ScrollBox_Push() |
//# - chg: ScrollBox_Push_P() (keine Rueckgabe mehr) |
//# - add: include <stdarg.h> |
//# |
//# 20.04.2013 OG - NEU |
//############################################################################ |
#ifndef _SCROLLBOX_H |
#define _SCROLLBOX_H |
#include <stdarg.h> // Notwendig! (OG) |
uint8_t ScrollBox_Create( uint8_t maxlines ); |
void ScrollBox_Destroy( void ); |
void ScrollBox_PushLine( void ); |
void ScrollBox_Push_P( uint8_t mode, const char *format, ... ); |
void ScrollBox_Push( uint8_t mode, const char *format, ... ); |
void ScrollBox_SetKey( uint8_t key, const char *keytext ); |
void ScrollBox_Refresh( void ); |
uint8_t ScrollBox_Show( void ); |
//void ScrollBox_Debug( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twi_slave.c |
---|
0,0 → 1,373 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.c |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : This is sample driver to AVRs TWI module. |
* It is interupt driveren. All functionality is controlled through |
* passing information to and from functions. Se main.c for samples |
* of how to use the driver. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
#if defined(__ICCAVR__) |
#include "ioavr.h" |
#include "inavr.h" |
#else |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#endif |
#include "twi_slave.h" |
// Emulate GCC ISR() statement in IAR |
#if defined(__ICCAVR__) |
#define PRAGMA(x) _Pragma( #x ) |
#define ISR(vec) PRAGMA( vector=vec ) __interrupt void handler_##vec(void) |
#endif |
static unsigned char TWI_buf[TWI_BUFFER_SIZE]; // Transceiver buffer. Set the size in the header file |
static unsigned char TWI_msgSize = 0; // Number of bytes to be transmitted. |
static unsigned char TWI_state = TWI_NO_STATE; // State byte. Default set to TWI_NO_STATE. |
// This is true when the TWI is in the middle of a transfer |
// and set to false when all bytes have been transmitted/received |
// Also used to determine how deep we can sleep. |
static volatile unsigned char TWI_busy = 0; |
union TWI_statusReg_t TWI_statusReg = {0}; // TWI_statusReg is defined in TWI_Slave.h |
/**************************************************************************** |
Call this function to set up the TWI slave to its initial standby state. |
Remember to enable interrupts from the main application after initializing the TWI. |
Pass both the slave address and the requrements for triggering on a general call in the |
same byte. Use e.g. this notation when calling this function: |
TWI_Slave_Initialise( (TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) ); |
The TWI module is configured to NACK on any requests. Use a TWI_Start_Transceiver function to |
start the TWI. |
****************************************************************************/ |
void twi_slave_init( unsigned char TWI_ownAddress ) |
{ |
TWAR = TWI_ownAddress; // Set own TWI slave address. Accept TWI General Calls. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins. |
(0<<TWIE)|(0<<TWINT)| // Disable TWI Interupt. |
(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Do not ACK on any requests, yet. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to test if the TWI_ISR is busy transmitting. |
****************************************************************************/ |
unsigned char twi_slave_busy( void ) |
{ |
return TWI_busy; |
} |
/**************************************************************************** |
Call this function to fetch the state information of the previous operation. The function will hold execution (loop) |
until the TWI_ISR has completed with the previous operation. If there was an error, then the function |
will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_status( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI has completed the transmission. |
return ( TWI_state ); // Return error state. |
} |
/**************************************************************************** |
Call this function to send a prepared message, or start the Transceiver for reception. Include |
a pointer to the data to be sent if a SLA+W is received. The data will be copied to the TWI buffer. |
Also include how many bytes that should be sent. Note that unlike the similar Master function, the |
Address byte is not included in the message buffers. |
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation, |
then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start_with_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char temp; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_msgSize = msgSize; // Number of data to transmit. |
for ( temp = 0; temp < msgSize; temp++ ) // Copy data that may be transmitted if the TWI Master requests data. |
{ |
TWI_buf[ temp ] = msg[ temp ]; |
} |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 1; |
} |
/**************************************************************************** |
Call this function to start the Transceiver without specifing new transmission data. Useful for restarting |
a transmission, or just starting the transceiver for reception. The driver will reuse the data previously put |
in the transceiver buffers. The function will hold execution (loop) until the TWI_ISR has completed with the |
previous operation, then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to read out the received data from the TWI transceiver buffer. I.e. first call |
TWI_Start_Transceiver to get the TWI Transceiver to fetch data. Then Run this function to collect the |
data when they have arrived. Include a pointer to where to place the data and the number of bytes |
to fetch in the function call. The function will hold execution (loop) until the TWI_ISR has completed |
with the previous operation, before reading out the data and returning. |
If there was an error in the previous transmission the function will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char i; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
if( TWI_statusReg.lastTransOK ) // Last transmission completed successfully. |
{ |
for ( i=0; i<msgSize; i++ ) // Copy data from Transceiver buffer. |
{ |
msg[ i ] = TWI_buf[ i ]; |
} |
TWI_statusReg.RxDataInBuf = FALSE; // Slave Receive data has been read from buffer. |
} |
return( TWI_statusReg.lastTransOK ); |
} |
// ********** Interrupt Handlers ********** // |
/**************************************************************************** |
This function is the Interrupt Service Routine (ISR), and called when the TWI interrupt is triggered; |
that is whenever a TWI event has occurred. This function should not be called directly from the main |
application. |
****************************************************************************/ |
ISR(TWI_vect) |
{ |
static unsigned char TWI_bufPtr; |
switch (TWSR) |
{ |
case TWI_STX_ADR_ACK: // Own SLA+R has been received; ACK has been returned |
// case TWI_STX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
case TWI_STX_DATA_ACK: // Data byte in TWDR has been transmitted; ACK has been received |
TWDR = TWI_buf[TWI_bufPtr++]; |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_STX_DATA_NACK: // Data byte in TWDR has been transmitted; NACK has been received. |
// I.e. this could be the end of the transmission. |
if (TWI_bufPtr == TWI_msgSize) // Have we transceived all expected data? |
{ |
TWI_statusReg.lastTransOK = TRUE; // Set status bits to completed successfully. |
} |
else // Master has sent a NACK before all data where sent. |
{ |
TWI_state = TWSR; // Store TWI State as errormessage. |
} |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Answer on next address match |
(0<<TWWC); // |
TWI_busy = 0; // Transmit is finished, we are not busy anymore |
break; |
case TWI_SRX_GEN_ACK: // General call address has been received; ACK has been returned |
// case TWI_SRX_GEN_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
TWI_statusReg.genAddressCall = TRUE; |
case TWI_SRX_ADR_ACK: // Own SLA+W has been received ACK has been returned |
// case TWI_SRX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
// Dont need to clear TWI_S_statusRegister.generalAddressCall due to that it is the default state. |
TWI_statusReg.RxDataInBuf = TRUE; |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Expect ACK on this transmission |
(0<<TWWC); |
TWI_busy = 1; |
break; |
case TWI_SRX_ADR_DATA_ACK: // Previously addressed with own SLA+W; data has been received; ACK has been returned |
case TWI_SRX_GEN_DATA_ACK: // Previously addressed with general call; data has been received; ACK has been returned |
// TODO: What is this? Seems to be no bounds checking! |
TWI_buf[TWI_bufPtr++] = TWDR; |
TWI_statusReg.lastTransOK = TRUE; // Set flag transmission successfull. |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Send ACK after next reception |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_SRX_STOP_RESTART: // A STOP condition or repeated START condition has been received while still addressed as Slave |
// Enter not addressed mode and listen to address match |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Enable interrupt and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Wait for new address match |
(0<<TWWC); // |
TWI_busy = 0; // We are waiting for a new address match, so we are not busy |
break; |
case TWI_SRX_ADR_DATA_NACK: // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
case TWI_SRX_GEN_DATA_NACK: // Previously addressed with general call; data has been received; NOT ACK has been returned |
case TWI_STX_DATA_ACK_LAST_BYTE: // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// case TWI_NO_STATE // No relevant state information available; TWINT = �0� |
case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition |
TWI_state = TWSR; //Store TWI State as errormessage, operation also clears noErrors bit |
TWCR = (1<<TWSTO)|(1<<TWINT); //Recover from TWI_BUS_ERROR, this will release the SDA and SCL pins thus enabling other devices to use the bus |
break; |
default: |
TWI_state = TWSR; // Store TWI State as errormessage, operation also clears the Success bit. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Acknowledge on any new requests. |
(0<<TWWC); // |
TWI_busy = 0; // Unknown status, so we wait for a new address match that might be something we can handle |
} |
} |
/* |
void example(){ |
unsigned char messageBuf[TWI_BUFFER_SIZE]; |
unsigned char TWI_slaveAddress; |
// LED feedback port - connect port B to the STK500 LEDS |
DDRB = 0xFF; // Set to ouput |
PORTB = 0x55; // Startup pattern |
// Own TWI slave address |
TWI_slaveAddress = 0x10; |
// Initialise TWI module for slave operation. Include address and/or enable General Call. |
TWI_Slave_Initialise( (unsigned char)((TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) )); |
SEI(); |
// Start the TWI transceiver to enable reseption of the first command from the TWI Master. |
TWI_Start_Transceiver(); |
// This example is made to work together with the AVR315 TWI Master application note. In adition to connecting the TWI |
// pins, also connect PORTB to the LEDS. The code reads a message as a TWI slave and acts according to if it is a |
// general call, or an address call. If it is an address call, then the first byte is considered a command byte and |
// it then responds differently according to the commands. |
// This loop runs forever. If the TWI is busy the execution will just continue doing other operations. |
for(;;) |
{ |
#ifdef POWER_MANAGEMENT_ENABLED |
// Sleep while waiting for TWI transceiver to complete or waiting for new commands. |
// If we have data in the buffer, we can't enter sleep because we have to take care |
// of it first. |
// If the transceiver is busy, we enter idle mode because it will wake up by all TWI |
// interrupts. |
// If the transceiver not is busy, we can enter power-down mode because next receive |
// should be a TWI address match and it wakes the device up from all sleep modes. |
if( ! TWI_statusReg.RxDataInBuf ) { |
if(TWI_Transceiver_Busy()) { |
MCUCR = (1<<SE)|(0<<SM2)|(0<<SM1)|(0<<SM0); // Enable sleep with idle mode |
} else { |
MCUCR = (1<<SE)|(0<<SM2)|(1<<SM1)|(0<<SM0); // Enable sleep with power-down mode |
} |
SLEEP(); |
} else { |
NOP(); // There is data in the buffer, code below takes care of it. |
} |
#else // No power management |
// Here you can add your own code that should be run while waiting for the TWI to finish |
NOP(); // Put own code here. |
#endif |
// Check if the TWI Transceiver has completed an operation. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
// Check if the last operation was successful |
if ( TWI_statusReg.lastTransOK ) |
{ |
// Check if the last operation was a reception |
if ( TWI_statusReg.RxDataInBuf ) |
{ |
TWI_Get_Data_From_Transceiver(messageBuf, 2); |
// Check if the last operation was a reception as General Call |
if ( TWI_statusReg.genAddressCall ) |
{ |
// Put data received out to PORTB as an example. |
PORTB = messageBuf[0]; |
} |
else // Ends up here if the last operation was a reception as Slave Address Match |
{ |
// Example of how to interpret a command and respond. |
// TWI_CMD_MASTER_WRITE stores the data to PORTB |
if (messageBuf[0] == TWI_CMD_MASTER_WRITE) |
{ |
PORTB = messageBuf[1]; |
} |
// TWI_CMD_MASTER_READ prepares the data from PINB in the transceiver buffer for the TWI master to fetch. |
if (messageBuf[0] == TWI_CMD_MASTER_READ) |
{ |
messageBuf[0] = PINB; |
TWI_Start_Transceiver_With_Data( messageBuf, 1 ); |
} |
} |
} |
else // Ends up here if the last operation was a transmission |
{ |
NOP(); // Put own code here. |
} |
// Check if the TWI Transceiver has already been started. |
// If not then restart it to prepare it for new receptions. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
TWI_Start_Transceiver(); |
} |
} |
else // Ends up here if the last operation completed unsuccessfully |
{ |
TWI_Act_On_Failure_In_Last_Transmission( TWI_Get_State_Info() ); |
} |
} |
} |
}*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twi_slave.h |
---|
0,0 → 1,121 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.h |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : Header file for TWI_slave.c |
* Include this file in the application. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
/**************************************************************************** |
TWI Status/Control register definitions |
****************************************************************************/ |
#define TWI_BUFFER_SIZE 4 // Reserves memory for the drivers transceiver buffer. |
// Set this to the largest message size that will be sent including address byte. |
/**************************************************************************** |
Global definitions |
****************************************************************************/ |
union TWI_statusReg_t // Status byte holding flags. |
{ |
unsigned char all; |
struct |
{ |
unsigned char lastTransOK:1; |
unsigned char RxDataInBuf:1; |
unsigned char genAddressCall:1; // TRUE = General call, FALSE = TWI Address; |
unsigned char unusedBits:5; |
}; |
}; |
extern union TWI_statusReg_t TWI_statusReg; |
/**************************************************************************** |
Function definitions |
****************************************************************************/ |
void twi_slave_init( unsigned char ); |
unsigned char twi_slave_busy( void ); |
unsigned char twi_slave_get_status( void ); |
void twi_slave_start_with_data( unsigned char * , unsigned char ); |
void twi_slave_start( void ); |
unsigned char twi_slave_get_data( unsigned char *, unsigned char ); |
/**************************************************************************** |
Bit and byte definitions |
****************************************************************************/ |
#define TWI_READ_BIT 0 // Bit position for R/W bit in "address byte". |
#define TWI_ADR_BITS 1 // Bit position for LSB of the slave address bits in the init byte. |
#define TWI_GEN_BIT 0 // Bit position for LSB of the general call bit in the init byte. |
#define TRUE 1 |
#define FALSE 0 |
/**************************************************************************** |
TWI State codes |
****************************************************************************/ |
// General TWI Master staus codes |
#define TWI_START 0x08 // START has been transmitted |
#define TWI_REP_START 0x10 // Repeated START has been transmitted |
#define TWI_ARB_LOST 0x38 // Arbitration lost |
// TWI Master Transmitter staus codes |
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received |
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received |
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received |
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received |
// TWI Master Receiver staus codes |
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received |
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received |
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted |
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted |
// TWI Slave Transmitter staus codes |
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned |
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received |
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received |
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// TWI Slave Receiver staus codes |
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned |
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned |
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned |
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave |
// TWI Miscellaneous status codes |
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = �0� |
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twimaster.c |
---|
0,0 → 1,202 |
/************************************************************************* |
* Title: I2C master library using hardware TWI interface |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: twimaster.c,v 1.3 2005/07/02 11:14:21 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device with hardware TWI |
* Usage: API compatible with I2C Software Library i2cmaster.h |
**************************************************************************/ |
#include <inttypes.h> |
#include <compat/twi.h> |
#include <i2cmaster.h> |
/* define CPU frequency in Mhz here if not defined in Makefile */ |
#ifndef F_CPU |
#define F_CPU 16000000UL |
#endif |
/* I2C clock in Hz */ |
#define SCL_CLOCK 400000L |
/************************************************************************* |
Initialization of the I2C bus interface. Need to be called only once |
*************************************************************************/ |
void i2c_init(void) |
{ |
/* initialize TWI clock: 100 kHz clock, TWPS = 0 => prescaler = 1 */ |
TWSR = 0; /* no prescaler */ |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ |
}/* i2c_init */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
return 0 = device accessible, 1= failed to access device |
*************************************************************************/ |
unsigned char i2c_start(unsigned char address) |
{ |
uint8_t twst; |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) return 1; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed and ACK/NACK has been received |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1; |
return 0; |
}/* i2c_start */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
If device is busy, use ack polling to wait until device is ready |
Input: address and transfer direction of I2C device |
*************************************************************************/ |
void i2c_start_wait(unsigned char address) |
{ |
uint8_t twst; |
while ( 1 ) |
{ |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) continue; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) |
{ |
/* device busy, send stop condition to terminate write operation */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
continue; |
} |
//if( twst != TW_MT_SLA_ACK) return 1; |
break; |
} |
}/* i2c_start_wait */ |
/************************************************************************* |
Issues a repeated start condition and sends address and transfer direction |
Input: address and transfer direction of I2C device |
Return: 0 device accessible |
1 failed to access device |
*************************************************************************/ |
unsigned char i2c_rep_start(unsigned char address) |
{ |
return i2c_start( address ); |
}/* i2c_rep_start */ |
/************************************************************************* |
Terminates the data transfer and releases the I2C bus |
*************************************************************************/ |
void i2c_stop(void) |
{ |
/* send stop condition */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
}/* i2c_stop */ |
/************************************************************************* |
Send one byte to I2C device |
Input: byte to be transfered |
Return: 0 write successful |
1 write failed |
*************************************************************************/ |
unsigned char i2c_write( unsigned char data ) |
{ |
uint8_t twst; |
// send data to the previously addressed device |
TWDR = data; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits |
twst = TW_STATUS & 0xF8; |
if( twst != TW_MT_DATA_ACK) return 1; |
return 0; |
}/* i2c_write */ |
/************************************************************************* |
Read one byte from the I2C device, request more data from device |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readAck(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readAck */ |
/************************************************************************* |
Read one byte from the I2C device, read is followed by a stop condition |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readNak(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readNak */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/xutils.c |
---|
0,0 → 1,765 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ xutils.c - erweiterte String-Funktionen, xprintf (Info nach History) |
//+ und weitere Hilfsfunktionen wie z.B. UTCdatetime2local() |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//############################################################################ |
//# HISTORY xutils.c |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P(), _strncpyat() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# - add: buffer sprintf_buffer[] |
//# - chg: PGMBUFF_SIZE und ARGBUFF_SIZE von 80 auf 40 geaendert |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() entfernt Leerzeichen auf der rechten Seite |
//# |
//# 14.05.2013 OG |
//# - chg: Kommentarkopf von UTCdatetime2local() aktualisiert |
//# |
//# 05.05.2013 OG |
//# - chg: UTCdatetime2local() auf Config.timezone/summertime umgestellt |
//# - add: include eeprom.h |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.c |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# - fix: _xvsnprintf() Darstellung kleiner negativer Nachkommazahlen (-1<z<0) |
//# |
//# 29.04.2013 OG |
//# - chg: Doku zu xprintf Ergaenzt bei 'bekannte Einschraenkungen' |
//# - chg: includes reduziert auf das Notwendige |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
//############################################################################ |
//# xprintf |
//# |
//# Diese Variante von printf ist angepasst auf das PKT: |
//# |
//# - Unterstuetzung von Festkomma-Integer Anzeige |
//# - Overflow-Anzeige durch '*' wenn eine Zahl die Format-Maske sprengt |
//# - progmen wird optional unterstuetz fuer den format-String als auch fuer |
//# String Argumente |
//# - strikte Einhaltung von Laengen einer Format-Maske |
//# (ggf. wird die Ausgabe gekuerzt) |
//# |
//# In diesem Source sind nur die Basis-xprintf zum Erzeugen von formatierten |
//# Strings. Die direkten Ausgabefunktionen auf den Screen sind in lcd.c |
//# als lcdx_printf_at() und lcdx_printf_at_P(). |
//# |
//# FORMAT ANGABEN: |
//# |
//# %d: dezimal int signed (Rechtsbuendige Ausgabe) (Wandlung via itoa) |
//# "%d" arg: 1234 -> "1234" |
//# "%5d" arg: 1234 -> " 1234" |
//# "%5.2d" arg: 1234 -> " 12.34" |
//# "%05d" arg: 123 -> "00123" |
//# "%3.2d" arg: -13 -> " -0.13" |
//# |
//# %u: dezimal int unsigned (Rechtsbuendige Ausgabe) (Wandlung via utoa) |
//# wie %d jedoch mittels utoa |
//# |
//# %h: hex int unsigned -> Hex-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%4h" |
//# %o: octal int unsigned -> Octal-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%2o" |
//# %b: binary int unsigned -> Binaer-Zahl Rechtsbuendig, Laenge wird unterstuetzt z.B. "%8b" |
//# |
//# %ld, %lu, %lh, %lo, %lb: |
//# wie die obigen jedoch fuer long-zahlen (via ltoa, ultoa) |
//# nur wenn define USE_XPRINTF_LONG gesetzt ist! |
//# Bespiele: "%ld", "%5.6ld" usw. |
//# |
//# %s: String aus RAM -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %S: String aus progmem -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# |
//# %c: einzelnes char -> Linksbuendig, Laenge wird unterstuetzt (z.B. "%8s") |
//# %%: Ausgabe von "%" |
//# |
//# |
//# BEISPIELE: |
//# |
//# vorhanden in: osd/osdata.c, lcd/lcd.c |
//# |
//# |
//# BEKANNTE EINSCHRAENKUNGEN: |
//# |
//# 1. padding '0' mit negativen Zahlen wenn padding aktiv ist ergibt z.B. "00-1.5" |
//# 2. der Entwickler muss auf eine korrekte Format-Maske achten. Andernfalls |
//# kommt es zu nicht vorhersehbaren Ausgaben. |
//# 3. nicht in ISR's verwenden - dazu muss ggf. eine Anpassung durchgefuert werden |
//# |
//# KOMPILIER OPTIONEN |
//# |
//# define: USE_XPRINTF_LONG |
//# Unterstuetzung von long int / unsigned long int ('l' Modifier in Maske) |
// Wird in der main.h gesetzt |
//############################################################################ |
#define USE_XPRINTF_LONG // Unterstuetzung von long integer - Achtung! Muss fuer PKT gesetzt sein |
// da in Sourcen verwendet! |
#include <avr/pgmspace.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <stdarg.h> |
#include <stdbool.h> |
#include "../main.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#define PGMBUFF_SIZE 40 // max. 40 chars bei format-String aus progmem |
#define ARGBUFF_SIZE 40 // buffer fuer xprintf Parameter (strings, itoa, utoa) (fuer ltoa ggf. anpassen) |
#define SPRINTF_BUFFER_SIZE 40 // max. 40 Chars fuer den Buffer fuer xsnprintf(), xsnprintf_P() |
char sprintf_buffer[SPRINTF_BUFFER_SIZE]; |
//---------------------- |
// xprintf parser data |
//---------------------- |
typedef struct |
{ |
uint8_t parse; // true / false |
char cmd; // char: d u h o b s S c % |
uint8_t prelen; // Vorkomma |
uint8_t declen; // decimal (Nachkomma) |
uint8_t point; // true / false |
uint8_t uselong; // true / false |
uint8_t mask; // true / false |
char pad; // ' ' oder '0' |
} xprintf_t; |
//---------------------- |
// Buffers |
//---------------------- |
char cmddigit[7]; |
char argbuff[ARGBUFF_SIZE]; |
char pgmbuff[PGMBUFF_SIZE]; |
//#################################################################################### |
//--------------------------------------------------------------------- |
// Basisfunktion von xprintf |
// Doku: siehe oben |
//--------------------------------------------------------------------- |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ) |
{ |
const char *p_fmt; // pointer auf den format-String |
char *p_dst; // pointer auf den destination buffer |
const char *p_str; // pointer auf einen arg-String wenn ein String ausgegeben werden soll |
char *p_cmddigit = 0; // pointer auf den Digit-Buffer fuer eine Maske (Laenge/Nachkommastellen) |
const char *p_fmtbuff; // pointer auf den format-Buffer (fuer progmem) |
const char *p_argbuff; // pointer auf den argbuffer |
uint8_t fmtlen, arglen, overflow; |
uint8_t i,j,dec; |
uint8_t dstcnt; |
uint8_t minus; |
xprintf_t cmd; // parser Daten |
p_fmtbuff = format; |
if( useprogmem ) // format von progmem in's RAM kopieren |
{ |
strncpy_P( pgmbuff, format, PGMBUFF_SIZE); |
pgmbuff[PGMBUFF_SIZE-1] = 0; |
p_fmtbuff = pgmbuff; |
} |
cmd.parse = false; |
p_fmt = p_fmtbuff-1; // -1 -> wird am Anfang der Schleife korrigiert |
p_dst = buffer; |
dstcnt = 0; |
do |
{ |
if( dstcnt >= n ) // max. Anzahl von Zeichen fuer Ziel 'buffer' erreicht? |
break; |
p_fmt++; // naechstes Zeichen von format |
//######################## |
//# 1. PARSE |
//######################## |
//------------------------ |
// START: parse cmd |
//------------------------ |
if( cmd.parse == false && *p_fmt == '%' ) |
{ |
memset( &cmd, 0, sizeof(xprintf_t) ); // init |
cmd.parse = true; |
cmd.pad = ' '; |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// NO parse: copy char |
//------------------------ |
if( cmd.parse == false ) |
{ |
*p_dst = *p_fmt; |
p_dst++; |
dstcnt++; |
continue; |
} |
//------------------------ |
// set: pad (eine '0' ganz am Anfang der Formatmaske) |
//------------------------ |
if( cmd.parse == true && *p_fmt == '0' && p_cmddigit == cmddigit ) |
{ |
cmd.pad = '0'; |
continue; |
} |
//------------------------ |
// set: vor/nach-kommastellen |
//------------------------ |
if( cmd.parse == true && *p_fmt >= '0' && *p_fmt <= '9' ) |
{ |
*p_cmddigit = *p_fmt; |
p_cmddigit++; |
continue; |
} |
//------------------------ |
// set: point |
//------------------------ |
if( cmd.parse == true && *p_fmt == '.' && cmd.point == false ) |
{ |
cmd.point = true; |
*p_cmddigit = 0; |
cmd.prelen = atoi( cmddigit ); |
p_cmddigit = cmddigit; |
continue; |
} |
//------------------------ |
// set: uselong |
//------------------------ |
#ifdef USE_XPRINTF_LONG |
if( cmd.parse == true && *p_fmt == 'l' ) |
{ |
cmd.uselong = true; |
continue; |
} |
#endif |
//------------------------ |
// END: parse cmd |
//------------------------ |
if( cmd.parse == true && (*p_fmt == 'd' || *p_fmt == 'u' || *p_fmt == 'x' || *p_fmt == 'b' || *p_fmt == 'o' || |
*p_fmt == 's' || *p_fmt == 'S' || *p_fmt == 'c' || *p_fmt == '%') ) |
{ |
cmd.cmd = *p_fmt; |
cmd.parse = false; |
*p_cmddigit = 0; |
if( cmd.point == false ) cmd.prelen = atoi(cmddigit); |
else cmd.declen = atoi(cmddigit); |
if( cmd.point || cmd.prelen>0 ) |
cmd.mask = true; |
} |
//######################## |
//# 2. EXECUTE |
//######################## |
//------------------------ |
// exec cmd: "d,u,x,b,o" |
//------------------------ |
if( cmd.cmd == 'd' || cmd.cmd == 'u' || cmd.cmd == 'x' || cmd.cmd == 'b' || cmd.cmd == 'o' ) |
{ |
if( cmd.uselong ) |
{ |
#ifdef USE_XPRINTF_LONG |
switch(cmd.cmd) |
{ |
case 'd': ltoa ( va_arg(ap, long) , argbuff, 10); break; // LONG dezimal int signed |
case 'u': ultoa( va_arg(ap, unsigned long), argbuff, 10); break; // LONG dezimal int unsigned |
case 'x': ultoa( va_arg(ap, unsigned long), argbuff, 16); break; // LONG hex int unsigned |
case 'b': ultoa( va_arg(ap, unsigned long), argbuff, 2); break; // LONG binary int unsigned |
case 'o': ultoa( va_arg(ap, unsigned long), argbuff, 8); break; // LONG octal int unsigned |
} |
#endif |
} |
else |
{ |
switch(cmd.cmd) |
{ |
case 'd': itoa( va_arg(ap, int) , argbuff, 10); break; // dezimal int signed |
case 'u': utoa( va_arg(ap, unsigned int), argbuff, 10); break; // dezimal int unsigned |
case 'x': utoa( va_arg(ap, unsigned int), argbuff, 16); break; // hex int unsigned |
case 'b': utoa( va_arg(ap, unsigned int), argbuff, 2); break; // binary int unsigned |
case 'o': utoa( va_arg(ap, unsigned int), argbuff, 8); break; // octal int unsigned |
} |
} |
minus = (argbuff[0] == '-'); |
arglen = strlen(argbuff); |
fmtlen = cmd.prelen + cmd.declen + (cmd.point ? 1 : 0); |
arglen = strlen(argbuff); |
overflow = cmd.mask && // Zahl zu gross -> "*" anzeigen statt der Zahl |
(arglen > cmd.prelen + cmd.declen || cmd.prelen < 1+minus); |
if( overflow ) // overflow: Zahl passt nicht in Maske |
{ // -> zeige '*.*' |
for( i=0; (i < fmtlen) && (dstcnt < n); i++) |
{ |
if( cmd.point && i==cmd.prelen ) *p_dst = '.'; |
else *p_dst = '*'; |
p_dst++; |
dstcnt++; |
} |
} |
else // else: if( overflow ) |
{ |
if( !cmd.mask ) // keine Maske: alles von der Zahl ausgeben |
fmtlen = arglen; |
p_argbuff = argbuff; |
if( minus ) // wenn Zahl negativ: merken und auf dem argbuff nehmen |
{ |
p_argbuff++; |
arglen--; |
} |
//----------------- |
// die Zahl wird 'Rueckwaerts' uebertragen |
//----------------- |
dec = -1; // wird am Anfang der Schleife auf 0 gesetzt |
j = 1; // zaehler fuer argbuff |
for( i=1; i<=fmtlen; i++ ) |
{ |
dec++; // Zaehler Dizmalstellen |
if( dstcnt+fmtlen-i <= n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( cmd.point && (dec == cmd.declen) ) // Dezimalpunkt setzen |
{ |
p_dst[fmtlen-i] = '.'; |
continue; |
} |
if( j <= arglen ) // Ziffer uebertragen aus argbuff |
{ |
p_dst[fmtlen-i] = p_argbuff[arglen-j]; |
j++; |
continue; |
} |
if( cmd.declen > 0 && // Nachkomma und 1. Vorkommastelle ggf. auf '0' |
(dec < cmd.declen || dec == cmd.declen+1) ) // setzen wenn die Zahl zu klein ist |
{ |
p_dst[fmtlen-i] = '0'; |
continue; |
} |
if( minus && ( (cmd.pad == ' ') || // ggf. Minuszeichen setzen |
(cmd.pad != ' ' && i == fmtlen) ) ) // Minuszeichen bei '0'-padding an erster Stelle setzen |
{ |
minus = false; |
p_dst[fmtlen-i] = '-'; |
continue; |
} |
p_dst[fmtlen-i] = cmd.pad; // padding ' ' oder '0' |
} // end: if( dstcnt+fmtlen-i <= n ) |
} |
p_dst += fmtlen; |
dstcnt += fmtlen; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "s", "S", "c" |
//------------------------ |
if( cmd.cmd == 's' || cmd.cmd == 'S' || cmd.cmd == 'c' ) |
{ |
switch(cmd.cmd) |
{ |
case 's': p_str = va_arg( ap, char *); // String aus dem RAM |
break; |
case 'S': strncpy_P( argbuff, va_arg( ap, char *), ARGBUFF_SIZE); // String liegt im progmem -> in's RAM kopieren |
argbuff[ARGBUFF_SIZE-1] = 0; |
p_str = argbuff; |
break; |
case 'c': argbuff[0] = va_arg( ap, int); // einzelnes char |
argbuff[1] = 0; |
p_str = argbuff; |
break; |
} |
fmtlen = cmd.prelen; |
arglen = strlen(p_str); |
if( !cmd.mask ) // keine Maske: alles vom String ausgeben |
fmtlen = arglen; |
for( i=0; i<fmtlen; i++) |
{ |
if( dstcnt < n ) // wenn Zielbuffer nicht ueberschritten |
{ |
if( *p_str ) // char uebertragen |
{ |
*p_dst = *p_str; |
p_str++; |
} |
else // padding |
{ |
*p_dst = ' '; |
} |
p_dst++; |
} |
dstcnt++; |
} |
continue; |
} |
//------------------------ |
// exec cmd: "%" |
//------------------------ |
if( cmd.cmd == '%' ) |
{ |
*p_dst = '%'; |
p_dst++; |
dstcnt++; |
continue; |
} |
} while( (dstcnt < n) && *p_fmt ); |
*(p_dst + (dstcnt < n ? 0 : -1)) = 0; // terminierende 0 im Ausgabebuffer setzen |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( false, buffer, n, format, ap); |
va_end(ap); |
} |
//--------------------------------------------------------------------- |
//--------------------------------------------------------------------- |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ) |
{ |
va_list ap; |
va_start(ap, format); |
_xvsnprintf( true, buffer, n, format, ap); |
va_end(ap); |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( false, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//----------------------------------------------------------- |
// buffered_sprintf_P( format, ...) |
// |
// Ausgabe direkt in einen internen Buffer. |
// Der Pointer auf den RAM-Buffer wird zurueckgegeben. |
// Abgesichert bzgl. Buffer-Overflow. |
// |
// Groesse des Buffers: PRINTF_BUFFER_SIZE |
// |
// Parameter: |
// format : String aus PROGMEM (siehe: xprintf in utils/xstring.h) |
// ... : Parameter fuer 'format' |
//----------------------------------------------------------- |
char * buffered_sprintf_P( const char *format, ... ) |
{ |
va_list ap; |
va_start( ap, format ); |
_xvsnprintf( true, sprintf_buffer, SPRINTF_BUFFER_SIZE, format, ap ); |
va_end(ap); |
return sprintf_buffer; |
} |
//-------------------------------------------------------------- |
// kopiert einen String von src auf dst mit fester Laenge und |
// ggf. Space paddings rechts |
// |
// - fuellt ggf. den dst-String auf size Laenge mit Spaces |
// - setzt Terminierung's 0 bei dst auf Position size |
//-------------------------------------------------------------- |
void strncpyfill( char *dst, const char *src, size_t size) |
{ |
uint8_t i; |
uint8_t pad = false; |
for( i=0; i<size; i++) |
{ |
if(*src == 0) pad = true; |
if( pad ) *dst = ' '; |
else *dst = *src; |
src++; |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// entfernt rechte Leerzeichen aus einem String |
//-------------------------------------------------------------- |
void strrtrim( char *dst) |
{ |
char *p; |
p = dst + strlen(dst) - 1; |
while( (p != dst) && (*p == ' ') ) p--; |
if( (*p != ' ') && (*p != 0) ) p++; |
*p = 0; |
} |
//-------------------------------------------------------------- |
// INTERN - fuer strncpyat(), strncpyat_P() |
//-------------------------------------------------------------- |
void _strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount, uint8_t progmem) |
{ |
uint8_t i; |
if( progmem ) |
strncpy_P( dst, src, size); |
else |
strncpy( dst, src, size); |
for( i=0; i<size; i++) |
{ |
if( *dst == 0) return; |
if( *dst == sepchar) |
{ |
sepcharcount--; |
if( sepcharcount==0 ) |
{ |
*dst = 0; |
return; |
} |
} |
dst++; |
} |
dst--; |
*dst = 0; |
} |
//-------------------------------------------------------------- |
// strncpyat( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in PROGMEM |
//-------------------------------------------------------------- |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, false); |
} |
//-------------------------------------------------------------- |
// strncpyat_P( dst, src, size, sepchar) |
// |
// kopiert einen String von 'src 'auf 'dst' mit max. Laenge 'size' |
// oder bis 'sepchar' gefunden wird. |
// |
// src in RAM |
//-------------------------------------------------------------- |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount) |
{ |
_strncpyat( dst, src, size, sepchar, sepcharcount, true); |
} |
//-------------------------------------------------------------- |
// UTCdatetime2local( PKTdatetime_t *dtbuffer, PKTdatetime_t dt ) |
// |
// konvertiert die UTC-Time 'dt' in die lokale Zeit und speichert |
// dieses in 'dtbuffer' ab. |
// |
// Parameter: |
// |
// dtdst: Pointer Destination (PKTdatetime_t) (Speicher muss alloziiert sein!) |
// dtsrc: Pointer Source (PKTdatetime_t) |
// |
// Hinweise: |
// |
// Schaltjahre (bzw. der 29.02.) werden nicht unterstuetzt |
//-------------------------------------------------------------- |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ) |
{ |
int8_t timeoffset; |
int32_t v; |
int8_t diff; |
// 01 02 03 04 05 06 07 08 09 10 11 12 Monat |
int8_t daymonth[] = {31,28,31,30,31,30,31,31,30,31,30,31}; |
//-------------------------- |
// timezone: Einstellbereich -12 .. 0 .. 12 (+1 = Berlin) |
// summertime: Einstellung: 0 oder 1 (0=Winterzeit, 1=Sommerzeit) |
//-------------------------- |
timeoffset = Config.timezone + Config.summertime; |
//timeoffset = 2; // solange noch nicht in PKT-Config: Berlin, Sommerzeit |
memcpy( dtdst, dtsrc, sizeof(PKTdatetime_t) ); // copy datetime to destination |
//-------------------------- |
// Zeitzonenanpassung |
//-------------------------- |
if( dtdst->year != 0 && dtdst->month >= 1 && dtdst->month <= 12 ) // nur wenn gueltiges Datum vorhanden |
{ |
//-------------------------- |
// 1. Sekunden |
//-------------------------- |
v = (int32_t)dtdst->seconds; |
v += timeoffset*3600; // Stunden korrigieren |
diff = 0; |
if( v > 86400 ) // Tagesueberschreitung? (86400 = 24*60*60 bzw. 24 Stunden) |
{ |
v -= 86400; |
diff++; // inc: Tag |
} |
else if( v < 0 ) // Tagesunterschreitung? |
{ |
v += 86400; |
diff--; // dec: Tag |
} |
dtdst->seconds = (uint32_t)v; // SET: seconds |
//-------------------------- |
// 2. Tag |
//-------------------------- |
v = (int32_t)dtdst->day; |
v += diff; |
diff = 0; |
if( v > daymonth[dtdst->month-1] ) // Monatsueberschreitung? |
{ |
v = 1; // erster Tag des Monats |
diff++; // inc: Monat |
} |
else if( v < 1 ) // Monatsunterschreitung? |
{ |
if( dtdst->month > 1 ) |
v = daymonth[dtdst->month-1-1]; // letzter Tag des vorherigen Monats |
else |
v = 31; // letzter Tag im Dezember des vorherigen Jahres |
diff--; // dec: Monat |
} |
dtdst->day = (uint8_t)v; // SET: day |
//-------------------------- |
// 3. Monat |
//-------------------------- |
v = (int32_t)dtdst->month; |
v += diff; |
diff = 0; |
if( v > 12 ) // Jahresueberschreitung? |
{ |
v = 1; |
diff++; // inc: Jahr |
} |
else if( v < 1 ) // Jahresunterschreitung? |
{ |
v = 12; |
diff--; // dec: Jahr |
} |
dtdst->month = (uint8_t)v; // SET: month |
//-------------------------- |
// 4. Jahr |
//-------------------------- |
dtdst->year += diff; // SET: year |
} |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/xutils.h |
---|
0,0 → 1,64 |
/***************************************************************************** |
* Copyright (C) 2013 Oliver Gemesi * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY xutils.h |
//# |
//# 12.04.2014 OG |
//# - chg: strncpyat(), strncpyat_P(), _strncpyat() erweitert um Parameter 'sepcharcount' |
//# |
//# 08.04.2014 OG |
//# - add: strncpyat(), strncpyat_P() |
//# |
//# 28.02.2014 OG |
//# - add: buffered_sprintf(), buffered_sprintf_P() |
//# |
//# 24.06.2013 OG |
//# - add: strrtrim() |
//# |
//# 04.05.2013 OG |
//# - chg: umbenannt zu xutils.h |
//# |
//# 03.05.2013 OG |
//# - add: UTCdatetime2local() |
//# |
//# 28.04.2013 OG - NEU |
//############################################################################ |
#ifndef _XUTILS_H |
#define _XUTILS_H |
#include <stdarg.h> |
void _xvsnprintf( uint8_t useprogmem, char *buffer, uint8_t n, const char *format, va_list ap ); |
void xsnprintf( char *buffer, uint8_t n, const char *format, ... ); |
void xsnprintf_P( char *buffer, uint8_t n, const char *format, ... ); |
char *buffered_sprintf( const char *format, ... ); |
char *buffered_sprintf_P( const char *format, ... ); |
void strncpyfill( char *dst, const char *src, size_t size); |
void strrtrim( char *dst); |
void strncpyat( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void strncpyat_P( char *dst, const char *src, size_t size, const char sepchar, uint8_t sepcharcount); |
void UTCdatetime2local( PKTdatetime_t *dtdst, PKTdatetime_t *dtsrc ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+maniacbug-mighty-1284p-68ed99c |
+ |
+Arduino |
+ |
+_doc |
+ |
+maniacbug-mighty-1284p-68ed99c.zip |
/Transportables_Koptertool/PKT/trunk/waypoints/waypoints.c |
---|
0,0 → 1,362 |
/*#######################################################################################*/ |
/* !!! THIS IS NOT FREE SOFTWARE !!! */ |
/*#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 2008 Ingo Busker, Holger Buss |
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY |
// + FOR NON COMMERCIAL USE ONLY |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdint.h> |
#include <string.h> |
#include <stdbool.h> |
//#include "91x_lib.h" |
#include "waypoints.h" |
#include "../mk-data-structs.h" |
#include "../uart/uart1.h" |
#include "../eeprom/eeprom.h" |
#include "../main.h" |
#include <util/delay.h> |
#ifdef USE_WAYPOINTS |
// the waypoints list |
NaviData_t *NaviData; |
//Point_t PointList[MAX_LIST_LEN]; |
uint8_t WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount |
uint8_t POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount |
uint8_t WPCount = 0; // number of waypoints |
uint8_t PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN |
uint8_t POICount = 0; |
uint8_t WPActive = false; |
uint8_t PointList_Init(void) |
{ |
return PointList_Clear(); |
} |
uint8_t PointList_Clear(void) |
{ |
uint8_t i; |
WPIndex = 0; // real list position are 1 ,2, 3 ... |
POIIndex = 0; // real list position are 1 ,2, 3 ... |
WPCount = 0; // no waypoints |
POICount = 0; |
PointCount = 0; // no contents |
WPActive = false; |
NaviData->WaypointNumber = WPCount; |
NaviData->WaypointIndex = 0; |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
Config.PointList[i].Position.Status = INVALID; |
Config.PointList[i].Position.Latitude = 0; |
Config.PointList[i].Position.Longitude = 0; |
Config.PointList[i].Position.Altitude = 0; |
Config.PointList[i].Heading = 361; // invalid value |
Config.PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered |
Config.PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
Config.PointList[i].Type = POINT_TYPE_INVALID; |
Config.PointList[i].Event_Flag = 0; // future implementation |
Config.PointList[i].AltitudeRate = 0; // no change of setpoint |
} |
return true; |
} |
uint8_t PointList_GetCount(void) |
{ |
return PointCount; // number of points in the list |
} |
Point_t* PointList_GetAt(uint8_t index) |
{ |
if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1])); // return pointer to this waypoint |
else return(NULL); |
} |
uint8_t PointList_SetAt(Point_t* pPoint) |
{ |
// if index is in range |
if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN)) |
{ |
// check list entry before update |
switch(Config.PointList[pPoint->Index-1].Type) |
{ |
case POINT_TYPE_INVALID: // was invalid |
switch(pPoint->Type) |
{ |
default: |
case POINT_TYPE_INVALID: |
// nothing to do |
break; |
case POINT_TYPE_WP: |
WPCount++; |
PointCount++; |
break; |
case POINT_TYPE_POI: |
POICount++; |
PointCount++; |
break; |
} |
break; |
case POINT_TYPE_WP: // was a waypoint |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
WPCount--; |
PointCount--; |
break; |
default: |
case POINT_TYPE_WP: |
//nothing to do |
break; |
case POINT_TYPE_POI: |
POICount++; |
WPCount--; |
break; |
} |
break; |
case POINT_TYPE_POI: // was a poi |
switch(pPoint->Type) |
{ |
case POINT_TYPE_INVALID: |
POICount--; |
PointCount--; |
break; |
case POINT_TYPE_WP: |
WPCount++; |
POICount--; |
break; |
case POINT_TYPE_POI: |
default: |
// nothing to do |
break; |
} |
break; |
} |
memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry |
NaviData->WaypointNumber = WPCount; |
return pPoint->Index; |
} |
else return(0); |
} |
// returns the pointer to the first waypoint within the list |
Point_t* PointList_WPBegin(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
if(WPActive == false) return(NULL); |
POIIndex = 0; // set invalid POI |
if(PointCount > 0) |
{ |
// search for first wp in list |
for(i = 0; i <MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
WPIndex = i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP in the list |
{ |
NaviData->WaypointIndex = 1; |
// update index to POI |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // some points in the list but no WP found |
{ |
NaviData->WaypointIndex = 0; |
//Check for an existing POI |
for(i = 0; i < MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID)) |
{ |
POIIndex = i + 1; |
break; |
} |
} |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns the last waypoint |
Point_t* PointList_WPEnd(void) |
{ |
uint8_t i; |
WPIndex = 0; // set list position invalid |
POIIndex = 0; // set invalid |
if(WPActive == false) return(NULL); |
if(PointCount > 0) |
{ |
// search backward! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
WPIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
if(WPIndex) // found a WP within the list |
{ |
NaviData->WaypointIndex = WPCount; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
} |
else // list contains some points but no WP in the list |
{ |
// search backward for a POI! |
for(i = 1; i <= MAX_LIST_LEN; i++) |
{ |
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID)) |
{ |
POIIndex = MAX_LIST_LEN - i + 1; |
break; |
} |
} |
NaviData->WaypointIndex = 0; |
} |
} |
else // no point in the list |
{ |
POIIndex = 0; |
NaviData->WaypointIndex = 0; |
} |
if(WPIndex) return(&(Config.PointList[WPIndex-1])); |
else return(NULL); |
} |
// returns a pointer to the next waypoint or NULL if the end of the list has been reached |
Point_t* PointList_WPNext(void) |
{ |
uint8_t wp_found = 0; |
if(WPActive == false) return(NULL); |
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list |
{ |
uint8_t i; |
for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry |
{ |
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs |
{ |
wp_found = i+1; |
break; |
} |
} |
} |
if(wp_found) |
{ |
WPIndex = wp_found; // update list position |
NaviData->WaypointIndex++; |
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint |
} |
else |
{ // no next wp found |
NaviData->WaypointIndex = 0; |
POIIndex = 0; |
return(NULL); |
} |
} |
void PointList_WPActive(uint8_t set) |
{ |
if(set) |
{ |
WPActive = true; |
PointList_WPBegin(); // uopdates POI index |
} |
else |
{ |
WPActive = false; |
POIIndex = 0; // disable POI also |
} |
} |
Point_t* PointList_GetPOI(void) |
{ |
return PointList_GetAt(POIIndex); |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/waypoints/waypoints.h |
---|
0,0 → 1,56 |
#ifndef _WAYPOINTS_H |
#define _WAYPOINTS_H |
//#include "ubx.h" |
#include "../mk-data-structs.h" |
#define POINT_TYPE_INVALID 255 |
#define POINT_TYPE_WP 0 |
#define POINT_TYPE_POI 1 |
#define INVALID 0x00 |
//typedef struct |
//{ |
// int32_t Longitude; // in 1E-7 deg |
// int32_t Latitude; // in 1E-7 deg |
// int32_t Altitude; // in mm |
// uint8_t Status;// validity of data |
//} __attribute__((packed)) GPS_Pos_t; |
//typedef struct |
//{ |
// GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details |
// int16_t Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List |
// uint8_t ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered |
// uint8_t HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered |
// uint8_t Event_Flag; // future implementation |
// uint8_t Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
// uint8_t Type; // typeof Waypoint |
// uint8_t WP_EventChannelValue; // |
// uint8_t AltitudeRate; // rate to change the setpoint |
// uint8_t reserve[8]; // reserve |
//} __attribute__((packed)) Point_t; |
// Init List, return TRUE on success |
uint8_t PointList_Init(void); |
// Clear List, return TRUE on success |
uint8_t PointList_Clear(void); |
// Returns number of points in the list |
uint8_t PointList_GetCount(void); |
// return pointer to point at position |
Point_t* PointList_GetAt(uint8_t index); |
// set a point in the list at index, returns its index on success, else 0 |
uint8_t PointList_SetAt(Point_t* pPoint); |
// goto the first WP in the list and return pointer to it |
Point_t* PointList_WPBegin(void); |
// goto the last WP in the list and return pointer to it |
Point_t* PointList_WPEnd(void); |
// goto next WP in the list and return pointer to it |
Point_t* PointList_WPNext(void); |
// enables/disables waypoint function |
void PointList_WPActive(uint8_t set); |
// returns pointer to actual POI |
Point_t* PointList_GetPOI(void); |
#endif // _WAYPOINTS_H |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wi232/Wi232.c |
---|
0,0 → 1,451 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.c |
//# |
//# 15.06.2014 OG |
//# -chg: umgestellt auf PKT_Progress_Init() / PKT_Progress_Next() |
//# |
//# 10.06.2014 OG |
//# KOMPLETTE SANIERUNG ab v3.80cX5 |
//# (ehemaliger Code in v3.80cX4 oder frueher) |
//# - add: Wi232_ConfigPC() - ehemals in connect.c als Port_USB2CFG_Wi() |
//# |
//# 06.06.2014 OG |
//# - add: Wi232_Write_Progress() |
//# - chg: Codeformattierung |
//# |
//# 05.06.2014 OG |
//# -chg: Codeformattierung |
//# |
//# 14.05.2014 OG |
//# - chg: InitErrorWi232(), InitWi232() - umgestellt auf dezimale Anzeige des |
//# Fehlercodes damit der einfacher im Source zu identifizieren ist |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <util/delay.h> |
#include <stdlib.h> |
#include <string.h> |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../uart/usart.h" |
#include "../uart/uart1.h" |
#include "../main.h" |
#include "../wi232/Wi232.h" |
#include "../timer/timer.h" |
#include "../eeprom/eeprom.h" |
#include "../setup/setup.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "../lipo/lipo.h" |
#include "../utils/menuctrl.h" |
//############################################################################ |
//############################################################################ |
static const char STR_WI232INIT[] PROGMEM = "Wi.232 Init"; |
//############################################################################ |
//############################################################################ |
//-------------------------------------------------------------- |
// lError = WriteWi232( Wi232Register, RegisterValue) |
// |
// set Register to Wi232, Register, Value |
// |
// Returns: |
// 0 = ACK, FF = NAK |
//-------------------------------------------------------------- |
uint8_t Wi232_Write( uint8_t Wi232Register, uint8_t RegisterValue ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
unsigned int v; |
if( RegisterValue < 0x80 ) |
{ |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(Wi232Register); |
USART_putc(RegisterValue); |
} |
else // RegisterValue >= 0x80 |
{ |
USART_putc(0xff); |
USART_putc(0x03); |
USART_putc(Wi232Register); |
USART_putc(0xfe); |
USART_putc(RegisterValue - 0x80); |
} |
do |
{ |
v = USART_getc(); // ACK erwartet |
_delay_ms(100); |
tc++; |
} while( (v == 0) && (tc != timeout) ); |
if( v != 0x06 ) |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
//PKT_Message_P( PSTR("Wi.232 NAK"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen (wird nicht mehr angezeigt da uebernommen von hoeheren Funktionen) |
return 0xFF; // return: ERROR |
} |
return 0; // return: OK (v==0x06) |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t Wi232_Write_Progress( uint8_t Wi232Register, uint8_t RegisterValue, uint8_t errorcode ) |
{ |
uint8_t nError; |
// nError = 0: ok |
// nError > 0: Fehler |
nError = Wi232_Write( Wi232Register, RegisterValue ); |
PKT_Progress_Next(); // Progressbar |
if( nError ) |
{ |
nError = errorcode; |
} |
return nError; |
} |
//-------------------------------------------------------------- |
// Registervalue = Wi232_Read( Wi232Register ) |
// |
// send Readcommand to Wi232, |
// |
// Returns: |
// Registervalue: 0 = timeout 0xFF = Syntaxerror |
//-------------------------------------------------------------- |
uint8_t Wi232_Read( uint8_t Wi232Register ) |
{ |
uint8_t timeout = 10; |
uint8_t tc = 0; |
uint8_t v; |
v = USART_getc(); // Zeichen loeschen |
USART_putc(0xff); |
USART_putc(0x02); |
USART_putc(0xfe); |
USART_putc(Wi232Register); |
_delay_ms(50); |
//lcd_printpns_at (0, 2, PSTR("read Wi232"),0); |
do |
{ |
v = USART_getc(); //ACK erwartet |
_delay_ms(100); |
tc++; |
} while( v==0 && tc!=timeout ); |
if( tc == timeout ) |
return 0; // Timeout |
if( v != 0x06 ) |
return 0xFF; // Syntaxerror |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Register*/ |
// lcd_print_hex(v,0); |
// v = USART_getc(); /*Value*/ |
// lcd_print_hex(v,0); |
return v; |
} |
//-------------------------------------------------------------- |
// foundbaud = Wi232_CheckBaud( Baudrate ) |
// |
// RUECKGABE: |
// =0: Error (Wi.232 not found at given Baud) |
// >0: Baudrate |
// |
// INFO: Baudrate |
// Baud_9600 1 |
// Baud_19200 2 |
// Baud_38400 3 |
// Baud_57600 4 |
// Baud_115200 5 |
// Baud_4800 6 |
// Baud_2400 7 |
//-------------------------------------------------------------- |
uint16_t Wi232_CheckBaud( uint8_t Baudrate ) // Suche Wi232 mit entsprechender Baudrate |
{ |
uint8_t RegisterWi232; |
//lcdx_cls_row( y, mode, yoffs ) |
lcdx_cls_row( 2, MNORMAL, 1 ); // Zeile loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, PSTR("%S %lu Baud"), strGet(STR_SEARCH), Baud_to_uint32(Baudrate) ); // "suche nnn Baud" |
SetBaudUart0( Baudrate ); // UART auf angegebene Baudrate setzen |
RegisterWi232 = Wi232_Read( regDiscover ); |
//--------------------------------------------------- |
// Hinweis zu RegisterWi232 nach regDiscover |
// == 0 : "no Wi.232 found" |
// == 0xFF: "Wi.232 Syntaxerror" |
// |
// >0 && < 0xff: Wi.232 Version (Wi.232 gefunden) |
//--------------------------------------------------- |
if( RegisterWi232 > 0 && RegisterWi232 < 0xFF ) |
{ |
return Baudrate; // Wi.232 gefunden |
} |
return 0; // Wi.232 nicht gefunden bei gegebener Baudrate |
} |
//-------------------------------------------------------------- |
// InitWI232( InitBaudrate ) |
// |
// set Wi232Register for Mikrokopter |
// |
// Returns: |
// 0 = ACK, FF = NAK ????????? |
//-------------------------------------------------------------- |
void Wi232_Initalize( void ) |
{ |
uint8_t foundbaud = 0; |
uint8_t nError = 0; |
uint8_t InitBaudrate = Config.PKT_Baudrate; |
//---------------------------- |
// 1. Kopter ausgeschaltet? |
//---------------------------- |
// Text1: "Kopter ausschalten!" |
// Text2: NULL |
// Head : "* ACHTUNG *" |
// Titel: "Wi.232 Init" |
if( !PKT_Ask_P( ASK_CANCEL_OK, strGet(STR_SWITCHOFFMK), NULL, strGet(STR_ATTENTION), STR_WI232INIT) ) |
{ |
return; |
} |
//---------------------------- |
// 2. Wi.232 suchen |
//---------------------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
Change_Output( Uart02Wi ); // Verbindung zu Wi232 herstellen |
SwitchToWi232(); // Serielle Kanaele Wi232 mit USB verbinden |
set_WI232CMD(); |
_delay_ms(100); |
//_delay_ms(200); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Old_Baudrate ); // versuche zuerst mit der 'alten' Baudrate (hohe Trefferwahrscheinlichkeit) |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_2400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_9600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_19200 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_38400 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_57600 ); |
if( !foundbaud ) foundbaud = Wi232_CheckBaud( Baud_115200 ); |
if( !foundbaud ) // gefunden? |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 not found"), true, 2000, true, true); // ERROR: max. 20 Sekunden anzeigen |
} |
//---------------------------- |
// 3. Wi.232 initialisieren |
//---------------------------- |
if( foundbaud > 0 ) |
{ |
//-------------- |
// Screen |
//-------------- |
PKT_Title_P( STR_WI232INIT, true, true ); // Titel - mit ShowLipo und clearscreen |
lcdx_printf_center_P( 2, MNORMAL, 0,1, STR_WI232INIT ); |
//PKT_Progress_Init( max, yoffs, width, height); |
PKT_Progress_Init( 18, 0, 0,0); // 18 Progress Steps |
// DEBUG - hier kann ggf. die _gefundene_ Baudrate vom Wi.232 angezeigt werden |
//lcdx_printf_center_P( 6, MNORMAL, 0,-4, PSTR("(found %lu Baud)"), Baud_to_uint32(foundbaud) ); // zeige Baudrate |
// DEBUG - und hier auf welche Baudrate das Wi.232 gesetzt werden soll |
//lcdx_printf_center_P( 7, MNORMAL, 0,-2, PSTR("(set %lu Baud)"), Baud_to_uint32(Config.PKT_Baudrate) ); // zeige Baudrate |
SetBaudUart0( foundbaud ); |
// wenn sich ein EEPROM-Wert ändert wird auch das Ram beschrieben damit die Änderung sofort wirksam wird |
//-------------------------------------------------------------------------------------------------------------------- |
// + Errorcode |
// | |
// | |
if( !nError ) nError = Wi232_Write_Progress( regNVTXCHANNEL,Config.WiTXRXChannel, 1); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regTXCHANNEL,Config.WiTXRXChannel , 2); // TX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVRXCHANNEL,Config.WiTXRXChannel, 3); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regRXCHANNEL,Config.WiTXRXChannel , 4); // RX Channel |
if( !nError ) nError = Wi232_Write_Progress( regNVSLPMODE ,Sleep_Awake , 5); // Sleepmode |
if( !nError ) nError = Wi232_Write_Progress( regNVPWRMODE,WbModeP15 , 6); // Transceiver Mode/Powermode |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: TWaitTime16 (=0x10 // 16 ms Delay (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVTXTO,Config.WiTXTO , 7); // UART Timeout |
if( !nError ) nError = Wi232_Write_Progress( regTXTO,Config.WiTXTO , 8); // UART Timeout |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: UartMTU64 (=64 // default=64, valid 1-144) |
if( !nError ) nError = Wi232_Write_Progress( regNVUARTMTU,Config.WiUartMTU , 9); // UART Buffer |
if( !nError ) nError = Wi232_Write_Progress( regUARTMTU,Config.WiUartMTU ,10); // UART Buffer |
//--------- |
// 06.06.2014 OG: nicht mehr unterstuetzt via setup.c (ggf. spaeter festen Standardwert einsetzen statt Config.xyz) |
// Standard: NetMode_Normal (=0x01 // Normalmode (default)) |
if( !nError ) nError = Wi232_Write_Progress( regNVNETMODE,Config.WiNetworkMode ,11); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNETMODE,Config.WiNetworkMode ,12); // Networkmode |
if( !nError ) nError = Wi232_Write_Progress( regNVUSECRC ,CRC_Enable ,13); // CRC |
if( !nError ) nError = Wi232_Write_Progress( regNVCSMAMODE,CSMA_En ,14); // CSMA |
if( !nError ) nError = Wi232_Write_Progress( regNVNETGRP,Config.WiNetworkGroup ,15); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNETGRP,Config.WiNetworkGroup ,16); // Networkgroup |
if( !nError ) nError = Wi232_Write_Progress( regNVDATARATE,InitBaudrate ,17); // Baudrate |
_delay_ms(200); |
if( !nError ) nError = Wi232_Write_Progress( regDATARATE,InitBaudrate ,18); // Baudrate |
_delay_ms(250); |
//_delay_ms(6000); // DEBUG |
if( nError ) |
{ |
//PKT_Message( text, error, timeout, beep, clearscreen) |
PKT_Message( buffered_sprintf_P(PSTR("Wi.232 Error: %d"),nError), true, 2000, true, true); // ERROR: 2000 = max. 20 Sekunden anzeigen |
} |
else |
{ |
//PKT_Message_P( text, error, timeout, beep, clearscreen) |
PKT_Message_P( PSTR("Wi.232 Init OK!"), false, 1000, true, true); // OK: 1000 = max. 10 Sekunden anzeigen |
WriteWiInitFlag(); // erfolgreiches Init merken |
} |
} |
clr_WI232CMD(); |
SetBaudUart0( Config.PKT_Baudrate ); // die richtige UART Baudrate wiederherstellen !! |
clear_key_all(); |
} |
//-------------------------------------------------------------- |
// Wi232_ConfigPC() |
// |
// Connect USB direct to Wi.232 in Progmode |
//-------------------------------------------------------------- |
void Wi232_ConfigPC( void ) |
{ |
lcd_cls (); |
lcd_frect( 0, 0, 127, 7, 1); // Titelzeile Invers |
lcdx_printf_at_P( 1, 0, MINVERS, 0,0, strGet(STR_WI232CONFIG) ); // Titeltext "Wi.232 Konfig." |
lcdx_printp_center( 2, strGet(STR_USBCONNECTED), MNORMAL, 0,-2); // "mit USB verbunden" |
lcdx_printp_center( 4, strGet(STR_SEEMKWIKI) , MNORMAL, 0,0); // "siehe MK-Wiki:" |
lcdx_printp_center( 5, PSTR("\"RadioTronix\"") , MNORMAL, 0,2); // "RadioTronix" |
lcd_rect_round( 0, 28, 127, 24, 1, R2); // Rahmen um Text |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline "Ende" |
Change_Output(USB2Wi); |
set_WI232CMD(); // Port D6 = CMD |
while(!get_key_press(1 << KEY_ESC)) |
{ |
show_Lipo(); |
} |
clr_WI232CMD(); // Port D6 = CMD |
if( Config.U02SV2 == 1 ) |
Change_Output(Uart02FC); |
else |
Change_Output(Uart02Wi); |
// 10.06.2014 OG: das Folgende war im orginalen Port_USB2CFG_Wi() nicht |
// vorhanden - das hatte den Effekt, dass wenn man diese |
// Funktion aufgerufen und wieder beendet hatte das |
// PKT-Updatetool kein PKT-Update mehr via PKT_CtrlHook() |
// durchfuehren konnte. Die USB Umschaltung war wohl nicht sauber. |
// |
uart1_init( UART_BAUD_SELECT(USART_BAUD,F_CPU) ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
SetBaudUart1( Config.PKT_Baudrate ); // USB wieder zuruecksetzen damit PKT wieder normal via USB erreichbar (u.a. fuer PKT-Updatetool) |
} |
//#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wi232/Wi232.h |
---|
0,0 → 1,192 |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY Wi232.h |
//# |
//# 10.06.2014 OG |
//# - add: Wi232_Initalize() |
//# - add: Wi232_ConfigPC() |
//# - del: InitWi232() |
//# |
//# 08.06.2014 OG |
//# - chg: InitWi232() - Parameteraenderung |
//# - del: Wi232_New_Baudrate |
//# - add: Source-Historie ergaenzt |
//############################################################################ |
#ifndef WI232_H_ |
#define WI232_H_ |
// Non-volatile Registers |
// Name Address Description Default |
#define regNVTXCHANNEL 0x00 // Transmit channel setting ## 0 ## |
#define regNVRXCHANNEL 0x01 // Receive channel setting ## 0 ## |
#define regNVPWRMODE 0x02 // Operating mode settings ## +13 dBm widebandmode ## |
#define regNVDATARATE 0x03 // UART data rate ## 2400bps ## |
#define regNVNETMODE 0x04 // Network mode (Normal/Slave) ## Normal ## |
#define regNVTXTO 0x05 // Transmit wait timeout ## ~16ms ## |
#define regNVNETGRP 0x06 // Network group ID ## 0x00 ## |
#define regNVUSECRC 0x08 // Enable/Disable CRC ## Enabled ## |
#define regNVUARTMTU 0x09 // Minimum transmission unit. ## 64 bytes ## |
#define regNVSHOWVER 0x0A // Enable/Disable start-up message ## Enabled ## |
#define regNVCSMAMODE 0x0B // Enable/Disable CSMA ## Enabled ## |
#define regNVSLPMODE 0x0D // Power state of module ## Awake ## |
#define regNVACKONWAKE 0x0E // Send ACK character to host on wake |
// Non-volatile Read Only Registers |
// Name Address Description |
#define regMAC0 0x22 // These registers form the unique 48-bit MAC address. |
#define regMAC1 0x23 // MAC |
#define regMAC2 0x24 // MAC |
#define regOUI0 0x25 // MAC |
#define regOUI1 0x26 // MAC |
#define regOUI2 0x27 // MAC |
#define regDiscover 0x78 // Versionsregister |
// Volatile Read/Write Registers |
// Name Address Description |
#define regTXCHANNEL 0x4B // Transmit channel setting |
#define regRXCHANNEL 0x4C // Receive channel setting |
#define regPWRMODE 0x4D // Operating mode settings |
#define regDATARATE 0x4E // UART data rate |
#define regNETMODE 0x4F // Network mode (Normal or Slave) |
#define regTXTO 0x50 // Transmit wait timeout |
#define regNETGRP 0x51 // Network group ID |
#define regUSECRC 0x53 // Enable/Disable CRC |
#define regUARTMTU 0x54 // Minimum transmission unit. |
#define regSHOWVER 0x55 // Enable/Disable start-up message |
#define regCSMAMODE 0x56 // Enable/disable CSMA |
#define regSLPMODE 0x58 // Power state of module |
#define regACKONWAKE 0x59 // Send ACK character to host on wake |
// Wideband Channels |
// regNVTXCHAN (0x00) regTXCHAN (0x4B) |
// Channel Number Frequency |
#define wChan0 0x00 // 868.300 MHz |
#define wChan1 0x01 // 868.95 MHz ## MK ## |
// Narrowband Channels |
// regNVRXCHAN (0x01) regRXCHAN (0x4C) |
// Channel Number Frequency |
#define nChan0 0x00 // 868.225 MHz |
#define nChan1 0x01 // 868.375 MHz ## MK ## |
#define nChan2 0x02 // 868.850 MHz |
#define nChan3 0x03 // 869.050 MHz |
#define nChan4 0x04 // 869.525 MHz |
#define nChan5 0x05 // 869.850 MHz |
// Power Mode |
// regNVPWRMODE (0x02) regPWRMODE (0x4D) |
// PM1 PM1 PM0 Mode |
#define NbModeN0 0x00 // 0 0 0 Narrowband Mode 0dBm power setting (typical) |
#define WbModeP5 0x01 // 0 0 1 Wideband Mode +5dBm power setting (typical) |
#define WbModeP10 0x02 // 0 1 0 Wideband Mode +10dBm power setting (typical) |
#define WbModeP15 0x03 // 0 1 1 Wideband Mode +15dBm power setting (typical) ## MK ## |
#define WbModeN0 0x04 // 1 0 0 Wideband Mode 0dBm power setting (typical) |
#define NbModeP5 0x05 // 1 0 1 Narrowband Mode +5dBm power setting (typical) |
#define NbModeP10 0x06 // 1 1 0 Narrowband Mode +10dBm power setting (typical) |
#define NbModeP15 0x07 // 1 1 1 Narrowband Mode +15dBm power setting (typical) |
// Wi232 UART Baudrate |
// regNVDATARATE (0x03) regDATARATE (0x4E) |
// Baud Rate BR2 BR1 BR0 |
#define Wi232_2400 Baud_2400 // 0 0 0* (default 2400) |
#define Wi232_9600 Baud_9600 // 0 0 1 |
#define Wi232_19200 Baud_19200 // 0 1 0 |
#define Wi232_38400 Baud_38400 // 0 1 1 |
#define Wi232_57600 Baud_57600 // 1 0 0 ## MK ## |
#define Wi232_115200 Baud_115200 // 1 0 1 |
#define Wi232_10400 0x06 // 1 1 0 |
#define Wi232_31250 0x07 // 1 1 1 |
// NetworkMode |
// regNVNETMODE (0x04) regNETMODE (0x4F) |
#define NetMode_Slave 0x00 // Slavemode |
#define NetMode_Normal 0x01 // Normalmode (default) |
// Transmit Wait Timeout |
// regNVTXTO (0x05) regTXTO (0x50) |
#define TWaitTimeFull 0x00 // full Buffer required |
#define TWaitTime16 0x10 // 16 ms Delay (default) |
// Network Group |
// regNVNETGRP (0x06) regNETGRP (0x51) |
#define NetWorkGroup 66 // default = 0, valid 0-127 ## MK = 66 ## |
// CRC Control |
// regNVUSECRC (0x08) regUSECRC (0x53) |
#define CRC_Disable 0x00 // no CRC check |
#define CRC_Enable 0x01 // CRC check (default) |
// UART minimum transmission unit |
// regNVUARTMTU (0x09) regUARTMTU (0x54) |
#define UartMTU64 64 // default=64, valid 1-144 |
// Verbose mode |
// regNVSHOWVER (0x0A) |
#define ShowVers_Dis 0x00 // do not show Startupmessage ## MK = 66 ## |
#define ShowVers_En 0x01 // show Startupmessage (default) |
// CSMA enable |
// regNVCSMAMODE (0x0B) regCSMAMODE (0x56) |
#define CSMA_Dis 0x00 // disable CSMA Carrier-sense multiple access |
#define CSMA_En 0x01 // enable CSMA (default) |
// Sleep control |
// regNVSLPMODE (0x0D) regSLPMODE (0x58) |
#define Sleep_Awake 0x00 // Sleepmode = Awake (default) |
#define Sleep 0x01 // Sleepmode = Sleep |
#define Sleep_Stby 0x02 // Sleepmode = Standby |
// ACK on Wake |
// regNVACKONWAKE (0x0D) regACKONWAKE (0x59) |
#define ACKwake_Dis 0x00 // disable ACK on Wake |
#define ACKwake_En 0x01 // enable ACK on Wake (default) |
//---------------------------------- |
// EXPORT |
//---------------------------------- |
void Wi232_Initalize( void ); |
void Wi232_ConfigPC( void ); |
#endif // WI232_H_ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wifly/PKT_WiFlyHQ.c |
---|
0,0 → 1,1406 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/** |
* @file WiFly RN-XV Library |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.c |
//# |
//# 03.06.2014 OG |
//# - fix: find_WiFly() - Benutzer kann Suche nach WiFly jetzt abbrechen |
//# (z.b. weil er kein WiFly angesteckt hat) |
//# von dieser Aenderungen betroffen sind auch noch einige Codechanges |
//# an anderen Funktionen. |
//# |
//# 15.04.2014 OG |
//# - chg: wifly_init() - Logik bei der Versionsverifikation von WIFLY_ADHOC |
//# geaendert da v2.38 ansonsten ggf. den Bach runter gegangen waere. |
//# AKTUELL UNGETESTET DA KEIN WIFLY ADHOC VORHANDEN! |
//# - add: Wifly_update() ein paar Kommentare im Code |
//# - add: KOMMENTAR zu define DEBUG_WIFLY ! (lesenswert!) |
//# |
//# 14.04.2014 Cebra |
//# - add: Defines für getestete WiFly Softwareversionen |
//# - add: wifly_init bei der Versionsabfrage erweitert |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: define DELAY_BEFORE_SAVE |
//# - add: define DELAY_BEFORE_REBOOT |
//# |
//# 02.04.2014 OG |
//# WIP: etliche weitere Aenderungen.... |
//# - chg: getVersion(): Rueckgabe/Logik geaendert |
//# - add: define DELAY_AFTER_MODUL_ON |
//# |
//# 01.04.2014 SH |
//# - chg: Nicht benötigte Progmem Strings auskommentiert |
//# |
//# 11.03.2014 SH |
//# - add: neue Funktionen wl_reset() und wl_update() |
//# - chg: wl_init() wurde komplett auf den neuen AP Modus geändert, die Init Routine vom Ad Hoc Modus befindet sich ausgeklammert darunter |
//# - chg: getVersion wurde angepasst + neuer Wert für resp_Version[] PROGMEM = "RN-171\r\n" anstatt WiFly |
//# - chg: setSSID() und setPassphrase() wurden um weiteren Parameter ergänzt |
//# - chg: createAdhocNetwork angepasst da Funktion setSSID() enthalten |
//# - del: ausgeklammerte Funktionen getConnection, getDHCPMode, getSpaceReplace, getopt_Asc und join |
//# |
//# 12.02.2014 OG |
//# - chg: readTimeout() Auskommentiert: "unused variable 'ind'" |
//# - chg: wl_init() Auskommentiert: "unused variable 'dhcpMode'" |
//# |
//# 02.07.2013 cebra |
//# - new: Routinen für WiFly WLAN-Modul |
//# |
//########################################################################### |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "PKT_WiFlyHQ.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../timer/timer.h" |
#include "../lcd/lcd.h" |
#include "../messages.h" |
#include "../pkt/pkt.h" |
#include "../utils/xutils.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//---------------------------------------------------- |
// DEBUG |
// |
// Wenn 'DEBUG_WIFLY' eingeschalte wird dann werden die Ausgaben |
// des WiFly auf dem PKT-Screen angezeigt - fast wie bei einem |
// Terminal Programm! |
// |
// Man kann dadurch genaueres erkennen wie die Kommunikation |
// ablaeuft und mit welchen Strings beispielsweise das WiFly |
// ein Ok quittiert. |
//---------------------------------------------------- |
//#define DEBUG_WIFLY |
//---------------------------------------------------- |
// unterstützte WiFly Softwareversionen |
//---------------------------------------------------- |
#define Adhoc_Version1 232 // getestet Version 2.32 |
#define Adhoc_Version2 238 // getestet Version 2.38 |
#define Apmode_Version1 441 // getestet Version 4.41 |
//---------------------------------------------------- |
// Einstellungen |
//---------------------------------------------------- |
static const char WIFLY_IP_ADDRESS[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_PORT[] PROGMEM = "2000"; |
static const char WIFLY_IP_GATEWAY[] PROGMEM = "169.254.1.1"; |
static const char WIFLY_IP_NETMASK[] PROGMEM = "255.255.0.0"; |
static const char WIFLY_VERSION_ADHOC[] PROGMEM = "2.38.3"; |
//static const char WIFLY_VERSION_ADHOC[] PROGMEM = "v2.32"; |
static const char WIFLY_VERSION_APMODE[] PROGMEM = "4.41"; |
//---------------------------------------------------- |
// Einsstellungen fuer Delay's |
//---------------------------------------------------- |
//#define WIFLY_DEFAULT_TIMEOUT 500 /* 500 milliseconds */ |
#define WIFLY_DEFAULT_TIMEOUT 100 // n milliseconds |
//---------------------------------------------------- |
//---------------------------------------------------- |
uint8_t progress_max = 0; |
uint8_t progress_act = 0; |
char progress_buffer[22]; |
char buffer[20]; |
char wifly_version_string[12]; |
uint16_t wifly_version; |
//---------------------------------------------------- |
// Texte |
//---------------------------------------------------- |
static const char STR_WIFLY_NOT_FOUND[] PROGMEM = "WiFly not found"; |
static const char RECEIVE_AOK[] PROGMEM = "AOK"; |
#define REPLACE_SPACE_CHAR '$' // ersetzt Spaces in einem String-Parameter des WiFly - '$' ist WiFly default (0x24) fuer SSID und Pwd |
//static const char [] PROGMEM = ""; |
//---------------------------------------------------- |
//############################################################################# |
//# |
//############################################################################# |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_begin( void ) |
{ |
lcd_cls(); |
lcd_setpos(0,0); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_wait( void ) |
{ |
set_beep( 25, 0xffff, BeepNormal ); |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) && !get_key_press(1 << KEY_ENTER) ); |
lcd_print_LF(); |
} |
//---------------------------------------------------- |
//---------------------------------------------------- |
void debug_message( uint8_t *text ) |
{ |
lcd_print_LF(); |
lcd_print( text, MINVERS ); |
debug_wait(); |
lcd_print_LF(); |
} |
//############################################################################# |
//# |
//############################################################################# |
/** Read the next character from the WiFly serial interface. |
* Waits up to timeout milliseconds to receive the character. |
* @param chp pointer to store the read character in |
* @param timeout the number of milliseconds to wait for a character |
* @retval true - character read |
* @retval false - timeout reached, character not read |
*/ |
bool readTimeout( char *chp, uint16_t timeout) |
{ |
char ch; |
timer2 = timeout; |
while( timer2>0 ) |
{ |
if( uart1_available() > 0 ) |
{ |
ch = uart1_getc(); |
*chp = ch; |
#ifdef DEBUG_WIFLY |
lcd_print_char( ch, MNORMAL); |
#endif |
return (true); |
} |
} |
return (false); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void flushRx( int timeout ) |
{ |
char ch; |
while( readTimeout( &ch, timeout) ); |
} |
/** |
* Read characters from the WiFly and match them against the |
* progmem string. Ignore any leading characters that don't match. Keep |
* reading, discarding the input, until the string is matched |
* or until no characters are received for the timeout duration. |
* @param str The string to match, in progmem. |
* @param timeout fail if no data received for this period (in milliseconds). |
* @retval true - a match was found |
* @retval false - no match found, timeout reached |
*/ |
bool match_P( const char *str, uint16_t timeout) |
{ |
const char *match = str; |
char ch, ch_P; |
ch_P = pgm_read_byte(match); |
if( ch_P == '\0' ) return true; /* Null string always matches */ |
while( readTimeout( &ch, timeout) ) |
{ |
// DEBUG |
//lcd_print_char( ch, MNORMAL ); |
if( ch == ch_P ) match++; |
else |
{ |
match = str; // Restart match |
if( ch == pgm_read_byte(match) ) match++; |
} |
ch_P = pgm_read_byte( match ); |
if( ch_P == '\0' ) return (true); |
} |
return (false); |
} |
//############################################################################# |
//# progress |
//############################################################################# |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_begin( uint8_t maxsteps ) |
{ |
progress_max = maxsteps; |
progress_act = 0; |
} |
//-------------------------------------------------------------- |
// wait4key: true / false |
//-------------------------------------------------------------- |
void progress_end( uint8_t wait4key ) |
{ |
progress_max = 0; |
progress_act = 0; |
if( wait4key ) |
{ |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
} |
//-------------------------------------------------------------- |
// msg: PROGMEM |
//-------------------------------------------------------------- |
void progress_show( const char *msg ) |
{ |
int8_t yoffs = 3; |
#ifdef DEBUG_WIFLY |
return; |
#endif |
if( progress_max ) |
{ |
progress_act++; |
lcdx_printf_center_P( 4, MNORMAL, 0,yoffs, PSTR("%d/%d"), progress_act, progress_max ); |
if( msg ) |
{ |
strncpyat_P( progress_buffer, msg, 20, ' ', 2); |
lcd_frect( 2, (5*8)+yoffs, 124, 7, 0); // Zeile 5 loeschen |
lcdx_print_center( 5, (uint8_t *)progress_buffer, MNORMAL, 0,yoffs); // Text zentriert; String im RAM |
} |
lcd_rect_round( 0, 28+yoffs, 127, 22, 1, R2); // Rahmen |
_delay_ms(700); // fuer Anzeige (Entschleunigung) |
} |
} |
//-------------------------------------------------------------- |
// gibt zentriert einen Text und loescht vorher die Zeile |
//-------------------------------------------------------------- |
void MsgCenter_P( uint8_t y, const char *text, uint8_t mode, int8_t yoffs ) |
{ |
lcdx_cls_row( y, MNORMAL, yoffs ); |
lcdx_printp_center( y, text, MNORMAL, 0, yoffs ); |
} |
//############################################################################# |
//# sendcmd |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void cmd_showerror( const char *cmdstr) |
{ |
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen ) |
PKT_Message_P( cmdstr, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t find_Prompt() |
{ |
char c; |
uint8_t state = 0; |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( state == 0 && (c == '>') ) { state = 1; continue; } |
if( state == 1 && (c == ' ') ) return true; |
if( state == 1 ) break; // Fehler: nicht vorgesehen |
} |
return false; |
} |
//-------------------------------------------------------------- |
// nicht verwendet... |
//-------------------------------------------------------------- |
uint8_t find_Prompt2() |
{ |
char c; |
timer2 = 800; // 8 Sekunden max. suchen |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) && timer2 ) |
{ |
if( c == '>' ) return true; |
} |
return false; |
} |
//-------------------------------------------------------------- |
// Das ist erstmal so uebernommen - ob das alles sein muss?? |
// |
// Put the WiFly into command mode |
// |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t cmd_EnterCommandMode( void ) |
{ |
uint8_t retry = 2; |
int8_t result = 0; // false |
while( retry && !result ) |
{ |
//_delay_ms(250); |
uart1_puts_p( PSTR("$$$") ); |
//_delay_ms(250); |
_delay_ms(100); |
uart1_putc('\r'); |
if( get_key_short(1 << KEY_ESC) ) // Abbruch durch Benutzer? |
{ |
return -1; // -1 = Abbruch Benutzer |
} |
result = find_Prompt(); // ggf. result = true |
if( !result ) |
{ |
retry--; |
_delay_ms(250); |
} |
} |
return result; |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// cmdstr : PROGMEM |
// strvalue : RAM |
// match_receive: PROGMEM |
// findprompt : true/false |
//-------------------------------------------------------------- |
uint8_t cmdsend( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
uint8_t result = true; |
const char *p; |
char c; |
progress_show( cmdstr ); |
//-------------------- |
// Senden... |
//-------------------- |
uart1_puts_p( cmdstr ); |
if( strvalue != NULL ) |
{ |
uart1_putc(' '); |
p = strvalue; |
while( *p ) |
{ |
c = *p; |
if( c == ' ' ) c = REPLACE_SPACE_CHAR; |
uart1_putc( c ); |
p++; |
} |
} |
uart1_putc('\r'); |
//-------------------- |
// Antwort... |
//-------------------- |
if( match_receive ) |
{ |
//if( !match_P( p, WIFLY_DEFAULT_TIMEOUT) ) |
if( !match_P( match_receive, 2000) ) |
{ |
cmd_showerror( cmdstr ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
result = false; |
} |
// DEBUG |
//lcdx_printf_at_P( 2, 7, MINVERS, 0,0, PSTR("%d"), result ); |
} |
#ifdef DEBUG_WIFLY |
debug_wait(); |
#endif |
// TODO: Kommandos die keinen Prompt zurueckliefern? (evtl. 'reset'?) |
//uart1_putc('\r'); // fuer cmd_Join ein extra Return... (bei den anderen Kommandos stoert es nicht) |
if( result && findprompt) |
{ |
result = find_Prompt(); |
} |
return( result ); |
} |
//-------------------------------------------------------------- |
// cmdsend_P() |
// |
// PARAMETER: |
//-------------------------------------------------------------- |
uint8_t cmdsend_P( const char *cmdstr, const char *strvalue, const char *match_receive, uint8_t findprompt ) |
{ |
strncpy_P( buffer, strvalue, 20); |
return cmdsend( cmdstr, buffer, match_receive, findprompt ); |
} |
//-------------------------------------------------------------- |
// Version wird anhand des Prompt's ermittelt |
// Implementiert als State-Machine |
//-------------------------------------------------------------- |
uint8_t cmd_GetVersion( void ) |
{ |
char c; |
uint8_t state = 0; |
char *dst = wifly_version_string; |
uint8_t n = 0; |
wifly_version = 0; |
uart1_putc('\r'); // erzwinge Prompt |
while( readTimeout( &c, WIFLY_DEFAULT_TIMEOUT) ) |
{ |
if( (state == 0) && (c == '<') ) |
{ |
state = 1; |
continue; |
} |
if( (state == 1) && (c != '>') ) |
{ |
// TODO: Ende vom Prompt im Fehlerfall catchen... |
if( n+1 >= sizeof(wifly_version_string) ) |
break; |
*dst = c; |
dst++; |
n++; |
continue; |
} |
if( (state == 1) && (c == '>') ) // Versionsstring-Ende erreicht |
{ |
*dst = 0; |
state = 2; |
break; |
} |
} |
if( state == 2 ) |
{ |
//uint16_t wifly_version; |
wifly_version = (wifly_version_string[0]-'0') * 100; |
wifly_version += (wifly_version_string[2]-'0') * 10; |
wifly_version += (wifly_version_string[3]-'0') * 1; |
} |
else |
{ |
cmd_showerror( PSTR("get version") ); // Fehler auf PKT Screen anzeigen und durch Benutzer bestaetigen lassen |
} |
return (state == 2); |
} |
//-------------------------------------------------------------- |
// cmd_Set() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_Set( const char *cmdstr ) |
{ |
return cmdsend( cmdstr, 0, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : RAM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr( const char *cmdstr, const char *str ) |
{ |
return cmdsend( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetStr() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// str : PROGMEM |
//-------------------------------------------------------------- |
uint8_t cmd_SetStr_P( const char *cmdstr, const char *str ) |
{ |
return cmdsend_P( cmdstr, str, RECEIVE_AOK, true ); |
} |
//-------------------------------------------------------------- |
// cmd_SetNum() |
// |
// PARAMETER: |
// cmdstr: PROGMEM |
// value : Dezimal |
//-------------------------------------------------------------- |
uint8_t cmd_SetNum( const char *cmdstr, const uint16_t value ) |
{ |
utoa( value, buffer, 10 ); |
return cmd_SetStr( cmdstr, buffer); |
} |
//-------------------------------------------------------------- |
// cmd_Reset() |
//-------------------------------------------------------------- |
uint8_t cmd_Reset( void ) |
{ |
uint8_t result = true; |
//_delay_ms( 500 ); |
result = cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), false ); |
_delay_ms( 1000 ); |
return result; |
// return cmdsend( PSTR("factory RESET"), 0, PSTR("Defaults"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Save() |
//-------------------------------------------------------------- |
uint8_t cmd_Save( void ) |
{ |
//_delay_ms( DELAY_BEFORE_SAVE ); |
_delay_ms( 200 ); |
return cmdsend( PSTR("save"), 0, PSTR("Storing"), true ); |
} |
//-------------------------------------------------------------- |
// cmd_Join() |
//-------------------------------------------------------------- |
uint8_t cmd_Join( const char *ssid ) |
{ |
uint8_t result = true; |
result = cmdsend( PSTR("join"), ssid, PSTR("Associated"), false ); |
if( result ) uart1_putc('\r');; |
if( result ) result = find_Prompt(); |
return result; |
} |
//-------------------------------------------------------------- |
// cmd_Reboot() |
//-------------------------------------------------------------- |
uint8_t cmd_Reboot( void ) |
{ |
//_delay_ms( DELAY_BEFORE_REBOOT ); |
_delay_ms( 300 ); |
cmdsend( PSTR("reboot"), 0, 0, false ); |
_delay_ms( 2000 ); |
return true; |
} |
//############################################################################# |
//# WiFly suchen |
//############################################################################# |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
void find_WiFly_searchmsg( const char *text, uint8_t baud ) |
{ |
lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcdx_printf_center_P( 2, MNORMAL, 0,0, PSTR("%S %lu Baud"), text, Baud_to_uint32(baud) ); // Text zentriert |
} |
//-------------------------------------------------------------- |
// RUECKGABE: |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
//-------------------------------------------------------------- |
int8_t find_WiFly_baud( uint8_t baud ) |
{ |
int8_t result = 1; |
find_WiFly_searchmsg( strGet(STR_SEARCH), baud ); |
// Anmerkung 06.04.2014 OG: |
// SetBaudUart1() (in uart/uart1.c) setzt nur die Baudrate des Uart. |
// Config.PKT_Baudrate wird durch SetBaudUart1() NICHT geaendert! |
SetBaudUart1( baud ); |
result = cmd_EnterCommandMode(); |
if( result > 0 ) |
{ |
return baud; // Baud gefunden, ok! |
} |
return result; // nicht gefunden oder Abbruch durch Benutzer |
} |
//-------------------------------------------------------------- |
// ok = find_WiFly() |
// |
// Sucht das WiFly mit verschiedenen Baudraten. |
// |
// Wenn das WiFly mit von Config.PKT_Baudrate (= 57600) abweichender |
// Baudrate gefunden wird (z.B. 9600 Baud) dann wird das WiFly |
// automatisch auf 57600 Baud umkonfiguriert damit nachfolgende |
// Funktionen das nicht mehr beachten muessen. |
// |
// RUECKGABE: |
// ok = true : WiFly gefunden und ist auf 57600 Baud |
// ok = false: kein WiFly gefunden |
// |
// >0: ok |
// =0: Fehler |
// -1: Abbruch Benutzer |
// |
// Anmerkung: |
// #define Baud_9600 1 |
// #define Baud_19200 2 |
// #define Baud_38400 3 |
// #define Baud_57600 4 |
// #define Baud_115200 5 |
// #define Baud_4800 6 |
// #define Baud_2400 7 |
//-------------------------------------------------------------- |
int8_t find_WiFly( void ) // search Wifly with all Baudrates |
{ |
int8_t result = 1; // 1 == true |
int8_t baud = 0; |
//inCommandMode = false; |
//exitCommand = 0; |
set_Modul_On( Wlan ); // WiFly einschalten |
timer2 = 60; // Delay damit das WiFly booten kann |
while( timer2 ); |
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline: "Ende" |
//------------------------ |
// suche WiFly mit versch. Baudraten |
//------------------------ |
if( baud==0 ) baud = find_WiFly_baud( Baud_57600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_9600 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_115200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_38400 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_19200 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_4800 ); |
if( baud==0 ) baud = find_WiFly_baud( Baud_2400 ); |
lcdx_cls_row( 7, MNORMAL, 0); // Keyline loeschen |
//------------------------ |
// Abbruch durch Benutzer? |
//------------------------ |
if( baud < 0 ) |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
return -1; // -1 = Abbruch Benutzer |
} |
//------------------------ |
// kein WiFly gefunden... |
//------------------------ |
if( baud==0 ) // WiFly nicht gefunden :-( |
{ |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
set_Modul_On( USB ); // wieder auf USB zurueckschalten |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( STR_WIFLY_NOT_FOUND, true, 3000, true, true ); // max. 30 Sekunden anzeigen |
return false; // 0 = nicht gefunden |
} |
//------------------------ |
// WiFly gefunden! |
//------------------------ |
find_WiFly_searchmsg( strGet(STR_FOUND), baud ); // Anzeige: "gefunden..." |
//------------------------ |
// WiFly Baud ok? |
// -> ggf. WiFly Baud umkonfigurieren |
//------------------------ |
if( Config.PKT_Baudrate != baud ) |
{ |
find_WiFly_searchmsg( strGet(STR_SET), Config.PKT_Baudrate ); // Anzeige: "setze..." |
if( result ) result = cmd_Set( PSTR("set u b 57600") ); // -> etwas unsauber hier einfach 57600 zu setzen und nicht Config.PKT_Baudrate |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); // true = uart-baud setzen |
SetBaudUart1( Config.PKT_Baudrate ); // orginal Baudrate von Uart1 wieder herstellen |
if( result ) result = cmd_EnterCommandMode(); |
} |
return result; |
} |
//############################################################################# |
//# |
//############################################################################# |
//-------------------------------------------------------------- |
// zeigt die WiFly-Version an |
//-------------------------------------------------------------- |
uint8_t wifly_version_show( void ) |
{ |
int8_t result = 1; // 1 = true |
ShowTitle_P( PSTR("WiFly Version"), true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); // Ergebnis in 'wifly_version_string' |
set_Modul_On( USB ); |
if( result ) |
{ |
// TODO: |
// Anzeige ob WiFly-Version kompatibel zum gewaehlten |
// Mode (AP-Mode, AdHoc) ist |
//lcd_frect( 0, 2*8, 126, 7, 0); // Zeile 2 loeschen |
lcd_rect_round( 0, 8*4+3, 127, 8*2, 1, R2); // Anzeige-Rahmen |
lcdx_printf_center_P( 5, MNORMAL, 0,0, PSTR("Version: v%s"), wifly_version_string ); |
lcd_printp_at( 12, 7, strGet(ENDE), MNORMAL); // Keyline |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !get_key_press(1 << KEY_ESC) ); // warte auf Taste... |
} |
else |
{ |
PKT_Message_P( PSTR("Version FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t _ask( uint8_t keyline ) |
{ |
uint8_t ask = 0; |
lcd_rect_round( 0, 34, 127, 16, 1, R2); // Rahmen |
lcd_printp_at( 12, 7, strGet(keyline), MNORMAL); // Keyline |
if( keyline == ENDE ) |
set_beep( 1000, 0xffff, BeepNormal ); // langer Error-Beep |
else |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
clear_key_all(); |
while( !ask ) // warte auf Ja/Nein |
{ |
if( get_key_press(1 << KEY_ENTER) ) ask = 1; // "Ja" |
if( get_key_press(1 << KEY_ESC) ) ask = 2; // "Nein" |
}; |
return( ask==1 ); |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_init_perform( uint8_t wiflymode ) //AP Mode |
{ |
uint8_t result = true; |
#ifndef DEBUG_WIFLY |
MsgCenter_P( 2, PSTR("INIT"), MNORMAL, 0 ); |
#endif |
//--------------- |
// WIFLY_ADHOC |
//--------------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
progress_begin( 9 ); |
//if( result ) result = cmd_Set( PSTR("set u m 1") ); // ist das notwendig? - (turn off echo) |
//if( result ) result = cmd_Set( PSTR("set sys printlvl 0") ); // ist das notwendig? |
//if( result ) result = cmd_Set( PSTR("set comm remote 0") ); // ist das notwendig? |
if( result ) result = cmd_Set( PSTR("set wlan join 4") ); // 4 = AdHoc |
if( result ) result = cmd_Set( PSTR("set ip dhcp 0") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_SetStr( PSTR("set wlan ssid"), Config.Wlan_SSID ); |
if( result ) result = cmd_SetNum( PSTR("set wlan chan"), Config.Wlan_Channel ); |
} |
//--------------- |
// WIFLY_APMODE |
//--------------- |
if( wiflymode == WIFLY_APMODE ) |
{ |
progress_begin( 13 ); |
if( result ) result = cmd_Set( PSTR("set wlan join 7") ); |
if( result ) result = cmd_Set( PSTR("set ip dhcp 4") ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip address"), WIFLY_IP_ADDRESS ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip gateway"), WIFLY_IP_GATEWAY ); |
if( result ) result = cmd_SetStr_P( PSTR("set ip netmask"), WIFLY_IP_NETMASK ); |
if( result ) result = cmd_Set( PSTR("set wlan rate 3") ); |
if( result ) result = cmd_SetStr( PSTR("set apmode ssid") , Config.Wlan_SSID ); |
if( result ) result = cmd_SetStr( PSTR("set apmode pass") , Config.Wlan_Password ); |
if( result ) result = cmd_SetNum( PSTR("set wlan channel"), Config.Wlan_Channel ); |
if( result ) result = cmd_Set( PSTR("set ip protocol 0x3") ); |
if( result ) result = cmd_Set( PSTR("set apmode link_monitor 0") ); |
} |
if( result ) result = cmd_Save(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
return( result ); |
} |
//-------------------------------------------------------------- |
// ok = wifly_init( wiflymode ) |
// |
// initialisiert das WiFly fuer AP-Mode oder AdHoc |
// |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_init( uint8_t wiflymode ) //AP Mode |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
uint8_t versionerror = false; |
char *title = (char *)PSTR("Init WiFly"); |
char *strMode; |
if( wiflymode == WIFLY_ADHOC ) strMode = (char *)PSTR("AdHoc"); |
else strMode = (char *)PSTR("AP-Mode"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Version pruefen |
//----------------------------- |
if( result ) |
{ |
//-------- |
// ADHOC |
//-------- |
if( wiflymode == WIFLY_ADHOC ) |
{ |
if( (wifly_version != Adhoc_Version1) // 2.32 |
&& (wifly_version != Adhoc_Version2) // 2.38 |
) |
{ |
versionerror = true; |
} |
} |
//-------- |
// APMODE |
//-------- |
if( wiflymode == WIFLY_APMODE && wifly_version != Apmode_Version1 ) versionerror = true; |
if( versionerror ) |
{ |
set_Modul_On( USB ); |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -1, PSTR("ERROR") ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("Version no %S!"), strMode ); // "Version no AP-Mode" / "AdHoc" |
ask = _ask( ENDE ); |
return false; |
} |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("WiFly v%s"), wifly_version_string ); // WiFly Versionsanzeige |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: OK") , strMode ); // "AP-Mode" oder "AdHoc" |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?") , title ); // "Init WiFly?" |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Init durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
result = wifly_init_perform( wiflymode ); |
} |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Init FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Init OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return( result ); |
} |
//-------------------------------------------------------------- |
// PARAMETER: |
// wiflymode: WIFLY_APMODE = Firmware wifly7-441.img |
// WIFLY_ADHOC = Firmware wifly7-2383.img |
//-------------------------------------------------------------- |
uint8_t wifly_update( uint8_t wiflymode ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Update WiFly"); |
const char *pStr; |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
if( result ) result = cmd_GetVersion(); |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
if( wiflymode == WIFLY_ADHOC ) pStr = WIFLY_VERSION_ADHOC; |
else pStr = WIFLY_VERSION_APMODE; |
ShowTitle_P( title, true ); |
//lcdx_printf_at_P( 0, 1, MINVERS, 0,0, PSTR("%d"), wifly_version ); |
lcdx_printf_center_P( 2, MNORMAL, 0, -4, PSTR("%S: v%s"), strGet(STR_VON), wifly_version_string ); |
lcdx_printf_center_P( 3, MNORMAL, 0, -2, PSTR("%S: v%S"), strGet(STR_NACH), pStr ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
//----------------------------- |
// Update durchfuehren |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
progress_begin( 16 ); |
} |
//---------------------------- |
// 1. Reset |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("Reset"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
if( result ) result = find_WiFly(); |
//---------------------------- |
// 2. mit Heimnetz verbinden |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("verbinde Heimnetz"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ip dhcp 1") ); |
if( result ) result = cmd_SetStr( PSTR("set wlan phrase"), Config.Wlan_HomePassword ); |
if( result ) result = cmd_Set( PSTR("set wlan channel 0") ); |
if( result ) result = cmd_Join( Config.Wlan_HomeSSID ); |
//---------------------------- |
// 3. FTP Download & Update |
//---------------------------- |
#ifndef DEBUG_WIFLY |
if( result ) MsgCenter_P( 2, PSTR("FTP Update"), MNORMAL, 0 ); |
#endif |
if( result ) result = cmd_Set( PSTR("set ftp address 198.175.253.161") ); |
if( result ) result = cmd_Set( PSTR("set ftp dir public") ); |
if( result ) result = cmd_Set( PSTR("set ftp user roving") ); |
if( result ) result = cmd_Set( PSTR("set ftp pass Pass123") ); |
if( result ) result = cmd_Set( PSTR("set ftp timeout 800") ); |
//if( result ) result = cmd_Set( PSTR("del config") ); |
if( result ) |
{ |
//----- |
// das kann einiges gekuerzt werden wenn das denn zuverlaessig laufen wuerde... |
// |
// hier wird auf v4.40 geprueft da laut Refrenz-Manual erst seit dieser Version |
// "cupdate" unterstuetzt wird. Bei einem Downgraden von v4.4 auf eine aeltere |
// Version ist ein clean des Dateisystems ('c') erforderlich da sonst das WiFly |
// nur via Hardware-Taster an GPIO9 wiedre zum Leben erweckt werden kann. |
// |
// Anmerkung: evtl. wuer auch ein "del config" reichen - das ist aber nicht |
// hinreichend getestet |
//----- |
if( wifly_version >= 440 ) |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp cupdate wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp cupdate wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
else |
{ |
if( wiflymode == WIFLY_ADHOC ) |
result = cmdsend( PSTR("ftp update wifly7-2383.img"), 0, PSTR("UPDATE OK"), false ); |
else |
result = cmdsend( PSTR("ftp update wifly7-441.img") , 0, PSTR("UPDATE OK"), false ); |
} |
} |
if( result ) result = cmd_Reset(); |
if( result ) { timer2 = 50; while(timer2); }; |
/* |
//----- |
// Baustelle / Experimente bzgl. FTP-Update |
//----- |
//debug_message("FTP END"); |
//debug_message("RESET"); |
//if( result ) result = cmd_Reboot(); |
if( result ) { timer2 = 100; while(timer2); }; |
if( result ) { timer2 = 200; while(timer2); }; |
debug_message("FINDPR"); |
uart1_putc('\r'); |
if( result ) result = find_Prompt(); |
if( result ) debug_message("FINDPR END: 1"); |
else debug_message("FINDPR END: 0"); |
*/ |
//progress_end( true ); // Progress abschalten & warten |
progress_end( false ); // Progress abschalten |
//---------------------------- |
// 4. Init |
//---------------------------- |
/* |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) set_Modul_On( USB ); |
if( result ) { timer2 = 10; while(timer2); }; |
if( result ) ShowTitle_P( title, true ); |
if( result ) result = result = find_WiFly(); |
if( result ) result = wifly_init_perform( wiflymode ); |
*/ |
set_Modul_On( USB ); |
if( !result ) |
PKT_Message_P( PSTR("Update FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
else |
PKT_Message_P( PSTR("Update OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
clear_key_all(); |
return result; |
} |
//-------------------------------------------------------------- |
//-------------------------------------------------------------- |
uint8_t wifly_reset( void ) |
{ |
int8_t result = 1; // 1 = true |
uint8_t ask = false; |
char *title = (char *)PSTR("Reset WiFly"); |
ShowTitle_P( title, true ); |
result = find_WiFly(); |
if( result < 0 ) // Abbruch durch Benutzer? |
{ |
clear_key_all(); |
return result; |
} |
//----------------------------- |
// Benutzer fragen |
//----------------------------- |
if( result ) |
{ |
ShowTitle_P( title, true ); |
lcdx_printf_center_P( 5, MNORMAL, 0, -1, PSTR("%S?"), title ); |
ask = _ask( NOYES ); |
if( !ask ) // "Nein" |
{ |
set_Modul_On( USB ); |
return false; |
} |
} |
ShowTitle_P( title, true ); |
if( result ) progress_begin( 2 ); |
if( result ) result = cmd_Reset(); |
if( result ) result = cmd_Reboot(); |
progress_end( false ); // Progress abschalten |
set_Modul_On( USB ); |
if( result ) |
{ |
//PKT_Message_P( *text, error, timeout, beep, clearscreen ) |
PKT_Message_P( PSTR("Reset OK!"), false, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
else |
{ |
PKT_Message_P( PSTR("Reset FAIL!"), true, 3000, true, true ); // max. 30 Sekunden anzeigen |
} |
clear_key_all(); |
return result; |
} |
//############################################################################# |
//# TEST / DEBUG |
//############################################################################# |
//-------------------------------------------------------------- |
// nur zum testen! |
//-------------------------------------------------------------- |
uint8_t wifly_test( void ) |
{ |
uint8_t result = true; |
return result; |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wifly/PKT_WiFlyHQ.h |
---|
0,0 → 1,89 |
/*- |
* Copyright (c) 2012,2013 Darran Hunt (darran [at] hunt dot net dot nz) |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* |
* THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY |
* AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
* THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; |
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR |
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
*/ |
/* Release history |
* |
* Version Date Description |
* 0.1 25-Mar-2012 First release. |
* 0.2 09-Apr-2012 Added features to support http servers. |
* - added an httpserver.ino example. |
* - added sendChunk() and sendChunkln() to send chunked HTTP bodies. |
* - added terminal() method for simple terminal access via debug stream |
* - replaced getFreeMemory() with simpler version that works with 0 bytes |
* - turned peek buffer into a circular buffer to fix bug with detecting |
* *CLOS* and *OPEN* after a partial match. |
* - Added new TCP connection detection via *OPEN* match from available(). |
* isConnected() can now be polled until a client connects. |
* - made the match() function public, handy for matching text in a stream. |
* - Added a getProtocol() function to get current set of protocols. |
* 0.3 21-Apr-2012 Added createAdhocNetwork() to create an Ad Hoc WiFi network. |
* Optimised the setopt() and getopt() function so they handle |
* integer conversions and refactored all of the set and get functions. |
* Added a multMatch_P() function to match serial data against multiple |
* progmem strings. |
* Added failure detection to the join() function to quickly detect |
* a failure rather than relying on a timeout. |
* Added setJoin() and getJoin() function for access to the wlan join parameter. |
* Refactored getres() to use the new multiMatch_P() function. |
* |
*/ |
/** |
* @mainpage WiFlyHQ WiFly RN-XV Arduino library |
* |
* This library provides functions for setting up and managing the WiFly module, |
* sending UDP packets, opening TCP connections and sending and receiving data |
* over the TCP connection. |
* |
* @author Harlequin-Tech |
*/ |
//############################################################################ |
//# HISTORY PKT_WiFlyHQ.h |
//# |
//# 13.04.2014 OG |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 02.04.2014 OG |
//# - add: Versionshistorie ergaenzt |
//########################################################################### |
#ifndef _WIFLYHQ_H_ |
#define _WIFLYHQ_H_ |
// Todo: Strings verfuegbar machen für connect.c |
//extern const char WIFLY_IP_ADDRESS[]; |
//extern const char WIFLY_IP_PORT[]; |
uint8_t wifly_update( uint8_t wiflymode ); |
uint8_t wifly_reset(void); |
uint8_t wifly_version_show( void ); |
uint8_t wifly_init( uint8_t wiflymode ); |
uint8_t wifly_test( void ); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wifly/wifly_setup.c |
---|
0,0 → 1,424 |
/* |
* wifly_setup.c |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
/***************************************************************************** |
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de * |
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
* Copyright (C) 2011 Harald Bongartz * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
* * |
* Credits to: * |
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
* http://www.mikrokopter.de * |
* Gregor "killagreg" Stobrawa for his version of the MK code * |
* Thomas Kaiser "thkais" for the original project. See * |
* http://www.ft-fanpage.de/mikrokopter/ * |
* http://forum.mikrokopter.de/topic-4061-1.html * |
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
* http://www.mylifesucks.de/oss/c-osd/ * |
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
*****************************************************************************/ |
//############################################################################ |
//# HISTORY wifly_setup.c |
//# |
//# 08.06.2014 OG |
//# - chg: Setup_WiFly() - Tipptext fuer Module-Installed ergaenzt |
//# |
//# 04.06.2014 OG |
//# - chg: Menuetext angepasst; Menue-Separator hinzugefuegt |
//# |
//# 31.05.2014 OG |
//# - chg: Setup's auf geaendertes Edit_String() umgestellt (Param 'Text' entfernt) |
//# |
//# 28.05.2014 OG |
//# - chg: Setup's auf das neue Edit_generic() umgestellt |
//# |
//# 13.04.2014 OG |
//# - ggf. Probleme beim Downgrade des WiFly auf v2.38 -> deaktiviert |
//# - FAST KOMPLETT NEUER CODE |
//# |
//# 03.04.2014 OG |
//# - add: #include "../pkt/pkt.h" |
//# |
//# 30.03.2014 OG |
//# - chg: Text von WL_INSTALLED geaendert von "Modul eingebaut?" auf "Modul vorhanden?" |
//# -> das Modul wird aufgesteckt und nicht eingebaut... |
//# - chg: ein paar englische Texte geaendert |
//# - chg: Sprache Hollaendisch vollstaendig entfernt |
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
//# |
//# 11.03.2014 SH |
//# - add: neue Menüpunkte Reset, Version und Update |
//# - chg: Menüpunkt Passwort wurde wieder aktiviert |
//# - chg: beim Menüpunkt Kanal fängt die Auswahl jetzt bei 0 an (Modul sucht sich automatisch den besten Kanal) |
//# |
//# 12.02.2014 OG |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'z'" |
//# - chg: Setup_WiFly() Auskommentiert: "unused variable 'i'" |
//# |
//# 02.07.2013 cebra |
//# - new: wifly_setup(): Setup für WiFly WLAN-Modul |
//########################################################################### |
#include "../cpu.h" |
#include <avr/io.h> |
#include <avr/interrupt.h> |
//#include <avr/pgmspace.h> |
//#include <string.h> |
//#include <util/delay.h> |
//#include <string.h> |
#include <avr/pgmspace.h> |
#include <stdbool.h> |
#include <stdlib.h> |
#include <stdarg.h> |
#include <util/delay.h> |
#include <inttypes.h> |
#include <string.h> |
#include "../main.h" |
#include "../messages.h" |
#include "../lcd/lcd.h" |
#include "../pkt/pkt.h" |
#include "../utils/menuctrl.h" |
#include "../utils/xutils.h" |
#include "../eeprom/eeprom.h" |
#include "../uart/uart1.h" |
#include "../setup/setup.h" |
#include "../wifly/PKT_WiFlyHQ.h" |
#include "wifly_setup.h" |
#ifdef USE_WLAN |
//char buffer[20]; // fuer Versionsabfrage |
//----------------------------- |
// Setup_WiFly() (WLAN) |
//----------------------------- |
#define WL_INSTALLED 1 |
#define WL_SSID 2 |
#define WL_PASSWORD 3 |
#define WL_CHANNEL 4 |
#define WL_INIT 5 |
#define WL_RESET 6 |
#define WL_VERSION 7 |
#define WL_UPDATE 8 |
#define WL_PCCONFIG 9 |
#define WL_WPA 10 |
#define WL_HOMESSID 11 |
#define WL_HOMEPASSWORD 12 |
#define WL_TEST 99 |
static const char WL_INSTALLED_de[] PROGMEM = "WiFly Modus"; |
static const char WL_INSTALLED_en[] PROGMEM = "WiFly mode"; |
static const char WL_SSID_de[] PROGMEM = "SSID"; |
#define WL_SSID_en WL_SSID_de |
static const char WL_PASSWORD_de[] PROGMEM = "Passwort"; |
static const char WL_PASSWORD_en[] PROGMEM = "Password"; |
static const char WL_HOMESSID_de[] PROGMEM = "Home SSID"; |
#define WL_HOMESSID_en WL_HOMESSID_de |
static const char WL_HOMEPASSWORD_de[] PROGMEM = "Home Passwort"; |
static const char WL_HOMEPASSWORD_en[] PROGMEM = "Home Password"; |
static const char WL_CHANNEL_de[] PROGMEM = "Kanal"; |
static const char WL_CHANNEL_en[] PROGMEM = "Channel"; |
static const char WL_VERSION_de[] PROGMEM = "zeige Version"; |
static const char WL_VERSION_en[] PROGMEM = "show version"; |
static const char WL_UPDATE_de[] PROGMEM = "Update"; |
#define WL_UPDATE_en WL_UPDATE_de |
static const char WL_PCCONFIG_de[] PROGMEM = "WiFly einschalten"; |
static const char WL_PCCONFIG_en[] PROGMEM = "WiFly power on"; |
static const char WL_WPA_de[] PROGMEM = "Sicherheit"; |
static const char WL_WPA_en[] PROGMEM = "Security"; |
//############################################################################ |
//-------------------------------------------------------------- |
// Setup_WiFly_MenuCreate() |
// |
// das Menue aendert sich je nachdem ob WiFly ein- oder |
// ausgeschaltet ist |
//-------------------------------------------------------------- |
void Setup_WiFly_MenuCreate( void ) |
{ |
//--------------- |
// Create |
//--------------- |
MenuCtrl_Create(); |
if( Config.UseWL == WIFLY_APMODE ) MenuCtrl_SetTitle_P( PSTR("WiFly AP-Mode")); |
else if( Config.UseWL == WIFLY_ADHOC ) MenuCtrl_SetTitle_P( PSTR("WiFly AdHoc")); |
else MenuCtrl_SetTitle_P( PSTR("WiFly")); |
//--------------- |
// Menuitems |
//--------------- |
MenuCtrl_PushML2_P( WL_INSTALLED , MENU_ITEM, NOFUNC , WL_INSTALLED_de , WL_INSTALLED_en ); |
if( Config.UseWL ) |
{ |
//MenuCtrl_Push_P( WL_TEST , MENU_ITEM, NOFUNC , PSTR("TEST") ); |
MenuCtrl_PushML2_P( WL_VERSION , MENU_ITEM, NOFUNC , WL_VERSION_de , WL_VERSION_en ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_SSID , MENU_ITEM, NOFUNC , WL_SSID_de , WL_SSID_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_PushML2_P( WL_PASSWORD, MENU_ITEM, NOFUNC , WL_PASSWORD_de , WL_PASSWORD_en ); |
MenuCtrl_PushML2_P( WL_CHANNEL , MENU_ITEM, NOFUNC , WL_CHANNEL_de , WL_CHANNEL_en ); |
if( Config.UseWL == WIFLY_APMODE ) |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AP-Mode") ); |
else |
MenuCtrl_Push_P( WL_INIT , MENU_ITEM, NOFUNC , PSTR("INIT: AdHoc") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_PushML2_P( WL_HOMESSID , MENU_ITEM, NOFUNC , WL_HOMESSID_de , WL_HOMESSID_en ); |
MenuCtrl_PushML2_P( WL_HOMEPASSWORD, MENU_ITEM, NOFUNC , WL_HOMEPASSWORD_de , WL_HOMEPASSWORD_en ); |
MenuCtrl_Push_P( WL_UPDATE , MENU_ITEM, NOFUNC , PSTR("UPDATE: WiFly") ); |
MenuCtrl_PushSeparator(); |
MenuCtrl_Push_P( WL_RESET , MENU_ITEM, NOFUNC , PSTR("RESET: WiFly") ); |
MenuCtrl_PushML2_P( WL_PCCONFIG , MENU_ITEM, &Port_WiFly2Wi , WL_PCCONFIG_de , WL_PCCONFIG_en ); |
//MenuCtrl_PushML2_P( WL_WPA , MENU_ITEM, NOFUNC , WL_WPA_de , WL_WPA_en ); |
if( Config.UseWL == WIFLY_ADHOC ) |
{ |
// aktuell kann es zu Problemen kommen wenn das WiFly |
// auf v2.38 gedowngraded wird (WiFly nicht mehr ansprechbar) |
// -> deshalb: deaktiviert |
MenuCtrl_ItemActive( WL_UPDATE, false ); |
} |
} |
} |
//-------------------------------------------------------------- |
// Setup_WiFly() |
//-------------------------------------------------------------- |
void Setup_WiFly( void ) |
{ |
uint8_t itemid; |
uint8_t UseWL; |
char string[20]; |
Setup_WiFly_MenuCreate(); |
//--------------- |
// Control |
//--------------- |
while( true ) |
{ |
MenuCtrl_Control( MENUCTRL_EVENT ); |
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
edit = 0; |
//-------------------- |
// TEST |
//-------------------- |
/* |
if( itemid == WL_TEST ) |
{ |
lcd_cls(); |
ShowTitle_P( PSTR("TEST"), true ); |
wifly_test(); |
} |
*/ |
//-------------------- |
// Wlan_INSTALLED |
//-------------------- |
if( itemid == WL_INSTALLED ) |
{ |
UseWL = Config.UseWL; |
Config.UseWL = Edit_generic( Config.UseWL, 0, 2, WlanMode, 1, strGet(STR_EXTSV2MODULE),NULL); |
if( UseWL != Config.UseWL ) // hat Benutzer Einstellung geaendert? |
{ |
MenuCtrl_Destroy(); // Menue aendern wegen wechsel Wlan vorhanden / nicht vorhanden |
Setup_WiFly_MenuCreate(); |
continue; |
} |
} |
//-------------------- |
// WL_VERSION |
//-------------------- |
if( itemid == WL_VERSION ) |
{ |
wifly_version_show(); |
} |
//-------------------- |
// WL_SSID |
//-------------------- |
if( itemid == WL_SSID ) |
{ |
strncpyfill( string, Config.Wlan_SSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_SSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_PASSWORD |
//-------------------- |
if( itemid == WL_PASSWORD) |
{ |
strncpyfill( string, Config.Wlan_Password, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_Password, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMESSID |
//-------------------- |
if( itemid == WL_HOMESSID ) |
{ |
strncpyfill( string, Config.Wlan_HomeSSID, wlan_ssid_length+1 ); // wlan_ssid_length |
Edit_String( string, wlan_ssid_length , EDIT_SSID ); |
if( edit == 1 ) |
{ |
strrtrim( EditString); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomeSSID, EditString, wlan_ssid_length+1 ); |
} |
} |
//-------------------- |
// WL_HOMEPASSWORD |
//-------------------- |
if( itemid == WL_HOMEPASSWORD) |
{ |
strncpyfill( string, Config.Wlan_HomePassword, wlan_password_length+1 ); // |
Edit_String( string, wlan_password_length , EDIT_WL_PASSWORD ); |
if( edit == 1 ) |
{ |
strrtrim( EditString ); // Leerzeichen rechts entfernen |
strncpy( Config.Wlan_HomePassword, EditString, wlan_password_length+1 ); |
} |
} |
//-------------------- |
// WL_CHANNEL |
//-------------------- |
if( itemid == WL_CHANNEL ) |
{ |
Config.Wlan_Channel = Edit_generic( Config.Wlan_Channel, 0,13,Show_uint3,1 ,NULL,NULL); |
} |
//-------------------- |
// WL_INIT |
//-------------------- |
if( itemid == WL_INIT ) |
{ |
wifly_init( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_RESET |
//-------------------- |
if( itemid == WL_RESET ) |
{ |
wifly_reset(); |
} |
//-------------------- |
// Wlan_UPDATE |
//-------------------- |
if( itemid == WL_UPDATE ) |
{ |
wifly_update( Config.UseWL ); // Config.UseWL = WIFLY_APMODE oder WIFLY_ADHOC |
} |
//-------------------- |
// Wlan_WPA |
//-------------------- |
//if( itemid == WL_WPA ) |
//{ |
// Config.Wlan_WPA = Edit_generic(Config.Wlan_WPA,0,1,WL3,WlanSecurity,1); |
//} |
} // end: while( true ) |
//--------------- |
// Destroy |
//--------------- |
MenuCtrl_Destroy(); |
} // end: Setup_WiFly() |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wifly/wifly_setup.h |
---|
0,0 → 1,18 |
/* |
* wifly_setup.h |
* |
* Created on: 02.07.2013 |
* Author: cebra |
*/ |
#ifndef WIFLY_SETUP_H_ |
#define WIFLY_SETUP_H_ |
#define WIFLY_APMODE 1 |
#define WIFLY_ADHOC 2 |
void Setup_WiFly( void ); |
#endif /* WIFLY_SETUP_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/wifly |
---|
Property changes: |
Added: svn:ignore |
+_old_source |
+ |
+_doc |
/Transportables_Koptertool/PKT/trunk |
---|
Property changes: |
Added: svn:ignore |
+_doc |
+ |
+Koptertool3.9 |
+ |
+nmealib |
+ |
+Resourcen |
+ |
+TinyGPS |
+ |
+LICENSE.TXT |
+ |
+Source_PKT384e_TEST!.zip |
+ |
+Source_PKT384a.zip |
+ |
+mkparameters.c |
+ |
+DIFF_PKT384a.txt |
+ |
+DIFF_PKT384e.txt |
+ |
+DIFF_PKT384d.txt |
+ |
+.refactorings |
+ |
+.settings |
+ |
+.directory |
+ |
+.cproject |
+ |
+.project |