mono - C# Serial Port BNO055 Sensor Communication -
i trying use c# , mono talk imu sensor (bno055) on raspberry pi. no matter do, cannot read , write sensor. can more experience let me know proper way is? thanks.
initialization:
private void connecttobno055() { gpio.write(resetpin, true); task.delay(650).wait(); port = new serialport(); port.portname = portname; port.baudrate = 115200; port.parity = parity.none; port.databits = 8; port.stopbits = stopbits.one; port.handshake = handshake.none; port.readtimeout = 5000; port.writetimeout = 5000; try { port.open(); while (!port.isopen) { task.delay(1).wait(); } setmode(operationmode.operation_mode_config); writeregister((byte)pageidregisterdefinition.bno055_page_id_addr, 0, false); byte chipid = readregister((byte)pageregisterdefinitionstart.bno055_chip_id_addr); if ((byte)i2caddress.bno055_id == chipid) { reset(); writeregister((byte)moderegisters.bno055_pwr_mode_addr, (byte)powermode.power_mode_normal); writeregister((byte)moderegisters.bno055_sys_trigger_addr, 0x0); setmode(operationmode.operation_mode_ndof); connected = true; console.writeline("connected imu."); console.writeline("calibrating imu. . ."); bno055.calibrationstatus lastcal; bno055.calibrationstatus cal = getcalibrationstatus(); console.writeline("system: " + cal.system.tostring() + ", magnometer: " + cal.mag.tostring() + ", gyroscope: " + cal.gyro.tostring() + ", accelerometer: " + cal.accel.tostring()); while (cal.system != 3 || cal.mag != 3 || cal.gyro != 3 || cal.accel != 3) { task.delay(250).wait(); lastcal = cal; cal = getcalibrationstatus(); if (cal.accel != lastcal.accel || cal.gyro != lastcal.gyro || cal.mag != lastcal.mag || cal.system != lastcal.system) { console.writeline("system: " + cal.system.tostring() + ", magnometer: " + cal.mag.tostring() + ", gyroscope: " + cal.gyro.tostring() + ", accelerometer: " + cal.accel.tostring()); } } calibrated = true; console.writeline("imu calibrated."); } else { console.writeline("unable connect imu."); } } catch (exception) { console.writeline("unable connect imu."); } }
io methods:
private void writeregister(byte address, byte data, bool ack = true) { byte[] writebuffer = new byte[] { 0xaa, 0x00, address, 1, data }; byte[] response; try { response = writedata(writebuffer, ack); if (ack) { if (0xee01 != (response[0] << 8 | response[1])) throw new exception(string.format("writeregister returned 0x{0:x2},0x{1:x2}", response[0], response[1])); } } catch (exception ex) { console.writeline(string.format("writeregister error: {0}", ex.message)); } } private byte readregister(byte register) { byte[] commandbuffer = new byte[] { 0xaa, 0x01, register, 1 }; byte[] readbuffer = new byte[1]; try { readbuffer = readdata(commandbuffer, 1); } catch (exception ex) { console.writeline(string.format("readregister error: {0}", ex.message)); } return readbuffer[0]; } private byte[] writedata(byte[] command, bool ack = true, int retries = 5) { byte[] ackbuffer = new byte[2]; bool done = false; while ((!done) && (retries-- > 0)) { port.write(command, 0, command.length); if (ack) { while (port.bytestoread != 2) { task.delay(1).wait(); } port.read(ackbuffer, 0, 2); done = 0xee07 != (ackbuffer[0] << 8 | ackbuffer[1]); } else done = true; } if (!done) throw new exception("writedata retries exceeded"); return ackbuffer; } private byte[] readdata(byte[] command, int readlength) { byte[] headerbuffer = new byte[2]; byte[] returnbuffer = new byte[readlength]; port.write(command, 0, command.length); while (port.bytestoread != 2) { task.delay(1).wait(); } port.read(headerbuffer, 0, 2); if (headerbuffer[0] != 0xbb) throw new exception(string.format("readdata error 0x{0:x2}{1:x2}", headerbuffer[0], headerbuffer[1])); if (headerbuffer[1] != readlength) throw new exception(string.format("readdata error: failed read {0} bytes. read {1}", readlength, headerbuffer[1])); while (port.bytestoread != readlength) { task.delay(1).wait(); } port.read(returnbuffer, 0, readlength); return returnbuffer; }
the issue here was trying read response sensor after writing data. solve problem, add loop waiting bytestoread property equal desired response length read.
Comments
Post a Comment