void read_rx() {
for (int x=0;x<RX_NUM_CHANNELS;++x) {
#if (RX_TYPE==RX_DSM2_1024)
// convert from 0-1024 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxValues[x], ((fixedpointnum)rxrawvalues[x]-512)<<7, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
#else
// convert from 0-2048 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxValues[x], ((fixedpointnum)rxrawvalues[x]-1024)<<6, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
#endif
}
}
void readrx(void)
{
int chan;
uint16_t data[8];
if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_RX_DR))) {
uint32_t t = lib_timers_gettimermicroseconds(packet_timer);
if (t > rx_timeout) {
if (boundprotocol != PROTO_NONE) {
if (++missed_packets > 500 && bind_phase == PHASE_JUST_BOUND) {
valid_packets = missed_packets = bad_packets = 0;
bind_phase = PHASE_LOST_BINDING;
prepare_to_bind();
}
} else switch_channel();
packet_timer = lib_timers_starttimer();
}
return;
}
packet_timer = lib_timers_starttimer();
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_RX_DR));
NRF24L01_ReadPayload(packet, V2X2_PAYLOAD_SIZE);
NRF24L01_FlushRx();
switch_channel();
if (!decode_packet(packet, data))
return;
for (chan = 0; chan < 8; ++chan) {
// data = pwmRead(chan);
// if (data < 750 || data > 2250)
// data = 1500;
// convert from 1000-2000 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxvalues[chan], ((fixedpointnum) data[chan] - 1500) * 131L, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
}
// reset the failsafe timer
global.failsafetimer = lib_timers_starttimer();
}
// read the acc and gyro a bunch of times and get an average of how far off they are.
// assumes the aircraft is sitting level and still.
// If both==false, only gyro is calibrated and accelerometer calibration not touched.
void calibrategyroandaccelerometer(bool both)
{
#ifdef X4_BUILD
uint8_t ledstatus;
#endif
// do some readings to initialize the gyro filters
for (int x = 0; x < 15; ++x) {
readgyro();
readacc();
}
// start the low pass at the first readout rather then at zero
for (int x = 0; x < 3; ++x) {
usersettings.gyrocalibration[x]= -global.gyrorate[x];
if(both)
usersettings.acccalibration[x]= -global.acc_g_vector[x];
}
/*
for (int x = 0; x < 3; ++x) {
usersettings.gyrocalibration[x] = 0;
if(both)
usersettings.acccalibration[x] = 0;
}
*/
fixedpointnum totaltime = 0;
// calibrate the gyro and acc
while (totaltime < (FIXEDPOINTCONSTANT(4) << TIMESLIVEREXTRASHIFT)) // 4 seconds
{
readgyro();
if(both) {
readacc();
global.acc_g_vector[ZINDEX] -= FIXEDPOINTONE; // vertical vector should be at 1g
}
calculatetimesliver();
totaltime += global.timesliver;
#ifdef X4_BUILD
// Rotating LED pattern
ledstatus = (uint8_t)((totaltime >> (FIXEDPOINTSHIFT+TIMESLIVEREXTRASHIFT-3))& 0x3);
switch(ledstatus) {
case 0:
x4_set_leds(X4_LED_FL);
break;
case 1:
x4_set_leds(X4_LED_FR);
break;
case 2:
x4_set_leds(X4_LED_RR);
break;
case 3:
x4_set_leds(X4_LED_RL);
break;
}
#endif
for (int x = 0; x < 3; ++x) {
lib_fp_lowpassfilter(&usersettings.gyrocalibration[x], -global.gyrorate[x], global.timesliver, FIXEDPOINTONEOVERONE, TIMESLIVEREXTRASHIFT);
if(both)
lib_fp_lowpassfilter(&usersettings.acccalibration[x], -global.acc_g_vector[x], global.timesliver, FIXEDPOINTONEOVERONE, TIMESLIVEREXTRASHIFT);
}
}
}
void read_rx() {
unsigned char chan;
for (chan=0; chan<RX_NUM_CHANNELS;++chan) // convert from 1000-2000 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxValues[channelindex[chan]], ((fixedpointnum)rxrawvalues[chan]-1500)*131L, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
}
void read_rx() {
for (int x=0;x<RX_NUM_CHANNELS;++x) {
// convert from 1000-2000 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxValues[x], ((fixedpointnum)rxrawvalues[x]-1500)*131L, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
}
}
