libopeninv-arduino/param_save.cpp at main · Modellfan/libopeninv-arduino · GitHub

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
/*
 * Arduino port of libopeninv param_save
 * Uses EEPROM for parameter persistence.
 */

#include <Arduino.h>
#include <EEPROM.h>
#include "params.h"
#include "param_save.h"
#include "my_string.h"

namespace
{
constexpr size_t kParamBlockSize = 2048;
constexpr int kEepromBase = 0;

typedef struct __attribute__((packed))
{
   uint16_t key;
   uint8_t dummy;
   uint8_t flags;
   uint32_t value;
} PARAM_ENTRY;

constexpr size_t kNumParams = (kParamBlockSize - 8) / sizeof(PARAM_ENTRY);
constexpr size_t kParamWords = kParamBlockSize / 4;

typedef struct __attribute__((packed))
{
   PARAM_ENTRY data[kNumParams];
   uint32_t crc;
   uint32_t padding;
} PARAM_PAGE;
}

// Simple CRC32 calculation
static uint32_t calculate_crc32(uint32_t *data, uint32_t length)
{
   uint32_t crc = 0xFFFFFFFF;

   for (uint32_t i = 0; i < length; i++)
   {
      crc ^= data[i];
      for (int j = 0; j < 32; j++)
      {
         if (crc & 1)
            crc = (crc >> 1) ^ 0xEDB88320;
         else
            crc = crc >> 1;
      }
   }

   return ~crc;
}

/**
* Save parameters to flash
*
* @return CRC of parameter flash page
*/
uint32_t parm_save()
{
   PARAM_PAGE parmPage;
   uint32_t idx;

   memset32((int*)&parmPage, 0xFFFFFFFF, kParamWords);

   // Copy parameter values and keys to block structure
   for (idx = 0; idx < kNumParams && idx < Param::PARAM_LAST; idx++)
   {
      if (Param::GetType((Param::PARAM_NUM)idx) == Param::TYPE_PARAM)
      {
         const Param::Attributes *pAtr = Param::GetAttrib((Param::PARAM_NUM)idx);
         parmPage.data[idx].flags = (uint8_t)Param::GetFlag((Param::PARAM_NUM)idx);
         parmPage.data[idx].key = pAtr->id;
         parmPage.data[idx].value = Param::Get((Param::PARAM_NUM)idx);
      }
   }

   parmPage.crc = calculate_crc32((uint32_t*)&parmPage, 2 * kNumParams);
   EEPROM.put(kEepromBase, parmPage);

   return parmPage.crc;
}

/**
* Load parameters from flash
*
* @retval 0 Parameters loaded successfully
* @retval -1 CRC error, parameters not loaded
*/
int parm_load()
{
   PARAM_PAGE parmPage;
   EEPROM.get(kEepromBase, parmPage);

   uint32_t crc = calculate_crc32((uint32_t*)&parmPage, 2 * kNumParams);

   if (crc == parmPage.crc)
   {
      int loaded = 0;
      for (unsigned int idxPage = 0; idxPage < kNumParams; idxPage++)
      {
         Param::PARAM_NUM idx = Param::NumFromId(parmPage.data[idxPage].key);
         if (idx != Param::PARAM_INVALID && Param::GetType((Param::PARAM_NUM)idx) == Param::TYPE_PARAM)
         {
            Param::SetFixed(idx, parmPage.data[idxPage].value);
            Param::SetFlagsRaw(idx, parmPage.data[idxPage].flags);
            loaded++;
         }
      }
      return 0;
   }

   // Check if EEPROM is erased (all 0xFF)
   if (parmPage.crc == 0xFFFFFFFF)
      return -1;

   return -1;
}