Tom N / LidarSpi

This is a class which contains function to interface with the MLX75320

Dependents:   MLX75320_API

LidarSpi.cpp@2:7edbd37d81fd, 2016-02-25 (annotated)

Committer:
TNU
Date:
Thu Feb 25 08:08:37 2016 +0000
Revision:
2:7edbd37d81fd
Parent:
0:dfe498e03679 
Updated getechos

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TNU 0:dfe498e03679 1 #include "mbed.h"
TNU 0:dfe498e03679 2 #include "LidarSpi.h"
TNU 0:dfe498e03679 3
TNU 0:dfe498e03679 4 //#include "ToolsClass.h"
TNU 0:dfe498e03679 5 #include <string.h>
TNU 0:dfe498e03679 6 #include "LidarSpi.h"
TNU 0:dfe498e03679 7 #include "MLX_BaseSPI.h"
TNU 0:dfe498e03679 8 #define SPIFREQ 8000
TNU 0:dfe498e03679 9 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr)
TNU 0:dfe498e03679 10 {
TNU 0:dfe498e03679 11 //resetPin.write(1);
TNU 0:dfe498e03679 12 chipS.write(1);
TNU 0:dfe498e03679 13
TNU 0:dfe498e03679 14 //8 bit
TNU 0:dfe498e03679 15 //Mode 1: CPPOL=0, CLPHA=1 => Default of lidar, mbed side receiving is bad near 25Mhz
TNU 0:dfe498e03679 16 // -> mbed does not read first bit, this shifts the entire message on MISO 1 bit
TNU 0:dfe498e03679 17 //Mode 3: CLPOL=1, CLPHA=1 => Transmission on mbed side good, Lidar chip returns CRC errors on the succesful tranmissions
TNU 0:dfe498e03679 18 // -> Cant't send anything to lidar without changing lidar SPI mode
TNU 0:dfe498e03679 19 device.format(8,1); //8 bit, CLPOL=1, CLPHA=1
TNU 0:dfe498e03679 20 device.frequency(16000000);
TNU 0:dfe498e03679 21 }
TNU 0:dfe498e03679 22
TNU 0:dfe498e03679 23
TNU 0:dfe498e03679 24 int LidarSpi::BasicRead(){
TNU 0:dfe498e03679 25 return device.write(0x6B);
TNU 0:dfe498e03679 26
TNU 0:dfe498e03679 27 }
TNU 0:dfe498e03679 28
TNU 0:dfe498e03679 29
TNU 0:dfe498e03679 30 int LidarSpi::SpiSetting(long freq, int mode, Serial* pc){
TNU 0:dfe498e03679 31 int cPha= mode & 0x01;
TNU 0:dfe498e03679 32 int cPol=(mode>>1) & 0x01;
TNU 0:dfe498e03679 33 uint32_t val=0;
TNU 0:dfe498e03679 34 int res=0;
TNU 0:dfe498e03679 35 res=ReadReg(0x10E, &val,pc);
TNU 0:dfe498e03679 36 if(res<0) return -1;
TNU 0:dfe498e03679 37 val=val>>16;
TNU 0:dfe498e03679 38 val=val | (cPol<<5) | (cPha<<6);
TNU 0:dfe498e03679 39 wait_us(4);
TNU 0:dfe498e03679 40 res=WriteReg(0x10E, val, pc);
TNU 0:dfe498e03679 41 wait_us(4);
TNU 0:dfe498e03679 42 device.format(8, mode);
TNU 0:dfe498e03679 43 res=ReadReg(0x10E, &val,pc);
TNU 0:dfe498e03679 44
TNU 0:dfe498e03679 45 device.frequency(freq);
TNU 0:dfe498e03679 46 return res;
TNU 0:dfe498e03679 47 }
TNU 0:dfe498e03679 48
TNU 0:dfe498e03679 49
TNU 0:dfe498e03679 50 //int LidarSpi::BasicTransfer(uint8_t* rData, uint16_t rSz, const uint8_t* tData, uint16_t tSz, const event_callback_t callback){
TNU 0:dfe498e03679 51 // device.transfer(tData,tSz,rData,rSz,callback,event=SPI_EVENT_COMPLETE);
TNU 0:dfe498e03679 52 // //device.transfer((uint8_t*)_cmd, (2 + (DISPLAY_WIDTH / DISPLAY_BUFFER_TYPE_SIZE * sizeof(DISPLAY_BUFFER_TYPE))) , (uint8_t*)NULL, 0, _internalEventCallback, SPI_EVENT_COMPLETE) != 0)
TNU 0:dfe498e03679 53 // device.transfer();
TNU 0:dfe498e03679 54 // return 0;
TNU 0:dfe498e03679 55 //}
TNU 0:dfe498e03679 56
TNU 0:dfe498e03679 57
TNU 0:dfe498e03679 58 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
TNU 0:dfe498e03679 59 chipS=0;
TNU 0:dfe498e03679 60 int i =0;
TNU 0:dfe498e03679 61 for(i=0; i< tSz;i++){
TNU 0:dfe498e03679 62 *rData=device.write(*tData);
TNU 0:dfe498e03679 63 rData++;
TNU 0:dfe498e03679 64 tData++;
TNU 0:dfe498e03679 65 }
TNU 0:dfe498e03679 66 chipS=1;
TNU 0:dfe498e03679 67 *rSz=i;
TNU 0:dfe498e03679 68 return 0;
TNU 0:dfe498e03679 69 }
TNU 0:dfe498e03679 70
TNU 0:dfe498e03679 71 int LidarSpi::TxPacketWord(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
TNU 0:dfe498e03679 72 chipS=0;
TNU 0:dfe498e03679 73 int i =0;
TNU 0:dfe498e03679 74 uint16_t* recPoint=(uint16_t*)rData;
TNU 0:dfe498e03679 75 uint16_t* transPoint=(uint16_t*)tData;
TNU 0:dfe498e03679 76
TNU 0:dfe498e03679 77 for(i=0; i< tSz/2;i++){
TNU 0:dfe498e03679 78 *recPoint=device.write(*transPoint);
TNU 0:dfe498e03679 79 recPoint++;
TNU 0:dfe498e03679 80 transPoint++;
TNU 0:dfe498e03679 81 }
TNU 0:dfe498e03679 82 chipS=1;
TNU 0:dfe498e03679 83 *rSz=i*2;
TNU 0:dfe498e03679 84 return 0;
TNU 0:dfe498e03679 85 }
TNU 0:dfe498e03679 86
TNU 0:dfe498e03679 87
TNU 0:dfe498e03679 88
TNU 0:dfe498e03679 89 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, Serial* pc){
TNU 0:dfe498e03679 90 chipS=0;
TNU 0:dfe498e03679 91 int i =0;
TNU 0:dfe498e03679 92 //pc->printf("Transmitting %d bytes...\n\r",tSz);
TNU 0:dfe498e03679 93 //pc->printf("Received: ");
TNU 0:dfe498e03679 94 for(i=0; i< tSz;i++){
TNU 0:dfe498e03679 95 //*(tData+i)=*(tData+i)+1; //<================Uncomment to write gibberish with wrong CRC
TNU 0:dfe498e03679 96 *(rData+i)=device.write(*(tData+i));
TNU 0:dfe498e03679 97 //pc->printf("%02X", *(rData+i));
TNU 0:dfe498e03679 98 //rData++;
TNU 0:dfe498e03679 99 //tData++;
TNU 0:dfe498e03679 100 }
TNU 0:dfe498e03679 101 //pc->printf("\n\r");
TNU 0:dfe498e03679 102 chipS=1;
TNU 0:dfe498e03679 103 *rSz=i;
TNU 0:dfe498e03679 104 return 0;
TNU 0:dfe498e03679 105 }
TNU 0:dfe498e03679 106
TNU 0:dfe498e03679 107 int LidarSpi::TxPacketSlow(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, uint16_t usDelay){
TNU 0:dfe498e03679 108 int res=TxPacket(rData, rSz, tData, tSz);
TNU 0:dfe498e03679 109 wait_us(usDelay);
TNU 0:dfe498e03679 110 return res;
TNU 0:dfe498e03679 111 }
TNU 0:dfe498e03679 112
TNU 0:dfe498e03679 113 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val)
TNU 0:dfe498e03679 114 {
TNU 0:dfe498e03679 115 int res;
TNU 0:dfe498e03679 116 uint16_t rSz;
TNU 0:dfe498e03679 117 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 118
TNU 0:dfe498e03679 119 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 120 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 121
TNU 0:dfe498e03679 122 // Encode the request and send it
TNU 0:dfe498e03679 123 res = MLX_ReqReadReg(tx, (uint32_t)reg);
TNU 0:dfe498e03679 124 if (res < 0)
TNU 0:dfe498e03679 125 return -1;
TNU 0:dfe498e03679 126
TNU 0:dfe498e03679 127 // tx[1].hdr = 0x0040;
TNU 0:dfe498e03679 128 // tx[1].crc = 0x106a;
TNU 0:dfe498e03679 129 res = MLX_EncodeStatusS(tx + 1, 0, 0);
TNU 0:dfe498e03679 130 if (res < 0)
TNU 0:dfe498e03679 131 return -2;
TNU 0:dfe498e03679 132
TNU 0:dfe498e03679 133 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx)/2);
TNU 0:dfe498e03679 134 wait_us(5);
TNU 0:dfe498e03679 135 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
TNU 0:dfe498e03679 136 if (res < 0)
TNU 0:dfe498e03679 137 return -3;
TNU 0:dfe498e03679 138
TNU 0:dfe498e03679 139 // Decode the response packet with register value
TNU 0:dfe498e03679 140 res = MLX_DecodeResS(rx + 1, val);
TNU 0:dfe498e03679 141 if (res < 0)
TNU 0:dfe498e03679 142 return res;
TNU 0:dfe498e03679 143 //return sizeof(tx);
TNU 0:dfe498e03679 144
TNU 0:dfe498e03679 145 return 0;
TNU 0:dfe498e03679 146 }
TNU 0:dfe498e03679 147
TNU 0:dfe498e03679 148 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val, Serial* pc)
TNU 0:dfe498e03679 149 {
TNU 0:dfe498e03679 150 int res;
TNU 0:dfe498e03679 151 uint16_t rSz;
TNU 0:dfe498e03679 152 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 153
TNU 0:dfe498e03679 154 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 155 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 156
TNU 0:dfe498e03679 157 // Encode the request and send it
TNU 0:dfe498e03679 158 res = MLX_ReqReadReg(tx, (uint32_t)reg);
TNU 0:dfe498e03679 159 if (res < 0)
TNU 0:dfe498e03679 160 return -1;
TNU 0:dfe498e03679 161
TNU 0:dfe498e03679 162 // tx[1].hdr = 0x0040;
TNU 0:dfe498e03679 163 // tx[1].crc = 0x106a;
TNU 0:dfe498e03679 164 res = MLX_EncodeStatusS(tx + 1, 0, 0);
TNU 0:dfe498e03679 165 if (res < 0)
TNU 0:dfe498e03679 166 return -2;
TNU 0:dfe498e03679 167
TNU 0:dfe498e03679 168 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
TNU 0:dfe498e03679 169 wait_us(5);
TNU 0:dfe498e03679 170 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
TNU 0:dfe498e03679 171 if (res < 0)
TNU 0:dfe498e03679 172 return -3;
TNU 0:dfe498e03679 173
TNU 0:dfe498e03679 174 pc->printf("Read register request and response\n\r");
TNU 0:dfe498e03679 175
TNU 0:dfe498e03679 176 pc->printf("MOSI ReadRegRequest:\t");
TNU 0:dfe498e03679 177 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 178 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 179 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 180 }
TNU 0:dfe498e03679 181 pc->printf("\n\r");
TNU 0:dfe498e03679 182 pc->printf("MISO ReadRegRequest:\t");
TNU 0:dfe498e03679 183 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 184 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 185 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 186 }
TNU 0:dfe498e03679 187 pc->printf("\n\r");
TNU 0:dfe498e03679 188 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 189
TNU 0:dfe498e03679 190
TNU 0:dfe498e03679 191 pc->printf("MOSI ReadRegResponse:\t");
TNU 0:dfe498e03679 192 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 193 uint8_t* pnt=(uint8_t*)(tx+1);
TNU 0:dfe498e03679 194 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 195 }
TNU 0:dfe498e03679 196 pc->printf("\n\r");
TNU 0:dfe498e03679 197 pc->printf("MISO ReadRegResponse:\t");
TNU 0:dfe498e03679 198 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 199 uint8_t* pnt=(uint8_t*)(rx+1);
TNU 0:dfe498e03679 200 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 201 }
TNU 0:dfe498e03679 202 pc->printf("\n\r");
TNU 0:dfe498e03679 203 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 204
TNU 0:dfe498e03679 205 // Decode the response packet with register value
TNU 0:dfe498e03679 206 res = MLX_DecodeResS(rx + 1, val);
TNU 0:dfe498e03679 207 if (res < 0)
TNU 0:dfe498e03679 208 if(res==-6) {
TNU 0:dfe498e03679 209 uint8_t isser=0;
TNU 0:dfe498e03679 210 uint16_t err=0;
TNU 0:dfe498e03679 211 MLX_DecodeStatusS(rx+1,&isser, &err);
TNU 0:dfe498e03679 212 pc->printf("Not a valid ReadRequestResponse-> Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,isser,err);
TNU 0:dfe498e03679 213 return res;
TNU 0:dfe498e03679 214 }
TNU 0:dfe498e03679 215 else return res;
TNU 0:dfe498e03679 216 //return sizeof(tx);
TNU 0:dfe498e03679 217 return 0;
TNU 0:dfe498e03679 218 }
TNU 0:dfe498e03679 219
TNU 0:dfe498e03679 220 int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
TNU 0:dfe498e03679 221 {
TNU 0:dfe498e03679 222 return WriteRegSpeed(reg, val, START_DELAY);
TNU 0:dfe498e03679 223 }
TNU 0:dfe498e03679 224
TNU 0:dfe498e03679 225 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 226 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed)
TNU 0:dfe498e03679 227 {
TNU 0:dfe498e03679 228 int res;
TNU 0:dfe498e03679 229 uint16_t rSz;
TNU 0:dfe498e03679 230 uint8_t iserr;
TNU 0:dfe498e03679 231 uint16_t error;
TNU 0:dfe498e03679 232 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 233
TNU 0:dfe498e03679 234 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 235 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 236
TNU 0:dfe498e03679 237 // Encode the request and send it
TNU 0:dfe498e03679 238 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 239 if (res < 0)
TNU 0:dfe498e03679 240 return res;
TNU 0:dfe498e03679 241
TNU 0:dfe498e03679 242 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2);
TNU 0:dfe498e03679 243 wait_us(5);
TNU 0:dfe498e03679 244 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
TNU 0:dfe498e03679 245 if (res < 0)
TNU 0:dfe498e03679 246 return res;
TNU 0:dfe498e03679 247
TNU 0:dfe498e03679 248
TNU 0:dfe498e03679 249
TNU 0:dfe498e03679 250 // Decode response (a status packet)
TNU 0:dfe498e03679 251 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 252 if (res < 0 || iserr)
TNU 0:dfe498e03679 253 return res;
TNU 0:dfe498e03679 254
TNU 0:dfe498e03679 255 return 0;
TNU 0:dfe498e03679 256 }
TNU 0:dfe498e03679 257
TNU 0:dfe498e03679 258 int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc)
TNU 0:dfe498e03679 259 {
TNU 0:dfe498e03679 260 return WriteRegSpeed(reg, val, START_DELAY, pc);
TNU 0:dfe498e03679 261 }
TNU 0:dfe498e03679 262
TNU 0:dfe498e03679 263 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 264 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed, Serial* pc)
TNU 0:dfe498e03679 265 {
TNU 0:dfe498e03679 266 int res;
TNU 0:dfe498e03679 267 uint16_t rSz;
TNU 0:dfe498e03679 268 uint8_t iserr;
TNU 0:dfe498e03679 269 uint16_t error;
TNU 0:dfe498e03679 270 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 271
TNU 0:dfe498e03679 272 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 273 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 274
TNU 0:dfe498e03679 275 // Encode the request and send it
TNU 0:dfe498e03679 276 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 277 if (res < 0)
TNU 0:dfe498e03679 278 return res;
TNU 0:dfe498e03679 279
TNU 0:dfe498e03679 280 /*
TNU 0:dfe498e03679 281 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), speed);
TNU 0:dfe498e03679 282 if (res < 0)
TNU 0:dfe498e03679 283 return res;
TNU 0:dfe498e03679 284 */
TNU 0:dfe498e03679 285 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
TNU 0:dfe498e03679 286 wait_us(5);
TNU 0:dfe498e03679 287 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
TNU 0:dfe498e03679 288 if (res < 0)
TNU 0:dfe498e03679 289 return -3;
TNU 0:dfe498e03679 290
TNU 0:dfe498e03679 291 //pc->printf("Write register request and response\n\r");
TNU 0:dfe498e03679 292
TNU 0:dfe498e03679 293 pc->printf("MOSI WriteRegRequest:\t 0x");
TNU 0:dfe498e03679 294 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 295 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 296 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 297 }
TNU 0:dfe498e03679 298 pc->printf("\n\r");
TNU 0:dfe498e03679 299 pc->printf("MISO WriteRegRequest:\t 0x");
TNU 0:dfe498e03679 300 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 301 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 302 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 303 }
TNU 0:dfe498e03679 304 pc->printf("\n\r");
TNU 0:dfe498e03679 305 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 306
TNU 0:dfe498e03679 307
TNU 0:dfe498e03679 308 pc->printf("MOSI WriteRegResponse:\t 0x");
TNU 0:dfe498e03679 309 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 310 uint8_t* pnt=(uint8_t*)(tx+1);
TNU 0:dfe498e03679 311 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 312 }
TNU 0:dfe498e03679 313 pc->printf("\n\r");
TNU 0:dfe498e03679 314 pc->printf("MISO WriteRegResponse:\t 0x");
TNU 0:dfe498e03679 315 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 316 uint8_t* pnt=(uint8_t*)(rx+1);
TNU 0:dfe498e03679 317 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 318 }
TNU 0:dfe498e03679 319 pc->printf("\n\r");
TNU 0:dfe498e03679 320 pc->printf("==========================================================\n\r");
TNU 0:dfe498e03679 321
TNU 0:dfe498e03679 322
TNU 0:dfe498e03679 323
TNU 0:dfe498e03679 324 // Decode response (a status packet)
TNU 0:dfe498e03679 325 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 326 if (res < 0 || iserr){
TNU 0:dfe498e03679 327 pc->printf("Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error);
TNU 0:dfe498e03679 328 return res;}
TNU 0:dfe498e03679 329
TNU 0:dfe498e03679 330 return 0;
TNU 0:dfe498e03679 331 }
TNU 0:dfe498e03679 332 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode)
TNU 0:dfe498e03679 333 {
TNU 0:dfe498e03679 334 trigger.write(0);
TNU 0:dfe498e03679 335 int res, a, b;
TNU 0:dfe498e03679 336 uint16_t rSz;
TNU 0:dfe498e03679 337 uint32_t cnt;
TNU 0:dfe498e03679 338 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 339 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 340 uint8_t iserr;
TNU 0:dfe498e03679 341 uint16_t err;
TNU 0:dfe498e03679 342 uint16_t idx = 0;
TNU 0:dfe498e03679 343 uint32_t val = 0;
TNU 0:dfe498e03679 344 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 345
TNU 0:dfe498e03679 346 uint16_t * u16ptr ;
TNU 0:dfe498e03679 347
TNU 0:dfe498e03679 348
TNU 0:dfe498e03679 349 // Required buffer space
TNU 0:dfe498e03679 350 if (maxN < MAXECH)
TNU 0:dfe498e03679 351 return -1;
TNU 0:dfe498e03679 352
TNU 0:dfe498e03679 353 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 354 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 355 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 356 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 357 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 358
TNU 0:dfe498e03679 359 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 360 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 361 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 362
TNU 0:dfe498e03679 363 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 364
TNU 0:dfe498e03679 365 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 366 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 367 if (res < 0)
TNU 0:dfe498e03679 368 goto END;
TNU 0:dfe498e03679 369 val = (val >> 16) | 1;
TNU 0:dfe498e03679 370 trigger.write(1);
TNU 0:dfe498e03679 371 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 372 if (res < 0)
TNU 0:dfe498e03679 373 goto END;
TNU 0:dfe498e03679 374
TNU 0:dfe498e03679 375 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 376 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 377
TNU 0:dfe498e03679 378
TNU 0:dfe498e03679 379
TNU 0:dfe498e03679 380 cnt = 0;
TNU 0:dfe498e03679 381 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 382 wait_us(50);
TNU 0:dfe498e03679 383 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 384 cnt++;
TNU 0:dfe498e03679 385 }
TNU 0:dfe498e03679 386 trigger.write(0);
TNU 0:dfe498e03679 387 // Encode the request and send it
TNU 0:dfe498e03679 388 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 389 if (res < 0)
TNU 0:dfe498e03679 390 goto END;
TNU 0:dfe498e03679 391
TNU 0:dfe498e03679 392 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 393 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 394 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 395 if (res < 0)
TNU 0:dfe498e03679 396 goto END;
TNU 0:dfe498e03679 397 //----PACKET EXCHANGE
TNU 0:dfe498e03679 398
TNU 0:dfe498e03679 399 // Optional status decoding
TNU 0:dfe498e03679 400 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 401 if (res < 0 || iserr)
TNU 0:dfe498e03679 402 goto END;
TNU 0:dfe498e03679 403
TNU 0:dfe498e03679 404 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 405 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 406
TNU 0:dfe498e03679 407
TNU 0:dfe498e03679 408 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 409 if (res < 0){
TNU 0:dfe498e03679 410 res = -7;
TNU 0:dfe498e03679 411 goto END;
TNU 0:dfe498e03679 412 }
TNU 0:dfe498e03679 413 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 414 {
TNU 0:dfe498e03679 415
TNU 0:dfe498e03679 416 wait_us(10);
TNU 0:dfe498e03679 417 //Tools::Wait(10);
TNU 0:dfe498e03679 418 // Clock the remaining long packets
TNU 0:dfe498e03679 419 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 420 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 421 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 422 if (res < 0)
TNU 0:dfe498e03679 423 goto END;
TNU 0:dfe498e03679 424
TNU 0:dfe498e03679 425 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 426 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 427 if (res < 0)
TNU 0:dfe498e03679 428 goto END;
TNU 0:dfe498e03679 429
TNU 0:dfe498e03679 430 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 431 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 432 for (b = 0; b < (128 / 2); ++b)
TNU 0:dfe498e03679 433 {
TNU 0:dfe498e03679 434 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 435 buf[a*(128 / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 436 }
TNU 0:dfe498e03679 437 }
TNU 0:dfe498e03679 438
TNU 0:dfe498e03679 439 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 440 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 441
TNU 0:dfe498e03679 442 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 443 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 444 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 445 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 446 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 447 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 448 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 449 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 450
TNU 0:dfe498e03679 451 ++idx;
TNU 0:dfe498e03679 452 }
TNU 0:dfe498e03679 453
TNU 0:dfe498e03679 454 res = idx;
TNU 0:dfe498e03679 455
TNU 0:dfe498e03679 456 END:
TNU 0:dfe498e03679 457
TNU 0:dfe498e03679 458 return res;
TNU 0:dfe498e03679 459 }
TNU 0:dfe498e03679 460
TNU 0:dfe498e03679 461
TNU 0:dfe498e03679 462 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode, Serial* pc)
TNU 0:dfe498e03679 463 {
TNU 0:dfe498e03679 464 trigger.write(0);
TNU 0:dfe498e03679 465 int res, a, b;
TNU 0:dfe498e03679 466 uint16_t rSz;
TNU 0:dfe498e03679 467 uint32_t cnt;
TNU 0:dfe498e03679 468 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 469 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 470 uint8_t iserr;
TNU 0:dfe498e03679 471 uint16_t err;
TNU 0:dfe498e03679 472 uint16_t idx = 0;
TNU 0:dfe498e03679 473 uint32_t val = 0;
TNU 0:dfe498e03679 474 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 475 memset(buf, 0, MAXCH*4*BYTES_PER_ECH/2);
TNU 0:dfe498e03679 476 uint16_t * u16ptr ;
TNU 0:dfe498e03679 477
TNU 0:dfe498e03679 478
TNU 0:dfe498e03679 479 // Required buffer space
TNU 0:dfe498e03679 480 if (maxN < MAXECH){
TNU 0:dfe498e03679 481 pc->printf("maxN too small\n\r");
TNU 0:dfe498e03679 482 return -1;
TNU 0:dfe498e03679 483 }
TNU 0:dfe498e03679 484
TNU 0:dfe498e03679 485 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 486 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 487 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 488 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 489 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 490
TNU 0:dfe498e03679 491 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 492 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 493 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 494
TNU 0:dfe498e03679 495 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 496
TNU 0:dfe498e03679 497 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 498 pc->printf("\tRead PORT_ACQU\n\r");
TNU 0:dfe498e03679 499 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 500 if (res < 0){
TNU 0:dfe498e03679 501 pc->printf("ReadReg Error\n\r");
TNU 0:dfe498e03679 502 goto END;}
TNU 0:dfe498e03679 503 val = (val >> 16) | 1;
TNU 0:dfe498e03679 504 pc->printf("\tWrite 1 to PORT_ACQU\n\r");
TNU 0:dfe498e03679 505 trigger.write(1);
TNU 0:dfe498e03679 506 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 507 if (res < 0){
TNU 0:dfe498e03679 508 pc->printf("WriteReg Error\n\r");
TNU 0:dfe498e03679 509 goto END;}
TNU 0:dfe498e03679 510
TNU 0:dfe498e03679 511 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 512
TNU 0:dfe498e03679 513 pc->printf("\tWait for PORT_READY bit\n\r");
TNU 0:dfe498e03679 514 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 515 while(res<0){
TNU 0:dfe498e03679 516 pc->printf("Error: reread state of ready PIN\n\r");
TNU 0:dfe498e03679 517 wait_us(4);
TNU 0:dfe498e03679 518 res=ReadReg(470, &val);
TNU 0:dfe498e03679 519 }
TNU 0:dfe498e03679 520 cnt = 0;
TNU 0:dfe498e03679 521 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 522 wait_us(50);
TNU 0:dfe498e03679 523 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 524 while(res<0){
TNU 0:dfe498e03679 525 wait_us(4);
TNU 0:dfe498e03679 526 res = ReadReg(470, &val);
TNU 0:dfe498e03679 527 }
TNU 0:dfe498e03679 528 cnt++;
TNU 0:dfe498e03679 529 }
TNU 0:dfe498e03679 530 pc->printf("Counter: %d\n\r", cnt);
TNU 0:dfe498e03679 531
TNU 0:dfe498e03679 532 // Encode the request and send it
TNU 0:dfe498e03679 533 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 534 if (res < 0){
TNU 0:dfe498e03679 535 pc->printf("ReqreadEch error\n\r");
TNU 0:dfe498e03679 536 goto END;}
TNU 0:dfe498e03679 537
TNU 0:dfe498e03679 538 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 539 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 540 pc->printf("\tSend ReqReadEch\n\r");
TNU 0:dfe498e03679 541 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 542 if (res < 0){
TNU 0:dfe498e03679 543 pc->printf("txPacketSlow Error\n\r");
TNU 0:dfe498e03679 544 goto END;}
TNU 0:dfe498e03679 545 /*
TNU 0:dfe498e03679 546 pc->printf("\tFirmware read request - processed\n\r");
TNU 0:dfe498e03679 547 pc->printf("Firmware readRequest MOSI: \t");
TNU 0:dfe498e03679 548 for(int i =0; i<sizeof(tx);i++) {
TNU 0:dfe498e03679 549 uint8_t* pnt=(uint8_t*)(&tx);
TNU 0:dfe498e03679 550 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 551 }
TNU 0:dfe498e03679 552 pc->printf("\n\r");
TNU 0:dfe498e03679 553 pc->printf("Firmware readRequest MISO: \t");
TNU 0:dfe498e03679 554 for(int i =0; i<sizeof(rx);i++) {
TNU 0:dfe498e03679 555 uint8_t* pnt=(uint8_t*)(&rx);
TNU 0:dfe498e03679 556 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 557 }
TNU 0:dfe498e03679 558 pc->printf("\n\r");
TNU 0:dfe498e03679 559 pc->printf("-- -- -- -- -- -- --\n\r");*/
TNU 0:dfe498e03679 560
TNU 0:dfe498e03679 561 //----PACKET EXCHANGE
TNU 0:dfe498e03679 562
TNU 0:dfe498e03679 563 // Optional status decoding
TNU 0:dfe498e03679 564 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 565 if (res < 0 || iserr){
TNU 0:dfe498e03679 566 pc->printf("Short status decode: Iss err?\n\r");
TNU 0:dfe498e03679 567 goto END;}
TNU 0:dfe498e03679 568
TNU 0:dfe498e03679 569 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 570 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 571 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 572 pc->printf("Value of npack:%d \n\r", nPack);
TNU 0:dfe498e03679 573 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 574 {
TNU 0:dfe498e03679 575
TNU 0:dfe498e03679 576 if (res < 0){
TNU 0:dfe498e03679 577 res = -7;
TNU 0:dfe498e03679 578 pc->printf("Problem creating echo status\n\r");
TNU 0:dfe498e03679 579 goto END;
TNU 0:dfe498e03679 580 }
TNU 0:dfe498e03679 581 wait_us(10);
TNU 0:dfe498e03679 582 //Tools::Wait(10);
TNU 0:dfe498e03679 583 // Clock the remaining long packets
TNU 0:dfe498e03679 584 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 585 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 586 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 587 if (res < 0){
TNU 0:dfe498e03679 588 pc->printf("Packet #%d => txPacket_long error\n\r",a);
TNU 0:dfe498e03679 589 goto END;}
TNU 0:dfe498e03679 590
TNU 0:dfe498e03679 591
TNU 0:dfe498e03679 592 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 593 res = MLX_DecodeResL2(&rxL);
TNU 2:7edbd37d81fd 594 if ((res < 0)){
TNU 0:dfe498e03679 595
TNU 0:dfe498e03679 596 pc->printf("\n\r");
TNU 0:dfe498e03679 597 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 598 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 599
TNU 0:dfe498e03679 600 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 601 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 602 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 603 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 604 }
TNU 0:dfe498e03679 605 pc->printf("\n\r");
TNU 0:dfe498e03679 606 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 607 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 608 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 609 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 610 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 611
TNU 0:dfe498e03679 612 }
TNU 0:dfe498e03679 613 pc->printf("\n\r");
TNU 0:dfe498e03679 614 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 615 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 2:7edbd37d81fd 616 goto END;
TNU 2:7edbd37d81fd 617 }
TNU 2:7edbd37d81fd 618 if(a==nPack-1){
TNU 2:7edbd37d81fd 619 pc->printf("Values in payload: \n\r");
TNU 2:7edbd37d81fd 620 for(int i=0; i<128;i++) pc->printf("value in payload[%d]: %u \n\r",i,u16ptr[i]);
TNU 2:7edbd37d81fd 621 }
TNU 0:dfe498e03679 622
TNU 0:dfe498e03679 623 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 624 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 2:7edbd37d81fd 625 for (b = 0; b < (128); ++b)
TNU 0:dfe498e03679 626 {
TNU 0:dfe498e03679 627 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 2:7edbd37d81fd 628 buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 629 }
TNU 0:dfe498e03679 630 //prevTX=txL;
TNU 0:dfe498e03679 631 //prevRX=rxL;
TNU 0:dfe498e03679 632 }
TNU 0:dfe498e03679 633
TNU 0:dfe498e03679 634 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 635 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 636
TNU 0:dfe498e03679 637 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 638 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 639 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 640 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 641 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 642 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 643 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 644 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 645
TNU 0:dfe498e03679 646 ++idx;
TNU 0:dfe498e03679 647 }
TNU 0:dfe498e03679 648
TNU 0:dfe498e03679 649 res = idx;
TNU 0:dfe498e03679 650 trigger.write(0);
TNU 0:dfe498e03679 651
TNU 0:dfe498e03679 652 END:
TNU 0:dfe498e03679 653 trigger.write(0);
TNU 0:dfe498e03679 654 return res;
TNU 0:dfe498e03679 655
TNU 0:dfe498e03679 656 }
TNU 0:dfe498e03679 657
TNU 0:dfe498e03679 658
TNU 0:dfe498e03679 659 int LidarSpi::LoadPatch ( uint16_t address, uint8_t *buf, uint16_t nBytes){
TNU 0:dfe498e03679 660 int res;
TNU 0:dfe498e03679 661 uint16_t rSz;
TNU 0:dfe498e03679 662 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 663 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 664 uint8_t iserr;
TNU 0:dfe498e03679 665 uint16_t err;
TNU 0:dfe498e03679 666 //uint32_t val = 0;
TNU 0:dfe498e03679 667 uint16_t nWordsRemaining;
TNU 0:dfe498e03679 668 uint16_t nPack;
TNU 0:dfe498e03679 669 uint16_t nWords = nBytes / 2;
TNU 0:dfe498e03679 670
TNU 0:dfe498e03679 671
TNU 0:dfe498e03679 672 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 673 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 674 memset(&rxL, 0, sizeof(rxL));
TNU 0:dfe498e03679 675
TNU 0:dfe498e03679 676 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
TNU 0:dfe498e03679 677 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
TNU 0:dfe498e03679 678 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
TNU 0:dfe498e03679 679
TNU 0:dfe498e03679 680 // Encode the request and send it
TNU 0:dfe498e03679 681 res = MLX_ReqWriteFW(tx, nPack, address);
TNU 0:dfe498e03679 682 if (res < 0)
TNU 0:dfe498e03679 683 goto END;
TNU 0:dfe498e03679 684
TNU 0:dfe498e03679 685 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 686 if (res < 0)
TNU 0:dfe498e03679 687 goto END;
TNU 0:dfe498e03679 688
TNU 0:dfe498e03679 689 // Optional status decoding
TNU 0:dfe498e03679 690 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 691 if (res < 0 || iserr)
TNU 0:dfe498e03679 692 goto END;
TNU 0:dfe498e03679 693
TNU 0:dfe498e03679 694 nWordsRemaining = nWords;
TNU 0:dfe498e03679 695 for (uint a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 696 {
TNU 0:dfe498e03679 697 uint size;
TNU 0:dfe498e03679 698 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
TNU 0:dfe498e03679 699 size = MLX_LONG2_DATA_SZ / 2;
TNU 0:dfe498e03679 700 else
TNU 0:dfe498e03679 701 size = nWordsRemaining;
TNU 0:dfe498e03679 702
TNU 0:dfe498e03679 703 res = MLX_WriteDataL2(&txL, size, a, &buf[a*MLX_LONG2_DATA_SZ]);
TNU 0:dfe498e03679 704 if (res < 0){
TNU 0:dfe498e03679 705 res = -7;
TNU 0:dfe498e03679 706 goto END;
TNU 0:dfe498e03679 707 }
TNU 0:dfe498e03679 708
TNU 0:dfe498e03679 709 //Tools::Wait(10);
TNU 0:dfe498e03679 710
TNU 0:dfe498e03679 711 // Clock the remaining long packets
TNU 0:dfe498e03679 712 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 713
TNU 0:dfe498e03679 714 // Decode the long responses
TNU 0:dfe498e03679 715 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 716 if (res < 0){
TNU 0:dfe498e03679 717 printf("LONG WRITE ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 718 res = -9;
TNU 0:dfe498e03679 719 goto END;
TNU 0:dfe498e03679 720 }
TNU 0:dfe498e03679 721
TNU 0:dfe498e03679 722 nWordsRemaining = nWords - size;
TNU 0:dfe498e03679 723 }
TNU 0:dfe498e03679 724
TNU 0:dfe498e03679 725 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
TNU 0:dfe498e03679 726 res = MLX_EncodeStatusL2(&txL, 0, 0);
TNU 0:dfe498e03679 727 if (res < 0) {
TNU 0:dfe498e03679 728 res = -7;
TNU 0:dfe498e03679 729 goto END;
TNU 0:dfe498e03679 730 }
TNU 0:dfe498e03679 731
TNU 0:dfe498e03679 732 //Tools::Wait(10);
TNU 0:dfe498e03679 733 // Clock the remaining long packets
TNU 0:dfe498e03679 734 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 735
TNU 0:dfe498e03679 736 if (res < 0)
TNU 0:dfe498e03679 737 goto END;
TNU 0:dfe498e03679 738
TNU 0:dfe498e03679 739 // Change jump table pointer.
TNU 0:dfe498e03679 740 res = WriteReg(0x1000, 0x7000); // write addr: 0x1000 value:0x7000
TNU 0:dfe498e03679 741 if (res < 0)
TNU 0:dfe498e03679 742 goto END;
TNU 0:dfe498e03679 743
TNU 0:dfe498e03679 744 END:
TNU 0:dfe498e03679 745 return res;
TNU 0:dfe498e03679 746 }
TNU 0:dfe498e03679 747
TNU 0:dfe498e03679 748 void LidarSpi::Trigger(int level){
TNU 0:dfe498e03679 749 trigger.write(level);
TNU 0:dfe498e03679 750 }