Issues with current model during copy / move operations in MODELSEL menu.

This commit is contained in:
bsongis
2011-11-25 11:04:38 +00:00
parent 5d9a2a483b
commit 9761da9de3
12 changed files with 576 additions and 556 deletions
+200 -57
View File
@@ -43,8 +43,8 @@ mode4 ail thr ele rud
EEGeneral g_eeGeneral;
ModelData g_model;
uint16_t g_tmr1Latency_max;
uint16_t g_tmr1Latency_min = 0x7ff;
uint8_t g_tmr1Latency_max;
uint8_t g_tmr1Latency_min;
uint16_t g_timeMain;
uint16_t g_time_per10;
@@ -256,12 +256,13 @@ int16_t Expo::expo(int16_t x)
#endif
void applyExpos(int16_t *anas)
void applyExpos(int16_t *anas, uint8_t phase)
{
static int16_t anas2[4]; // values before expo, to ensure same expo base when multiple expo lines are used
memcpy(anas2, anas, sizeof(anas2));
uint8_t phase = getFlightPhase();
if (phase == 255)
phase = getFlightPhase();
int8_t cur_chn = -1;
for (uint8_t i=0; i<DIM(g_model.expoData); i++) {
@@ -448,10 +449,8 @@ void setTrimValue(uint8_t phase, uint8_t idx, int16_t trim)
STORE_MODELVARS;
}
uint8_t getTrimFlightPhase(uint8_t idx, int8_t phase) // TODO uint8_t phase?
uint8_t getTrimFlightPhase(uint8_t idx, uint8_t phase)
{
if (phase == -1) phase = getFlightPhase();
for (uint8_t i=0; i<MAX_PHASES; i++) {
if (phase == 0) return 0;
int16_t trim = getTrimValue(phase, idx);
@@ -674,7 +673,7 @@ uint8_t checkTrim(uint8_t event)
if (k>=0 && k<8) { // && (event & _MSK_KEY_REPT))
//LH_DWN LH_UP LV_DWN LV_UP RV_DWN RV_UP RH_DWN RH_UP
uint8_t idx = k/2;
uint8_t phase = getTrimFlightPhase(idx);
uint8_t phase = getTrimFlightPhase(idx, getFlightPhase());
int16_t before = getTrimValue(phase, idx);
int8_t v = (s==0) ? min(32, abs(before)/4+1) : 1 << (s-1); // 1=>1 2=>2 3=>4 4=>8
bool thro = (((2-(g_eeGeneral.stickMode&1)) == idx) && g_model.thrTrim);
@@ -817,7 +816,7 @@ void getADC_bandgap()
{
#if defined(PCBSTD)
ADMUX=0x1E|ADC_VREF_TYPE; // Switch MUX to internal 1.22V reference
_delay_us(5); // short delay to stablise reference voltage
_delay_us(5); // short delay to stabilise reference voltage
ADCSRA|=0x40;
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10; // grab a sample
@@ -1093,13 +1092,13 @@ int32_t act [MAX_MIXERS] = {0};
uint8_t swOn [MAX_MIXERS] = {0};
uint8_t mixWarning;
inline void evalTrims()
inline void evalTrims(uint8_t phase)
{
for (uint8_t i=0; i<NUM_STICKS; i++) {
// do trim -> throttle trim if applicable
int16_t v = anas[i];
int32_t vv = 2*RESX;
int16_t trim = getTrimValue(getTrimFlightPhase(i), i);
int16_t trim = getTrimValue(getTrimFlightPhase(i, phase), i);
if (IS_THROTTLE(i) && g_model.thrTrim) {
if (g_eeGeneral.throttleReversed)
trim = -trim;
@@ -1113,7 +1112,7 @@ inline void evalTrims()
}
}
uint8_t evalSticks()
uint8_t evalSticks(uint8_t phase)
{
#ifdef HELI
uint16_t d = 0;
@@ -1178,10 +1177,10 @@ uint8_t evalSticks()
}
/* EXPOs */
applyExpos(anas);
applyExpos(anas, phase);
/* TRIMs */
evalTrims();
evalTrims(phase);
return anaCenter;
}
@@ -1204,10 +1203,9 @@ void evalFunctions()
}
}
void perOut(int16_t *chanOut)
void perOut(int16_t *chanOut, uint8_t phase)
{
uint8_t phase = getFlightPhase();
uint8_t anaCenter = evalSticks();
uint8_t anaCenter = evalSticks(phase);
//===========BEEP CENTER================
anaCenter &= g_model.beepANACenter;
@@ -1348,10 +1346,6 @@ void perOut(int16_t *chanOut)
{
#define DEL_MULT 256
//if(init) {
//act[i]=(int32_t)v*DEL_MULT;
//swTog = false;
//}
int16_t diff = v-act[i]/DEL_MULT;
if(swTog) {
@@ -1472,53 +1466,113 @@ void perOut(int16_t *chanOut)
}
}
#ifdef DISPLAY_USER_DATA
char userDataDisplayBuf[TELEM_SCREEN_BUFFER_SIZE];
#endif
void perMain()
{
static uint16_t lastTMR;
tick10ms = (get_tmr10ms() != lastTMR);
lastTMR = get_tmr10ms();
int16_t last_chans512[NUM_CHNOUT];
int16_t next_chans512[NUM_CHNOUT];
static uint8_t last_phase = 0;
#define MAX_ACT 0xffff
static uint16_t fp_act[MAX_PHASES] = {0};
static uint8_t s_fade_flight_phases = 0;
static uint8_t s_last_phase = 255;
uint8_t phase = getFlightPhase();
static uint8_t fading_out_timer = 0;
if (last_phase != phase) {
fading_out_timer = 10 * max(g_model.phaseData[last_phase].fadeOut, g_model.phaseData[phase].fadeIn);
last_phase = phase;
if (s_last_phase != phase) {
if (s_last_phase == 255) {
fp_act[phase] = MAX_ACT;
}
else {
if (g_model.phaseData[s_last_phase].fadeOut) {
s_fade_flight_phases |= (1<<s_last_phase);
}
else {
fp_act[s_last_phase] = 0;
s_fade_flight_phases &= ~(1<<s_last_phase);
}
if (g_model.phaseData[phase].fadeIn) {
s_fade_flight_phases |= (1<<phase);
}
else {
fp_act[phase] = MAX_ACT;
s_fade_flight_phases &= ~(1<<phase);
}
}
s_last_phase = phase;
// printf("s_fade_flight_phases=%d\n", s_fade_flight_phases); fflush(stdout);
}
if (fading_out_timer) {
memcpy(last_chans512, g_chans512, sizeof(g_chans512));
}
int16_t next_chans512[NUM_CHNOUT];
perOut(next_chans512);
for (uint8_t i=0; i<NUM_CHNOUT; i++) {
cli();
if (fading_out_timer) {
g_chans512[i] = last_chans512[i] + (next_chans512[i] - last_chans512[i]) / fading_out_timer;
fading_out_timer--;
if (s_fade_flight_phases) {
int32_t sum_chans512[NUM_CHNOUT] = {0};
int32_t weight = 0;
for (uint8_t p=0; p<MAX_PHASES; p++) {
if (s_fade_flight_phases & (1<<p)) {
perOut(next_chans512, p);
// printf("perOut(%d - %d)=>%d\n", p, fp_act[p], next_chans512[2]);
for (uint8_t i=0; i<NUM_CHNOUT; i++) {
sum_chans512[i] += (int32_t)next_chans512[i] * fp_act[p];
}
weight += fp_act[p];
}
}
// printf("sum=%d, weight=%d ", sum_chans512[2], weight); fflush(stdout);
assert(weight);
for (uint8_t i=0; i<NUM_CHNOUT; i++) {
next_chans512[i] = ((int32_t)sum_chans512[i] / weight);
}
// printf("output = %d\n", next_chans512[2]); fflush(stdout);
}
else {
g_chans512[i] = next_chans512[i];
perOut(next_chans512, phase);
}
for (uint8_t i=0; i<NUM_CHNOUT; i++) {
cli();
g_chans512[i] = next_chans512[i];
sei();
}
#ifdef EEPROM_ASYNC_WRITE
if (!eeprom_buffer_size) {
if (theFile.isWriting())
theFile.nextWriteStep();
else if (s_eeDirtyMsk)
eeCheck();
}
#endif
if(!tick10ms) return; //make sure the rest happen only every 10ms.
if (s_fade_flight_phases) {
for (uint8_t p=0; p<MAX_PHASES; p++) {
// printf("f_act[%d]=%d\n", p, fp_act[p]);
if (s_fade_flight_phases & (1<<p)) {
if (p == phase) {
uint16_t delta = (MAX_ACT / 100) / g_model.phaseData[p].fadeIn;
if (MAX_ACT - fp_act[p] > delta)
fp_act[p] += delta;
else {
fp_act[p] = MAX_ACT;
s_fade_flight_phases -= (1<<p);
}
}
else {
uint16_t delta = (MAX_ACT / 100) / g_model.phaseData[p].fadeOut;
if (fp_act[p] > delta)
fp_act[p] -= delta;
else {
fp_act[p] = 0;
s_fade_flight_phases -= (1<<p);
}
}
}
}
}
evalFunctions();
if (s_noHi) s_noHi--;
@@ -1537,10 +1591,98 @@ void perMain()
if (trimsCheckTimer > 0)
trimsCheckTimer -= 1;
#ifndef EEPROM_ASYNC_WRITE
eeCheck();
#endif
#if defined (FRSKY)
/***** TEST CODE - Fr-Sky SD/MMC card / User Data experiments *****/
#if defined (PCBV3)
/* Use light switch (on) to open telemtry test log file */
static FRESULT result;
static int8_t testLogOpen = 0;
if(getSwitch(g_eeGeneral.lightSw,0))
{
// while(1); // Test WDT
if ((testLogOpen==0) // if we know we haven't started logging ...
|| ((testLogOpen==1) && !g_oLogFile.fs)) // ... or we thought we had, but the file got closed
{
result = f_mount(0, &FATFS_Obj);
if (result!=FR_OK)
{
testLogOpen = -1;
beepAgain = result - 1;
beepKey();
}
else
{
// create new log file using filename set up in pers.cpp::resetTelemetry()
result = f_open(&g_oLogFile, g_logFilename, FA_OPEN_ALWAYS | FA_WRITE);
if (result!=FR_OK)
{
testLogOpen = -2;
beepAgain = result - 1;
beepKey();
}
else
{
f_lseek(&g_oLogFile, g_oLogFile.fsize); // append
testLogOpen = 1;
beepWarn2();
}
}
}
}
else
{
if (testLogOpen==1)
{
f_close(&g_oLogFile);
beepWarn2();
}
testLogOpen = 0;
}
#ifdef DISPLAY_USER_DATA
////////////////
// Write raw user data into on-screen display line buffer
char userDataRxBuffer[21]; // Temp buffer used to collect fr-sky user data
// retrieve bytes from user data buffer and insert into display string,
// scrolling at the 21 character mark (edge of screen)
uint8_t numbytes = frskyGetUserData(userDataRxBuffer, 21); // Get as many bytes as we can
static uint8_t displayBufferIndex;
for (uint8_t byt=0; byt < numbytes; byt++)
{
displayBufferIndex++;
if (displayBufferIndex > 20)
{
for (int xx=0; xx<20; xx++) // scroll one char left
userDataDisplayBuf[xx] = userDataDisplayBuf[xx+1];
displayBufferIndex = 20;
}
userDataDisplayBuf[displayBufferIndex] = userDataRxBuffer[byt];
// Write the raw byte out to log file, if open
if (testLogOpen && (g_oLogFile.fs != 0))
f_putc(userDataRxBuffer[byt], &g_oLogFile);
}
////////////////
#endif
/***** END TEST CODE - Fr-Sky User Data experiments *****/
// PCBV3
#endif
// FRSKY
#endif
lcd_clear();
uint8_t evt=getEvent();
evt = checkTrim(evt);
@@ -1950,22 +2092,23 @@ uint16_t DEBUG2 = 0;
void instantTrim()
{
uint8_t phase = getFlightPhase();
for (uint8_t i=0; i<NUM_STICKS; i++) {
if (!IS_THROTTLE(i)) {
// don't instant trim the throttle stick
uint8_t phase = getTrimFlightPhase(i);
uint8_t trim_phase = getTrimFlightPhase(i, phase);
s_noStickInputs = true;
evalSticks();
evalSticks(phase);
s_noStickInputs = false;
int16_t trim = (anas[i] + trims[i]) / 2;
// printf("anas[%d]=%d, trims[%d]=%d, trim=%d, subtrim=%d => trim=%d\n", i, anas[i], i, trims[i], g_model.phaseData[phase].trim[i], g_model.subtrim[i], trim);
if (trim < TRIM_EXTENDED_MIN) {
trim = TRIM_EXTENDED_MIN;
}
if (trim > TRIM_EXTENDED_MAX) {
trim = TRIM_EXTENDED_MAX;
}
setTrimValue(phase, i, trim);
setTrimValue(trim_phase, i, trim);
}
}
@@ -1978,7 +2121,7 @@ void moveTrimsToOffsets() // copy state of 3 primary to subtrim
int16_t zero_chans512[NUM_CHNOUT];
s_noStickInputs = true;
perOut(zero_chans512); // do output loop - zero input sticks
perOut(zero_chans512, getFlightPhase()); // do output loop - zero input sticks
s_noStickInputs = false;
for (uint8_t i=0; i<NUM_CHNOUT; i++)
@@ -2001,27 +2144,27 @@ void moveTrimsToOffsets() // copy state of 3 primary to subtrim
#if defined (PCBV4)
// Rotary encoder interrupts
uint8_t volatile g_rotenc1, g_rotenc2 = 0;
volatile uint8_t g_rotenc[2] = {0};
ISR(INT2_vect)
{
uint8_t input = PIND & 0b00001100;
if (input == 0 || input == 0b00001100) g_rotenc1--;
if (input == 0 || input == 0b00001100) g_rotenc[0]--;
}
ISR(INT3_vect)
{
uint8_t input = PIND & 0b00001100;
if (input == 0 || input == 0b00001100) g_rotenc1++;
if (input == 0 || input == 0b00001100) g_rotenc[0]++;
}
ISR(INT5_vect)
{
uint8_t input = PINE & 0b01100000;
if (input == 0 || input == 0b01100000) g_rotenc2++;
if (input == 0 || input == 0b01100000) g_rotenc[1]++;
}
ISR(INT6_vect)
{
uint8_t input = PINE & 0b01100000;
if (input == 0 || input == 0b01100000) g_rotenc2--;
if (input == 0 || input == 0b01100000) g_rotenc[1]--;
}
#endif
@@ -2188,7 +2331,7 @@ int main(void)
clearKeyEvents(); //make sure no keys are down before proceeding
perOut(g_chans512);
perOut(g_chans512, getFlightPhase());
lcdSetRefVolt(g_eeGeneral.contrast);
g_LightOffCounter = g_eeGeneral.lightAutoOff*500; //turn on light for x seconds - no need to press key Issue 152