//BIND_TX
uint32_t bind_Flysky() {
unsigned long timeout_timer=lib_timers_starttimer();
uint32_t _id=0;
// set channel 0;
A7105_Strobe(0xA0);
A7105_WriteRegister(A7105_0F_PLL_I,00);
A7105_Strobe(A7105_RX);
while(!_id && lib_timers_gettimermicroseconds(timeout_timer) < BIND_READ_TOGGLE_TIMEOUT)
{
if( lib_timers_gettimermicroseconds(0) % 524288 > 262144)
leds_set(LED2 | LED4);
else
leds_set(LED1 | LED3);
char mode = A7105_ReadRegister(A7105_00_MODE);
if(mode & A7105_MODE_TRER_MASK || mode & (1<<6) || mode & ( 1<<5) )
{
A7105_Strobe(A7105_RST_RDPTR);
A7105_Strobe(A7105_RX);
continue;
}
A7105_ReadPayload((uint8_t*)&packet, sizeof(packet));
A7105_Strobe(A7105_RST_RDPTR);
if ( packet[0] == 170 ) // 170
{
//set found tx
_id=( ( packet[1] << 0 | packet[2] <<8 | packet[3] << 16 | packet[4]<<24 ) );
}
A7105_Strobe(A7105_RX);
}
return _id;
}
void bind() {
/* if no id found, bind */
while (usersettings.flysky_id==0) {
usersettings.flysky_id=bind_Flysky();
}
/* if id found toggle read and bind until one frame is readed */
/* in that way, it possible to bind with an other TX even if one tx id is already registered */
uint8_t state=READ_STATE;
while (state!=DONE) {
unsigned long timeout_timer=lib_timers_starttimer();
if (state==READ_STATE) {
init_channels();
if (_readrx()) state=DONE;
while (state==READ_STATE && lib_timers_gettimermicroseconds(timeout_timer) < BIND_READ_TOGGLE_TIMEOUT) {
if( lib_timers_gettimermicroseconds(0) % 500000 > 250000)
leds_set(LED1 | LED5);
else
leds_set(LED2 | LED6);
if (_readrx()) state=DONE;
}
if (state==READ_STATE) {
state=BIND_STATE;
}
}
if (state==BIND_STATE) {
uint32_t _id=bind_Flysky();
if (_id) {
usersettings.flysky_id=_id;
}
state=READ_STATE;
}
}
}
void throttle_callback(unsigned char interruptnumber,unsigned char newstate) {
// call back will get called any time the pin changes
process_callback(THROTTLE_INDEX);
// reset the failsafe timer
global.failsafeTimer=lib_timers_starttimer();
}
void serialsumcallback(unsigned char interruptnumber, unsigned char newstate) {
// gke
static unsigned long starttime = 0;
static unsigned char chan = 0;
unsigned int width;
static unsigned int glitches=0;
if (newstate) {
width = lib_timers_gettimermicrosecondsandreset(&starttime);
if (width > 3000) {
global.failsafeTimer=lib_timers_starttimer(); // reset the failsafe timer
chan = 0;
} else if (chan < RX_NUM_CHANNELS) {
if (width>900 && width<2050)
rxrawvalues[chan] = width;
// else
// {
// global.debugValue[1] = width;
// global.debugValue[0] = ++glitches;
// }
chan++;
}
}
}
// this callback will get called whenever we receive a character on our dsm2 serial port
void dsm2serialcallback(unsigned char c) {
// there is a 11ms delay between packets. We need to use this delay to determine when we are
// getting new data.
unsigned long microsecondssincelastchar=lib_timers_gettimermicrosecondsandreset(&dsm2timer);
if (microsecondssincelastchar>5000) {
// this is a new packet. Skip this first byte
dsm2state=DSM2STATE_NEWFRAMESARTED;
} else if (dsm2state==DSM2STATE_NEWFRAMESARTED) {
// skip the 2nd byte too
dsm2state=DSM2STATE_WAITINGFORFIRSTCHAR;
} else if (dsm2state==DSM2STATE_WAITINGFORFIRSTCHAR) {
dsm2firstchar=c;
dsm2state=DSM2STATE_WAITINGFORSECONDCHAR;
} else {
unsigned char channel = 0x0F & (dsm2firstchar >> DSM2_CHAN_SHIFT);
if (channel < RX_NUM_CHANNELS) {
channel=dsm2channelindex[channel];
rxrawvalues[channel] = ((unsigned int)(dsm2firstchar & DSM2_CHAN_MASK) << 8) + c;
}
dsm2state=DSM2STATE_WAITINGFORFIRSTCHAR;
// reset the failsafe timer
global.failsafeTimer=lib_timers_starttimer();
}
}
static void prepare_to_bind(void)
{
packet_timer = lib_timers_starttimer();
tryprotocol = PROTO_V2X2;
for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
rf_channels[i] = v2x2_freq_hopping[0][i];
}
nfreqchannels = V2X2_NFREQCHANNELS;
rx_timeout = 1000L;
boundprotocol = PROTO_NONE;
}
int _readrx(void) {
for (int i=0;i<21;i++) packet[i]=0;
channel=tx_channels[chanrow][chancol]-1-chanoffset;
A7105_Strobe(A7105_STANDBY);
A7105_Strobe(A7105_RST_RDPTR);
A7105_WriteRegister(0x0F, channel);
A7105_Strobe(A7105_RX);
chancol = (chancol + 1) % 16;
unsigned long pause;
uint8_t x;
pause=lib_timers_starttimer();
while(1){
if (lib_timers_gettimermicroseconds(pause) > 2000) {
// Red_LED_OFF;
chancol = (chancol + 1) % 16;
channel=tx_channels[chanrow][chancol]-1-chanoffset;
break;
}
if(A7105_ReadRegister(A7105_00_MODE) & A7105_MODE_TRER_MASK){
continue;
}
x=A7105_ReadRegister(0x00);
if (!(bitRead(x,5)==0)&& !(bitRead(x,6)==0)){
continue;
}
Read_Packet();
if (convert2id(packet[1],packet[2],packet[3],packet[4])!=usersettings.flysky_id) {
// Serial.println("bad id");
continue;
}
decodepacket();
// reset the failsafe timer
global.failsafetimer = lib_timers_starttimer();
return 1;
}
return 0;
}
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();
}
// Returns whether the data was successfully decoded
static bool decode_packet(uint8_t *packet, uint16_t *data)
{
switch (boundprotocol) {
case PROTO_NONE:
decode_bind_packet(packet);
break;
case PROTO_V2X2:
// Decode packet
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
return false;
}
if (packet[7] != usersettings.txid[0] ||
packet[8] != usersettings.txid[1] ||
packet[9] != usersettings.txid[2])
{
bad_packets++;
return false;
}
// Restore regular interval
rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
// TREA order in packet to MultiWii order is handled by
// correct assignment to channelindex
// Throttle 0..255 to 1000..2000
data[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
for (int i = 1; i < 4; ++i) {
uint8_t a = packet[i];
data[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
}
uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
for (int i = 4; i < 8; ++i) {
data[v2x2_channelindex[i]] = 1000 + ((packet[14] & flags[i-4]) ? 1000 : 0);
}
packet_timer = lib_timers_starttimer();
if (++valid_packets > 50) bind_phase = PHASE_BOUND;
return true;
}
return false;
}
char readgps()
{ // returns 1 if new data was read
if (lib_timers_gettimermicroseconds(gpstimer)<50000) return(0); // 20 hz
gpstimer=lib_timers_starttimer();
unsigned char shiftedaddress=I2C_GPS_ADDRESS<<1;
lib_i2c_start_wait(shiftedaddress+I2C_WRITE);
if (lib_i2c_write(I2C_GPS_STATUS_00))
return(0);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
unsigned char gps_status= lib_i2c_readnak();
global.gps_num_satelites = (gps_status & 0xf0) >> 4;
if (gps_status & I2C_GPS_STATUS_3DFIX)
{ //Check is we have a good 3d fix (numsats>5)
long longvalue;
unsigned char *ptr=(unsigned char *)&longvalue;
lib_i2c_rep_start(shiftedaddress+I2C_WRITE);
lib_i2c_write(I2C_GPS_LOCATION);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readack();
global.gps_current_latitude=lib_fp_multiply(longvalue,27488L); // convert from 10,000,000 m to fixedpointnum
ptr=(unsigned char *)&longvalue;
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readnak();
global.gps_current_longitude=lib_fp_multiply(longvalue,27488L); // convert from 10,000,000 m to fixedpointnum
int intvalue;
ptr= (unsigned char *)&intvalue;
lib_i2c_rep_start(shiftedaddress+I2C_WRITE);
lib_i2c_write(I2C_GPS_GROUND_SPEED);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
global.gps_current_speed=intvalue*665L; // convert fro cm/s to fixedpointnum to m/s
ptr= (unsigned char *)&intvalue;
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readnak();
global.gps_current_altitude=((fixedpointnum)intvalue)<<FIXEDPOINTSHIFT;
lib_i2c_stop();
return(1);
}
lib_i2c_stop();
return(0);
}
void initgps()
{
gpstimer=lib_timers_starttimer();
}
// It all starts here:
int main(void) {
// start with default user settings in case there's nothing in eeprom
default_user_settings();
// try to load settings from eeprom
read_user_settings_from_eeprom();
// set our LED as a digital output
lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT);
//initialize the libraries that require initialization
lib_timers_init();
lib_i2c_init();
// pause a moment before initializing everything. To make sure everything is powered up
lib_timers_delaymilliseconds(100);
// initialize all other modules
init_rx();
init_outputs();
serial_init();
init_gyro();
init_acc();
init_baro();
init_compass();
init_gps();
init_imu();
// set the default i2c speed to 400 KHz. If a device needs to slow it down, it can, but it should set it back.
lib_i2c_setclockspeed(I2C_400_KHZ);
// initialize state
global.state.armed=0;
global.state.calibratingCompass=0;
global.state.calibratingAccAndGyro=0;
global.state.navigationMode=NAVIGATION_MODE_OFF;
global.failsafeTimer=lib_timers_starttimer();
// run loop
for(;;) {
// check to see what switches are activated
check_checkbox_items();
// check for config program activity
serial_check_for_action();
calculate_timesliver();
// run the imu to estimate the current attitude of the aircraft
imu_calculate_estimated_attitude();
// arm and disarm via rx aux switches
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes
if (!global.state.armed) {
if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) {
global.state.armed=1;
#if (GPS_TYPE!=NO_GPS)
navigation_set_home_to_current_location();
#endif
global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX];
global.home.location.altitude=global.baroRawAltitude;
}
} else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0;
}
#if (GPS_TYPE!=NO_GPS)
// turn on or off navigation when appropriate
if (global.state.navigationMode==NAVIGATION_MODE_OFF) {
if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on
navigation_set_destination(global.home.location.latitude,global.home.location.longitude);
global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME;
} else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on
navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude);
global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD;
}
} else { // we are currently navigating
// turn off navigation if desired
if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) {
global.state.navigationMode=NAVIGATION_MODE_OFF;
// we will be turning control back over to the pilot.
reset_pilot_control();
}
}
#endif
// read the receiver
read_rx();
// turn on the LED when we are stable and the gps has 5 satelites or more
#if (GPS_TYPE==NO_GPS)
lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON);
#else
lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON);
#endif
// get the angle error. Angle error is the difference between our current attitude and our desired attitude.
// It can be set by navigation, or by the pilot, etc.
fixedpointnum angleError[3];
// let the pilot control the aircraft.
get_angle_error_from_pilot_input(angleError);
#if (GPS_TYPE!=NO_GPS)
// read the gps
unsigned char gotNewGpsReading=read_gps();
// if we are navigating, use navigation to determine our desired attitude (tilt angles)
if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating
navigation_set_angle_error(gotNewGpsReading,angleError);
}
#endif
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) {
// We are probably on the ground. Don't accumnulate error when we can't correct it
reset_pilot_control();
// bleed off integrated error by averaging in a value of zero
lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
}
#ifndef NO_AUTOTUNE
// let autotune adjust the angle error if the pilot has autotune turned on
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) {
autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning
} else {
autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning
}
#endif
// This gets reset every loop cycle
// keep a flag to indicate whether we shoud apply altitude hold. The pilot can turn it on or
// uncrashability/autopilot mode can turn it on.
global.state.altitudeHold=0;
if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) {
global.state.altitudeHold=1;
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) {
// we just turned on alt hold. Remember our current alt. as our target
global.altitudeHoldDesiredAltitude=global.altitude;
global.integratedAltitudeError=0;
}
}
fixedpointnum throttleOutput;
#ifndef NO_AUTOPILOT
// autopilot is available
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) {
// let autopilot know to transition to the starting state
autopilot(AUTOPILOT_STARTING);
} else {
// autopilot normal run state
autopilot(AUTOPILOT_RUNNING);
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
// tell autopilot that we just stopped autotuning
autopilot(AUTOPILOT_STOPPING);
} else {
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
}
#else
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
#endif
#ifndef NO_UNCRASHABLE
uncrashable(gotNewGpsReading,angleError,&throttleOutput);
#endif
#if (BAROMETER_TYPE!=NO_BAROMETER)
// check for altitude hold and adjust the throttle output accordingly
if (global.state.altitudeHold) {
global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver);
lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high
// do pid for the altitude hold and add it to the throttle output
throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]);
}
#endif
#ifndef NO_AUTOTHROTTLE
if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) {
if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) {
// Divide the throttle by the throttleOutput by the z component of the down vector
// This is probaly the slow way, but it's a way to do fixed point division
fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]);
recriprocal=lib_fp_multiply(recriprocal,recriprocal);
throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA;
}
}
#endif
#ifndef NO_FAILSAFE
// if we don't hear from the receiver for over a second, try to land safely
if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) {
throttleOutput=FPFAILSAFEMOTOROUTPUT;
// make sure we are level!
angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX];
angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX];
}
#endif
// calculate output values. Output values will range from 0 to 1.0
// calculate pid outputs based on our angleErrors as inputs
fixedpointnum pidOutput[3];
// Gain Scheduling essentialy modifies the gains depending on
// throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
// 1.0 when at mid throttle, and .5 when at zero throttle. This helps
// eliminate the wobbles when decending at low throttle.
fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
for (int x=0;x<3;++x) {
global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver);
// don't let the integrated error get too high (windup)
lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT);
// do the attitude pid
pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4);
// add gain scheduling.
pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]);
}
lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1
compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes
#if (NUM_SERVOS>0)
// do not update servos during unarmed calibration of sensors which are sensitive to vibration
if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs();
#endif
write_motor_outputs();
}
void init_rx() {
dsm2timer=lib_timers_starttimer();
lib_serial_initport(RX_DSM2_SERIAL_PORT,DSM2_BAUD);
lib_serial_setrxcallback(RX_DSM2_SERIAL_PORT,dsm2serialcallback);
}
// each checkbox item has a checkboxvalue. The bits in this value represent low, medium, and high checkboxes
// for each of the aux switches, just as they show up in most config programs.
void checkcheckboxitems(void)
{
global.previousactivecheckboxitems = global.activecheckboxitems;
global.activecheckboxitems = 0;
uint16_t mask = 0; // a mask of what aux states are true
#if (RXNUMCHANNELS>4)
if (global.rxvalues[AUX1INDEX] < FPAUXMIDRANGELOW) // low
mask |= (1 << 0);
else if (global.rxvalues[AUX1INDEX] > FPAUXMIDRANGEHIGH) // high
mask |= (1 << 2);
else
mask |= (1 << 1); // mid
#endif
#if (RXNUMCHANNELS>5)
if (global.rxvalues[AUX2INDEX] < FPAUXMIDRANGELOW) // low
mask |= (1 << 3);
else if (global.rxvalues[AUX2INDEX] > FPAUXMIDRANGEHIGH) // high
mask |= (1 << 5);
else
mask |= (1 << 4); //mid
#endif
#if (RXNUMCHANNELS>6)
if (global.rxvalues[AUX3INDEX] < FPAUXMIDRANGELOW) // low
mask |= (1 << 6);
else if (global.rxvalues[AUX3INDEX] > FPAUXMIDRANGEHIGH) // high
mask |= (1 << 8);
else
mask |= (1 << 7); //mid
#endif
#if (RXNUMCHANNELS>7)
if (global.rxvalues[AUX4INDEX] < FPAUXMIDRANGELOW) // low
mask |= (1 << 9);
else if (global.rxvalues[AUX4INDEX] > FPAUXMIDRANGEHIGH) // high
mask |= (1 << 11);
else
mask |= (1 << 10); //mid
#endif
for (int x = 0; x < NUMCHECKBOXES; ++x) {
if (usersettings.checkboxconfiguration[x] & mask)
global.activecheckboxitems |= (1 << x);
}
#if (defined(STICK_ARM) | defined (STICK_DISARM))
// figure out where the sticks are
unsigned int stickmask = 0;
static uint32_t stickCounterArm = 0;
static uint32_t stickCounterDisArm = 0;
if (global.rxvalues[ROLLINDEX] < FPSTICKLOW)
stickmask |= STICK_COMMAND_ROLL_LOW;
else if (global.rxvalues[ROLLINDEX] > FPSTICKHIGH)
stickmask |= STICK_COMMAND_ROLL_HIGH;
if (global.rxvalues[PITCHINDEX] < FPSTICKLOW)
stickmask |= STICK_COMMAND_PITCH_LOW;
else if (global.rxvalues[PITCHINDEX] > FPSTICKHIGH)
stickmask |= STICK_COMMAND_PITCH_HIGH;
if (global.rxvalues[YAWINDEX] < FPSTICKLOW)
stickmask |= STICK_COMMAND_YAW_LOW;
else if (global.rxvalues[YAWINDEX] > FPSTICKHIGH)
stickmask |= STICK_COMMAND_YAW_HIGH;
// If the sticks are in the right positions, set the arm or disarm checkbox value
// Start with the previous value in case the sticks aren't doing anything special
global.activecheckboxitems = (global.activecheckboxitems & ~CHECKBOXMASKARM) |(global.previousactivecheckboxitems & CHECKBOXMASKARM);
if ((stickmask & (STICK_ARM)) == STICK_ARM) {
if (stickCounterArm == 0)
stickCounterArm = lib_timers_starttimer();
if (lib_timers_gettimermicroseconds(stickCounterArm) > STICK_ARM_TIME * 1000)
global.activecheckboxitems |= CHECKBOXMASKARM;
}
else if ((stickmask & (STICK_DISARM)) == STICK_DISARM) {
if (stickCounterDisArm == 0)
stickCounterDisArm = lib_timers_starttimer();
if (lib_timers_gettimermicroseconds(stickCounterDisArm) > STICK_ARM_TIME * 1000)
global.activecheckboxitems &= ~CHECKBOXMASKARM;
}
else
{
stickCounterArm = 0;
stickCounterDisArm = 0;
}
#endif
}
