Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@3:9ed1d493c235, 2016-03-04 (annotated)

Committer:
TNU
Date:
Fri Mar 04 11:53:52 2016 +0000
Revision:
3:9ed1d493c235
Parent:
1:e3ace426cee5
Child:
5:87e211a23654
Use dataReady pin in gettrace; change shiftr

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 3:9ed1d493c235 9 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr, PinName smpl):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr), sampling(smpl)
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 PACK_LONG2 prevRX;
TNU 0:dfe498e03679 572 PACK_LONG2 prevTX;
TNU 0:dfe498e03679 573 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 574 memset(&prevRX,0,sizeof(prevRX));
TNU 0:dfe498e03679 575 memset(&prevTX,0,sizeof(prevTX));
TNU 0:dfe498e03679 576 pc->printf("Value of npack:%d \n\r", nPack);
TNU 0:dfe498e03679 577 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 578 {
TNU 0:dfe498e03679 579
TNU 0:dfe498e03679 580 if (res < 0){
TNU 0:dfe498e03679 581 res = -7;
TNU 0:dfe498e03679 582 pc->printf("Problem creating echo status\n\r");
TNU 0:dfe498e03679 583 goto END;
TNU 0:dfe498e03679 584 }
TNU 0:dfe498e03679 585 wait_us(10);
TNU 0:dfe498e03679 586 //Tools::Wait(10);
TNU 0:dfe498e03679 587 // Clock the remaining long packets
TNU 0:dfe498e03679 588 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 589 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 590 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 591 if (res < 0){
TNU 0:dfe498e03679 592 pc->printf("Packet #%d => txPacket_long error\n\r",a);
TNU 0:dfe498e03679 593 goto END;}
TNU 0:dfe498e03679 594
TNU 0:dfe498e03679 595
TNU 0:dfe498e03679 596 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 597 res = MLX_DecodeResL2(&rxL);
TNU 1:e3ace426cee5 598 if ((res < 0)){
TNU 0:dfe498e03679 599
TNU 0:dfe498e03679 600 pc->printf("\n\r");
TNU 0:dfe498e03679 601 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 602 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 603
TNU 0:dfe498e03679 604 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 605 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 606 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 607 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 608 }
TNU 0:dfe498e03679 609 pc->printf("\n\r");
TNU 0:dfe498e03679 610 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 611 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 612 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 613 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 614 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 615
TNU 0:dfe498e03679 616 }
TNU 0:dfe498e03679 617 pc->printf("\n\r");
TNU 0:dfe498e03679 618 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 619 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 620 goto END;}
TNU 0:dfe498e03679 621
TNU 0:dfe498e03679 622 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 623 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 1:e3ace426cee5 624 for (b = 0; b < (128); ++b)
TNU 0:dfe498e03679 625 {
TNU 0:dfe498e03679 626 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 1:e3ace426cee5 627 buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 628 }
TNU 0:dfe498e03679 629 //prevTX=txL;
TNU 0:dfe498e03679 630 //prevRX=rxL;
TNU 0:dfe498e03679 631 }
TNU 0:dfe498e03679 632
TNU 0:dfe498e03679 633 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 634 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 635
TNU 0:dfe498e03679 636 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 637 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 638 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 639 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 640 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 641 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 642 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 643 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 644
TNU 0:dfe498e03679 645 ++idx;
TNU 0:dfe498e03679 646 }
TNU 0:dfe498e03679 647
TNU 0:dfe498e03679 648 res = idx;
TNU 0:dfe498e03679 649 trigger.write(0);
TNU 0:dfe498e03679 650
TNU 0:dfe498e03679 651 END:
TNU 0:dfe498e03679 652 trigger.write(0);
TNU 0:dfe498e03679 653 return res;
TNU 0:dfe498e03679 654 }
TNU 0:dfe498e03679 655
TNU 0:dfe498e03679 656
TNU 0:dfe498e03679 657
TNU 0:dfe498e03679 658
TNU 0:dfe498e03679 659 int LidarSpi::GetTrace ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc){
TNU 3:9ed1d493c235 660 trigger.write(0);
TNU 0:dfe498e03679 661 int res, a, b, i;
TNU 0:dfe498e03679 662 uint32_t cnt;
TNU 0:dfe498e03679 663 uint16_t rSz;
TNU 0:dfe498e03679 664 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 665 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 666 uint8_t iserr;
TNU 0:dfe498e03679 667 uint16_t err;
TNU 0:dfe498e03679 668 uint32_t val = 0;
TNU 0:dfe498e03679 669
TNU 0:dfe498e03679 670 uint16_t * u16ptr;
TNU 0:dfe498e03679 671 pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 672 // Required buffer space
TNU 0:dfe498e03679 673 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 674 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 675 return -1;}
TNU 0:dfe498e03679 676
TNU 0:dfe498e03679 677 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 678 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 679 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 680 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 681 pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 682 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 683 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 684
TNU 0:dfe498e03679 685 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 686 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 687
TNU 0:dfe498e03679 688
TNU 0:dfe498e03679 689 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 690 if (res < 0)
TNU 0:dfe498e03679 691 goto END;
TNU 0:dfe498e03679 692 val >>= 16;
TNU 0:dfe498e03679 693 pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 694 cnt = 0;
TNU 0:dfe498e03679 695 // Count how many channels are selected
TNU 0:dfe498e03679 696 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 697 if (val & 0x1)
TNU 0:dfe498e03679 698 cnt++;
TNU 0:dfe498e03679 699 val >>= 1;
TNU 0:dfe498e03679 700 }
TNU 3:9ed1d493c235 701
TNU 0:dfe498e03679 702 nPack *= cnt;
TNU 0:dfe498e03679 703 nPack /= 16;
TNU 3:9ed1d493c235 704
TNU 0:dfe498e03679 705 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 706 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 707 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 708
TNU 0:dfe498e03679 709 if (res < 0){
TNU 0:dfe498e03679 710 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 711 goto END;}
TNU 0:dfe498e03679 712
TNU 0:dfe498e03679 713 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 714 nPack /= 4;
TNU 0:dfe498e03679 715 }
TNU 0:dfe498e03679 716 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 717 nPack /= 4;
TNU 0:dfe498e03679 718 }
TNU 0:dfe498e03679 719 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 720 nPack /= 2;
TNU 0:dfe498e03679 721 }
TNU 0:dfe498e03679 722 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 723 nPack /= 1;
TNU 0:dfe498e03679 724 }
TNU 0:dfe498e03679 725 else {
TNU 0:dfe498e03679 726 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 727 }
TNU 0:dfe498e03679 728
TNU 3:9ed1d493c235 729
TNU 0:dfe498e03679 730 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 731 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 732 if (res < 0){
TNU 0:dfe498e03679 733 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 734 goto END;}
TNU 0:dfe498e03679 735 val = (val>>16) | 1;
TNU 3:9ed1d493c235 736 trigger.write(1);
TNU 0:dfe498e03679 737 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 738 if (res < 0){
TNU 0:dfe498e03679 739 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 740 goto END;}
TNU 0:dfe498e03679 741
TNU 0:dfe498e03679 742 // Wait till PORT_READY bit is set.
TNU 3:9ed1d493c235 743
TNU 3:9ed1d493c235 744
TNU 0:dfe498e03679 745 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 746 cnt = 0;
TNU 3:9ed1d493c235 747 /*
TNU 0:dfe498e03679 748 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 749 wait_us(50);
TNU 0:dfe498e03679 750 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 751 cnt++;
TNU 3:9ed1d493c235 752 }*/
TNU 3:9ed1d493c235 753
TNU 3:9ed1d493c235 754 while((!dataReady.read())&& (cnt<500)){
TNU 3:9ed1d493c235 755 wait_us(50);
TNU 3:9ed1d493c235 756 cnt++;
TNU 0:dfe498e03679 757 }
TNU 3:9ed1d493c235 758
TNU 3:9ed1d493c235 759
TNU 3:9ed1d493c235 760 trigger.write(0);
TNU 3:9ed1d493c235 761 pc-printf("Count: %d\n\r", cnt);
TNU 0:dfe498e03679 762
TNU 0:dfe498e03679 763 // Encode the request and send it
TNU 0:dfe498e03679 764 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 765 if (res < 0)
TNU 0:dfe498e03679 766 goto END;
TNU 0:dfe498e03679 767
TNU 0:dfe498e03679 768
TNU 0:dfe498e03679 769 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 770 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 771 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 772 if (res < 0)
TNU 0:dfe498e03679 773 goto END;
TNU 0:dfe498e03679 774 //----PACKET EXCHANGE
TNU 0:dfe498e03679 775 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 776
TNU 0:dfe498e03679 777
TNU 0:dfe498e03679 778 // Optional status decoding
TNU 0:dfe498e03679 779 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 780 if (res < 0 || iserr)
TNU 0:dfe498e03679 781 goto END;
TNU 0:dfe498e03679 782
TNU 0:dfe498e03679 783
TNU 0:dfe498e03679 784 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 785 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 786 //device.format(16,1);
TNU 3:9ed1d493c235 787 //trigger.write(1);
TNU 0:dfe498e03679 788 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 789 {
TNU 0:dfe498e03679 790 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 791 if (res < 0){
TNU 0:dfe498e03679 792 res = -7;
TNU 0:dfe498e03679 793 goto END;
TNU 0:dfe498e03679 794 }
TNU 0:dfe498e03679 795
TNU 0:dfe498e03679 796 //Tools::Wait(10);
TNU 0:dfe498e03679 797 wait_us(10);
TNU 0:dfe498e03679 798
TNU 0:dfe498e03679 799 // Clock the remaining long packets
TNU 0:dfe498e03679 800 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 801 // pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
TNU 0:dfe498e03679 802 // pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
TNU 0:dfe498e03679 803 if (res < 0){
TNU 0:dfe498e03679 804 res = -8;
TNU 0:dfe498e03679 805 goto END;
TNU 0:dfe498e03679 806 }
TNU 0:dfe498e03679 807
TNU 0:dfe498e03679 808 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 809 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 810 if ((res < 0)){
TNU 0:dfe498e03679 811 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 812 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 813 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 814 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 815 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 816 }
TNU 0:dfe498e03679 817 pc->printf("\n\r");
TNU 0:dfe498e03679 818 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 819 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 820 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 821 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 822 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 823
TNU 0:dfe498e03679 824 }
TNU 0:dfe498e03679 825 pc->printf("\n\r");
TNU 0:dfe498e03679 826 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 827 //printf("last RXL = \n");
TNU 0:dfe498e03679 828 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 829 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 830 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 831 // pc->printf("\n");
TNU 0:dfe498e03679 832 //}
TNU 0:dfe498e03679 833 //printf("\n");
TNU 0:dfe498e03679 834 res = -9;
TNU 0:dfe498e03679 835 goto END;
TNU 0:dfe498e03679 836 }
TNU 0:dfe498e03679 837
TNU 0:dfe498e03679 838 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 839 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 840 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 841 {
TNU 0:dfe498e03679 842 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 843 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 844 }
TNU 0:dfe498e03679 845
TNU 0:dfe498e03679 846 }
TNU 0:dfe498e03679 847 trigger.write(0);
TNU 0:dfe498e03679 848 device.format(8,1);
TNU 0:dfe498e03679 849
TNU 0:dfe498e03679 850 END:
TNU 0:dfe498e03679 851 device.format(8,1);
TNU 0:dfe498e03679 852 return res;
TNU 0:dfe498e03679 853 }
TNU 0:dfe498e03679 854
TNU 0:dfe498e03679 855 int LidarSpi::GetTraceOne ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){
TNU 0:dfe498e03679 856 int res, a, b, i;
TNU 0:dfe498e03679 857 uint32_t cnt;
TNU 0:dfe498e03679 858 uint16_t rSz;
TNU 0:dfe498e03679 859 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 860 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 861 uint8_t iserr;
TNU 0:dfe498e03679 862 uint16_t err;
TNU 0:dfe498e03679 863 uint32_t val = 0;
TNU 0:dfe498e03679 864
TNU 0:dfe498e03679 865 uint16_t * u16ptr;
TNU 0:dfe498e03679 866 //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 867 // Required buffer space
TNU 0:dfe498e03679 868 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 869 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 870 return -1;}
TNU 0:dfe498e03679 871
TNU 0:dfe498e03679 872 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 873 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 874 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 875 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 876 //pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 877 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 878 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 879
TNU 0:dfe498e03679 880 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 881 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 882
TNU 0:dfe498e03679 883
TNU 0:dfe498e03679 884 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 885 if (res < 0)
TNU 0:dfe498e03679 886 goto END;
TNU 0:dfe498e03679 887 val >>= 16;
TNU 0:dfe498e03679 888 //pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 889 cnt = 0;
TNU 0:dfe498e03679 890 // Count how many channels are selected
TNU 0:dfe498e03679 891 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 892 if (val & 0x1)
TNU 0:dfe498e03679 893 cnt++;
TNU 0:dfe498e03679 894 val >>= 1;
TNU 0:dfe498e03679 895 }
TNU 0:dfe498e03679 896 nPack *= cnt;
TNU 0:dfe498e03679 897 nPack /= 16;
TNU 0:dfe498e03679 898
TNU 0:dfe498e03679 899 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 900 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 901 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 902
TNU 0:dfe498e03679 903 if (res < 0){
TNU 0:dfe498e03679 904 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 905 goto END;}
TNU 0:dfe498e03679 906
TNU 0:dfe498e03679 907 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 908 nPack /= 4;
TNU 0:dfe498e03679 909 }
TNU 0:dfe498e03679 910 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 911 nPack /= 4;
TNU 0:dfe498e03679 912 }
TNU 0:dfe498e03679 913 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 914 nPack /= 2;
TNU 0:dfe498e03679 915 }
TNU 0:dfe498e03679 916 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 917 nPack /= 1;
TNU 0:dfe498e03679 918 }
TNU 0:dfe498e03679 919 else {
TNU 0:dfe498e03679 920 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 921 }
TNU 0:dfe498e03679 922
TNU 0:dfe498e03679 923
TNU 0:dfe498e03679 924 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 925 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 926 if (res < 0){
TNU 0:dfe498e03679 927 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 928 goto END;}
TNU 0:dfe498e03679 929 val = (val>>16) | 1;
TNU 0:dfe498e03679 930 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 931 if (res < 0){
TNU 0:dfe498e03679 932 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 933 goto END;}
TNU 0:dfe498e03679 934
TNU 0:dfe498e03679 935 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 936 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 937 cnt = 0;
TNU 0:dfe498e03679 938 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 939 wait_us(50);
TNU 0:dfe498e03679 940 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 941 cnt++;
TNU 0:dfe498e03679 942 }
TNU 0:dfe498e03679 943
TNU 0:dfe498e03679 944 // Encode the request and send it
TNU 0:dfe498e03679 945 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 946 if (res < 0)
TNU 0:dfe498e03679 947 goto END;
TNU 0:dfe498e03679 948
TNU 0:dfe498e03679 949
TNU 0:dfe498e03679 950 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 951 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 952 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 953 if (res < 0)
TNU 0:dfe498e03679 954 goto END;
TNU 0:dfe498e03679 955
TNU 0:dfe498e03679 956 pc->printf("MOSI ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 957 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 958 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 959 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 960 }
TNU 0:dfe498e03679 961 pc->printf("\n");
TNU 0:dfe498e03679 962 pc->printf("MISO ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 963 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 964 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 965 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 966 }
TNU 0:dfe498e03679 967 pc->printf("\n");
TNU 0:dfe498e03679 968 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
TNU 0:dfe498e03679 969 //----PACKET EXCHANGE
TNU 0:dfe498e03679 970 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 971
TNU 0:dfe498e03679 972
TNU 0:dfe498e03679 973 // Optional status decoding
TNU 0:dfe498e03679 974 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 975 if (res < 0 || iserr)
TNU 0:dfe498e03679 976 goto END;
TNU 0:dfe498e03679 977
TNU 0:dfe498e03679 978
TNU 0:dfe498e03679 979 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 980 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 981 //device.format(16,1);
TNU 0:dfe498e03679 982 trigger.write(1);
TNU 0:dfe498e03679 983 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 984 {
TNU 0:dfe498e03679 985 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 986 if (res < 0){
TNU 0:dfe498e03679 987 res = -7;
TNU 0:dfe498e03679 988 goto END;
TNU 0:dfe498e03679 989 }
TNU 0:dfe498e03679 990
TNU 0:dfe498e03679 991 //Tools::Wait(10);
TNU 0:dfe498e03679 992 wait_us(10);
TNU 0:dfe498e03679 993
TNU 0:dfe498e03679 994 // Clock the remaining long packets
TNU 0:dfe498e03679 995 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 996 // pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
TNU 0:dfe498e03679 997 // pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
TNU 0:dfe498e03679 998 if (res < 0){
TNU 0:dfe498e03679 999 res = -8;
TNU 0:dfe498e03679 1000 goto END;
TNU 0:dfe498e03679 1001 }
TNU 0:dfe498e03679 1002
TNU 0:dfe498e03679 1003
TNU 0:dfe498e03679 1004 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 1005 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 1006 if ((res < 0)){
TNU 0:dfe498e03679 1007 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 1008 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 1009 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1010 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1011 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1012 }
TNU 0:dfe498e03679 1013 pc->printf("\n\r");
TNU 0:dfe498e03679 1014 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1015 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 1016 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1017 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1018 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1019
TNU 0:dfe498e03679 1020 }
TNU 0:dfe498e03679 1021 pc->printf("\n\r");
TNU 0:dfe498e03679 1022 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1023 //printf("last RXL = \n");
TNU 0:dfe498e03679 1024 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 1025 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 1026 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 1027 // pc->printf("\n");
TNU 0:dfe498e03679 1028 //}
TNU 0:dfe498e03679 1029 //printf("\n");
TNU 0:dfe498e03679 1030 res = -9;
TNU 0:dfe498e03679 1031 goto END;
TNU 0:dfe498e03679 1032 }
TNU 0:dfe498e03679 1033
TNU 0:dfe498e03679 1034 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 1035 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 1036 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 1037 {
TNU 0:dfe498e03679 1038 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 1039 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 1040 }
TNU 0:dfe498e03679 1041 if(a<64){
TNU 0:dfe498e03679 1042 pc->printf("Trace packet %d MOSI: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1043 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1044 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1045 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1046 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1047 }
TNU 0:dfe498e03679 1048 pc->printf("\n\r");
TNU 0:dfe498e03679 1049 pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1050 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1051 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1052 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1053 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1054 }
TNU 0:dfe498e03679 1055 pc->printf("\n\r");
TNU 0:dfe498e03679 1056 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1057 }
TNU 0:dfe498e03679 1058
TNU 0:dfe498e03679 1059 }
TNU 0:dfe498e03679 1060 trigger.write(0);
TNU 0:dfe498e03679 1061 device.format(8,1);
TNU 0:dfe498e03679 1062
TNU 0:dfe498e03679 1063 END:
TNU 0:dfe498e03679 1064 device.format(8,1);
TNU 0:dfe498e03679 1065 return res;
TNU 0:dfe498e03679 1066 }
TNU 0:dfe498e03679 1067
TNU 0:dfe498e03679 1068
TNU 0:dfe498e03679 1069 int LidarSpi::LoadPatch ( uint16_t address, uint8_t *buf, uint16_t nBytes){
TNU 0:dfe498e03679 1070 int res;
TNU 0:dfe498e03679 1071 uint16_t rSz;
TNU 0:dfe498e03679 1072 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 1073 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 1074 uint8_t iserr;
TNU 0:dfe498e03679 1075 uint16_t err;
TNU 0:dfe498e03679 1076 //uint32_t val = 0;
TNU 0:dfe498e03679 1077 uint16_t nWordsRemaining;
TNU 0:dfe498e03679 1078 uint16_t nPack;
TNU 0:dfe498e03679 1079 uint16_t nWords = nBytes / 2;
TNU 0:dfe498e03679 1080
TNU 0:dfe498e03679 1081
TNU 0:dfe498e03679 1082 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 1083 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 1084 memset(&rxL, 0, sizeof(rxL));
TNU 0:dfe498e03679 1085
TNU 0:dfe498e03679 1086 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
TNU 0:dfe498e03679 1087 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
TNU 0:dfe498e03679 1088 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
TNU 0:dfe498e03679 1089
TNU 0:dfe498e03679 1090 // Encode the request and send it
TNU 0:dfe498e03679 1091 res = MLX_ReqWriteFW(tx, nPack, address);
TNU 0:dfe498e03679 1092 if (res < 0)
TNU 0:dfe498e03679 1093 goto END;
TNU 0:dfe498e03679 1094
TNU 0:dfe498e03679 1095 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 1096 if (res < 0)
TNU 0:dfe498e03679 1097 goto END;
TNU 0:dfe498e03679 1098
TNU 0:dfe498e03679 1099 // Optional status decoding
TNU 0:dfe498e03679 1100 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 1101 if (res < 0 || iserr)
TNU 0:dfe498e03679 1102 goto END;
TNU 0:dfe498e03679 1103
TNU 0:dfe498e03679 1104 nWordsRemaining = nWords;
TNU 0:dfe498e03679 1105 for (uint a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 1106 {
TNU 0:dfe498e03679 1107 uint size;
TNU 0:dfe498e03679 1108 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
TNU 0:dfe498e03679 1109 size = MLX_LONG2_DATA_SZ / 2;
TNU 0:dfe498e03679 1110 else
TNU 0:dfe498e03679 1111 size = nWordsRemaining;
TNU 0:dfe498e03679 1112
TNU 0:dfe498e03679 1113 res = MLX_WriteDataL2(&txL, size, a, &buf[a*MLX_LONG2_DATA_SZ]);
TNU 0:dfe498e03679 1114 if (res < 0){
TNU 0:dfe498e03679 1115 res = -7;
TNU 0:dfe498e03679 1116 goto END;
TNU 0:dfe498e03679 1117 }
TNU 0:dfe498e03679 1118
TNU 0:dfe498e03679 1119 //Tools::Wait(10);
TNU 0:dfe498e03679 1120
TNU 0:dfe498e03679 1121 // Clock the remaining long packets
TNU 0:dfe498e03679 1122 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 1123
TNU 0:dfe498e03679 1124 // Decode the long responses
TNU 0:dfe498e03679 1125 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 1126 if (res < 0){
TNU 0:dfe498e03679 1127 printf("LONG WRITE ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 1128 res = -9;
TNU 0:dfe498e03679 1129 goto END;
TNU 0:dfe498e03679 1130 }
TNU 0:dfe498e03679 1131
TNU 0:dfe498e03679 1132 nWordsRemaining = nWords - size;
TNU 0:dfe498e03679 1133 }
TNU 0:dfe498e03679 1134
TNU 0:dfe498e03679 1135 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
TNU 0:dfe498e03679 1136 res = MLX_EncodeStatusL2(&txL, 0, 0);
TNU 0:dfe498e03679 1137 if (res < 0) {
TNU 0:dfe498e03679 1138 res = -7;
TNU 0:dfe498e03679 1139 goto END;
TNU 0:dfe498e03679 1140 }
TNU 0:dfe498e03679 1141
TNU 0:dfe498e03679 1142 //Tools::Wait(10);
TNU 0:dfe498e03679 1143 // Clock the remaining long packets
TNU 0:dfe498e03679 1144 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 1145
TNU 0:dfe498e03679 1146 if (res < 0)
TNU 0:dfe498e03679 1147 goto END;
TNU 0:dfe498e03679 1148
TNU 0:dfe498e03679 1149 // Change jump table pointer.
TNU 0:dfe498e03679 1150 res = WriteReg(0x1000, 0x7000); // write addr: 0x1000 value:0x7000
TNU 0:dfe498e03679 1151 if (res < 0)
TNU 0:dfe498e03679 1152 goto END;
TNU 0:dfe498e03679 1153
TNU 0:dfe498e03679 1154 END:
TNU 0:dfe498e03679 1155 return res;
TNU 0:dfe498e03679 1156 }
TNU 0:dfe498e03679 1157
TNU 0:dfe498e03679 1158 void LidarSpi::Trigger(int level){
TNU 0:dfe498e03679 1159 trigger.write(level);
TNU 0:dfe498e03679 1160 }