2017-05-30 22:58:20 +00:00
# include "ch.h"
# include "hal.h"
2017-09-07 19:56:00 +00:00
# include "tracking.h"
2017-06-12 14:46:03 +00:00
# include "debug.h"
2017-05-30 22:58:20 +00:00
# include "config.h"
2017-06-15 06:08:48 +00:00
# include "ublox.h"
2017-05-30 22:58:20 +00:00
# include "bme280.h"
# include "padc.h"
# include "pac1720.h"
# include "radio.h"
# include "flash.h"
2017-06-12 14:46:03 +00:00
# include "watchdog.h"
2017-09-11 04:45:19 +00:00
# include "image.h"
2017-05-30 22:58:20 +00:00
static trackPoint_t trackPoints [ 2 ] ;
static trackPoint_t * lastTrackPoint ;
static systime_t nextLogEntryTimer ;
2017-06-12 14:46:03 +00:00
static module_conf_t trac_conf = { . name = " TRAC " } ; // Fake config needed for watchdog tracking
2017-07-26 22:35:48 +00:00
static bool threadStarted = false ;
2017-09-11 04:45:19 +00:00
static bool tracking_useGPS = false ;
2017-06-12 14:46:03 +00:00
2017-05-30 22:58:20 +00:00
/**
* Returns most recent track point witch is complete .
*/
trackPoint_t * getLastTrackPoint ( void )
{
return lastTrackPoint ;
}
2017-09-11 04:45:19 +00:00
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 ;
}
}
2017-06-12 14:46:03 +00:00
/**
* Returns next free log entry address in memory . Returns 0 if all cells are
* filled with data
*/
2017-09-11 04:45:19 +00:00
static trackPoint_t * getNextFreeLogAddress ( void )
2017-05-30 22:58:20 +00:00
{
2017-09-11 04:45:19 +00:00
trackPoint_t * tp ;
for ( uint16_t i = 0 ; ( tp = getLogBuffer ( i ) ) ! = NULL ; i + + )
if ( tp - > id = = 0xFFFFFFFF )
return tp ;
2017-06-12 14:46:03 +00:00
2017-09-11 04:45:19 +00:00
return NULL ;
2017-06-12 14:46:03 +00:00
}
/**
* Returns next free log entry address in memory . Returns 0 if all cells are
* filled with data
*/
2017-09-11 04:45:19 +00:00
static trackPoint_t * getLastLog ( void )
2017-06-12 14:46:03 +00:00
{
2017-09-11 04:45:19 +00:00
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 ;
2017-06-12 14:46:03 +00:00
}
2017-09-11 04:45:19 +00:00
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
2017-06-12 14:46:03 +00:00
}
/**
* 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
2017-09-11 04:45:19 +00:00
trackPoint_t * address = getNextFreeLogAddress ( ) ;
if ( address = = NULL ) // Memory completly used, erase oldest data
2017-06-12 14:46:03 +00:00
{
eraseOldestLogData ( ) ;
address = getNextFreeLogAddress ( ) ;
}
2017-09-11 04:45:19 +00:00
if ( address = = NULL ) // Something went wront at erasing the memory
2017-06-12 14:46:03 +00:00
{
TRACE_ERROR ( " TRAC > Erasing flash failed " ) ;
return ;
}
2017-05-30 22:58:20 +00:00
// Write data into flash
TRACE_INFO ( " TRAC > Flash write (ADDR=%08x) " , address ) ;
2017-09-11 04:45:19 +00:00
flashSectorBegin ( flashSectorAt ( ( uint32_t ) address ) ) ;
flashWrite ( ( uint32_t ) address , ( char * ) tp , sizeof ( trackPoint_t ) ) ;
flashSectorEnd ( flashSectorAt ( ( uint32_t ) address ) ) ;
2017-05-30 22:58:20 +00:00
// Verify
2017-09-11 04:45:19 +00:00
if ( flashCompare ( ( uint32_t ) address , ( char * ) tp , sizeof ( trackPoint_t ) ) )
2017-05-30 22:58:20 +00:00
TRACE_INFO ( " TRAC > Flash write OK " )
else
TRACE_ERROR ( " TRAC > Flash write failed " ) ;
2017-06-12 14:46:03 +00:00
}
2017-05-30 22:58:20 +00:00
2017-06-12 14:46:03 +00:00
void waitForNewTrackPoint ( void )
{
uint32_t old_id = getLastTrackPoint ( ) - > id ;
while ( old_id = = getLastTrackPoint ( ) - > id )
chThdSleepMilliseconds ( 1000 ) ;
2017-05-30 22:58:20 +00:00
}
/**
* Tracking Module ( Thread )
*/
2017-06-12 14:46:03 +00:00
THD_FUNCTION ( trackingThread , arg ) {
2017-05-30 22:58:20 +00:00
( void ) arg ;
uint32_t id = 1 ;
2017-09-19 01:36:38 +00:00
lastTrackPoint = & trackPoints [ 0 ] ;
2017-05-30 22:58:20 +00:00
2017-06-12 14:46:03 +00:00
// Fill initial values by PAC1720 and BME280 and RTC
2017-05-30 22:58:20 +00:00
// 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 ;
2017-09-11 04:45:19 +00:00
// 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 ) ;
2017-10-07 04:55:11 +00:00
id = lastLogPoint - > id + 1 ;
2017-09-11 04:45:19 +00:00
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 " ) ;
2017-06-12 14:46:03 +00:00
}
2017-09-11 04:45:19 +00:00
lastTrackPoint - > gps_lock = GPS_LOG ; // Tell other threads that it has been taken from log
2017-05-30 22:58:20 +00:00
lastTrackPoint - > gps_sats = 0 ;
lastTrackPoint - > gps_ttff = 0 ;
// Debug last stored GPS position
2017-09-11 04:45:19 +00:00
if ( lastLogPoint ! = NULL ) {
2017-05-30 22:58:20 +00:00
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
2017-06-12 14:46:03 +00:00
TRACE_ERROR ( " TRAC > No BME280 found " ) ;
2017-05-30 22:58:20 +00:00
lastTrackPoint - > air_press = 0 ;
lastTrackPoint - > air_hum = 0 ;
lastTrackPoint - > air_temp = 0 ;
}
2017-09-11 04:45:19 +00:00
/*
* 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 )
2017-09-12 01:04:40 +00:00
gimage_id = lastLogPoint - > id_image + 1 ;
2017-09-11 04:45:19 +00:00
2017-05-30 22:58:20 +00:00
systime_t time = chVTGetSystemTimeX ( ) ;
while ( true )
{
TRACE_INFO ( " TRAC > Do module TRACKING MANAGER cycle " ) ;
2017-06-12 14:46:03 +00:00
trac_conf . wdg_timeout = chVTGetSystemTimeX ( ) + S2ST ( 600 ) ; // TODO: Implement more sophisticated method
2017-05-30 22:58:20 +00:00
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 } ;
2017-09-11 04:45:19 +00:00
// Switch on GPS is enough power is available and GPS is needed by any position thread
2017-05-30 22:58:20 +00:00
uint16_t batt = getBatteryVoltageMV ( ) ;
2017-09-11 04:45:19 +00:00
if ( batt > = gps_on_vbat & & tracking_useGPS )
2017-05-30 22:58:20 +00:00
{
// Switch on GPS
GPS_Init ( ) ;
// Search for lock as long enough power is available
do {
batt = getBatteryVoltageMV ( ) ;
gps_get_fix ( & gpsFix ) ;
2017-09-05 06:35:23 +00:00
} 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)
2017-05-30 22:58:20 +00:00
2017-09-05 06:35:23 +00:00
if ( batt < gps_off_vbat ) // Switch off GPS at low batt
2017-05-30 22:58:20 +00:00
GPS_Deinit ( ) ;
}
if ( isGPSLocked ( & gpsFix ) ) { // GPS locked
2017-07-01 01:56:36 +00:00
// Switch off GPS (if cycle time is more than 60 seconds)
2017-09-05 06:35:23 +00:00
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 " ) ;
}
2017-05-30 22:58:20 +00:00
// 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 ;
2017-09-01 00:02:30 +00:00
tp - > gps_lock = GPS_LOCKED ;
2017-05-30 22:58:20 +00:00
tp - > gps_sats = gpsFix . num_svs ;
} else { // GPS lost (keep GPS switched on)
// Debug
2017-09-05 06:35:23 +00:00
if ( batt < gps_off_vbat ) {
2017-09-01 00:02:30 +00:00
TRACE_WARN ( " TRAC > GPS sampling finished GPS LOW BATT " ) ;
} else {
TRACE_WARN ( " TRAC > GPS sampling finished GPS LOSS " ) ;
}
2017-05-30 22:58:20 +00:00
// 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 ;
2017-09-11 04:45:19 +00:00
// Mark GPS loss (or low batt, GPS switch off)
if ( tracking_useGPS )
2017-10-07 04:55:11 +00:00
tp - > gps_lock = batt < gps_off_vbat | | batt < gps_on_vbat ? GPS_LOWBATT : GPS_LOSS ;
2017-09-11 04:45:19 +00:00
else
tp - > gps_lock = GPS_OFF ;
2017-05-30 22:58:20 +00:00
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 ( ) ;
2017-09-01 00:02:30 +00:00
tp - > adc_rbat = pac1720_getAvgRbat ( ) ;
2017-05-30 22:58:20 +00:00
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 ;
}
2017-09-11 04:45:19 +00:00
// Set last time ID
tp - > id_image = gimage_id ;
2017-05-30 22:58:20 +00:00
// Trace data
TRACE_INFO ( " TRAC > New tracking point available (ID=%d) \r \n "
" %s Time %04d-%02d-%02d %02d:%02d:%02d \r \n "
2017-08-28 23:21:32 +00:00
" %s Pos %d.%05d %d.%05d Alt %dm \r \n "
2017-05-30 22:58:20 +00:00
" %s Sats %d TTFF %dsec \r \n "
2017-09-01 00:02:30 +00:00
" %s ADC Vbat=%d.%03dV Vsol=%d.%03dV VUSB=%d.%03dV Pbat=%dmW Rbat=%dmOhm \r \n "
2017-09-11 04:45:19 +00:00
" %s AIR p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%% ImageID=%d " ,
2017-05-30 22:58:20 +00:00
tp - > id ,
TRACE_TAB , tp - > time . year , tp - > time . month , tp - > time . day , tp - > time . hour , tp - > time . minute , tp - > time . day ,
2017-08-28 23:21:32 +00:00
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 ,
2017-05-30 22:58:20 +00:00
TRACE_TAB , tp - > gps_sats , tp - > gps_ttff ,
2017-09-01 00:02:30 +00:00
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 ,
2017-09-11 04:45:19 +00:00
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
2017-05-30 22:58:20 +00:00
) ;
// Append logging (timeout)
2017-09-11 04:45:19 +00:00
if ( nextLogEntryTimer < = chVTGetSystemTimeX ( ) )
2017-05-30 22:58:20 +00:00
{
writeLogTrackPoint ( tp ) ;
2017-09-05 06:35:23 +00:00
nextLogEntryTimer + = log_cycle_time ;
2017-05-30 22:58:20 +00:00
}
// Switch last recent track point
lastTrackPoint = tp ;
id + + ;
2017-09-05 06:35:23 +00:00
time = chThdSleepUntilWindowed ( time , time + track_cycle_time ) ; // Wait until time + cycletime
2017-05-30 22:58:20 +00:00
}
}
2017-09-11 04:45:19 +00:00
void init_tracking_manager ( bool useGPS )
2017-05-30 22:58:20 +00:00
{
2017-09-11 04:45:19 +00:00
if ( useGPS )
tracking_useGPS = true ;
2017-07-26 22:35:48 +00:00
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
}
2017-06-12 14:46:03 +00:00
}
2017-05-30 22:58:20 +00:00
}