@@ -222,17 +222,17 @@ void Lidarlitev3hp::stateMachine(void)
222222 break ; // pedantic.
223223 }
224224 case LIDARLITE_STATE_POLLING:
225- {
225+ {
226226 lidarlite_i2c_dma_buf[0 ] = STATUS;
227227 if (HAL_I2C_Master_Transmit_DMA (hi2c_, address_, lidarlite_i2c_dma_buf, 1 )==HAL_OK) {
228228 i2cState_ = LIDARLITE_STATE_STATUS_CMD;
229- } else {
230- i2cState_ = LIDARLITE_STATE_ERROR;
231- }
229+ } else {
230+ i2cState_ = LIDARLITE_STATE_ERROR;
231+ }
232232 break ;
233- }
233+ }
234234 case LIDARLITE_STATE_STATUS_CMD:
235- {
235+ {
236236 // Start read of Status
237237 lidarlite_i2c_dma_buf[0 ] = 0 ;
238238 if (HAL_I2C_Master_Receive_DMA (hi2c_, address_, lidarlite_i2c_dma_buf,1 ) == HAL_OK) {
@@ -243,13 +243,13 @@ void Lidarlitev3hp::stateMachine(void)
243243 break ;
244244 }
245245 case LIDARLITE_STATE_STATUS_READ:
246- {
246+ {
247247 // Check status
248- status_ = lidarlite_i2c_dma_buf[0 ];
248+ status_ = lidarlite_i2c_dma_buf[0 ];
249249 if ((status_ & 0x01 )==0x00 )
250250 {
251- drdy_ = time64.Us ();
252- // status good, start a read
251+ drdy_ = time64.Us ();
252+ // status good, start a read
253253 lidarlite_i2c_dma_buf[0 ] = FULL_DELAY_HIGH;
254254
255255 if (HAL_I2C_Master_Transmit_DMA (hi2c_, address_, lidarlite_i2c_dma_buf,1 ) == HAL_OK) {
@@ -278,51 +278,51 @@ void Lidarlitev3hp::stateMachine(void)
278278 i2cState_ = LIDARLITE_STATE_DATA_READ;
279279 } else {
280280 i2cState_ = LIDARLITE_STATE_ERROR;
281- }
281+ }
282282 break ;
283283 }
284284 case LIDARLITE_STATE_DATA_READ:
285285 {
286- // Reading range data complete
287- uint16_t urange = (lidarlite_i2c_dma_buf[0 ]<<8 ) | lidarlite_i2c_dma_buf[1 ]; // cm
288-
289- RangePacket p;
290- p.header .timestamp = drdy_;
291- p.header .complete = time64.Us ();
292- p.header .status = status_;
293- p.range = ((float )urange)/100.0 ;
294- p.min_range = 0.0 ;
295- p.max_range = 40.0 ;
296- p.type = rosflight_firmware::SensorRangeType::ROSFLIGHT_RANGE_LIDAR;
297- // no specific check for status because we did that before starting the read.
298- write ((uint8_t *) &p, sizeof (p));
286+ // Reading range data complete
287+ uint16_t urange = (lidarlite_i2c_dma_buf[0 ]<<8 ) | lidarlite_i2c_dma_buf[1 ]; // cm
288+
289+ RangePacket p;
290+ p.header .timestamp = drdy_;
291+ p.header .complete = time64.Us ();
292+ p.header .status = status_;
293+ p.range = ((float )urange)/100.0 ;
294+ p.min_range = 0.0 ;
295+ p.max_range = 40.0 ;
296+ p.type = rosflight_firmware::SensorRangeType::ROSFLIGHT_RANGE_LIDAR;
297+ // no specific check for status because we did that before starting the read.
298+ write ((uint8_t *) &p, sizeof (p));
299299
300300 // Start Next Measurement
301301 lidarlite_i2c_dma_buf[0 ] = ACQ_COMMAND;
302302 lidarlite_i2c_dma_buf[1 ] = 0x04 ;
303303 if (HAL_I2C_Master_Transmit_DMA (hi2c_, address_, lidarlite_i2c_dma_buf, 2 )==HAL_OK) {
304304 i2cState_ = LIDARLITE_STATE_MEASURE_CMD;
305- } else {
306- i2cState_ = LIDARLITE_STATE_ERROR;
307- }
308- break ;
309- }
305+ } else {
306+ i2cState_ = LIDARLITE_STATE_ERROR;
307+ }
308+ break ;
309+ }
310310 case LIDARLITE_STATE_MEASURE_CMD:
311- {
311+ {
312312 i2cState_ = LIDARLITE_STATE_IDLE;
313313 break ;
314- }
314+ }
315315 case LIDARLITE_STATE_ERROR:
316316 {
317317 break ;
318- }
318+ }
319319 default :
320- {
321- i2cState_ = LIDARLITE_STATE_ERROR;
320+ {
321+ i2cState_ = LIDARLITE_STATE_ERROR;
322322 break ; // pedantic.
323+ }
323324 }
324325}
325- }
326326
327327bool Lidarlitev3hp::display (void )
328328{
0 commit comments