Pārlūkot izejas kodu

Tweaks to HAL codestyle

Scott Lahteine 7 gadus atpakaļ
vecāks
revīzija
f3dbe19669

+ 47
- 100
Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp Parādīt failu

@@ -54,8 +54,7 @@ unsigned char SoftwareSerial::_receive_buffer[_SS_MAX_RX_BUFF];
54 54
 volatile uint8_t SoftwareSerial::_receive_buffer_tail = 0;
55 55
 volatile uint8_t SoftwareSerial::_receive_buffer_head = 0;
56 56
 
57
-typedef struct _DELAY_TABLE
58
-{
57
+typedef struct _DELAY_TABLE {
59 58
   long baud;
60 59
   uint16_t rx_delay_centering;
61 60
   uint16_t rx_delay_intrabit;
@@ -64,16 +63,12 @@ typedef struct _DELAY_TABLE
64 63
 } DELAY_TABLE;
65 64
 
66 65
 // rough delay estimation
67
-static const DELAY_TABLE table[] =
68
-{
69
-  //baud    |rxcenter|rxintra |rxstop  |tx
70
-  { 250000,   2,      4,       4,       4,   }, //Done but not good due to instruction cycle error
71
-  { 115200,   4,      8,       8,       8,   }, //Done but not good due to instruction cycle error
66
+static const DELAY_TABLE table[] = {
67
+  //baud    |rxcenter|rxintra |rxstop  |tx { 250000,   2,      4,       4,       4,   }, //Done but not good due to instruction cycle error { 115200,   4,      8,       8,       8,   }, //Done but not good due to instruction cycle error
72 68
   //{ 74880,   69,       139,       62,      162,  }, // estimation
73 69
 //  { 57600,   100,       185,      1,       208,  }, // Done but not good due to instruction cycle error
74 70
   //{ 38400,   13,      26,      26,      26,  }, // Done
75
-  //{ 19200,   26,      52,      52,      52,  }, // Done
76
-  { 9600,    52,      104,     104,     104, }, // Done
71
+  //{ 19200,   26,      52,      52,      52,  }, // Done { 9600,    52,      104,     104,     104, }, // Done
77 72
   //{ 4800,    104,     208,     208,     208, },
78 73
   //{ 2400,    208,     417,     417,     417, },
79 74
   //{ 1200,    416,    833,      833,     833,},
@@ -85,7 +80,7 @@ static const DELAY_TABLE table[] =
85 80
 
86 81
 #if 0
87 82
 /* static */
88
-inline void SoftwareSerial::tunedDelay(uint32_t count) {
83
+inline void SoftwareSerial::tunedDelay(const uint32_t count) {
89 84
 
90 85
   asm volatile(
91 86
 
@@ -101,20 +96,18 @@ inline void SoftwareSerial::tunedDelay(uint32_t count) {
101 96
 
102 97
 }
103 98
 #else
104
-inline void SoftwareSerial::tunedDelay(uint32_t count) {
99
+inline void SoftwareSerial::tunedDelay(const uint32_t count) {
105 100
   delayMicroseconds(count);
106 101
 }
107 102
 #endif
108 103
 
109 104
 // This function sets the current object as the "listening"
110 105
 // one and returns true if it replaces another
111
-bool SoftwareSerial::listen()
112
-{
106
+bool SoftwareSerial::listen() {
113 107
   if (!_rx_delay_stopbit)
114 108
     return false;
115 109
 
116
-  if (active_object != this)
117
-  {
110
+  if (active_object != this) {
118 111
     if (active_object)
119 112
       active_object->stopListening();
120 113
 
@@ -130,10 +123,8 @@ bool SoftwareSerial::listen()
130 123
 }
131 124
 
132 125
 // Stop listening. Returns true if we were actually listening.
133
-bool SoftwareSerial::stopListening()
134
-{
135
-  if (active_object == this)
136
-  {
126
+bool SoftwareSerial::stopListening() {
127
+  if (active_object == this) {
137 128
     setRxIntMsk(false);
138 129
     active_object = NULL;
139 130
     return true;
@@ -144,14 +135,12 @@ bool SoftwareSerial::stopListening()
144 135
 //
145 136
 // The receive routine called by the interrupt handler
146 137
 //
147
-void SoftwareSerial::recv()
148
-{
138
+void SoftwareSerial::recv() {
149 139
   uint8_t d = 0;
150 140
 
151 141
   // If RX line is high, then we don't see any start bit
152 142
   // so interrupt is probably not for us
153
-  if (_inverse_logic ? rx_pin_read() : !rx_pin_read())
154
-  {
143
+  if (_inverse_logic ? rx_pin_read() : !rx_pin_read()) {
155 144
     // Disable further interrupts during reception, this prevents
156 145
     // triggering another interrupt directly after we return, which can
157 146
     // cause problems at higher baudrates.
@@ -160,38 +149,31 @@ void SoftwareSerial::recv()
160 149
     // Wait approximately 1/2 of a bit width to "center" the sample
161 150
     tunedDelay(_rx_delay_centering);
162 151
     // Read each of the 8 bits
163
-    for (uint8_t i=8; i > 0; --i)
164
-    {
165
-    tunedDelay(_rx_delay_intrabit);
152
+    for (uint8_t i=8; i > 0; --i) {
153
+      tunedDelay(_rx_delay_intrabit);
166 154
       d >>= 1;
167
-      if (rx_pin_read())
168
-        d |= 0x80;
155
+      if (rx_pin_read()) d |= 0x80;
169 156
     }
170 157
 
171
-    if (_inverse_logic)
172
-      d = ~d;
158
+    if (_inverse_logic) d = ~d;
173 159
 
174 160
     // if buffer full, set the overflow flag and return
175 161
     uint8_t next = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF;
176
-    if (next != _receive_buffer_head)
177
-    {
162
+    if (next != _receive_buffer_head) {
178 163
       // save new data in buffer: tail points to where byte goes
179 164
       _receive_buffer[_receive_buffer_tail] = d; // save new byte
180 165
       _receive_buffer_tail = next;
181 166
     }
182
-    else
183
-    {
167
+    else {
184 168
       _buffer_overflow = true;
185 169
     }
186
-  tunedDelay(_rx_delay_stopbit);
170
+    tunedDelay(_rx_delay_stopbit);
187 171
     // Re-enable interrupts when we're sure to be inside the stop bit
188
-  setRxIntMsk(true);//__enable_irq();//
189
-
172
+    setRxIntMsk(true);  //__enable_irq();//
190 173
   }
191 174
 }
192 175
 
193
-uint32_t SoftwareSerial::rx_pin_read()
194
-{
176
+uint32_t SoftwareSerial::rx_pin_read() {
195 177
   return digitalRead(_receivePin);
196 178
 }
197 179
 
@@ -200,12 +182,9 @@ uint32_t SoftwareSerial::rx_pin_read()
200 182
 //
201 183
 
202 184
 /* static */
203
-inline void SoftwareSerial::handle_interrupt()
204
-{
185
+inline void SoftwareSerial::handle_interrupt() {
205 186
   if (active_object)
206
-  {
207 187
     active_object->recv();
208
-  }
209 188
 }
210 189
 extern "C" void intWrapper() {
211 190
   SoftwareSerial::handle_interrupt();
@@ -219,23 +198,19 @@ SoftwareSerial::SoftwareSerial(pin_t receivePin, pin_t transmitPin, bool inverse
219 198
   _rx_delay_stopbit(0),
220 199
   _tx_delay(0),
221 200
   _buffer_overflow(false),
222
-  _inverse_logic(inverse_logic)
223
-{
201
+  _inverse_logic(inverse_logic) {
224 202
   setTX(transmitPin);
225 203
   setRX(receivePin);
226
-
227 204
 }
228 205
 
229 206
 //
230 207
 // Destructor
231 208
 //
232
-SoftwareSerial::~SoftwareSerial()
233
-{
209
+SoftwareSerial::~SoftwareSerial() {
234 210
   end();
235 211
 }
236 212
 
237
-void SoftwareSerial::setTX(pin_t tx)
238
-{
213
+void SoftwareSerial::setTX(pin_t tx) {
239 214
   // First write, then set output. If we do this the other way around,
240 215
   // the pin would be output low for a short while before switching to
241 216
   // output hihg. Now, it is input with pullup for a short while, which
@@ -244,36 +219,30 @@ void SoftwareSerial::setTX(pin_t tx)
244 219
   digitalWrite(tx, _inverse_logic ? LOW : HIGH);
245 220
   pinMode(tx,OUTPUT);
246 221
   _transmitPin = tx;
247
-
248 222
 }
249 223
 
250
-void SoftwareSerial::setRX(pin_t rx)
251
-{
224
+void SoftwareSerial::setRX(pin_t rx) {
252 225
   pinMode(rx, INPUT_PULLUP); // pullup for normal logic!
253 226
   //if (!_inverse_logic)
254 227
   // digitalWrite(rx, HIGH);
255 228
   _receivePin = rx;
256 229
   _receivePort = LPC1768_PIN_PORT(rx);
257 230
   _receivePortPin = LPC1768_PIN_PIN(rx);
258
-/*  GPIO_T * rxPort = digitalPinToPort(rx);
231
+  /* GPIO_T * rxPort = digitalPinToPort(rx);
259 232
   _receivePortRegister = portInputRegister(rxPort);
260 233
   _receiveBitMask = digitalPinToBitMask(rx);*/
261
-
262 234
 }
263 235
 
264 236
 //
265 237
 // Public methods
266 238
 //
267 239
 
268
-void SoftwareSerial::begin(long speed)
269
-{
240
+void SoftwareSerial::begin(long speed) {
270 241
   _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = 0;
271 242
 
272
-  for(uint8_t i = 0; i < sizeof(table)/sizeof(table[0]); ++i)
273
-  {
243
+  for(uint8_t i = 0; i < sizeof(table)/sizeof(table[0]); ++i) {
274 244
     long baud = table[i].baud;
275
-    if(baud == speed)
276
-    {
245
+    if (baud == speed) {
277 246
       _rx_delay_centering = table[i].rx_delay_centering;
278 247
       _rx_delay_intrabit = table[i].rx_delay_intrabit;
279 248
       _rx_delay_stopbit = table[i].rx_delay_stopbit;
@@ -289,29 +258,24 @@ void SoftwareSerial::begin(long speed)
289 258
 
290 259
 }
291 260
 
292
-void SoftwareSerial::setRxIntMsk(bool enable)
293
-{
261
+void SoftwareSerial::setRxIntMsk(bool enable) {
294 262
   if (enable)
295 263
     GpioEnableInt(_receivePort,_receivePin,CHANGE);
296 264
   else
297 265
     GpioDisableInt(_receivePort,_receivePin);
298 266
 }
299 267
 
300
-void SoftwareSerial::end()
301
-{
268
+void SoftwareSerial::end() {
302 269
   stopListening();
303 270
 }
304 271
 
305 272
 
306 273
 // Read data from buffer
307
-int SoftwareSerial::read()
308
-{
309
-  if (!isListening())
310
-    return -1;
274
+int SoftwareSerial::read() {
275
+  if (!isListening()) return -1;
311 276
 
312 277
   // Empty buffer?
313
-  if (_receive_buffer_head == _receive_buffer_tail)
314
-    return -1;
278
+  if (_receive_buffer_head == _receive_buffer_tail) return -1;
315 279
 
316 280
   // Read from "head"
317 281
   uint8_t d = _receive_buffer[_receive_buffer_head]; // grab next byte
@@ -319,16 +283,13 @@ int SoftwareSerial::read()
319 283
   return d;
320 284
 }
321 285
 
322
-int SoftwareSerial::available()
323
-{
324
-  if (!isListening())
325
-    return 0;
286
+int SoftwareSerial::available() {
287
+  if (!isListening()) return 0;
326 288
 
327 289
   return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) % _SS_MAX_RX_BUFF;
328 290
 }
329 291
 
330
-size_t SoftwareSerial::write(uint8_t b)
331
-{
292
+size_t SoftwareSerial::write(uint8_t b) {
332 293
   // By declaring these as local variables, the compiler will put them
333 294
   // in registers _before_ disabling interrupts and entering the
334 295
   // critical timing sections below, which makes it a lot easier to
@@ -337,36 +298,25 @@ size_t SoftwareSerial::write(uint8_t b)
337 298
   bool inv = _inverse_logic;
338 299
   uint16_t delay = _tx_delay;
339 300
 
340
-  if(inv)
341
-    b = ~b;
301
+  if (inv) b = ~b;
342 302
 
343 303
   cli();  // turn off interrupts for a clean txmit
344 304
 
345 305
   // Write the start bit
346
-  if (inv)
347
-    digitalWrite(_transmitPin, 1);
348
-  else
349
-    digitalWrite(_transmitPin, 0);
306
+  digitalWrite(_transmitPin, !!inv);
350 307
 
351 308
   tunedDelay(delay);
352 309
 
353 310
   // Write each of the 8 bits
354
-  for (uint8_t i = 8; i > 0; --i)
355
-  {
356
-    if (b & 1) // choose bit
357
-      digitalWrite(_transmitPin, 1); // send 1 //(GPIO_Desc[_transmitPin].P)->DOUT |= GPIO_Desc[_transmitPin].bit;
358
-    else
359
-      digitalWrite(_transmitPin, 0); // send 0 //(GPIO_Desc[_transmitPin].P)->DOUT &= ~GPIO_Desc[_transmitPin].bit;
360
-
311
+  for (uint8_t i = 8; i > 0; --i) {
312
+    digitalWrite(_transmitPin, b & 1); // send 1 //(GPIO_Desc[_transmitPin].P)->DOUT |= GPIO_Desc[_transmitPin].bit;
313
+                                       // send 0 //(GPIO_Desc[_transmitPin].P)->DOUT &= ~GPIO_Desc[_transmitPin].bit;
361 314
     tunedDelay(delay);
362 315
     b >>= 1;
363 316
   }
364 317
 
365 318
   // restore pin to natural state
366
-  if (inv)
367
-    digitalWrite(_transmitPin, 0);
368
-  else
369
-    digitalWrite(_transmitPin, 1);
319
+  digitalWrite(_transmitPin, !inv);
370 320
 
371 321
   sei(); // turn interrupts back on
372 322
   tunedDelay(delay);
@@ -374,18 +324,15 @@ size_t SoftwareSerial::write(uint8_t b)
374 324
   return 1;
375 325
 }
376 326
 
377
-void SoftwareSerial::flush()
378
-{
379
-  if (!isListening())
380
-    return;
327
+void SoftwareSerial::flush() {
328
+  if (!isListening()) return;
381 329
 
382 330
   cli();
383 331
   _receive_buffer_head = _receive_buffer_tail = 0;
384 332
   sei();
385 333
 }
386 334
 
387
-int SoftwareSerial::peek()
388
-{
335
+int SoftwareSerial::peek() {
389 336
   if (!isListening())
390 337
     return -1;
391 338
 

+ 2
- 2
Marlin/src/HAL/HAL_LPC1768/arduino.cpp Parādīt failu

@@ -165,9 +165,9 @@ void eeprom_write_byte(unsigned char *pos, unsigned char value) {
165 165
 
166 166
 unsigned char eeprom_read_byte(uint8_t * pos) { return '\0'; }
167 167
 
168
-void eeprom_read_block (void *__dst, const void *__src, size_t __n) { }
168
+void eeprom_read_block(void *__dst, const void *__src, size_t __n) { }
169 169
 
170
-void eeprom_update_block (const void *__src, void *__dst, size_t __n) { }
170
+void eeprom_update_block(const void *__src, void *__dst, size_t __n) { }
171 171
 
172 172
 char *dtostrf (double __val, signed char __width, unsigned char __prec, char *__s) {
173 173
   char format_string[20];

+ 14
- 20
Marlin/src/HAL/HAL_STM32F7/EmulatedEeprom.cpp Parādīt failu

@@ -80,23 +80,18 @@ static bool eeprom_initialised = false;
80 80
 
81 81
 
82 82
 void eeprom_init() {
83
-  if(!eeprom_initialised) {
83
+  if (!eeprom_initialised) {
84 84
     HAL_FLASH_Unlock();
85 85
 
86 86
     __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
87 87
 
88 88
     /* EEPROM Init */
89
-    if(EE_Initialise() != EE_OK)
90
-    {
91
-      while(1) {
92
-        HAL_Delay(1);
93
-      }
94
-    }
89
+    if (EE_Initialise() != EE_OK)
90
+      for (;;) HAL_Delay(1); // Spin forever until watchdog reset
95 91
 
96 92
     HAL_FLASH_Lock();
97 93
     eeprom_initialised = true;
98 94
   }
99
-
100 95
 }
101 96
 
102 97
 void eeprom_write_byte(unsigned char *pos, unsigned char value) {
@@ -106,39 +101,38 @@ void eeprom_write_byte(unsigned char *pos, unsigned char value) {
106 101
 
107 102
   HAL_FLASH_Unlock();
108 103
   __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
109
-  if(EE_WriteVariable(eeprom_address, (uint16_t) value) != EE_OK) {
110
-    while(1) {
111
-      HAL_Delay(1);
112
-    }
113
-  }
104
+
105
+  if (EE_WriteVariable(eeprom_address, (uint16_t) value) != EE_OK)
106
+      for (;;) HAL_Delay(1); // Spin forever until watchdog reset
107
+
114 108
   HAL_FLASH_Lock();
115 109
 }
116 110
 
117 111
 unsigned char eeprom_read_byte(unsigned char *pos) {
118 112
   uint16_t data = 0xFF;
119
-  uint16_t eeprom_address = (unsigned) pos;
113
+  uint16_t eeprom_address = (unsigned)pos;
120 114
 
121 115
   eeprom_init();
122 116
 
123
-  if(EE_ReadVariable(eeprom_address, &data) != EE_OK) {
124
-    return (char) data;
117
+  if (EE_ReadVariable(eeprom_address, &data) != EE_OK) {
118
+    return (unsigned char)data;
125 119
   }
126
-  return (char)data;
120
+  return (unsigned char)data;
127 121
 }
128 122
 
129
-void eeprom_read_block (void *__dst, const void *__src, size_t __n) {
123
+void eeprom_read_block(void *__dst, const void *__src, size_t __n) {
130 124
   uint16_t data = 0xFF;
131 125
   uint16_t eeprom_address = (unsigned) __src;
132 126
 
133 127
   eeprom_init();
134 128
 
135
-  for(uint8_t c = 0; c < __n; c++) {
129
+  for (uint8_t c = 0; c < __n; c++) {
136 130
     EE_ReadVariable(eeprom_address+c, &data);
137 131
     *((uint8_t*)__dst + c) = data;
138 132
   }
139 133
 }
140 134
 
141
-void eeprom_update_block (const void *__src, void *__dst, size_t __n) {
135
+void eeprom_update_block(const void *__src, void *__dst, size_t __n) {
142 136
 
143 137
 }
144 138
 

+ 11
- 15
Marlin/src/HAL/I2cEeprom.cpp Parādīt failu

@@ -72,10 +72,10 @@
72 72
 // Public functions
73 73
 // --------------------------------------------------------------------------
74 74
 
75
-static bool eeprom_initialised = false;
76 75
 static uint8_t eeprom_device_address = 0x50;
77 76
 
78 77
 static void eeprom_init(void) {
78
+  static bool eeprom_initialised = false;
79 79
   if (!eeprom_initialised) {
80 80
     Wire.begin();
81 81
     eeprom_initialised = true;
@@ -100,27 +100,25 @@ void eeprom_write_byte(unsigned char *pos, unsigned char value) {
100 100
 
101 101
 // WARNING: address is a page address, 6-bit end will wrap around
102 102
 // also, data can be maximum of about 30 bytes, because the Wire library has a buffer of 32 bytes
103
-void eeprom_update_block(const void* pos, void* eeprom_address, size_t n) {
104
-  uint8_t eeprom_temp[32] = {0};
105
-  uint8_t flag = 0;
106
-
103
+void eeprom_update_block(const void *pos, void* eeprom_address, size_t n) {
107 104
   eeprom_init();
108 105
 
109 106
   Wire.beginTransmission(eeprom_device_address);
110 107
   Wire.write((int)((unsigned)eeprom_address >> 8));   // MSB
111 108
   Wire.write((int)((unsigned)eeprom_address & 0xFF)); // LSB
112 109
   Wire.endTransmission();
110
+
111
+  uint8_t *ptr = (uint8_t*)pos;
112
+  uint8_t flag = 0;
113 113
   Wire.requestFrom(eeprom_device_address, (byte)n);
114
-  for (byte c = 0; c < n; c++) {
115
-    if (Wire.available()) eeprom_temp[c] = Wire.read();
116
-    flag |= (eeprom_temp[c] ^ *((uint8_t*)pos + c));
117
-  }
114
+  for (byte c = 0; c < n && Wire.available(); c++)
115
+    flag |= Wire.read() ^ ptr[c];
118 116
 
119 117
   if (flag) {
120 118
     Wire.beginTransmission(eeprom_device_address);
121 119
     Wire.write((int)((unsigned)eeprom_address >> 8));   // MSB
122 120
     Wire.write((int)((unsigned)eeprom_address & 0xFF)); // LSB
123
-    Wire.write((uint8_t*)(pos), n);
121
+    Wire.write((uint8_t*)pos, n);
124 122
     Wire.endTransmission();
125 123
 
126 124
     // wait for write cycle to complete
@@ -132,18 +130,16 @@ void eeprom_update_block(const void* pos, void* eeprom_address, size_t n) {
132 130
 
133 131
 unsigned char eeprom_read_byte(unsigned char *pos) {
134 132
   byte data = 0xFF;
135
-  unsigned eeprom_address = (unsigned) pos;
133
+  unsigned eeprom_address = (unsigned)pos;
136 134
 
137
-  eeprom_init ();
135
+  eeprom_init();
138 136
 
139 137
   Wire.beginTransmission(eeprom_device_address);
140 138
   Wire.write((int)(eeprom_address >> 8));   // MSB
141 139
   Wire.write((int)(eeprom_address & 0xFF)); // LSB
142 140
   Wire.endTransmission();
143 141
   Wire.requestFrom(eeprom_device_address, (byte)1);
144
-  if (Wire.available())
145
-    data = Wire.read();
146
-  return data;
142
+  return Wire.available() ? Wire.read() : 0xFF;
147 143
 }
148 144
 
149 145
 // maybe let's not read more than 30 or 32 bytes at a time!

+ 2
- 2
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp Parādīt failu

@@ -541,7 +541,7 @@
541 541
     // increment to first segment destination
542 542
     LOOP_XYZE(i) raw[i] += diff[i];
543 543
 
544
-    for(;;) {  // for each mesh cell encountered during the move
544
+    for (;;) {  // for each mesh cell encountered during the move
545 545
 
546 546
       // Compute mesh cell invariants that remain constant for all segments within cell.
547 547
       // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
@@ -591,7 +591,7 @@
591 591
       const float z_sxy0 = z_xmy0 * diff[X_AXIS],                                     // per-segment adjustment to z_cxy0
592 592
                   z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS];  // per-segment adjustment to z_cxym
593 593
 
594
-      for(;;) {  // for all segments within this mesh cell
594
+      for (;;) {  // for all segments within this mesh cell
595 595
 
596 596
         if (--segments == 0)                      // if this is last segment, use rtarget for exact
597 597
           COPY(raw, rtarget);

Notiek ielāde…
Atcelt
Saglabāt