kopia lustrzana https://github.com/DL7AD/pecanpico9
385 wiersze
12 KiB
C
385 wiersze
12 KiB
C
#include "ch.h"
|
|
#include "hal.h"
|
|
|
|
#include "tracking.h"
|
|
#include "debug.h"
|
|
#include "config.h"
|
|
#include "ublox.h"
|
|
#include "bme280.h"
|
|
#include "padc.h"
|
|
#include "pac1720.h"
|
|
#include "radio.h"
|
|
#include "flash.h"
|
|
#include "watchdog.h"
|
|
#include "image.h"
|
|
|
|
static trackPoint_t trackPoints[2];
|
|
static trackPoint_t* lastTrackPoint;
|
|
static systime_t nextLogEntryTimer;
|
|
static module_conf_t trac_conf = {.name = "TRAC"}; // Fake config needed for watchdog tracking
|
|
static bool threadStarted = false;
|
|
static bool tracking_useGPS = false;
|
|
|
|
/**
|
|
* Returns most recent track point witch is complete.
|
|
*/
|
|
trackPoint_t* getLastTrackPoint(void)
|
|
{
|
|
return lastTrackPoint;
|
|
}
|
|
|
|
trackPoint_t* getLogBuffer(uint16_t id)
|
|
{
|
|
if(sizeof(trackPoint_t)*id < LOG_SECTOR_SIZE-sizeof(trackPoint_t))
|
|
{
|
|
return (trackPoint_t*)(LOG_FLASH_ADDR1 + id * sizeof(trackPoint_t));
|
|
} else if((id-(LOG_SECTOR_SIZE/sizeof(trackPoint_t)))*sizeof(trackPoint_t) < LOG_SECTOR_SIZE-sizeof(trackPoint_t)) {
|
|
return (trackPoint_t*)(LOG_FLASH_ADDR2 + (id-(LOG_SECTOR_SIZE/sizeof(trackPoint_t))) * sizeof(trackPoint_t));
|
|
} else { // Outside of memory address allocation
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Returns next free log entry address in memory. Returns 0 if all cells are
|
|
* filled with data
|
|
*/
|
|
static trackPoint_t* getNextFreeLogAddress(void)
|
|
{
|
|
trackPoint_t* tp;
|
|
for(uint16_t i=0; (tp = getLogBuffer(i)) != NULL; i++)
|
|
if(tp->id == 0xFFFFFFFF)
|
|
return tp;
|
|
|
|
return NULL;
|
|
}
|
|
|
|
/**
|
|
* Returns next free log entry address in memory. Returns 0 if all cells are
|
|
* filled with data
|
|
*/
|
|
static trackPoint_t* getLastLog(void)
|
|
{
|
|
trackPoint_t* last = NULL;
|
|
trackPoint_t* tp;
|
|
for(uint16_t i=0; (tp = getLogBuffer(i)) != NULL; i++) {
|
|
if(tp->id == 0xFFFFFFFF)
|
|
return last; // Found last entry
|
|
last = tp;
|
|
}
|
|
if(last->id != 0xFFFFFFFF)
|
|
return last; // All memory entries are use, so the very last one must be the most recent one.
|
|
return NULL; // There is no log entry in memory
|
|
}
|
|
|
|
/**
|
|
* Erases oldest data
|
|
*/
|
|
static void eraseOldestLogData(void)
|
|
{
|
|
// Determine which sector holds the oldest data
|
|
trackPoint_t pt1, pt2;
|
|
flashRead(LOG_FLASH_ADDR1, (char*)&pt1, sizeof(trackPoint_t));
|
|
flashRead(LOG_FLASH_ADDR2, (char*)&pt2, sizeof(trackPoint_t));
|
|
|
|
if(pt1.id < pt2.id) // Erase sector 10
|
|
{
|
|
TRACE_INFO("TRAC > Erase flash %08x", LOG_FLASH_ADDR1);
|
|
flashErase(LOG_FLASH_ADDR1, LOG_SECTOR_SIZE);
|
|
} else { // Erase sector 11
|
|
TRACE_INFO("TRAC > Erase flash %08x", LOG_FLASH_ADDR2);
|
|
flashErase(LOG_FLASH_ADDR2, LOG_SECTOR_SIZE);
|
|
}
|
|
}
|
|
|
|
static void writeLogTrackPoint(trackPoint_t* tp)
|
|
{
|
|
// Get address to write on
|
|
trackPoint_t* address = getNextFreeLogAddress();
|
|
if(address == NULL) // Memory completly used, erase oldest data
|
|
{
|
|
eraseOldestLogData();
|
|
address = getNextFreeLogAddress();
|
|
}
|
|
if(address == NULL) // Something went wront at erasing the memory
|
|
{
|
|
TRACE_ERROR("TRAC > Erasing flash failed");
|
|
return;
|
|
}
|
|
|
|
// Write data into flash
|
|
TRACE_INFO("TRAC > Flash write (ADDR=%08x)", address);
|
|
flashSectorBegin(flashSectorAt((uint32_t)address));
|
|
flashWrite((uint32_t)address, (char*)tp, sizeof(trackPoint_t));
|
|
flashSectorEnd(flashSectorAt((uint32_t)address));
|
|
|
|
// Verify
|
|
if(flashCompare((uint32_t)address, (char*)tp, sizeof(trackPoint_t)))
|
|
TRACE_INFO("TRAC > Flash write OK")
|
|
else
|
|
TRACE_ERROR("TRAC > Flash write failed");
|
|
}
|
|
|
|
void waitForNewTrackPoint(void)
|
|
{
|
|
uint32_t old_id = getLastTrackPoint()->id;
|
|
while(old_id == getLastTrackPoint()->id)
|
|
chThdSleepMilliseconds(1000);
|
|
}
|
|
|
|
/**
|
|
* Tracking Module (Thread)
|
|
*/
|
|
THD_FUNCTION(trackingThread, arg) {
|
|
(void)arg;
|
|
|
|
uint32_t id = 1;
|
|
lastTrackPoint = &trackPoints[0];
|
|
|
|
// Fill initial values by PAC1720 and BME280 and RTC
|
|
|
|
// Time
|
|
ptime_t rtc;
|
|
getTime(&rtc);
|
|
lastTrackPoint->time.year = rtc.year;
|
|
lastTrackPoint->time.month = rtc.month;
|
|
lastTrackPoint->time.day = rtc.day;
|
|
lastTrackPoint->time.hour = rtc.hour;
|
|
lastTrackPoint->time.minute = rtc.minute;
|
|
lastTrackPoint->time.second = rtc.second;
|
|
|
|
// Get last tracking point from memory
|
|
TRACE_INFO("TRAC > Read last track point from flash memory");
|
|
trackPoint_t* lastLogPoint = getLastLog();
|
|
|
|
if(lastLogPoint != NULL) { // If there has been stored a trackpoint, then get the last know GPS fix
|
|
TRACE_INFO("TRAC > Found track point in flash memory ID=%d", lastLogPoint->id);
|
|
lastTrackPoint->gps_lat = lastLogPoint->gps_lat;
|
|
lastTrackPoint->gps_lon = lastLogPoint->gps_lon;
|
|
lastTrackPoint->gps_alt = lastLogPoint->gps_alt;
|
|
} else {
|
|
TRACE_INFO("TRAC > No track point found in flash memory");
|
|
}
|
|
|
|
lastTrackPoint->gps_lock = GPS_LOG; // Tell other threads that it has been taken from log
|
|
lastTrackPoint->gps_sats = 0;
|
|
lastTrackPoint->gps_ttff = 0;
|
|
|
|
// Debug last stored GPS position
|
|
if(lastLogPoint != NULL) {
|
|
TRACE_INFO(
|
|
"TRAC > Last GPS position (from memory)\r\n"
|
|
"%s Latitude: %d.%07ddeg\r\n"
|
|
"%s Longitude: %d.%07ddeg\r\n"
|
|
"%s Altitude: %d Meter",
|
|
TRACE_TAB, lastTrackPoint->gps_lat/10000000, (lastTrackPoint->gps_lat > 0 ? 1:-1)*lastTrackPoint->gps_lat%10000000,
|
|
TRACE_TAB, lastTrackPoint->gps_lon/10000000, (lastTrackPoint->gps_lon > 0 ? 1:-1)*lastTrackPoint->gps_lon%10000000,
|
|
TRACE_TAB, lastTrackPoint->gps_alt
|
|
);
|
|
} else {
|
|
TRACE_INFO("TRAC > No GPS position in memory");
|
|
}
|
|
|
|
// Voltage/Current
|
|
lastTrackPoint->adc_vsol = getSolarVoltageMV();
|
|
lastTrackPoint->adc_vbat = getBatteryVoltageMV();
|
|
lastTrackPoint->adc_vusb = getUSBVoltageMV();
|
|
lastTrackPoint->adc_pbat = pac1720_getPbat();
|
|
|
|
bme280_t bme280;
|
|
|
|
// Atmosphere condition
|
|
if(BME280_isAvailable(BME280_ADDRESS_INT)) {
|
|
BME280_Init(&bme280, BME280_ADDRESS_INT);
|
|
lastTrackPoint->air_press = BME280_getPressure(&bme280, 256);
|
|
lastTrackPoint->air_hum = BME280_getHumidity(&bme280);
|
|
lastTrackPoint->air_temp = BME280_getTemperature(&bme280);
|
|
} else { // No internal BME280 found
|
|
TRACE_ERROR("TRAC > No BME280 found");
|
|
lastTrackPoint->air_press = 0;
|
|
lastTrackPoint->air_hum = 0;
|
|
lastTrackPoint->air_temp = 0;
|
|
}
|
|
|
|
/*
|
|
* Get last image ID. This is important because Habhub does mix up different
|
|
* images with the same it. So it is good to use a new image ID when the
|
|
* tracker has been reset.
|
|
*/
|
|
if(lastLogPoint != NULL)
|
|
gimage_id = lastLogPoint->id_image+1;
|
|
|
|
systime_t time = chVTGetSystemTimeX();
|
|
while(true)
|
|
{
|
|
TRACE_INFO("TRAC > Do module TRACKING MANAGER cycle");
|
|
trac_conf.wdg_timeout = chVTGetSystemTimeX() + S2ST(600); // TODO: Implement more sophisticated method
|
|
|
|
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 = {{0,0,0,0,0,0,0},0,0,0,0,0};
|
|
|
|
// Switch on GPS is enough power is available and GPS is needed by any position thread
|
|
uint16_t batt = getBatteryVoltageMV();
|
|
if(batt >= gps_on_vbat && tracking_useGPS)
|
|
{
|
|
// Switch on GPS
|
|
GPS_Init();
|
|
|
|
// Search for lock as long enough power is available
|
|
do {
|
|
batt = getBatteryVoltageMV();
|
|
gps_get_fix(&gpsFix);
|
|
} while(!isGPSLocked(&gpsFix) && batt >= gps_off_vbat && chVTGetSystemTimeX() <= time + track_cycle_time - S2ST(3)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-3sec in order to keep synchronization)
|
|
|
|
if(batt < gps_off_vbat) // Switch off GPS at low batt
|
|
GPS_Deinit();
|
|
}
|
|
|
|
if(isGPSLocked(&gpsFix)) { // GPS locked
|
|
|
|
// Switch off GPS (if cycle time is more than 60 seconds)
|
|
if(track_cycle_time >= S2ST(60)) {
|
|
TRACE_INFO("TRAC > Switch off GPS");
|
|
GPS_Deinit();
|
|
} else {
|
|
TRACE_INFO("TRAC > Keep GPS switched of because cycle < 60sec");
|
|
}
|
|
|
|
// Debug
|
|
TRACE_INFO("TRAC > GPS sampling finished GPS LOCK");
|
|
|
|
// 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 = GPS_LOCKED;
|
|
tp->gps_sats = gpsFix.num_svs;
|
|
|
|
} else { // GPS lost (keep GPS switched on)
|
|
|
|
// Debug
|
|
if(batt < gps_off_vbat) {
|
|
TRACE_WARN("TRAC > GPS sampling finished GPS LOW BATT");
|
|
} else {
|
|
TRACE_WARN("TRAC > GPS sampling finished GPS LOSS");
|
|
}
|
|
|
|
// Take time from internal RTC
|
|
getTime(&rtc);
|
|
tp->time.year = rtc.year;
|
|
tp->time.month = rtc.month;
|
|
tp->time.day = rtc.day;
|
|
tp->time.hour = rtc.hour;
|
|
tp->time.minute = rtc.minute;
|
|
tp->time.second = rtc.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 GPS loss (or low batt, GPS switch off)
|
|
if(tracking_useGPS)
|
|
tp->gps_lock = batt < gps_off_vbat ? GPS_LOWBATT : GPS_LOSS;
|
|
else
|
|
tp->gps_lock = GPS_OFF;
|
|
tp->gps_sats = 0;
|
|
|
|
}
|
|
|
|
tp->id = id; // Serial ID
|
|
tp->gps_ttff = ST2S(chVTGetSystemTimeX() - time); // Time to first fix
|
|
|
|
// Power management
|
|
tp->adc_vsol = getSolarVoltageMV();
|
|
tp->adc_vbat = getBatteryVoltageMV();
|
|
tp->adc_vusb = getUSBVoltageMV();
|
|
tp->adc_pbat = pac1720_getAvgPbat();
|
|
tp->adc_rbat = pac1720_getAvgRbat();
|
|
|
|
bme280_t bme280;
|
|
|
|
// Atmosphere condition
|
|
if(BME280_isAvailable(BME280_ADDRESS_INT)) {
|
|
BME280_Init(&bme280, BME280_ADDRESS_INT);
|
|
tp->air_press = BME280_getPressure(&bme280, 256);
|
|
tp->air_hum = BME280_getHumidity(&bme280);
|
|
tp->air_temp = BME280_getTemperature(&bme280);
|
|
} else { // No internal BME280 found
|
|
TRACE_ERROR("TRAC > Internal BME280 not available");
|
|
tp->air_press = 0;
|
|
tp->air_hum = 0;
|
|
tp->air_temp = 0;
|
|
}
|
|
|
|
// Set last time ID
|
|
tp->id_image = gimage_id;
|
|
|
|
// 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.%05d %d.%05d Alt %dm\r\n"
|
|
"%s Sats %d TTFF %dsec\r\n"
|
|
"%s ADC Vbat=%d.%03dV Vsol=%d.%03dV VUSB=%d.%03dV Pbat=%dmW Rbat=%dmOhm\r\n"
|
|
"%s AIR p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%% ImageID=%d",
|
|
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 > 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->adc_vusb/1000, (tp->adc_vusb%1000), tp->adc_pbat, tp->adc_rbat,
|
|
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, tp->id_image
|
|
);
|
|
|
|
// Append logging (timeout)
|
|
if(nextLogEntryTimer <= chVTGetSystemTimeX())
|
|
{
|
|
writeLogTrackPoint(tp);
|
|
nextLogEntryTimer += log_cycle_time;
|
|
}
|
|
|
|
// Switch last recent track point
|
|
lastTrackPoint = tp;
|
|
id++;
|
|
|
|
time = chThdSleepUntilWindowed(time, time + track_cycle_time); // Wait until time + cycletime
|
|
}
|
|
}
|
|
|
|
void init_tracking_manager(bool useGPS)
|
|
{
|
|
if(useGPS)
|
|
tracking_useGPS = true;
|
|
|
|
if(!threadStarted)
|
|
{
|
|
threadStarted = true;
|
|
|
|
TRACE_INFO("TRAC > Startup tracking thread");
|
|
thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(2*1024), "TRA", NORMALPRIO, trackingThread, NULL);
|
|
if(!th) {
|
|
// Print startup error, do not start watchdog for this thread
|
|
TRACE_ERROR("TRAC > Could not startup thread (not enough memory available)");
|
|
} else {
|
|
register_thread_at_wdg(&trac_conf);
|
|
trac_conf.wdg_timeout = chVTGetSystemTimeX() + S2ST(1);
|
|
chThdSleepMilliseconds(300); // Wait a little bit until tracking manager has initialized first dataset
|
|
}
|
|
}
|
|
}
|
|
|