//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 }