desktop: fixed ADC value for airspeed

This commit is contained in:
Andrew Tridgell 2011-11-25 15:48:52 +11:00 committed by Pat Hickey
parent 98534e71f7
commit 78af52083b

View File

@ -36,6 +36,9 @@ struct ADC_UDR2 {
case 0xB7: next_value = channels[6]; break; case 0xB7: next_value = channels[6]; break;
case 0xF7: next_value = channels[7]; break; case 0xF7: next_value = channels[7]; break;
} }
if (cmd != 0) {
idx = 1;
}
return *this; return *this;
} }
@ -43,13 +46,15 @@ struct ADC_UDR2 {
read from UDR2 fetches a byte from the channel read from UDR2 fetches a byte from the channel
*/ */
operator int() { operator int() {
switch (idx) { uint8_t ret;
case 0: idx++; return(value>>8); if (idx & 1) {
case 1: idx++; return(value&0xFF); ret = (value&0xFF);
value = next_value;
} else {
ret = (value>>8);
} }
value = next_value; idx ^= 1;
idx=0; return ret;
return(value>>8);
} }
/* /*