-
Notifications
You must be signed in to change notification settings - Fork 1
/
flashlog.cpp
188 lines (165 loc) · 11.5 KB
/
flashlog.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
#include "timesync.h"
#include "flashsize.h"
#include "stm32f10x_flash.h"
#include "flashlog.h"
#include "format.h"
static uint8_t PageSizeLog2 = 0; // 10; // [log2([B])]
static uint32_t PageSize = 0; // 1<<PageSizeLog2; // [Bytes]
static uint32_t PageMask = 0; // PageSize-1;
// const uint32_t PageSizeWords = PageSize/sizeof(uint32_t); // [Words]
static uint16_t Pages = 0; // number of Flash pages to be used for logging
static uint32_t *FirstPage = 0; // address of the first Flash page available for logging
static uint16_t WriteIdx = 0; // index of the current Flash page being written
static uint32_t *WriteAddr = 0; // current address to write next record
static uint32_t *PrevPacket = 0; // most recent packet stored
static uint32_t PrevTime = 0; // [sec] and timestamup of this packet
static uint16_t ReadIdx = 0; // index of the current Flash page being read
static uint32_t *ReadAddr = 0; // current address to read the next packet
// static uint32_t *getPageAddr(uint16_t Idx) { return FlashStart + ((uint32_t)(getFlashSizeKB()-1-Pages+Idx)*PageSizeWords); } // address of the given page (the last page is reserved for parameters)
static uint32_t *getPageAddr(uint16_t Idx) { return FirstPage + ((uint32_t)Idx<<(PageSizeLog2-2)); } // address of the given page (the last page is reserved for parameters)
static uint32_t *getPageAddr(uint32_t *Addr) { return (uint32_t *)((uint32_t)Addr&(~PageMask)); }
static bool isPageAddr(uint32_t *Addr) { return ((uint32_t)Addr&PageMask)==0; }
extern char __etext; // end of code in the Flash: linker symbol
static void FlashLog_Init(void)
{ PageSizeLog2 = getFlashPageSizeLog2();
PageSize = 1<<PageSizeLog2;
PageMask = PageSize-1;
uint32_t CodeEnd = (uint32_t)(&__etext); // [Bytes] end of code in Flash
uint32_t CodePages = ((CodeEnd-(uint32_t)FlashStart)+PageMask)>>PageSizeLog2; // [pages] size of the code in Flash
uint16_t FlashPages = getFlashSizeKB()>>(PageSizeLog2-10);
uint16_t FreePages = FlashPages-CodePages; // [pages]
Pages = FreePages-1;
FirstPage = (uint32_t *)((CodeEnd+PageMask)&(~PageMask)); }
uint8_t FlashLog_Print(char *Output)
{ uint8_t Len=0;
Len+=Format_String(Output+Len, "FlashWrite: ");
Len+=Format_UnsDec(Output+Len, WriteIdx);
Output[Len++]='/';
Len+=Format_UnsDec(Output+Len, Pages);
Output[Len++]=' ';
Len+=Format_HHMMSS(Output+Len, PrevTime);
Output[Len++]=' '; Output[Len++]='@';
Len+=Format_Hex(Output+Len, (uint32_t)WriteAddr);
Output[Len++]='\r'; Output[Len++]='\n';
Output[Len]=0; return Len; }
// Every Flash log page would start with the following uint32_t words:
// 0. header word of the position packets: written just after the page is erased
// 1. ...
// 2. Time of the first stored position: written just after page is erased
// 3. Time of the last stored position: written when last postion is stored
template <class OGNx_Packet>
static void Read(OGNx_Packet &Packet, uint32_t *ReadAddr)
{ uint32_t *PageAddr = getPageAddr(ReadAddr);
Packet.HeaderWord = PageAddr[0];
memcpy(Packet.Data, ReadAddr, 4*sizeof(uint32_t)); }
template <class OGNx_Packet>
static bool Write(OGNx_Packet &Packet, uint32_t Time, bool NewPage=0) // write the Packet (without Header) into the Flash log
{ int16_t Delay=0;
vTaskDelay(1); // be as close to the start of the tick as possible
taskDISABLE_INTERRUPTS(); // disable all interrupts: Flash can not be read while being erased
FLASH_Unlock();
uint32_t *PageAddr = getPageAddr(WriteAddr); // address of the beginning of the page
if(NewPage && (PageAddr!=WriteAddr) ) // if jumping to a new page was requested (and not at the start of a page)
{ WriteIdx++; if(WriteIdx>=Pages) WriteIdx=0; // increment the page index
WriteAddr = getPageAddr(WriteIdx); // WriteAddr at the beginning of the page
PageAddr = getPageAddr(WriteAddr); }
if(PageAddr==WriteAddr) // if at the beginning of the page
{ FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
// FLASH_ErasePage((uint32_t)WriteAddr); Delay=15; // erages the new Flash page
while(FLASH_ErasePage((uint32_t)WriteAddr)!=FLASH_COMPLETE); Delay=20; // erages the new Flash page
FLASH_ProgramWord((uint32_t)WriteAddr, Packet.HeaderWord); WriteAddr+=2; // Write packet header word
FLASH_ProgramWord((uint32_t)WriteAddr, Time); WriteAddr+=2; // write the time of the first record on page
if(PageAddr[2]!=Time) // if Flash erase or write failed
{ WriteAddr=PageAddr; // keep WriteAddr at the start of the page
FLASH_Lock(); // re-lock the Flash
taskENABLE_INTERRUPTS(); // restore RTOS interrupts
return 0; } // if page erase failed, then return failure
}
PrevPacket=WriteAddr; // keep the address of the last position record
PrevTime=Time; // and its Time
for(uint8_t Idx=0; Idx<4; Idx++) // write to Flash the four words of position (but not the header)
{ FLASH_ProgramWord((uint32_t)WriteAddr, Packet.Data[Idx]); WriteAddr++; }
if(isPageAddr(WriteAddr)) // if at the end of the page
{ FLASH_ProgramWord((uint32_t)(PageAddr+3), Time); // write the time of the last record on page
WriteIdx++; if(WriteIdx>=Pages) WriteIdx=0; // push PageIdx to the next Flash page
WriteAddr = getPageAddr(WriteIdx); } // set the WriteAddr to the start of this page
FLASH_Lock();
if(Delay) // when page is erased we hole the CPU for some 15-20ms
{ TimeSync_CorrRef(-Delay); } // correct the time refernece bu the delay
taskENABLE_INTERRUPTS(); // restore RTOS interrupts
return 1; } // return success
uint16_t FlashLog_OpenForWrite(void)
{ if(PageSize==0) FlashLog_Init();
uint32_t Latest=0; WriteIdx=0; // find the page with the latest end-timestamp
for(uint16_t Idx=0; Idx<Pages; Idx++)
{ uint32_t *Page=getPageAddr(Idx);
uint32_t First=Page[2]; // uint32_t Last=Page[3];
if(First>Latest) { Latest=First; WriteIdx=Idx; }
if(Latest==0xFFFFFFFF) break; } // if this is an erased page stop the search
if(Latest!=0xFFFFFFFF)
{ WriteIdx++; if(WriteIdx>=Pages) WriteIdx=0; } // take the page following the latest or the unfinished one
WriteAddr=getPageAddr(WriteIdx);
return Pages<<(PageSizeLog2-10); } // [KB] return the number of Flash space available for logging
template <class OGNx_Packet>
static bool Process(OGNx_Packet &Packet, uint32_t Time) // process position packet: decide whether to store it or not
{ uint32_t TimeDelta = Time-PrevTime; // [sec] time since previously stored packet
if( (PrevPacket==0) || (TimeDelta>=50) ) return 1; // [sec]
int16_t Climb = Packet.DecodeClimbRate(); // [0.1m/s]
if(abs(Climb)>=100) return 1; // if climb/decent rate more than 10m/s
OGNx_Packet *Prev = (OGNx_Packet *)(PrevPacket-1); // address (tweaked) of the previously stored packet
int32_t AltDelta=Packet.DecodeAltitude()-Prev->DecodeAltitude(); // [m] altitude change
if(abs(AltDelta)>=20) return 1; // if more than 50m altitude change
int16_t PrevClimb = Prev->DecodeClimbRate(); // [0.1m/s]
int32_t DistDeltaV = (int32_t)(Climb-PrevClimb)*TimeDelta; // [0.1m]
if(abs(DistDeltaV)>=200) return 1;
int16_t Speed = Packet.DecodeSpeed(); // [0.1m/s]
int16_t PrevSpeed = Prev->DecodeSpeed(); // [0.1m/s]
int32_t DistDeltaH = (int32_t)(Speed-PrevSpeed)*TimeDelta; // [0.1m] speed change * time since last recorded packet
if(abs(DistDeltaH)>=200) return 1; // if extrapolation error more than 50m
int16_t Turn = Packet.DecodeTurnRate(); // [0.1deg/s]
int16_t CFaccel = ((int32_t)Turn*Speed*229+0x10000)>>17; // [0.1m/s^2] centrifugal acceleration in turn
if(abs(CFaccel)>=50) return 1; // CFaccel at or above 5m/s^2 (0.5g)
int16_t PrevTurn = Prev->DecodeTurnRate(); // [0.1deg/s]
int16_t PrevCFaccel = ((int32_t)PrevTurn*PrevSpeed*229+0x10000)>>17; // [0.1m/s^2]
int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*TimeDelta*TimeDelta/2; // [0.1m]
if(abs(DistDeltaR)>=200) return 1; // [0.1m]
return 0; }
template <class OGNx_Packet>
bool FlashLog_Process(OGNx_Packet &Packet, uint32_t Time) // process position packet: decide whether to store it or not
{ if(WriteAddr==0) FlashLog_OpenForWrite();
if(Process(Packet, Time)==0) return 0; // decide whetehr to save this packet
return Write(Packet, Time); } // return 1 if this packet was saved in the flash
template
bool FlashLog_Process<OGN_Packet>(OGN_Packet &Packet, uint32_t Time);
bool FlashLog_PageIsValid(const uint32_t *Page)
{ uint32_t First=Page[2]; uint32_t Last=Page[3];
if(First>=Last) return 0;
if(First<1500000000) return 0;
if( (Last!=0xFFFFFFFF) && ((Last-First)>(PageSize/16*50)) ) return 0;
OGN_Packet *Packet = (OGN_Packet *)Page;
if(Packet->Header.NonPos) return 0;
if(Packet->Header.Relay) return 0;
if(!Packet->goodAddrParity()) return 0;
return 1; }
uint16_t FlashLog_OpenForRead(uint32_t *StartTime)
{ if(PageSize==0) FlashLog_Init();
uint32_t Earliest=0xFFFFFFFF; ReadIdx=0; uint16_t Count=0; // find the page with the latest end-timestamp
for(uint16_t Idx=0; Idx<Pages; Idx++)
{ uint32_t *Page=getPageAddr(Idx);
if(!FlashLog_PageIsValid(Page)) continue;
uint32_t First=Page[2]; // uint32_t Last=Page[3];
if(First<Earliest) { Earliest=First; ReadIdx=Idx; }
Count++; }
ReadAddr=getPageAddr(ReadIdx);
if(StartTime) *StartTime=Earliest;
return Count; } // [KB] return the number of Flash space available for logging
uint32_t FlashLog_ReadPage(const uint32_t * &Page)
{ if(ReadAddr==0) FlashLog_OpenForRead();
uint16_t StartIdx=ReadIdx;
for( ; ; )
{ Page=ReadAddr;
ReadIdx++; if(ReadIdx>=Pages) ReadIdx=0;
ReadAddr=getPageAddr(ReadIdx);
if(FlashLog_PageIsValid(Page)) return PageSize;
if(ReadIdx==StartIdx) break; }
return 0; }