Esempio n. 1
0
void hexbright::update() {
  unsigned long now;

#if (DEBUG==DEBUG_LOOP)
  unsigned long start_time=micros();
#endif


#ifdef STROBE  
  while (true) {
    do {
      now = micros();
    } while (next_strobe > now && // not ready for strobe
	     continue_time > now); // not ready for update

    if (next_strobe <= now) {
      if (now - next_strobe <26) {
	digitalWriteFast(DPIN_DRV_EN, HIGH);
	delayMicroseconds(strobe_duration);
	digitalWriteFast(DPIN_DRV_EN, LOW);
      }
      next_strobe += strobe_delay;
    }
    if(continue_time <= now) {
      if(strobe_delay>update_delay && // we strobe less than once every 8333 microseconds
	 next_strobe-continue_time < 4000) // and the next strobe is within 4000 microseconds (may occur before we return)
	continue;
      else
	break;
    }
  } // do nothing... (will short circuit once every 70 minutes (micros maxint))
#else
    do {
      now = micros();
    } while (continue_time > now); // not ready for update
#endif  

  // if we're in debug mode, let us know if our loops are too large
#if (DEBUG!=DEBUG_OFF && DEBUG!=DEBUG_PRINT)
  static int i=0;
#if (DEBUG==DEBUG_LOOP)
  static unsigned long last_time = 0;
  if(!i) {
    Serial.print("Time used: ");
    Serial.print(start_time-last_time);
    Serial.println("/8333");
  }
  last_time = now;
#endif
  if(now-continue_time>5000 && !i) {
    // This may be caused by too much processing for our update_delay, or by too many print statements)
    //  If you're triggering this, your button and light will react more slowly, and some accelerometer
    //  data is being missed.
    Serial.println("WARNING: code is too slow");
  }
  if (!i)
    i=1000/update_delay; // display loop output every second
  else
    i--;
#endif
  
  
  // power saving modes described here: http://www.atmel.com/Images/2545s.pdf
  //run overheat protection, time display, track battery usage
  
#ifdef LED
  // regardless of desired led state, turn it off so we can read the button
  _led_off(RLED);
  delayMicroseconds(50); // let the light stabilize...
  read_button();
  // turn on (or off) the leds, if appropriate
  adjust_leds();
#ifdef PRINT_NUMBER
  update_number();
#endif
#else
  read_button();
#endif
  
  read_thermal_sensor(); // takes about .2 ms to execute (fairly long, relative to the other steps)
  read_charge_state();
  read_avr_voltage();

#ifdef ACCELEROMETER
  read_accelerometer();
  find_down();
#endif
  detect_overheating();
  detect_low_battery();
  apply_max_light_level();
  
  // change light levels as requested
  adjust_light();

  // advance time at the same rate as values are changed in the accelerometer.
  //  advance continue_time here, so the first run through short-circuits, 
  //  meaning we will read hardware immediately after power on.
  continue_time = continue_time+(1000*update_delay);
}
Esempio n. 2
0
void updateDisplay(void)
{
    uint32_t now = micros();
    static uint8_t previousArmedState = 0;

    bool updateNow = (int32_t)(now - nextDisplayUpdateAt) >= 0L;
    if (!updateNow) {
        return;
    }

    nextDisplayUpdateAt = now + DISPLAY_UPDATE_FREQUENCY;

    bool armedState = ARMING_FLAG(ARMED) ? true : false;
    bool armedStateChanged = armedState != previousArmedState;
    previousArmedState = armedState;

    if (armedState) {
        if (!armedStateChanged) {
            return;
        }
        pageState.pageIdBeforeArming = pageState.pageId;
        pageState.pageId = PAGE_ARMED;
        pageState.pageChanging = true;
    } else {
        if (armedStateChanged) {
            pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
            pageState.pageId = pageState.pageIdBeforeArming;
        }

        pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) ||
                (((int32_t)(now - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
        if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) {
            pageState.cycleIndex++;
            pageState.cycleIndex = pageState.cycleIndex % CYCLE_PAGE_ID_COUNT;
            pageState.pageId = cyclePageIds[pageState.cycleIndex];
        }
    }

    if (pageState.pageChanging) {
        handlePageChange();
        pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
        pageState.nextPageAt = now + PAGE_CYCLE_FREQUENCY;
    }

    switch(pageState.pageId) {
        case PAGE_WELCOME:
            showWelcomePage();
            break;
        case PAGE_ARMED:
            showArmedPage();
            break;
        case PAGE_BATTERY:
            showBatteryPage();
            break;
        case PAGE_SENSORS:
            showSensorsPage();
            break;
        case PAGE_RX:
            showRxPage();
            break;
        case PAGE_PROFILE:
            showProfilePage();
            break;
#ifdef GPS
        case PAGE_GPS:
            if (feature(FEATURE_GPS)) {
                showGpsPage();
            } else {
                pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
            }
            break;
#endif
#ifdef ENABLE_DEBUG_OLED_PAGE
        case PAGE_DEBUG:
            showDebugPage();
            break;
#endif
    }
    if (!armedState) {
        updateTicker();
    }
}
void loop(void) {
    loopCount++;

    if (digitalRead(BTN_PIN) == LOW) {
        leftCounter = 0;
        rightCounter = 0;
        delay(1000);
        driveWheel(WHEEL_LEFT, 150);
        driveWheel(WHEEL_RIGHT, 150);
        printDelay = millis();
    }

    tmpTime = micros();
    if ((leftReadedTime + ANALOG_READ_DELAY < tmpTime)
        || (leftReadedTime > tmpTime)
    ) {
        tmpValue = analogRead(LEFT_WHEEL_ENCODER_PIN);
        tmpIndex = leftReadedCount % ANALOG_READ_BUFFER_COUNT;
        leftReaded[tmpIndex] = tmpValue;

        tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
        tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];

        if (tmpValue <= LOW_VALUE_THRESHOLD) {
            if (leftEncoderState) {
                leftEncoderState = 0;
            }
        } else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
            if (!leftEncoderState) {
                leftEncoderState = 1;
                leftCounter++;
            }
        }

        leftReadedCount++;
        leftReadedTime = tmpTime;
    }

    tmpTime = micros();
    if ((rightReadedTime + ANALOG_READ_DELAY < tmpTime)
        || (rightReadedTime > tmpTime)
    ) {
        tmpValue = analogRead(RIGHT_WHEEL_ENCODER_PIN);
        tmpIndex = rightReadedCount % ANALOG_READ_BUFFER_COUNT;
        rightReaded[tmpIndex] = tmpValue;

        tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
        tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];

        if (tmpValue <= LOW_VALUE_THRESHOLD) {
            if (rightEncoderState) {
                rightEncoderState = 0;
            }
        } else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
            if (!rightEncoderState) {
                rightEncoderState = 1;
                rightCounter++;
            }
        }

        rightReadedCount++;
        rightReadedTime = tmpTime;
    }

    tmpTime = micros();
    if ((sensorReadedTime + SENSOR_READ_DELAY < tmpTime)
        || (leftReadedTime > tmpTime)
    ) {
        tmpIndex = (sensorReadedCount % SENSOR_READ_BUFFER_COUNT);
        for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
            analogSensorValue[i][tmpIndex] = analogRead(laserSensors[i]);
        }
        sensorReadedCount++;
        sensorReadedTime = tmpTime;
    }

    if (printDelay + 1000 < millis()) {
        stopWheel(WHEEL_LEFT);
        stopWheel(WHEEL_RIGHT);

        Serial.print("left=");
        Serial.print(leftCounter);
        Serial.print(" right=");
        Serial.print(rightCounter);
        Serial.print(" loops=");
        Serial.print(loopCount);

        for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
            Serial.print("\tS");
            Serial.print(i);
            Serial.print("=");
            Serial.print(getSensorValue(i));
        }
        Serial.println();

        loopCount = 0;
        printDelay = millis();
    }
}
Esempio n. 4
0
//
// The strategy is as follows: We will read pulses, and every pulse is alternating
// short or long pulse. Every bit consists of two pulses.
// We therefore expect twice as many pulses (and states) as we have bits
// We start with the
//
void kopouReceiver::interruptHandler() {
	// This handler is written as one big fuction as that is the fastest
	
	if (!_enabled) {
		return;
	}

	static byte receivedBit;			// Contains "bit" receiving (until changed this is previous bit)
	static kopouCode receivedCode;		// Contains received code
	static kopouCode previousCode;		// Contains previous received code
	static byte repeats = 0;			// The number of times the an identical code is received in a row.
	static unsigned long edgeTimeStamp[3] = {0, };	// Timestamp of edges
	static unsigned int min1Period, max1Period, min2Period, max2Period;

	// Allow for large error-margin. ElCheapo-hardware :(
	// By default 1T is 120µs, but for maximum compatibility go as low as 100µs
	min1Period =  90; // Lower limit for 0 period is 0.3 times measured period; high signals can "linger" a bit sometimes, making low signals quite short.
	max1Period = 180; // Upper limit 
	min2Period = 180; // Lower limit for a 1 bit
	max2Period = 300; // Upper limit
	
	// Filter out too short pulses. This method works as a low pass filter.
	edgeTimeStamp[1] = edgeTimeStamp[2];
	edgeTimeStamp[2] = micros();

	if (_state >= 0 && 
		((edgeTimeStamp[2]-edgeTimeStamp[1] < min1Period) ||			// Filter shorts
		 (edgeTimeStamp[2]-edgeTimeStamp[1] > max2Period)) )			// Filter Long
	{
		RESET_STATE;
		return;
	}

	//unsigned int duration = edgeTimeStamp[1] - edgeTimeStamp[0];
	unsigned int duration = edgeTimeStamp[2] - edgeTimeStamp[1];
	//edgeTimeStamp[0] = edgeTimeStamp[1];

	// Note that if state>=0, duration is always >= 1 period.

	if (_state == -1) {
		// There is no Stopbit: just send it 100 times
		if ((duration > 500) && (duration < 750)) { 
			// Sync signal received.. Preparing for decoding
			receivedCode.period = duration / 5;
#if STATISTICS==1
			receivedCode.minPeriod = duration / 5;
			receivedCode.maxPeriod = duration / 5;
#endif
			receivedBit = 0;
			receivedCode.address= 0;
			receivedCode.unit= 0;
			receivedCode.level= 1;			// Done in direct connect sniffer also
		}
		else {
			return;
		}
	} else
	//if (_state == 0) {
	//	
	//} else
	// If we are here, start bit recognized and we begin decoding the address
	if (_state < 32) { // As long as we are busyg with the address part
		if ((_state % 2) == 0) {
			if (duration < max1Period) {		// first pulse of nul bit
				receivedBit = 0 ;				// Set that we expect a null bit (short-long)
			}
			else {
				receivedBit = 1;				// We expect a 1 bit (long-short)
			}
		}
		else {									// This is the second pulse
			if (duration > min2Period) {		// second pulse is short! -> 0 bit
				if (receivedBit != 0) { 
					RESET_STATE;
					return;
				}
			}
			else {								// second pulse is short -> expect a 1-bit
				if (receivedBit != 1) { 
					RESET_STATE;
					return;
				}
			}
			receivedCode.address = receivedCode.address *2 + receivedBit;
		}
	} else
	// Start with the unit code
	if (_state < 48) { // As long as we are busy with the unit part ..
		if ((_state % 2) == 0) {
			if (duration < max1Period) {		// first pulse of nul bit
				receivedBit = 0 ;				// Set that we expect a null bit (short-long)
			}
			else {
				receivedBit = 1;				// We expect a 1 bit (long-short)
			}
		}
		else {									// This is the second pulse
			if (duration > min2Period) {		// second pulse is short! -> 0 bit
				if (receivedBit != 0) { 
					RESET_STATE;
					return;
				}
#if STATISTICS==1
				if (duration > receivedCode.maxPeriod) receivedCode.maxPeriod = duration;
#endif
				}
			else {								// second pulse is short -> expect a 1-bit
				if (receivedBit != 1) { 
					RESET_STATE;
					return;
				}
#if STATISTICS==1
				if (duration < receivedCode.minPeriod) receivedCode.minPeriod = duration;
#endif
			}
			receivedCode.unit = receivedCode.unit *2 + receivedBit;
		}
	} 
	else { // Otherwise the entire sequence is invalid
		RESET_STATE;
		return;
	}

   _state++;
	
	// OK, the complete address should be in
	if(_state == 48) {
		
		// a valid signal was found!
		if (
			receivedCode.address != previousCode.address ||
			receivedCode.unit != previousCode.unit ||
			receivedCode.level != previousCode.level 
		) {
			repeats=0;
			previousCode = receivedCode;
		}		
		repeats++;
				
		if (repeats >= _minRepeats) {
			if (!_inCallback) {
				_inCallback = true;
				(_callback)(receivedCode);
				_inCallback = false;
			}
			// Reset after callback.
			RESET_STATE;
			return;
		}		
		// Reset for next round
		RESET_STATE;
		return;
	}
	return;
}
/**
 * Update GPS topic
 *  Function is called on each GPS update
 */
