mirror
/
pecanpico10
kopia lustrzana https://github.com/DL7AD/pecanpico10
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
pecanpico10/tracker/software/source/threads/collector.c

624 wiersze
20 KiB
C
Czysty Wina Historia

/**
* @file collector.c
* @brief Telemetry data collector.
*
* @addtogroup telemetry
* @{
*/
#include "ch.h"
#include "hal.h"
#include "collector.h"
#include "debug.h"
#include "config.h"
#include "ublox.h"
#include "bme280.h"
#include "padc.h"
#include "pac1720.h"
#include "ov5640.h"
#include "radio.h"
#include "watchdog.h"
#include "pi2c.h"
#include "si446x.h"
#include "pflash.h"
#include "pkttypes.h"
/*===========================================================================*/
/* Module local variables. */
/*===========================================================================*/
static dataPoint_t dataPoints[2];
static dataPoint_t* lastDataPoint;
static bool threadStarted = false;
static uint8_t useGPS = 0;
static uint8_t useCFG = 0;
static uint8_t bme280_error;
/**
* Returns most recent data point which is complete.
*/
dataPoint_t* getLastDataPoint(void) {
return lastDataPoint;
}
/**
*
*/
void waitForNewDataPoint(void) {
uint32_t old_id = getLastDataPoint()->id;
while(old_id == getLastDataPoint()->id)
chThdSleep(TIME_S2I(1));
}
/**
* @brief Determine best fallback data when GPS not operable.
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to current @p datapoint structure
* @param[in] ltp pointer to prior @p datapoint structure
* @param[in] state state to set in current datapoint
*
* @notapi
*/
static void getPositionFallback(dataPoint_t* tp,
dataPoint_t* ltp,
gpsState_t state) {
tp->gps_state = state;
tp->gps_time = 0;
tp->gps_lat = 0;
tp->gps_lon = 0;
tp->gps_alt = 0;
if(hasGPSacquiredLock(ltp)
|| ltp->gps_state == GPS_FIXED
|| ltp->gps_state == GPS_LOG) {
tp->gps_lat = ltp->gps_lat;
tp->gps_lon = ltp->gps_lon;
tp->gps_alt = ltp->gps_alt;
tp->gps_time = ltp->gps_time;
}
ptime_t time;
getTime(&time);
if(time.year != RTC_BASE_YEAR)
/* The RTC has been set so use RTC time. */
tp->gps_time = date2UnixTimestamp(&time);
}
/**
* @brief Acquire GPS position and time data.
* @notes The GPS is switched on only if a service requires it.
* @notes The GPS model used can be controlled by barometric pressure.
* @notes The model for high/low pressure is set in config.c.
* @notes Model switching enables more reliable lock at low altitude.
* @notes The switch to airborne model is then made at high altitude.
*
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to current @p datapoint structure
* @param[in] ltp pointer to prior @p datapoint structure
* @param[in] timeout time limit to wait for acquisition
*
* @notapi
*/
static void aquirePosition(dataPoint_t* tp, dataPoint_t* ltp,
sysinterval_t timeout) {
sysinterval_t start = chVTGetSystemTime();
gpsFix_t gpsFix = {0};
/*
* Switch on GPS if...
* position/time is requested by a service and power is available.
*/
uint16_t batt = stm32_get_vbat();
if(batt < conf_sram.gps_on_vbat) {
getPositionFallback(tp, ltp, GPS_LOWBATT1);
return;
}
/* Try to switch on GPS. */
if(!GPS_Init()) {
GPS_Deinit();
getPositionFallback(tp, ltp, GPS_ERROR);
return;
}
/* If a Pa pressure is set then GPS model depends on BME reading.
* If BME is OK then stationary model will be used until Pa < airborne.
* Then airborne model will be set.
* If the BME is not OK then airborne model will be used immediately.
*/
bool dynamic = conf_sram.gps_pressure != 0;
TRACE_INFO("COLL > GPS %s in dynamic mode at %dPa", dynamic
? "is" : "is not", conf_sram.gps_pressure);
/*
* Search for GPS lock within the timeout period and while battery is good.
* Search timeout=cycle-1sec (-3sec in order to keep synchronization)
*/
do {
batt = stm32_get_vbat();
gps_set_model(dynamic);
gps_get_fix(&gpsFix);
} while(!isGPSLocked(&gpsFix)
&& batt >= conf_sram.gps_off_vbat
&& chVTIsSystemTimeWithin(start, start + timeout));
if(batt < conf_sram.gps_off_vbat) {
/*
* GPS was switched on but battery fell below threshold.
* Switch off GPS.
*/
GPS_Deinit();
TRACE_WARN("COLL > GPS acquisition stopped due low battery");
getPositionFallback(tp, ltp, GPS_LOWBATT2);
return;
}
if(!isGPSLocked(&gpsFix)) {
/*
* GPS was switched on but it failed to get a lock in timeout period.
* Keep GPS switched on.
*/
TRACE_WARN("COLL > GPS sampling finished GPS LOSS");
getPositionFallback(tp, ltp, GPS_LOSS);
return;
}
/*
* GPS locked successfully.
* Output SV info.
* Switch off GPS (unless cycle is less than 60 seconds).
*/
gps_svinfo_t svinfo;
if(gps_get_sv_info(&svinfo, sizeof(svinfo))) {
TRACE_INFO("GPS > Space Vehicle info iTOW=%d numCh=%02d globalFlags=%d",
svinfo.iTOW, svinfo.numCh, svinfo.globalFlags);
uint8_t i;
for(i = 0; i < svinfo.numCh; i++) {
gps_svchn_t *sat = &svinfo.svinfo[i];
TRACE_INFO("GPS > Satellite info chn=%03d svid=%03d flags=0x%02x"
" quality=%02d cno=%03d elev=%03d azim=%06d, prRes=%06d",
sat->chn, sat->svid, sat->flags, sat->flags,
sat->quality, sat->cno, sat->elev, sat->azim, sat->prRes);
}
} else {
TRACE_ERROR("GPS > Error getting Space Vehicle info");
}
/* Switch off GPS (if cycle time is more than 60 seconds). */
if(timeout < TIME_S2I(60)) {
TRACE_INFO("COLL > Keep GPS switched on because cycle < 60sec");
tp->gps_state = GPS_LOCKED2;
} else if(conf_sram.gps_onper_vbat != 0 && batt >= conf_sram.gps_onper_vbat) {
TRACE_INFO("COLL > Keep GPS switched on because VBAT >= %dmV", conf_sram.gps_onper_vbat);
tp->gps_state = GPS_LOCKED2;
} else {
TRACE_INFO("COLL > Switching off GPS");
GPS_Deinit();
tp->gps_state = GPS_LOCKED1;
}
// Debug
TRACE_INFO("COLL > GPS sampling finished GPS LOCK");
// Calibrate RTC
setTime(&gpsFix.time);
// Take time from GPS
tp->gps_time = date2UnixTimestamp(&gpsFix.time);
// Set new GPS fix
tp->gps_lat = gpsFix.lat;
tp->gps_lon = gpsFix.lon;
tp->gps_alt = gpsFix.alt;
tp->gps_sats = gpsFix.num_svs;
tp->gps_pdop = (gpsFix.pdop+3)/5;
tp->gps_ttff = TIME_I2S(chVTGetSystemTime() - start); // Time to first fix
}
/**
* @brief Get voltage status and save in datapoint.
* @notes The battery and solar voltages are read and stored.
*
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to a @p datapoint structure
*
* @notapi
*/
static void measureVoltage(dataPoint_t* tp)
{
tp->adc_vbat = stm32_get_vbat();
tp->adc_vsol = stm32_get_vsol();
pac1720_get_avg(&tp->pac_vbat, &tp->pac_vsol, &tp->pac_pbat, &tp->pac_psol);
}
/**
* @brief Get sensor status and save in datapoint.
* @notes The active/installed sensor(s) are read and stored.
*
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to a @p datapoint structure
*
* @api
*/
void getSensors(dataPoint_t* tp) {
// Measure BME280
bme280_error = 0;
bme280_t handle;
// Internal BME280
if(BME280_isAvailable(BME280_I1)) {
BME280_Init(&handle, BME280_I1);
tp->sen_i1_press = BME280_getPressure(&handle, 32);
tp->sen_i1_hum = BME280_getHumidity(&handle);
tp->sen_i1_temp = BME280_getTemperature(&handle);
} else { // No internal BME280 found
TRACE_ERROR("COLL > Internal BME280 I1 not found");
tp->sen_i1_press = 0;
tp->sen_i1_hum = 0;
tp->sen_i1_temp = 0;
bme280_error |= 0x1;
}
#if ENABLE_EXTERNAL_I2C == TRUE
// External BME280 Sensor 1
if(BME280_isAvailable(BME280_E1)) {
BME280_Init(&handle, BME280_E1);
tp->sen_e1_press = BME280_getPressure(&handle, 32);
tp->sen_e1_hum = BME280_getHumidity(&handle);
tp->sen_e1_temp = BME280_getTemperature(&handle);
} else { // No external BME280 found
TRACE_WARN("COLL > External BME280 E1 not found");
tp->sen_e1_press = 0;
tp->sen_e1_hum = 0;
tp->sen_e1_temp = 0;
bme280_error |= 0x4;
}
// External BME280 Sensor 2
if(BME280_isAvailable(BME280_E2)) {
BME280_Init(&handle, BME280_E2);
tp->sen_e2_press = BME280_getPressure(&handle, 32);
tp->sen_e2_hum = BME280_getHumidity(&handle);
tp->sen_e2_temp = BME280_getTemperature(&handle);
} else { // No external BME280 found
TRACE_WARN("COLL > External BME280 E2 not found");
tp->sen_e2_press = 0;
tp->sen_e2_hum = 0;
tp->sen_e2_temp = 0;
bme280_error |= 0x10;
}
#else
/* Set status to "not fitted" for E1 & E2. */
bme280_error |= 0x28;
#endif
// Measure various temperature sensors
/* TODO: Add LLD API to radio. */
tp->stm32_temp = stm32_get_temp();
tp->si446x_temp = Si446x_getLastTemperature(PKT_RADIO_1);
// Measure light intensity from OV5640
tp->light_intensity = OV5640_getLastLightIntensity() & 0xFFFF;
}
/**
* @brief Get GPIO port status and save in datapoint.
* @notes The input state or current output state is read.
* @notes The GPIO mode is determined by the port user.
*
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to a @p datapoint structure
* @param[out] tp gpio field is updated with current GPIO states.
*
* @notapi
*/
static void getGPIO(dataPoint_t* tp) {
tp->gpio = pktReadIOlines();
}
/**
* @brief Update/set system status data.
* @notes The system hardware health/status is read.
* @notes This covers the main hardware units...
* @notes I2C, GPS, Power, Camera & Environment.
*
* @post The provided data point (record) is updated.
*
* @param[in] tp pointer to a @p datapoint structure
*
* @api
*/
void setSystemStatus(dataPoint_t* tp) {
/*
* Set system errors.
*
* Bit usage:
* - 0:1 I2C status
* - 2:2 GPS status
* - 3:4 pac1720 status
* - 5:7 OV5640 status
* - 8:9 BMEi1 status (0 = OK, 1 = Fail, 2 = Not fitted)
* - 9:10 BMEe1 status (0 = OK, 1 = Fail, 2 = Not fitted)
* - 10:11 BMEe2 status (0 = OK, 1 = Fail, 2 = Not fitted)
*/
tp->sys_error = 0;
tp->sys_error |= (I2C_hasError() & 0x1) << 0;
tp->sys_error |= (tp->gps_state == GPS_ERROR) << 2;
tp->sys_error |= (pac1720_hasError() & 0x3) << 3;
tp->sys_error |= (OV5640_hasError() & 0x7) << 5;
tp->sys_error |= (bme280_error & BME_ALL_STATUS_MASK)
<< BME_ALL_STATUS_SHIFT;
// Set system time
tp->sys_time = TIME_I2S(chVTGetSystemTime());
}
/*===========================================================================*/
/* Data collector thread. */
/*===========================================================================*/
THD_FUNCTION(collectorThread, arg) {
uint8_t *useGPS = arg;
uint32_t id = 0;
// Read time from RTC
ptime_t time;
getTime(&time);
dataPoints[0].gps_time = date2UnixTimestamp(&time);
dataPoints[1].gps_time = date2UnixTimestamp(&time);
lastDataPoint = &dataPoints[0];
// Get last data point from memory
TRACE_INFO("COLL > Read last data point from flash memory");
dataPoint_t* lastLogPoint = flash_getNewestLogEntry();
if(lastLogPoint != NULL) { // If there is stored data point, then get it.
dataPoints[0].reset = lastLogPoint->reset+1;
dataPoints[1].reset = lastLogPoint->reset+1;
unixTimestamp2Date(&time, lastDataPoint->gps_time);
lastDataPoint->gps_lat = lastLogPoint->gps_lat;
lastDataPoint->gps_lon = lastLogPoint->gps_lon;
lastDataPoint->gps_alt = lastLogPoint->gps_alt;
lastDataPoint->gps_sats = lastLogPoint->gps_sats;
lastDataPoint->gps_ttff = lastLogPoint->gps_ttff;
TRACE_INFO(
"COLL > Last data point (from memory)\r\n"
"%s Reset %d ID %d\r\n"
"%s Time %04d-%02d-%02d %02d:%02d:%02d\r\n"
"%s Latitude: %d.%07ddeg\r\n"
"%s Longitude: %d.%07ddeg\r\n"
"%s Altitude: %d Meter",
TRACE_TAB, lastLogPoint->reset, lastLogPoint->id,
TRACE_TAB, time.year, time.month, time.day, time.hour,
time.minute, time.day,
TRACE_TAB, lastDataPoint->gps_lat/10000000,
(lastDataPoint->gps_lat > 0
? 1:-1)*lastDataPoint->gps_lat%10000000,
TRACE_TAB, lastDataPoint->gps_lon/10000000,
(lastDataPoint->gps_lon > 0
? 1:-1)*lastDataPoint->gps_lon%10000000,
TRACE_TAB, lastDataPoint->gps_alt
);
lastDataPoint->gps_state = GPS_LOG; // Mark dataPoint as LOG packet
} else {
TRACE_INFO("COLL > No data point found in flash memory");
/* State indicates that no valid stored position is available. */
lastDataPoint->gps_state = GPS_OFF;
/* Continue get telemetry and acquire position and time if required. */
}
// Measure telemetry
measureVoltage(lastDataPoint);
getSensors(lastDataPoint);
getGPIO(lastDataPoint);
setSystemStatus(lastDataPoint);
// Write data point to Flash memory
flash_writeLogDataPoint(lastDataPoint);
// Wait for position threads to start
chThdSleep(TIME_MS2I(500));
sysinterval_t cycle_time = chVTGetSystemTime();
/* TODO: The collector will be revised when the overall scheduler is implemented.
* Then app execution will be managed by the scheduler alone.
* i.e. There will be no schedule control within an app itself.
*/
// Determine cycle time and if GPS should be used.
sysinterval_t data_cycle_time = TIME_S2I(600); // Default.
if(conf_sram.pos_pri.thread_conf.active && conf_sram.pos_sec.thread_conf.active) { // Both position threads are active
data_cycle_time = conf_sram.pos_pri.thread_conf.cycle < conf_sram.pos_sec.thread_conf.cycle ? conf_sram.pos_pri.thread_conf.cycle : conf_sram.pos_sec.thread_conf.cycle; // Choose the smallest cycle
(*useGPS)++;
} else if(conf_sram.pos_pri.thread_conf.active) { // Only primary position thread is active
data_cycle_time = conf_sram.pos_pri.thread_conf.cycle;
(*useGPS)++;
} else if(conf_sram.pos_sec.thread_conf.active) { // Only secondary position thread is active
data_cycle_time = conf_sram.pos_sec.thread_conf.cycle;
(*useGPS)++;
} else if(conf_sram.aprs.thread_conf.active && conf_sram.aprs.digi.beacon) { // DIGI beacon is active
data_cycle_time = conf_sram.aprs.digi.cycle;
if(conf_sram.aprs.digi.gps) {
(*useGPS)++;
}
} else { // There must be an error
TRACE_ERROR("COLL > Data collector started but no position thread is active");
}
while(true) { /* Primary loop. */
/* TODO: Separate collector from GPS (put GPS in its own thread).
* That will allow collection to run per its schedule (won't be stalled by GPS).
*/
TRACE_INFO("COLL > Do module DATA COLLECTOR cycle");
dataPoint_t* tp = &dataPoints[(id+1) % 2]; // Current data point (the one which is processed now)
dataPoint_t* ltp = &dataPoints[ id % 2]; // Last data point
// Gather telemetry and system status data
measureVoltage(tp);
getSensors(tp);
getGPIO(tp);
setSystemStatus(tp);
/*
* Enable GPS position acquisition if...
* a) The RTC was not set then enable GPS temporarily to set it.
* b) If any app is using GPS for position.
*/
getTime(&time);
if(*useGPS == 0 && time.year == RTC_BASE_YEAR) {
/*
* There are no apps requiring GPS position but the uC RTC is not set.
* Enable the GPS and get a lock which results in setting the RTC.
*/
TRACE_INFO("COLL > Acquire time using GPS");
aquirePosition(tp, ltp, data_cycle_time - TIME_S2I(3));
/* RTC is set in aquirePosition(...). */
if(!hasGPSacquiredLock(tp)) {
/* Acquisition failed. Wait and then try again. */
TRACE_INFO("COLL > Time acquisition from GPS failed");
chThdSleep(TIME_S2I(60));
continue;
}
TRACE_INFO("COLL > Time acquired from GPS");
/* Switch GPS off. */
GPS_Deinit();
}
/* Check if any app requires position. */
if(*useGPS > 0) {
TRACE_INFO("COLL > Acquire position using GPS");
aquirePosition(tp, ltp, data_cycle_time - TIME_S2I(3));
/* RTC is set in aquirePosition(...). */
} else {
/*
* No threads using GPS.
* Update datapoint time from RTC.
* Set fixed location.
*/
TRACE_INFO("COLL > Using fixed location");
getTime(&time);
unixTimestamp2Date(&time, tp->gps_time);
tp->gps_alt = conf_sram.alt;
tp->gps_lat = conf_sram.lat;
tp->gps_lon = conf_sram.lon;
tp->gps_state = GPS_FIXED;
}
tp->id = ++id; // Serial ID
// Trace data
unixTimestamp2Date(&time, tp->gps_time);
TRACE_INFO( "COLL > New data point available (ID=%d)\r\n"
"%s Time %04d-%02d-%02d %02d:%02d:%02d\r\n"
"%s Pos %d.%05d %d.%05d Alt %dm\r\n"
"%s Sats %d TTFF %dsec\r\n"
"%s ADC Vbat=%d.%03dV Vsol=%d.%03dV Pbat=%dmW\r\n"
"%s AIR p=%d.%01dPa T=%d.%02ddegC phi=%d.%01d%%\r\n"
"%s IOP IO1=%d IO2=%d IO3=%d IO4=%d",
tp->id,
TRACE_TAB, time.year, time.month, time.day, time.hour, time.minute, time.day,
TRACE_TAB, tp->gps_lat/10000000, (tp->gps_lat > 0 ? 1:-1)*(tp->gps_lat/100)%100000, tp->gps_lon/10000000, (tp->gps_lon > 0 ? 1:-1)*(tp->gps_lon/100)%100000, tp->gps_alt,
TRACE_TAB, tp->gps_sats, tp->gps_ttff,
TRACE_TAB, tp->adc_vbat/1000, (tp->adc_vbat%1000), tp->adc_vsol/1000, (tp->adc_vsol%1000), tp->pac_pbat,
TRACE_TAB, tp->sen_i1_press/10, tp->sen_i1_press%10, tp->sen_i1_temp/100, tp->sen_i1_temp%100, tp->sen_i1_hum/10, tp->sen_i1_hum%10,
TRACE_TAB, tp->gpio & 1, (tp->gpio >> 1) & 1, (tp->gpio >> 2) & 1, (tp->gpio >> 3) & 1
);
// Write data point to Flash memory
flash_writeLogDataPoint(tp);
// Switch last data point
lastDataPoint = tp;
// Wait until cycle
cycle_time = chThdSleepUntilWindowed(cycle_time, cycle_time + data_cycle_time);
}
}
/**
* Telemetry config (Thread)
* TODO: Make this service thread run on system level time schedule to send config packets.
* Any thread sending telemetry would request this service be started.
*/
THD_FUNCTION(configThread, arg) {
//uint8_t *useCFG = arg;
(void)arg;
while(true) chThdSleep(TIME_S2I(1));
}
/**
* GPS operation (Thread)
* TODO: To provide GPS status and data.
*/
THD_FUNCTION(gpsThread, arg) {
//uint8_t *useCFG = arg;
(void)arg;
while(true) chThdSleep(TIME_S2I(1));
}
/**
*
*/
void init_data_collector() {
if(!threadStarted) {
threadStarted = true;
TRACE_INFO("COLL > Startup data collector thread");
thread_t *th = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(10*1024),
"COL", LOWPRIO,
collectorThread, &useGPS);
if(!th) {
// Print startup error, do not start watchdog for this thread
TRACE_ERROR("COLL > Could not start"
" thread (not enough memory available)");
} else {
chThdSleep(TIME_MS2I(300)); // Wait a little bit until data collector has initialized first dataset
}
TRACE_INFO("CFG > Startup telemetry config thread");
th = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(2*1024),
"CFG", LOWPRIO,
configThread, &useCFG);
if(!th) {
// Print startup error, do not start watchdog for this thread
TRACE_ERROR("CFG > Could not start"
" thread (not enough memory available)");
} else {
chThdSleep(TIME_MS2I(300));
}
TRACE_INFO("GPS > Startup GPS manager thread");
th = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(2*1024),
"GPS", LOWPRIO,
gpsThread, NULL);
if(!th) {
// Print startup error, do not start watchdog for this thread
TRACE_ERROR("GPS > Could not start"
" thread (not enough memory available)");
} else {
chThdSleep(TIME_MS2I(300));
}
}
}
/** @} */
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik