Page 339 - 2

prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /
512 instead of /500
prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128 prop2: max is 128 result pro
p1: max is 128
dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now
dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now
} else { // YAW
rcCommand[axis] = tmp;
}
if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE],MINCHECK,2000);
tmp = (uint32_t)(tmp-MINCHECK)*2559/(2000-MINCHECK); // [MINCHECK;2000] -> [0;2559]
tmp2 = tmp/256; // range [0;9]
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tm
p2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTL
E]
#if defined(HEADFREE)
if(f.HEADFREE_MODE) { //to optimize
float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.017
4533
float cosDiff = cos(radDiff);
float sinDiff = sin(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#endif

// query at most one multiplexed analog channel per MWii cycle
static uint8_t analogReader =0;
switch (analogReader++ % (3+VBAT_CELLS_NUM)) {
case 0:
{
#if defined(POWERMETER_HARD)
static uint32_t lastRead = currentTime;
static uint8_t ind = 0;
static uint16_t pvec[PSENSOR_SMOOTH], psum;
uint16_t p = analogRead(PSENSORPIN);
//LCDprintInt16(p); LCDcrlf();
//debug[0] = p;
#if PSENSOR_SMOOTH != 1
psum += p;
psum -= pvec[ind];
pvec[ind++] = p;

- 339 -