int writeDatalink(float frequency){ if (time - lastTime > frequency) { lastTime = time; struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock(); if (statusData == 0){ return 0; } statusData->lat = getLatitude(); statusData->lon = getLongitude(); statusData->millis = getUTCTime(); statusData->groundSpeed = getSpeed(); statusData->heading = getHeading(); statusData->lastCommandSent = lastCommandSentCode; statusData->errorCodes = getErrorCodes(); statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4)); statusData->steeringSetpoint = getPWM(1); statusData->throttleSetpoint = getPWM(2); statusData->steeringOutput = sData; statusData->throttleOutput = tData; statusData->debugChar = debugChar; statusData->debugInt = debugInt; statusData->debugFloat = debugFloat; if (BLOCKING_MODE) { sendTelemetryBlock(statusData); destroyTelemetryBlock(statusData); } else { return pushOutboundTelemetryQueue(statusData); } } else if (time < lastTime){ lastTime = time; return 0; } return 0; }
/** * Tracking Module (Thread) */ THD_FUNCTION(moduleTRACKING, arg) { // Print infos module_params_t* parm = (module_params_t*)arg; TRACE_INFO("TRAC > Startup module TRACKING MANAGER"); TRACE_INFO("TRAC > Module TRACKING MANAGER info\r\n" "%s Cycle: %d sec", TRACE_TAB, parm->cycle ); uint32_t id = 1; lastTrackPoint = &trackPoints[0]; systime_t time = chVTGetSystemTimeX(); while(true) { parm->lastCycle = chVTGetSystemTimeX(); // Watchdog timer TRACE_INFO("TRAC > Do module TRACKING MANAGER cycle"); trackPoint_t* tp = &trackPoints[id % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Current track point trackPoint_t* ltp = &trackPoints[(id-1) % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Last track point // Search for GPS satellites gpsFix_t gpsFix; GPS_Init(); do { gps_get_fix(&gpsFix); } while(!isGPSLocked(&gpsFix) && chVTGetSystemTimeX() <= time + S2ST(parm->cycle-3)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-1sec in order to keep synchronization) if(isGPSLocked(&gpsFix)) { // GPS locked // Switch off GPS GPS_Deinit(); // Debug TRACE_INFO("TRAC > GPS sampling finished GPS LOCK"); TRACE_GPSFIX(&gpsFix); // Calibrate RTC setTime(gpsFix.time); // Take time from GPS tp->time.year = gpsFix.time.year; tp->time.month = gpsFix.time.month; tp->time.day = gpsFix.time.day; tp->time.hour = gpsFix.time.hour; tp->time.minute = gpsFix.time.minute; tp->time.second = gpsFix.time.second; // Set new GPS fix tp->gps_lat = gpsFix.lat; tp->gps_lon = gpsFix.lon; tp->gps_alt = gpsFix.alt; tp->gps_lock = isGPSLocked(&gpsFix); tp->gps_sats = gpsFix.num_svs; } else { // GPS lost (keep GPS switched on) // Debug TRACE_WARN("TRAC > GPS sampling finished GPS LOSS"); // Take time from internal RTC ptime_t time; getTime(&time); tp->time.year = time.year; tp->time.month = time.month; tp->time.day = time.day; tp->time.hour = time.hour; tp->time.minute = time.minute; tp->time.second = time.second; // Take GPS fix from old lock tp->gps_lat = ltp->gps_lat; tp->gps_lon = ltp->gps_lon; tp->gps_alt = ltp->gps_alt; // Mark gpsloss tp->gps_lock = false; tp->gps_sats = 0; } tp->id = id; // Serial ID tp->gps_ttff = ST2S(chVTGetSystemTimeX() - time); // Time to first fix // Power management tp->adc_solar = getSolarVoltageMV(); tp->adc_battery = getBatteryVoltageMV(); tp->adc_charge = pac1720_getAverageChargePower(); tp->adc_discharge = pac1720_getAverageDischargePower(); bme280_t bmeInt; bme280_t bmeExt; // Atmosphere condition if(BME280_isAvailable(BME280_ADDRESS_INT)) { BME280_Init(&bmeInt, BME280_ADDRESS_INT); tp->air_press = BME280_getPressure(&bmeInt, 256); tp->air_hum = BME280_getHumidity(&bmeInt); tp->air_temp = BME280_getTemperature(&bmeInt); } else { // No internal BME280 found TRACE_ERROR("TRAC > Internal BME280 not available"); tp->air_press = 0; tp->air_hum = 0; tp->air_temp = 0; } // Balloon condition if(BME280_isAvailable(BME280_ADDRESS_EXT)) { BME280_Init(&bmeExt, BME280_ADDRESS_EXT); tp->bal_press = BME280_getPressure(&bmeExt, 256); tp->bal_hum = BME280_getHumidity(&bmeExt); tp->bal_temp = BME280_getTemperature(&bmeExt); } else { // No external BME280 found TRACE_WARN("TRAC > External BME280 not available"); tp->bal_press = 0; tp->bal_hum = 0; tp->bal_temp = 0; } // Trace data TRACE_INFO( "TRAC > New tracking point available (ID=%d)\r\n" "%s Time %04d-%02d-%02d %02d:%02d:%02d\r\n" "%s Pos %d.%07d %d.%07d Alt %dm\r\n" "%s Sats %d TTFF %dsec\r\n" "%s ADC Vbat=%d.%03dV Vsol=%d.%03dV Pin=%dmW Pout=%dmW\r\n" "%s Air p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%\r\n" "%s Ball p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%", tp->id, TRACE_TAB, tp->time.year, tp->time.month, tp->time.day, tp->time.hour, tp->time.minute, tp->time.day, TRACE_TAB, tp->gps_lat/10000000, tp->gps_lat%10000000, tp->gps_lon/10000000, tp->gps_lon%10000000, tp->gps_alt, TRACE_TAB, tp->gps_sats, tp->gps_ttff, TRACE_TAB, tp->adc_battery/1000, (tp->adc_battery%1000), tp->adc_solar/1000, (tp->adc_solar%1000), tp->adc_charge, tp->adc_discharge, TRACE_TAB, tp->air_press/10, tp->air_press%10, tp->air_temp/100, tp->air_temp%100, tp->air_hum/10, tp->air_hum%10, TRACE_TAB, tp->bal_press/10, tp->bal_press%10, tp->bal_temp/100, tp->bal_temp%100, tp->bal_hum/10, tp->bal_hum%10 ); // Switch last recent track point lastTrackPoint = tp; id++; time = chThdSleepUntilWindowed(time, time + S2ST(parm->cycle)); // Wait until time + cycletime } }