void onNewGPSData(void)
{
    static uint32_t lastGPSNewDataTime;
    static int32_t previousLat;
    static int32_t previousLon;
    static int32_t previousAlt;
    static bool isFirstGPSUpdate = true;

    gpsLocation_t newLLH;
    uint32_t currentTime = micros();

    newLLH.lat = gpsSol.llh.lat;
    newLLH.lon = gpsSol.llh.lon;
    newLLH.alt = gpsSol.llh.alt;

    if (sensors(SENSOR_GPS)) {
        if (!(STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) {
            isFirstGPSUpdate = true;
            return;
        }

        if ((currentTime - lastGPSNewDataTime) > MS2US(INAV_GPS_TIMEOUT_MS)) {
            isFirstGPSUpdate = true;
        }

#if defined(INAV_ENABLE_AUTO_MAG_DECLINATION)
        /* Automatic magnetic declination calculation - do this once */
        static bool magDeclinationSet = false;
        if (posControl.navConfig->inav.automatic_mag_declination && !magDeclinationSet) {
            magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units
            magDeclinationSet = true;
        }
#endif

        /* Process position update if GPS origin is already set, or precision is good enough */
        // FIXME: use HDOP here
        if ((posControl.gpsOrigin.valid) || (gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) {
            /* Convert LLH position to local coordinates */
            geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE);

            /* If not the first update - calculate velocities */
            if (!isFirstGPSUpdate) {
                float dT = US2S(getGPSDeltaTimeFilter(currentTime - lastGPSNewDataTime));

                /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
                float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
                if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelNE) {
                    posEstimator.gps.vel.V.X = gpsSol.velNED[0];
                    posEstimator.gps.vel.V.Y = gpsSol.velNED[1];
                }
                else {
                    posEstimator.gps.vel.V.X = (posEstimator.gps.vel.V.X + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f;
                    posEstimator.gps.vel.V.Y = (posEstimator.gps.vel.V.Y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f;
                }

                if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelD) {
                    posEstimator.gps.vel.V.Z = - gpsSol.velNED[2];   // NEU
                }
                else {
                    posEstimator.gps.vel.V.Z = (posEstimator.gps.vel.V.Z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
                }

#if defined(INAV_ENABLE_GPS_GLITCH_DETECTION)
                /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */
                if (detectGPSGlitch(currentTime)) {
                    posEstimator.gps.glitchRecovery = false;
                    posEstimator.gps.glitchDetected = true;
                }
                else {
                    /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */
                    posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected;
                    posEstimator.gps.glitchDetected = false;
                }
#endif

                /* FIXME: use HDOP/VDOP */
                posEstimator.gps.eph = INAV_GPS_EPH;
                posEstimator.gps.epv = INAV_GPS_EPV;

                /* Indicate a last valid reading of Pos/Vel */
                posEstimator.gps.lastUpdateTime = currentTime;
            }

            previousLat = gpsSol.llh.lat;
            previousLon = gpsSol.llh.lon;
            previousAlt = gpsSol.llh.alt;
            isFirstGPSUpdate = false;

            lastGPSNewDataTime = currentTime;
        }
    }
    else {
        posEstimator.gps.lastUpdateTime = 0;
    }
}
Esempio n. 6
0
void resumeRxSignal(void)
{
    suspendRxSignalUntil = micros();
    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
    failsafeOnRxResume();
}
Esempio n. 7
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;
    serialPort_t* loopbackPort = NULL;

    systemInit();
#ifdef USE_LAME_PRINTF
    init_printf(NULL, _putc);
#endif

    checkFirstTime(false);
    readEEPROM();

    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);
    initBoardAlignment();

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);

    mixerInit(); // this will set core.useServo var depending on mixer type
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
    pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
    pwm_params.useServos = core.useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
    if (feature(FEATURE_3D))
        pwm_params.idlePulse = mcfg.neutral3d;
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
        break;
    }

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
    for (i = 0; i < RC_CHANS; i++)
        rcData[i] = 1502;
    rcReadRawFunc = pwmReadRawRC;
    core.numRCChannels = MAX_INPUTS;

    if (feature(FEATURE_SERIALRX)) {
        switch (mcfg.serialrx_type) {
            case SERIALRX_SPEKTRUM1024:
            case SERIALRX_SPEKTRUM2048:
                spektrumInit(&rcReadRawFunc);
                break;
            case SERIALRX_SBUS:
                sbusInit(&rcReadRawFunc);
                break;
            case SERIALRX_SUMD:
                sumdInit(&rcReadRawFunc);
                break;
        }
    } else { // spektrum and GPS are mutually exclusive
        // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
        // gpsInit will return if FEATURE_GPS is not enabled.
        // Sanity check below - protocols other than NMEA do not support baud rate autodetection
        if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
            mcfg.gps_baudrate = 0;
        gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    serialInit(mcfg.serial_baudrate);

    if (feature(FEATURE_SOFTSERIAL)) {
      setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
#ifdef SOFTSERIAL_LOOPBACK
      loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
      serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
#endif
    }

    if (feature(FEATURE_TELEMETRY))
        initTelemetry();

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = CALIBRATING_ACC_CYCLES;
    calibratingG = CALIBRATING_GYRO_CYCLES;
    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLES_25 = 1;

    // loopy
    while (1) {
        loop();
#ifdef SOFTSERIAL_LOOPBACK
        if (loopbackPort) {
            while (serialTotalBytesWaiting(loopbackPort)) {
    
                uint8_t b = serialRead(loopbackPort);
                serialWrite(loopbackPort, b);
                //serialWrite(core.mainport, b);
            };
        }
#endif
    }
}
Esempio n. 8
0
void Pedals::patWatchdog(){
    watchdogTimer = micros();   //pat the watchdog
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////FONCTIONS DHT///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void DHTSensor::readSensor()
{
//Initialisation
	unsigned long 	CurrentTime;
	unsigned long 	MeasurementTime;
	uint16_t 		readData = 0;
	uint16_t 		Humidity = 0;
	uint16_t 		Temperature = 0;
	Temp = -1;
	Hum = -1;
//Echantillon de requete:
	//Signal d'envoi:
	digitalWrite(Pin, LOW);
	pinMode(Pin, OUTPUT);
	//Preparation du capteur:
	if(SensorType == 11) delay(18);
	else delayMicroseconds(800);
	pinMode(Pin, INPUT);
	//Mise en Mode Lecture:
	digitalWrite(Pin, HIGH);
//Lecture du Capteur
	for(int8_t i = -3; i < 2*40; i++)
	{
		CurrentTime = micros();
		do
		{
			MeasurementTime = micros()-CurrentTime;
			if (MeasurementTime > 90)
			{
				ErrorLevel = 2;
				return;
			}
		}
		while(digitalRead(Pin) == (i&1)?HIGH:LOW);		
		if(i>=0 && (i&1))
		{
			readData <<= 1;
			if (MeasurementTime > 30)
			{
				readData |= 1;
			}
		}
		switch(i)
		{
			case 31:
				Humidity = readData;
				break;
			case 63:
				Temperature = readData;
				readData = 0;
				break;
		}
	}
//Verification de fin de lecture:
	if((uint8_t)(((uint8_t)Humidity)+(Humidity>>8)+((uint8_t)Temperature)+(Temperature>>8)) != readData)
	{
		ErrorLevel = 3;
		return;
	}
//Enregistrement des donnees:
	if(SensorType == 11)
	{
		Hum = Humidity>>8;
		Temp = Temperature>>8;
	}
Esempio n. 10
0
void delayMicroseconds(uint32_t us)
{
    uint32_t now = micros();
    while (micros() - now < us);
}
Esempio n. 11
0
void Vco::calibrate()
{

  //
  // measure the frequencies of all input dac values..  
  //

//  const int inputPin = 4;  // v2 : patch wire
  const int inputPin = A1; // v3 : onboard trace..
  pinMode(inputPin, INPUT);

  const uint16_t* dac_inputPtr;
  dac_inputPtr = dac_input;
  int i = 0;

  for (uint16_t idx_octave = 0; idx_octave < nrOctaves; idx_octave++)
  {
    for (uint16_t idx_note = 0; idx_note < notesPerOctave; idx_note++)
    {
      // set dac value
      uint16_t dacValue = (*dac_inputPtr++) - 79;
      analogWrite(msPwmPin, (dacValue>>8)&0xFF);
      analogWrite(lsPwmPin, dacValue&0xFF);

      //delay(100); // unnecessary since we always measure between falling edges using the newly configured slope.

      bool bit = PINC & 0b00000010; // arduino pin A1
      //bool bit = PIND & 0b00010000; // arduino pin 4
      //bool bit = PINB & 0b00000001; // arduino pin 8
      bool lastBit = bit;
      uint16_t edgesMeasured = 0; // = periods measured + 1
      uint32_t startTime;
      uint32_t lastFalling = 0; // index of last falling edge
      uint32_t lastRising = 0; // index of last rising edge

      // sample the thing at super high speed. don't store anything, just check for falling edges
      for (uint32_t idx_sample = 0; idx_sample < maxCalibSamples; idx_sample++)
      {
        //bool sample = digitalRead(inputPin);
        // optimized hardcoded
        bit = PINC & 0b00000010; // arduino pin A1
        //bit = PIND & 0b00010000; // arduino pin 4
        //bit = PINB & 0b00000001; // arduino pin 8

        if (bit && !lastBit)
        {
          // rising edge!
          lastRising = idx_sample;
        }

        if (!bit && lastBit)
        {
          // this is a sanity check. a miniscule falling edge may also be present around the half of the slope.. (!)
          if (idx_sample - lastRising > (1<<(9-idx_octave)))
          {
            if (edgesMeasured == 0)
            {
              startTime = micros();
            }
            // falling edge!
            edgesMeasured++;
            // the higher the octave the more periods we need to have to remain accurate.. micros() really adds an offset.. 
            // also, we don't want this to take forever for the low octaves.. the user is waiting for the synth to boot.. 
            if ((edgesMeasured > 64) || (edgesMeasured == (uint16_t) (1<<idx_octave)+1))
            {
              break;
            }
          }
          lastFalling = idx_sample;
        }

        lastBit = bit;
      } // for sample

      uint32_t elapsedTime = micros() - startTime;
      uint32_t measuredPeriod = elapsedTime / (edgesMeasured - 1);
//      Serial.println(measuredPeriod);
      f_measured[i++] = 64000000. / (float) measuredPeriod;
      Serial.print(elapsedTime); Serial.print("/"); Serial.println(edgesMeasured);

    } // for note in octave
  } // for octave

  //
  // now compute the piecewise linear dac->frequency response, and use this to generate the calibration table.
  //

  const int highest_note = 104;
  int j = 0;
  for (int i=12; i<highest_note; i++)
  {
    float f_synth = 8.1757989156 * pow(2.,i/12.); 
    if (f_synth >= f_measured[j+1])
    {
      j++;
    }
    float df = f_measured[j+1] - f_measured[j];
    float ddac = dac_input[j+1] - dac_input[j];
    float f_frac = (f_synth - f_measured[j])/df;
    float dac_synth_ = dac_input[j] + f_frac*ddac;
    uint16_t dac_synth__ = (uint16_t) (dac_synth_ + 0.5);
    Serial.println(dac_synth__);
    freq_table[i] = dac_synth__;
  }

}
Esempio n. 12
0
// return values:
// DHTLIB_OK
// DHTLIB_ERROR_TIMEOUT
int dht::_readSensor(uint8_t pin, uint8_t wakeupDelay)
{
    // INIT BUFFERVAR TO RECEIVE DATA
    uint8_t mask = 128;
    uint8_t idx = 0;

    // replace digitalRead() with Direct Port Reads.
    // reduces footprint ~100 bytes => portability issue?
    // direct port read is about 3x faster
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
    volatile uint8_t *PIR = portInputRegister(port);

    // EMPTY BUFFER
    for (uint8_t i = 0; i < 5; i++) bits[i] = 0;

    // REQUEST SAMPLE
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW); // T-be
    delay(wakeupDelay);
    digitalWrite(pin, HIGH);   // T-go
    delayMicroseconds(40);
    pinMode(pin, INPUT);

    // GET ACKNOWLEDGE or TIMEOUT
    uint16_t loopCntLOW = DHTLIB_TIMEOUT;
    while ((*PIR & bit) == LOW )  // T-rel
    {
        if (--loopCntLOW == 0) return DHTLIB_ERROR_TIMEOUT;
    }

    uint16_t loopCntHIGH = DHTLIB_TIMEOUT;
    while ((*PIR & bit) != LOW )  // T-reh
    {
        if (--loopCntHIGH == 0) return DHTLIB_ERROR_TIMEOUT;
    }

    // READ THE OUTPUT - 40 BITS => 5 BYTES
    for (uint8_t i = 40; i != 0; i--)
    {
        loopCntLOW = DHTLIB_TIMEOUT;
        while ((*PIR & bit) == LOW )
        {
            if (--loopCntLOW == 0) return DHTLIB_ERROR_TIMEOUT;
        }

        uint32_t t = micros();

        loopCntHIGH = DHTLIB_TIMEOUT;
        while ((*PIR & bit) != LOW )
        {
            if (--loopCntHIGH == 0) return DHTLIB_ERROR_TIMEOUT;
        }

        if ((micros() - t) > 40)
        {
            bits[idx] |= mask;
        }
        mask >>= 1;
        if (mask == 0)   // next byte?
        {
            mask = 128;
            idx++;
        }
    }
    pinMode(pin, OUTPUT);
    digitalWrite(pin, HIGH);

    return DHTLIB_OK;
}
Esempio n. 13
0
void Task::markJustCalled() {
    this->lastCallTimeMicros = micros();
}
Esempio n. 14
0
void hexbright::set_strobe_delay(unsigned long delay) {
  strobe_delay = delay;
  next_strobe = micros()+strobe_delay;
}
Esempio n. 15
0
void osdUpdate(void)
{
    TIME_SECTION_BEGIN(0);

    uint32_t now = micros();

    static bool armed = false;
    static uint32_t armedAt = 0;

    bool armedNow = fcStatus.fcState & (1 << FC_STATE_ARM);
    if (armed != armedNow) {
        if (armedNow) {
            armedAt = now;
        }

        armed = armedNow;
    }

    if (armed) {
        if (now > armedAt) {

            fcStatus.armedDuration = (now - armedAt) / 1000;
        }
    }

    osdClearScreen();


    // test all timers, setting corresponding bits
    uint32_t timActive = 0;
    for(timId_e timId = 0; timId < timTimerCount; timId++) {
        if(cmp32(now, timerState[timId].val) < 0)
            continue;  // not ready yet
        timActive |= 1 << timId;
        // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
        // max delay is limited to 5s
        if(cmp32(now, timerState[timId].val) >= OSD_MS(100) || cmp32(now, timerState[timId].val) < OSD_HZ(5000) ) {
            timerState[timId].val = now;
        }
    }

    // update all timers
    for(unsigned i = 0; i < ARRAYLEN(timerTable); i++) {
        int timId = timerTable[i].timId;

        bool updateNow = timActive & (1 << timId);
        if (!updateNow) {
            continue;
        }
        timerState[timId].toggle = !timerState[timId].toggle;
        timerState[timId].counter++;
        timerState[timId].val += timerTable[i].delay;
    }

    bool showNowOrFlashWhenFCTimeoutOccured = !mspClientStatus.timeoutOccured || (mspClientStatus.timeoutOccured && timerState[tim10Hz].toggle);

    osdSetElementFlashOnDisconnectState(showNowOrFlashWhenFCTimeoutOccured);

    for (uint8_t elementIndex = 0; elementIndex < MAX_OSD_ELEMENT_COUNT; elementIndex++) {
        element_t *element = &osdElementConfig()->elements[elementIndex];

        osdDrawTextElement(element);
    }

    //
    // flash the logo for a few seconds
    // then leave it on for a few more before turning it off
    //

    static bool splashDone = false;
    if (!splashDone) {
        if (
            (timerState[tim1Hz].counter > 3 && timerState[tim1Hz].counter < 6) ||
            (timerState[tim1Hz].counter <= 3 && timerState[tim10Hz].toggle)
        ) {
            osdDisplaySplash();
        }

        if (timerState[tim1Hz].counter >= 10) {
            splashDone = true;
        }
    }

    if (timerState[tim2Hz].toggle && splashDone) {
        if (!osdIsCameraConnected()) {
            osdPrintAt(14, 4, "NO");
            osdPrintAt(12, 5, "CAMERA");
        }
    }

    TIME_SECTION_END(0);

    osdHardwareUpdate();

}
Esempio n. 16
0
void SysTick_Handler(void)
{
    uint8_t index;
    uint32_t currentTime;

    sysTickCycleCounter = *DWT_CYCCNT;
    sysTickUptime++;

    watchDogsTick();

    if ((systemReady         == true)  &&
        (cliBusy             == false) &&
        (accelCalibrating    == false) &&
        (escCalibrating      == false) &&
        (magCalibrating      == false) &&
        (mpu6000Calibrating  == false))

    {
        #ifdef _DTIMING
//    	    LA2_ENABLE;
        #endif

        frameCounter++;
        if (frameCounter > FRAME_COUNT)
            frameCounter = 1;

        ///////////////////////////////

        currentTime = micros();
        deltaTime1000Hz = currentTime - previous1000HzTime;
        previous1000HzTime = currentTime;

        readMPU6000();

        accelSum500Hz[XAXIS] += rawAccel[XAXIS].value;
        accelSum500Hz[YAXIS] += rawAccel[YAXIS].value;
        accelSum500Hz[ZAXIS] += rawAccel[ZAXIS].value;

        accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
        accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
        accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;

        gyroSum500Hz[ROLL ] += rawGyro[ROLL ].value;
        gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
        gyroSum500Hz[YAW  ] += rawGyro[YAW  ].value;

        ///////////////////////////////

        if ((frameCounter % COUNT_500HZ) == 0)
        {
            frame_500Hz = true;

            for (index = 0; index < 3; index++)
            {
            	accelSummedSamples500Hz[index] = accelSum500Hz[index];
            	accelSum500Hz[index] = 0;

            	gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
                gyroSum500Hz[index] = 0;
            }
        }

        ///////////////////////////////

        if ((frameCounter % COUNT_100HZ) == 0)
        {
            frame_100Hz = true;

            for (index = 0; index < 3; index++)
            {
                accelSummedSamples100Hz[index] = accelSum100Hz[index];
                accelSum100Hz[index] = 0;
            }

            if (!newTemperatureReading)
			{
				readTemperatureRequestPressure();
			    newTemperatureReading = true;
			}
			else
			{
			    readPressureRequestTemperature();
			    newPressureReading = true;
			}
        }

        ///////////////////////////////

        if ((frameCounter % COUNT_50HZ) == 0)
            frame_50Hz = true;

        ///////////////////////////////

        if (((frameCounter + 1) % COUNT_10HZ) == 0)
            newMagData = readMag();

        if ((frameCounter % COUNT_10HZ) == 0)
            frame_10Hz = true;

        ///////////////////////////////

        if ((frameCounter % COUNT_5HZ) == 0)
            frame_5Hz = true;

        ///////////////////////////////

        if ((frameCounter % COUNT_1HZ) == 0)
            frame_1Hz = true;

        ///////////////////////////////////

        executionTime1000Hz = micros() - currentTime;

        ///////////////////////////////

        #ifdef _DTIMING
//            LA2_DISABLE;
        #endif
    }
}
Esempio n. 17
0
void suspendRxSignal(void)
{
    suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
    failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
}
Esempio n. 18
0
File: rf433.c Progetto: ray66rus/rpi
static inline void await(unsigned us) {
	unsigned long long await_timer = micros() + us;
	while(micros() < await_timer) ; 
}
Esempio n. 19
0
void serialCom() {
  uint8_t c,cc,port,state,bytesTXBuff;
  static uint8_t offset[UART_NUMBER];
  static uint8_t dataSize[UART_NUMBER];
  static uint8_t c_state[UART_NUMBER];
  uint32_t timeMax; // limit max time in this function in case of GPS

  timeMax = micros();
  for(port=0;port<UART_NUMBER;port++) {
    CURRENTPORT=port;
    #define RX_COND
    #if defined(SERIAL_RX) && (UART_NUMBER > 1)
      #define RX_COND && (RX_SERIAL_PORT != port)
    #endif
    cc = SerialAvailable(port);
    while (cc-- RX_COND) {
      bytesTXBuff = SerialUsedTXBuff(port); // indicates the number of occupied bytes in TX buffer
      if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin)
      c = SerialRead(port);
      #ifdef SUPPRESS_ALL_SERIAL_MSP
        evaluateOtherData(c); // no MSP handling, so go directly
      #else //SUPPRESS_ALL_SERIAL_MSP
        state = c_state[port];
        // regular data handling to detect and handle MSP and other data
        if (state == IDLE) {
          if (c=='$') state = HEADER_START;
          else evaluateOtherData(c); // evaluate all other incoming serial data
        } else if (state == HEADER_START) {
          state = (c=='M') ? HEADER_M : IDLE;
        } else if (state == HEADER_M) {
          state = (c=='<') ? HEADER_ARROW : IDLE;
        } else if (state == HEADER_ARROW) {
          if (c > INBUF_SIZE) {  // now we are expecting the payload size
            state = IDLE;
            continue;
          }
          dataSize[port] = c;
          checksum[port] = c;
          offset[port] = 0;
          indRX[port] = 0;
          state = HEADER_SIZE;  // the command is to follow
        } else if (state == HEADER_SIZE) {
          cmdMSP[port] = c;
          checksum[port] ^= c;
          state = HEADER_CMD;
        } else if (state == HEADER_CMD) {
          if (offset[port] < dataSize[port]) {
            checksum[port] ^= c;
            inBuf[offset[port]++][port] = c;
          } else {
            if (checksum[port] == c) // compare calculated and transferred checksum
              evaluateCommand(cmdMSP[port]); // we got a valid packet, evaluate it
            state = IDLE;
            cc = 0; // no more than one MSP per port and per cycle
          }
        }
        c_state[port] = state;

        // SERIAL: try to detect a new nav frame based on the current received buffer
        #if defined(GPS_SERIAL)
        if (GPS_SERIAL == port) {
          static uint32_t GPS_last_frame_seen; //Last gps frame seen at this time, used to detect stalled gps communication
          if (GPS_newFrame(c)) {
            //We had a valid GPS data frame, so signal task scheduler to switch to compute
            if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; //Blink GPS update
            GPS_last_frame_seen = timeMax;
            GPS_Frame = 1;
          }
  
          // Check for stalled GPS, if no frames seen for 1.2sec then consider it LOST
          if ((timeMax - GPS_last_frame_seen) > 1200000) {
            //No update since 1200ms clear fix...
            f.GPS_FIX = 0;
            GPS_numSat = 0;
          }
        }
        if (micros()-timeMax>250) return;  // Limit the maximum execution time of serial decoding to avoid time spike
        #endif
      #endif // SUPPRESS_ALL_SERIAL_MSP
    } // while
  } // for
}
Esempio n. 20
0
/**
  Pin change interrupt routine that identifies 1 and 0 LightwaveRF bits
  and constructs a message when a valid packet of data is received.
**/
void lw_process_bits() { 
  // Don't process bits when a message is already ready
  if (lw_got_message) return;
  
  byte v = digitalRead(lw_rx_pin); // the current value
  unsigned long curr = micros(); // the current time in microseconds
  
  // Calculate pulse duration in 50 microsecond units
  unsigned int dur = (curr-lw_prev)/50;
  lw_prev = curr;
  
  // See if pulse corresponds to expected bit length
  if (dur < 6) {
    // inter 1 bit gap - do nothing
  } else if (dur < 11) {
    // potential 1 bit
    if (!v) { // value now zero as 1 pulse ended
      // 1 bit
      if (!lw_p_started) {
        // Start of message
        lw_p_started = true;
        lw_b_started = false;
        lw_num_bytes = 0;
      } else if (!lw_b_started) {
        // byte started
        lw_b_started = true;
        lw_num_bits = 0;
        lw_byte = 0;
      } else {
        // a valid 1 bit
        lw_byte = (lw_byte << 1) | 1;
        if (++lw_num_bits == 8) { // Test for complete byte
		  // Add the byte to the message
		  lw_b_started = false;
		  lw_msg[lw_num_bytes++] = lw_byte;
        }
      }
    } else {
      // Too short for a zero bit
      lw_p_started = false;
	  if (lw_num_bytes > 0) lw_num_invalid_packets[0]++;
    }
  } else if (dur > 20 && dur < 28) {
    // potential 0 bit
    if (v) {
      // 0 bit
      if (!lw_b_started) {
        // Zero bit where byte start bit expected
        lw_p_started = false;
		if (lw_num_bytes > 0) lw_num_invalid_packets[1]++;
      } else if (lw_p_started) {
        // Valid 0 bit
        lw_byte = (lw_byte << 1);
        if (++lw_num_bits == 8) {
          // Add the byte to the message
          lw_msg[lw_num_bytes++] = lw_byte;
          lw_b_started = false;
        }
      }
    } else {
      // Too long for a 1 bit
      lw_p_started = false;
	  if (lw_num_bytes > 0) lw_num_invalid_packets[2]++;
    }
  } else {
     // Not a valid length for a bit
	 if (lw_p_started && lw_num_bytes > 0) lw_num_invalid_packets[3]++;
	 lw_p_started = false;
  }
  
  // See if we have the whole message
  if (lw_p_started && lw_num_bytes == lw_msg_len) {
    lw_got_message = true;
    lw_p_started = false;
  }
}
Esempio n. 21
0
void setup()
{
  pinMode(ledPin, OUTPUT);      // sets the digital pin as output
  pinMode(testPin, OUTPUT);
  Serial.begin(57600);        // connect to the serial port
  Serial.println("Arduino Audio Loopback");

  //--------------------------------------------------------------------------
  // INPUT - ADC
  //--------------------------------------------------------------------------

  // Table 24-5. ADC Prescaler selections
  //
  // ADPS2 ADPS1 ADPS0 Division Factor
  //   0     0     0           2
  //   0     0     1           2
  //   0     1     0           4
  //   0     1     1           8
  //   1     0     0          16
  //   1     0     1          32
  //   1     1     0          64
  //   1     1     1         128
  // set adc prescaler to 64 for 19kHz sampling frequency
  cbi(ADCSRA, ADPS2);
  sbi(ADCSRA, ADPS1);
  sbi(ADCSRA, ADPS0);

  // ADMUX - ADC Multiplexer Selection Register
  sbi(ADMUX,ADLAR);  // 8-Bit ADC in ADCH Register
  //   Table 24-3.  Voltage Reference Selections for ADC
  //   REFS1     REFS0   Voltage Reference Selection
  //     0         0     AREF, Internal Vref turned off
  //     0         1     AVCC with external capacitor at AREF pin
  //     1         0     Reserved
  //     1         1     Internal 1.1V Voltage Reference with external cap. at AREF pin
  cbi(ADMUX,REFS1);
  sbi(ADMUX,REFS0);  // VCC Reference

  // Table 24-4. Input Channel Selections
  //  MUX3...0                     Single Ended Input
  //  0000                         ADC0
  //  0001                         ADC1
  //  0010                         ADC2
  //  0011                         ADC3
  //  0100                         ADC4
  //  0101                         ADC5
  //  0110                         ADC6
  //  0111                         ADC7
  //  1000                         ADC8(1)
  //  1001                         (reserved)
  //  1010                         (reserved)
  //  1011                         (reserved)
  //  1100                         (reserved)
  //  1101                         (reserved)
  //  1110                         1.1V (VBG)
  //  1111                         0V (GND)
  // Set Input Multiplexer to Channel 0
  cbi(ADMUX,MUX0);
  cbi(ADMUX,MUX1);
  cbi(ADMUX,MUX2);
  cbi(ADMUX,MUX3);


  //--------------------------------------------------------------------------
  // OUTPUT - PWM
  //--------------------------------------------------------------------------

  // TCCR2A - Timer/Counter Control Register A
  //   Table 18-3. Compare Output Mode, Fast PWM Mode(1)
  //   COM2A1      COM2A0    Description
  //       0         0       Normal port operation, OC2A disconnected.
  //       0         1       WGM22 = 0: Normal Port Operation, OC0A Disconnected.
  //                         WGM22 = 1: Toggle OC2A on Compare Match.
  //       1         0       Clear OC2A on Compare Match, set OC2A at BOTTOM,
  //                         (non-inverting mode).
  //       1         1       Set OC2A on Compare Match, clear OC2A at BOTTOM,
  //                         (inverting mode).
  // Timer2 PWM Mode set to fast PWM 
  sbi (TCCR2A, COM2A1);
  cbi (TCCR2A, COM2A0);

  // Table 18-8.   Waveform Generation Mode Bit Description
  //                                     Timer/Counter
  //                                     Mode of                 Update of  TOV Flag
  //  Mode     WGM2    WGM1     WGM0     Operation           TOP  OCRx at  Set on(1)(2)
  //    0        0        0       0      Normal             0xFF Immediate    MAX
  //    1        0        0       1      PWM, Phase         0xFF    TOP     BOTTOM
  //                                     Correct
  //    2        0        1       0      CTC                OCRA Immediate    MAX
  //    3        0        1       1      Fast PWM           0xFF  BOTTOM      MAX
  //    4        1        0       0      Reserved           ----   ----      ----
  //    5        1        0       1      PWM, Phase         OCRA    TOP     BOTTOM
  //                                     Correct
  //    6        1        1       0      Reserved           ----   ----      ----
  //    7        1        1       1      Fast PWM           OCRA  BOTTOM      TOP
  cbi (TCCR2B, WGM22);
  sbi (TCCR2A, WGM21);
  sbi (TCCR2A, WGM20);

  // TCCR2B - Timer/Counter Control Register B
  // Table 18-9. Clock Select Bit Description
  //     CS22        CS21          CS20       Description
  //       0          0              0        No clock source (Timer/Counter stopped).
  //       0          0              1        clkT2S/(No prescaling)
  //       0          1              0        clkT2S/8 (From prescaler)
  //       0          1              1        clkT2S/32 (From prescaler)
  //       1          0              0        clkT2S/64 (From prescaler)
  //       1          0              1        clkT2S/128 (From prescaler)
  //       1          1              0        clkT2S/256 (From prescaler)
  //       1          1              1        clkT2S/1024 (From prescaler)
  // Timer2 Clock Prescaler to : 1 
  cbi (TCCR2B, CS22);
  cbi (TCCR2B, CS21);
  sbi (TCCR2B, CS20);

  // 1 is output, 0 is input
  // Timer2 PWM Port Enable
  sbi(DDRB,3);                    // set digital pin 11 to output

  //cli();                         // disable interrupts to avoid distortion
  // TIMSK0 - Timer/Counter Interrupt Mask Register
  // - Bit 0 - TOIE0: Timer/Counter0 Overflow Interrupt Enable
  //cbi (TIMSK0,TOIE0);              // disable Timer0 !!! delay is off now
  // TIMSK1 - Timer/Counter1 Interrupt Mask Register
  // - Bit 0 - TOIE1: Timer/Counter1, Overflow Interrupt Enable
  sbi (TIMSK2,TOIE2);              // enable Timer2 Interrupt


  //--------------------------------------------------------------------------
  // Timer 1
  //--------------------------------------------------------------------------
  
  // Timer1 - Clock Select: No prescaling
  //   Table 16-5.  Clock Select Bit Description
  //   CS12      CS11    CS10      Description
  //     0        0        0       No clock source (Timer/Counter stopped).
  //     0        0        1       clkI/O/1 (No prescaling)
  //     0        1        0       clkI/O/8 (From prescaler)
  //     0        1        1       clkI/O/64 (From prescaler)
  //     1        0        0       clkI/O/256 (From prescaler)
  //     1        0        1       clkI/O/1024 (From prescaler)
  //     1        1        0       External clock source on T1 pin. Clock on falling edge.
  //     1        1        1       External clock source on T1 pin. Clock on rising edge.
  cbi(TCCR1B, CS12);
  cbi(TCCR1B, CS11);
  sbi(TCCR1B, CS10);

  // Timer1 - Overflow Interrupt: Enable
  sbi (TIMSK1,TOIE1);

  // Timer1 - Waveform Generation Mode: normal
  cbi (TCCR1B, WGM13);
  cbi (TCCR1B, WGM12);
  cbi (TCCR1A, WGM11);
  cbi (TCCR1A, WGM10);

  // Timer1 - Compare Output Mode: normal port operation
  cbi(TCCR1A, COM1A1);
  cbi(TCCR1A, COM1A0);
  cbi(TCCR1A, COM1B1);
  cbi(TCCR1A, COM1B0);


  //--------------------------------------------------------------------------
  // Variables initialization
  //--------------------------------------------------------------------------

  ltime = micros();

  // interrupt count on timer 2
  icnt = 0;
  icnt1 = 0;
}
Esempio n. 22
0
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
{
    if (!g.compass_enabled) {
        cliSerial->printf("Compass: ");
        print_enabled(false);
        return (0);
    }

    if (!compass.init()) {
        cliSerial->println("Compass initialisation failed!");
        return 0;
    }
    ahrs.init();
    ahrs.set_fly_forward(true);
    ahrs.set_wind_estimation(true);
    ahrs.set_compass(&compass);

    // we need the AHRS initialised for this test
    ins.init(ins_sample_rate);
    ahrs.reset();

    uint16_t counter = 0;
    float heading = 0;

    print_hit_enter();

    while(1) {
        hal.scheduler->delay(20);
        if (micros() - fast_loopTimer_us > 19000UL) {
            fast_loopTimer_us       = micros();

            // INS
            // ---
            ahrs.update();

            if(counter % 5 == 0) {
                if (compass.read()) {
                    // Calculate heading
                    const Matrix3f &m = ahrs.get_dcm_matrix();
                    heading = compass.calculate_heading(m);
                    compass.learn_offsets();
                }
            }

            counter++;
            if (counter>20) {
                if (compass.healthy()) {
                    const Vector3f &mag_ofs = compass.get_offsets();
                    const Vector3f &mag = compass.get_field();
                    cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
                                        (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                        (double)mag.x, (double)mag.y, (double)mag.z,
                                        (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
                } else {
                    cliSerial->println("compass not healthy");
                }
                counter=0;
            }
        }
        if (cliSerial->available() > 0) {
            break;
        }
    }

    // save offsets. This allows you to get sane offset values using
    // the CLI before you go flying.
    cliSerial->println("saving offsets");
    compass.save_offsets();
    return (0);
}
Esempio n. 23
0
void getEstimatedAltitude(void)
{
    static int8_t   VarioTab[VarioTabsize];
    static uint8_t  Vidx = 0, IniStep = 0, IniCnt = 0;
    static uint32_t LastBarotime = 0;
    static float    AvgHz = 0, LastEstAltBaro = 0, SNRcorrect, SNRavg = 0;
    float           NewVal, EstAltBaro;
    uint32_t        TimeTemp;
    uint8_t         i;

    if (!GroundAltInitialized)
    {
        if (newbaroalt)
        {
            TimeTemp     = micros();
            NewVal       = (float)(TimeTemp - LastBarotime);
            LastBarotime = TimeTemp;
            switch(IniStep)                                                   // Casemachine here for further extension
            {
            case 0:
                IniCnt++;
                if(IniCnt == 50)                                              // Waste 50 Cycles to let things (buffers) settle then ini some vars and proceed
                {
                    for (i = 0; i < VarioTabsize; i++) VarioTab[i] = 0;
                    EstAlt = GroundAlt = vario = 0;
                    IniCnt = 0;
                    IniStep++;
                }
                break;
            case 1:
                GroundAlt += BaroAlt;
                AvgHz     += NewVal;
                IniCnt++;
                if (IniCnt == 50)                                             // Gather 50 values
                {
                    GroundAlt *= 0.02f;
                    AvgHz      = 50000000.0f / AvgHz;                         // Calculate Average Hz here since we skip Baro temp readout every 2nd read
                    GroundAltInitialized = true;
                    SonarStatus = 0;
                }
                break;
            }
        }
    }
    else
    {
        if (sensors(SENSOR_SONAR))
        {
            if (SonarStatus) NewVal = sonarAlt;
            switch(SonarStatus)
            {
            case 0:
                SNRavg  = 0;
                IniStep = 0;
                break;
            case 1:
                if (!IniStep)
                {
                    IniStep = 1;
                    SNRavg  = NewVal;
                }
                else SNRavg += 0.2f * (NewVal - SNRavg);                      // Adjust Average during accepttimer (ca. 550ms so ca. 20 cycles)
                SNRcorrect = EstAlt + GroundAlt - SNRavg;                     // Calculate baro/sonar displacement on 1st contact
                break;
            case 2:
                if (newbaroalt) BaroAlt = (SNRcorrect + NewVal) * cfg.snr_cf + BaroAlt * (1 - cfg.snr_cf); // Set weight / make transition smoother
                break;
            }
        }
        EstAlt += vario * ACCDeltaTimeINS;
        if (newbaroalt)
        {
            EstAltBaro     = BaroAlt - GroundAlt;
            VarioTab[Vidx] = constrain((int16_t)(EstAltBaro - LastEstAltBaro), -127, 127);
            Vidx++;
            if (Vidx == VarioTabsize) Vidx = 0;
            LastEstAltBaro = EstAltBaro;
            NewVal = 0;
            for (i = 0; i < VarioTabsize; i++) NewVal += (float)VarioTab[i];
            NewVal = (NewVal * AvgHz)/(float)VarioTabsize;
            vario  = vario  * cfg.accz_vcf + NewVal     * (1.0f - cfg.accz_vcf);
            EstAlt = EstAlt * cfg.accz_acf + EstAltBaro * (1.0f - cfg.accz_acf);
            if (cfg.bar_dbg)
            {
                debug[0] = EstAltBaro * 10;
                debug[1] = EstAlt     * 10;
                debug[2] = NewVal;
                debug[3] = vario;
            }
        }
    }
}
Esempio n. 24
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;
    bool sensorsOK = false;
    initEEPROM();
    readEEPROM();
    SetSysClock(mcfg.emf_avoidance);
    //hw_revision = 1;
    systemInit();
    delay(100);
    activateConfig();
    i2cInit(I2C_DEVICE);
    // Do muc pin
    adc_params.powerAdcChannel = 0;
    mcfg.power_adc_channel = 0;


    // configure rssi ADC
    adc_params.rssiAdcChannel = 0;
    mcfg.rssi_adc_channel = 0;

    adcInit(&adc_params);
    // Kiem tra Volt cua Pin
    batteryInit();
    initBoardAlignment();
    sensorsSet(SENSORS_SET);
    sensorsOK = sensorsAutodetect();

    imuInit();
    mixerInit(); // xem lai
    serialInit(115200);//(mcfg.serial_baudrate);

    pwm_params.motorPwmRate = 400;
    pwm_params.pwmFilter = mcfg.pwm_filter;
    pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors
    pwm_params.syncPWM = feature(FEATURE_SYNCPWM);
    pwm_params.fastPWM = feature(FEATURE_FASTPWM);
    pwm_params.servoCenterPulse = mcfg.midrc;
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    pwm_params.adcChannel = 0;
    pwmInit(&pwm_params);
    for (i = 0; i < RC_CHANS; i++)
        rcData[i] = 1502;
    rcReadRawFunc = pwmReadRawRC;
    core.numRCChannels = MAX_INPUTS;
    gpsInit(mcfg.gps_baudrate);


    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = CALIBRATING_ACC_CYCLES;
    calibratingG = CALIBRATING_GYRO_CYCLES;
    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLE = 1;

    while (1) {
        loop();
    }
}
Esempio n. 25
0
 void init()
 {
     prev_ = micros();
 }
Esempio n. 26
0
int main(void)
{
	///////////////////////////////////////////////////////////////////////////

	uint32_t currentTime;

    systemReady = false;

    systemInit();

    systemReady = true;

    evrPush(EVR_StartingMain, 0);

    while (1)
    {
    	evrCheck();

    	///////////////////////////////

        if (frame_50Hz)
        {
        	frame_50Hz = false;

        	currentTime      = micros();
			deltaTime50Hz    = currentTime - previous50HzTime;
			previous50HzTime = currentTime;

			processFlightCommands();

            if (eepromConfig.useMs5611 == true)
			{
				if (newTemperatureReading && newPressureReading)
                {
                    d1Value = d1.value;
                    d2Value = d2.value;

                    calculateMs5611Temperature();
                    calculateMs5611PressureAltitude();

                    newTemperatureReading = false;
                    newPressureReading    = false;
                }
           }
           else
           {
			   if (newTemperatureReading && newPressureReading)
			   {
				   uncompensatedTemperatureValue = uncompensatedTemperature.value;
				   uncompensatedPressureValue    = uncompensatedPressure.value;

				   calculateBmp085Temperature();
				   calculateBmp085PressureAltitude();

				   newTemperatureReading = false;
				   newPressureReading    = false;
			   }
           }

            sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);

            executionTime50Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_10Hz)
        {
        	frame_10Hz = false;

        	currentTime      = micros();
			deltaTime10Hz    = currentTime - previous10HzTime;
			previous10HzTime = currentTime;

			sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]);
			sensors.mag10Hz[YAXIS] =   (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
			sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);

			newMagData = false;
			magDataUpdate = true;

        	batMonTick();

            cliCom();

            if (eepromConfig.mavlinkEnabled == true)
            {
				mavlinkSendAttitude();
				mavlinkSendVfrHud();
			}

        	executionTime10Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_500Hz)
        {
        	frame_500Hz = false;

       	    currentTime       = micros();
       	    deltaTime500Hz    = currentTime - previous500HzTime;
       	    previous500HzTime = currentTime;

       	    dt500Hz = (float)deltaTime500Hz * 0.000001f;  // For integrations in 500 Hz loop

            if (eepromConfig.useMpu6050 == true)
            {
			    computeMpu6050TCBias();

                sensors.accel500Hz[XAXIS] =  ((float)accelData500Hz[XAXIS] - accelTCBias[XAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;
                sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - accelTCBias[YAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;
                sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - accelTCBias[ZAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;

                sensors.gyro500Hz[ROLL ] =  ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU6050_GYRO_SCALE_FACTOR;
                sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU6050_GYRO_SCALE_FACTOR;
                sensors.gyro500Hz[YAW  ] = -((float)gyroData500Hz[YAW  ] - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * MPU6050_GYRO_SCALE_FACTOR;
			}
			else
            {
				sensors.accel500Hz[XAXIS] = -((float)accelData500Hz[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
			    sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
			    sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];

                // HJI sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]);
                // HJI sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]);
                // HJI sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]);

                computeMpu3050TCBias();

                sensors.gyro500Hz[ROLL ] =  ((float)gyroData500Hz[ROLL ]  - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU3050_GYRO_SCALE_FACTOR;
			    sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH]  - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU3050_GYRO_SCALE_FACTOR;
                sensors.gyro500Hz[YAW  ] = -((float)gyroData500Hz[YAW  ]  - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * MPU3050_GYRO_SCALE_FACTOR;
		    }

            MargAHRSupdate( sensors.gyro500Hz[ROLL],   sensors.gyro500Hz[PITCH],  sensors.gyro500Hz[YAW],
                            sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
                            sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                            magDataUpdate,
                            dt500Hz );

            magDataUpdate = false;

            computeAxisCommands(dt500Hz);
            mixTable();
            writeMotors();

            if (eepromConfig.receiverType == SPEKTRUM)
            	writeServos();

            executionTime500Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_100Hz)
        {
        	frame_100Hz = false;

        	currentTime       = micros();
			deltaTime100Hz    = currentTime - previous100HzTime;
			previous100HzTime = currentTime;

			dt100Hz = (float)deltaTime100Hz * 0.000001f;  // For integrations in 100 Hz loop

            sensors.accel100Hz[XAXIS] = sensors.accel500Hz[XAXIS];  // No sensor averaging so use the 500 Hz value
			sensors.accel100Hz[YAXIS] = sensors.accel500Hz[YAXIS];  // No sensor averaging so use the 500 Hz value
			sensors.accel100Hz[ZAXIS] = sensors.accel500Hz[ZAXIS];  // No sensor averaging so use the 500 Hz value

        	// HJI sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]);
			// HJI sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]);
			// HJI sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]);

			createRotationMatrix();
            bodyAccelToEarthAccel();
            vertCompFilter(dt100Hz);

            if (armed == true)
            {
				if ( eepromConfig.activeTelemetry == 1 )
                {
            	    // 500 Hz Accels
            	    telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
            	            	                             sensors.accel500Hz[YAXIS],
            	            			                     sensors.accel500Hz[ZAXIS]);
                }

                if ( eepromConfig.activeTelemetry == 2 )
                {
            	    // 500 Hz Gyros
            	    telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
            	            			                     sensors.gyro500Hz[PITCH],
            	            					             sensors.gyro500Hz[YAW  ]);
                }

                if ( eepromConfig.activeTelemetry == 4 )
                {
            	    // 500 Hz Attitudes
            	    telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
            	            			                     sensors.attitude500Hz[PITCH],
            	            			                     sensors.attitude500Hz[YAW  ]);
                }

                if ( eepromConfig.activeTelemetry == 8 )
                {
               	    // Vertical Variables
            	    telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
            	    		                                        sensors.pressureAlt50Hz,
            	    		                                        hDotEstimate,
            	    		                                        hEstimate);
                }

                if ( eepromConfig.activeTelemetry == 16)
                {
               	    // Vertical Variables
            	    telemPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f\n", verticalVelocityCmd,
            	    		                                                   hDotEstimate,
            	    		                                                   hEstimate,
            	    		                                                   ms5611Temperature,
            	    		                                                   verticalModeState,
            	    		                                                   throttleCmd);
                }

            }

            executionTime100Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_5Hz)
        {
        	frame_5Hz = false;

        	currentTime     = micros();
			deltaTime5Hz    = currentTime - previous5HzTime;
			previous5HzTime = currentTime;

			if (batMonVeryLowWarning > 0)
			{
				BEEP_TOGGLE;
				batMonVeryLowWarning--;
			}

        	executionTime5Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_1Hz)
        {
        	frame_1Hz = false;

        	currentTime     = micros();
			deltaTime1Hz    = currentTime - previous1HzTime;
			previous1HzTime = currentTime;

			if (execUp == false)
			    execUpCount++;

			if ((execUpCount == 5) && (execUp == false))
			{
				execUp = true;

				LED0_OFF;
				LED1_OFF;

				pwmEscInit();

				homeData.magHeading = sensors.attitude500Hz[YAW];
			}

			if (batMonLowWarning > 0)
			{
				BEEP_TOGGLE;
				batMonLowWarning--;
			}

            if (eepromConfig.mavlinkEnabled == true)
            {
				mavlinkSendHeartbeat();
				mavlinkSendSysStatus();
			}

			executionTime1Hz = micros() - currentTime;
        }

        ////////////////////////////////
    }

    ///////////////////////////////////////////////////////////////////////////
}
Esempio n. 27
0
bool IMU::processAngles(float angles[], float rates[])
{			
	accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);  //400µs
		
	//Filter
	accXf = filterX.update(accX);
	accYf = filterY.update(accY);
	accZf = filterZ.update(accZ);
	
	// Angular rates provided by gyro
	gyroXrate = (float) (gyroX-gyroXoffset)/131.0; //140 µsec on Arduino MEGA
	gyroYrate = -((float) (gyroY-gyroYoffset)/131.0);
	gyroZrate = ((float) (gyroZ-gyroZoffset)/131.0);

	//Angle provided by accelerometer
	//Time taken: 400 µsec on Arduino MEGA
	accYangle = (atan2f(accXf,accZf)+PI)*RAD_TO_DEG; // 
	accXangle = (atan2f(accYf,accZf)+PI)*RAD_TO_DEG; 
	
	//Integration of gyro rates to get the angles
	gyroXangle += gyroXrate*(float)(micros()-timer)/1000000;
	gyroYangle += gyroYrate*(float)(micros()-timer)/1000000;
	gyroZangle += gyroZrate*(float)(micros()-timer)/1000000;
	
	//Angle calculation through Complementary filter
	//Time taken: 200 µsec on Arduino MEGA
	dt = (float)(micros()-timer)/1000000.0;
	compAngleX = alpha_gyro*(compAngleX+(gyroXrate*dt))   +   c*accXangle; 
	compAngleY = alpha_gyro*(compAngleY+(gyroYrate*dt))  +   c*accYangle;
	//compAngleX0 = 0.997*(compAngleX0+(gyroXrate*(float)(micros()-timer)/1000000))   +   (1-0.997)*accXangle; 

	timer = micros(); 
	
	//45 deg rotation for roll and pitch (depending how your IMU is fixed to your quad)
	angles[0]=  -rac22* compAngleX + rac22*compAngleY + ROLL_OFFSET;
	angles[1]=  -rac22* compAngleX - rac22*compAngleY +2*rac22*PI*RAD_TO_DEG + PITCH_OFFSET;
	angles[2]=  gyroZangle;

	rates[0]=   -  rac22* gyroXrate + rac22*gyroYrate;
	rates[1]= - rac22* gyroXrate - rac22*gyroYrate;
	rates[2]=  gyroZrate;
		
	//////* Print Data  for vibration measurements*/ //Todo: Extract function
	//switch (j)
	  //{

	////	Frequency print
	  //case 1: 
		   //dtostrf(compAngleX - 180  ,6,2,StrAnglesvib);
		    //Serial.print(StrAnglesvib); 
		   //break;
	  //case 2:
		   //Serial.print("  ");
		   //break;
	  //case 3:
	  		//dtostrf(gyroXangle -180,6,2,StrAnglesvib);	
			//Serial.println(StrAnglesvib);
		   //j=0;
		   //break;
	  //}	   
	//j++;

	//Return false if the angle are not in the acceptable range
	if (abs(angles[0]) < ROLL_MAX_IMU && abs(angles[1]) < PITCH_MAX_IMU)
	{
		return true;
	}
	else
	{
		return false;
	}
}
Esempio n. 28
0
void nikon::_pulseOff(unsigned long startDelay) {
    unsigned long endDelay = micros() + startDelay;
    while(micros() < endDelay);
}
Esempio n. 29
0
void mavlinkSendPosition(void)
{
    uint16_t msgLength;
    uint8_t gpsFixType = 0;
    
    if (!sensors(SENSOR_GPS))
        return;

    if (!STATE(GPS_FIX)) {
        gpsFixType = 1;
    }
    else {
        if (GPS_numSat < 5) {
            gpsFixType = 2;
        }
        else {
            gpsFixType = 3;
        }
    }
        
    mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
        // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
        micros(),
        // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
        gpsFixType,
        // lat Latitude in 1E7 degrees
        GPS_coord[LAT],
        // lon Longitude in 1E7 degrees
        GPS_coord[LON],
        // alt Altitude in 1E3 meters (millimeters) above MSL
        GPS_altitude * 1000,
        // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
        65535,
        // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
        65535,
        // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
        GPS_speed,
        // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
        GPS_ground_course * 10,
        // satellites_visible Number of satellites visible. If unknown, set to 255
        GPS_numSat);
    msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
    mavlinkSerialWrite(mavBuffer, msgLength);

    // Global position
    mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
        // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
        micros(),
        // lat Latitude in 1E7 degrees
        GPS_coord[LAT],
        // lon Longitude in 1E7 degrees
        GPS_coord[LON],
        // alt Altitude in 1E3 meters (millimeters) above MSL
        GPS_altitude * 1000,
        // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR)
        (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000,
#else
        GPS_altitude * 1000,
#endif
        // Ground X Speed (Latitude), expressed as m/s * 100
        0,
        // Ground Y Speed (Longitude), expressed as m/s * 100
        0,
        // Ground Z Speed (Altitude), expressed as m/s * 100
        0,
        // heading Current heading in degrees, in compass units (0..360, 0=north)
        DECIDEGREES_TO_DEGREES(attitude.values.yaw)
    );
    msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
    mavlinkSerialWrite(mavBuffer, msgLength);

    mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
        // latitude Latitude (WGS84), expressed as * 1E7
        GPS_home[LAT],
        // longitude Longitude (WGS84), expressed as * 1E7
        GPS_home[LON],
        // altitude Altitude(WGS84), expressed as * 1000
        0); // FIXME
    msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
    mavlinkSerialWrite(mavBuffer, msgLength);
}
Esempio n. 30
0
void AP_ADC_ADS7844::read()
{
	AP_ADC_ADS7844::read((uint32_t)micros());
}