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);
}
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();
}
}
//
// 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;
}
}
void resumeRxSignal(void)
{
suspendRxSignalUntil = micros();
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxResume();
}
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
}
}
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;
}
void delayMicroseconds(uint32_t us)
{
uint32_t now = micros();
while (micros() - now < us);
}
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__;
}
}
// 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;
}
void Task::markJustCalled() {
this->lastCallTimeMicros = micros();
}
void hexbright::set_strobe_delay(unsigned long delay) {
strobe_delay = delay;
next_strobe = micros()+strobe_delay;
}
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();
}
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
}
}
void suspendRxSignal(void)
{
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
}
static inline void await(unsigned us) {
unsigned long long await_timer = micros() + us;
while(micros() < await_timer) ;
}
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
}
/**
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;
}
}
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;
}
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
cliSerial->printf("Compass: "******"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);
}
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;
}
}
}
}
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();
}
}
void init()
{
prev_ = micros();
}
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;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
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;
}
}
void nikon::_pulseOff(unsigned long startDelay) {
unsigned long endDelay = micros() + startDelay;
while(micros() < endDelay);
}
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);
}
void AP_ADC_ADS7844::read()
{
AP_ADC_ADS7844::read((uint32_t)micros());
}