From b83599e94de895b16a556923696b16520727f1d7 Mon Sep 17 00:00:00 2001 From: nynzzz <91736289+nynzzz@users.noreply.github.com> Date: Sun, 13 Oct 2024 23:38:40 +0200 Subject: [PATCH 1/3] Integrated State class --- src/Qbead.h | 282 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 184 insertions(+), 98 deletions(-) diff --git a/src/Qbead.h b/src/Qbead.h index 1666328..f40e275 100644 --- a/src/Qbead.h +++ b/src/Qbead.h @@ -1,12 +1,10 @@ #ifndef QBEAD_H #define QBEAD_H - #include #include #include #include - #include // default configs @@ -74,33 +72,137 @@ float sign(float x) { else return -1; } -// z = cos(t) -// x = cos(p)sin(t) -// y = sin(p)sin(t) -float phi(float x, float y, float z) { - float ll = x * x + y * y + z * z; - float l = sqrt(ll); - float phi = atan2(y, x); - return phi; +// State Class Definition +class State { +private: + float x, y, z; // Cartesian coordinates + float theta, phi; // Spherical coordinates + + void cartesianToSpherical(); + void sphericalToCartesian(); + +public: + State(); + State(float x_init, float y_init, float z_init); + State(float theta_init, float phi_init); + + void setX(float new_x); + void setY(float new_y); + void setZ(float new_z); + void setXYZ(float new_x, float new_y, float new_z); + + void setTheta(float new_theta); + void setPhi(float new_phi); + void setThetaPhi(float new_theta, float new_phi); + + float getX() const; + float getY() const; + float getZ() const; + + float getTheta() const; + float getPhi() const; + + void printState(); + void Xgate(); +}; + +State::State() : x(0), y(0), z(1) { + cartesianToSpherical(); +} + +State::State(float x_init, float y_init, float z_init) : x(x_init), y(y_init), z(z_init) { + cartesianToSpherical(); +} + +State::State(float theta_init, float phi_init) : theta(theta_init), phi(phi_init) { + sphericalToCartesian(); +} + +void State::cartesianToSpherical() { + float l = sqrt(x * x + y * y + z * z); + if (l == 0) l = 1; + theta = acos(z / l) * 180 / PI; + phi = atan2(y, x) * 180 / PI; + if (phi < 0) phi += 360; +} + +void State::sphericalToCartesian() { + float r = 1; + x = r * sin(theta * PI / 180.0) * cos(phi * PI / 180.0); + y = r * sin(theta * PI / 180.0) * sin(phi * PI / 180.0); + z = r * cos(theta * PI / 180.0); +} + +void State::setX(float new_x) { + x = new_x; + cartesianToSpherical(); } -float theta(float x, float y, float z) { - float ll = x * x + y * y + z * z; - float l = sqrt(ll); - float theta = acos(z / l); - return theta; +void State::setY(float new_y) { + y = new_y; + cartesianToSpherical(); } -void connect_callback(uint16_t conn_handle) -{ - // Get the reference to current connection - BLEConnection* connection = Bluefruit.Connection(conn_handle); +void State::setZ(float new_z) { + z = new_z; + cartesianToSpherical(); +} - char central_name[32] = { 0 }; - connection->getPeerName(central_name, sizeof(central_name)); +void State::setXYZ(float new_x, float new_y, float new_z) { + x = new_x; + y = new_y; + z = new_z; + cartesianToSpherical(); +} - Serial.print("Connected to "); // TODO take care of cases where Serial is not available - Serial.println(central_name); +void State::setTheta(float new_theta) { + theta = new_theta; + sphericalToCartesian(); +} + +void State::setPhi(float new_phi) { + phi = new_phi; + sphericalToCartesian(); +} + +void State::setThetaPhi(float new_theta, float new_phi) { + theta = new_theta; + phi = new_phi; + sphericalToCartesian(); +} + +float State::getX() const { + return x; +} + +float State::getY() const { + return y; +} + +float State::getZ() const { + return z; +} + +float State::getTheta() const { + return theta; +} + +float State::getPhi() const { + return phi; +} + +void State::printState() { + Serial.print("Cartesian: x = "); Serial.print(x); + Serial.print(", y = "); Serial.print(y); + Serial.print(", z = "); Serial.println(z); + + Serial.print("Spherical: theta = "); Serial.print(theta); + Serial.print(", phi = "); Serial.println(phi); +} + +void State::Xgate() { + theta = fmod(theta + 180.0, 360.0); + sphericalToCartesian(); } namespace Qbead { @@ -136,7 +238,6 @@ class Qbead { LSM6DS3 imu; Adafruit_NeoPixel pixels; - BLEService bleservice; BLECharacteristic blecharcol; BLECharacteristic blecharsph; @@ -150,20 +251,18 @@ class Qbead { const uint8_t ix, iy, iz; const bool sx, sy, sz; float rbuffer[3]; - float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g - float t_acc, p_acc; // theta and phi according to gravity + float rx, ry, rz; // filtered and raw acc, in units of g float T_imu; // last update from the IMU - float t_ble, p_ble; // theta and phi - uint32_t c_ble; // color + State state; static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { - singletoninstance->c_ble = (data[2] << 16) | (data[1] << 8) | data[0]; + singletoninstance->state.setXYZ((data[0] / 255.0) * 2 - 1, (data[1] / 255.0) * 2 - 1, (data[2] / 255.0) * 2 - 1); } - static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len){ - singletoninstance->t_ble = data[0]*180/255; - singletoninstance->p_ble = data[1]*360/255; + static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { + singletoninstance->state.setTheta(data[0] * 180 / 255); + singletoninstance->state.setPhi(data[1] * 360 / 255); } void begin() { @@ -175,6 +274,8 @@ class Qbead { clear(); setBrightness(10); + state.setXYZ(0, 0, 1); // Ensure the state starts pointing along the z-axis + Serial.println("qbead on XIAO BLE Sense + LSM6DS3 compiled on " __DATE__ " at " __TIME__); if (!imu.begin()) { Serial.println("IMU error"); @@ -203,9 +304,9 @@ class Qbead { blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY); blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN); blecharacc.setUserDescriptor("xyz acceleration"); - blecharacc.setFixedLen(3*sizeof(float)); + blecharacc.setFixedLen(3 * sizeof(float)); blecharacc.begin(); - blecharacc.write(zerobuffer20, 3*sizeof(float)); + blecharacc.write(zerobuffer20, 3 * sizeof(float)); startBLEadv(); } @@ -221,13 +322,13 @@ class Qbead { leg = nlegs - leg; // invert direction for the phi angle, because the PCB is set up as a left-handed coordinate system leg = leg % nlegs; if (leg == 0) { - pixels.setPixelColor(pixel, color); + pixels.setPixelColor(pixel, color); } else if (pixel == 0) { - pixels.setPixelColor(0, color); + pixels.setPixelColor(0, color); } else if (pixel == 6) { - pixels.setPixelColor(6, color); + pixels.setPixelColor(6, color); } else { - pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color); + pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color); } } @@ -235,17 +336,54 @@ class Qbead { pixels.setBrightness(b); } + void readIMU() { + rbuffer[0] = imu.readFloatAccelX(); + rbuffer[1] = imu.readFloatAccelY(); + rbuffer[2] = imu.readFloatAccelZ(); + rx = (1 - 2 * sx) * rbuffer[ix]; + ry = (1 - 2 * sy) * rbuffer[iy]; + rz = (1 - 2 * sz) * rbuffer[iz]; + + float T_new = micros(); + float delta = T_new - T_imu; + T_imu = T_new; + const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable + if (delta > 100000) { + state.setXYZ(rx, ry, rz); + } else { + float d = delta / T; + float newX = d * rx + (1 - d) * state.getX(); + float newY = d * ry + (1 - d) * state.getY(); + float newZ = d * rz + (1 - d) * state.getZ(); + state.setXYZ(newX, newY, newZ); + } + + state.printState(); + + rbuffer[0] = state.getX(); + rbuffer[1] = state.getY(); + rbuffer[2] = state.getZ(); + blecharacc.write(rbuffer, 3*sizeof(float)); + for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) + { + if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) ) + { + blecharacc.notify(rbuffer, 3*sizeof(float)); + } + } + } + void setBloch_deg(float theta, float phi, uint32_t color) { if (theta < 0 || theta > 180 || phi < 0 || phi > 360) { return; } - float theta_section = theta / theta_quant; + float theta_section = state.getTheta() / theta_quant; if (theta_section < 0.5) { setLegPixelColor(0, 0, color); } else if (theta_section > nsections - 0.5) { setLegPixelColor(0, nsections, color); } else { - float phi_leg = phi / phi_quant; + float phi_leg = state.getPhi() / phi_quant; int theta_int = theta_section + 0.5; theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range int phi_int = phi_leg + 0.5; @@ -261,9 +399,9 @@ class Qbead { float theta_section = theta / theta_quant; float phi_leg = phi / phi_quant; int theta_int = theta_section + 0.5; - theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range + theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range int phi_int = phi_leg + 0.5; - phi_int = phi_int > nlegs - 1 ? 0 : phi_int; + phi_int = phi_int % nlegs; float p = (theta_section - theta_int); int theta_direction = sign(p); @@ -273,63 +411,11 @@ class Qbead { q = q * q; uint8_t rc = redch(c); - uint8_t bc = bluech(c); uint8_t gc = greench(c); + uint8_t bc = bluech(c); - setLegPixelColor(phi_int, theta_int, color(q * rc, q * bc, q * gc)); - setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * bc, p * gc)); - } - - void readIMU() { - rbuffer[0] = imu.readFloatAccelX(); - rbuffer[1] = imu.readFloatAccelY(); - rbuffer[2] = imu.readFloatAccelZ(); - rx = (1-2*sx)*rbuffer[ix]; - ry = (1-2*sy)*rbuffer[iy]; - rz = (1-2*sz)*rbuffer[iz]; - - float T_new = micros(); - float delta = T_new - T_imu; - T_imu = T_new; - const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable - if (delta > 100000) { - x = rx; - y = ry; - z = rz; - } else { - float d = delta/T; - x = d*rx+(1-d)*x; - y = d*ry+(1-d)*y; - z = d*rz+(1-d)*z; - } - - t_acc = theta(x, y, z)*180/3.14159; - p_acc = phi(x, y, z)*180/3.14159; - if (p_acc<0) {p_acc+=360;}// to bring it to [0,360] range - - Serial.print(x); - Serial.print("\t"); - Serial.print(y); - Serial.print("\t"); - Serial.print(z); - Serial.print("\t-1\t1\t"); - Serial.print(t_acc); - Serial.print("\t"); - Serial.print(p_acc); - Serial.print("\t-360\t360\t"); - Serial.println(); - - rbuffer[0] = x; - rbuffer[1] = y; - rbuffer[2] = z; - blecharacc.write(rbuffer, 3*sizeof(float)); - for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) - { - if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) ) - { - blecharacc.notify(rbuffer, 3*sizeof(float)); - } - } + setLegPixelColor(phi_int, theta_int, color(q * rc, q * gc, q * bc)); + setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * gc, p * bc)); } void startBLEadv(void) @@ -366,4 +452,4 @@ Qbead *Qbead::singletoninstance = nullptr; } // end namespace -#endif // QBEAD_H +#endif // QBEAD_H \ No newline at end of file From 78ed02f2965bc3182b1d8fcf2f7fb7caa87c99f5 Mon Sep 17 00:00:00 2001 From: nynzzz <91736289+nynzzz@users.noreply.github.com> Date: Mon, 14 Oct 2024 00:27:52 +0200 Subject: [PATCH 2/3] Minor changes to .ino files to make sure they compile and work properly --- examples/BLE_reader/BLE_reader.ino | 2 +- examples/IMU_reader/IMU_reader.ino | 3 ++- src/Qbead.h | 30 ++++++++++++++++++++---------- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/examples/BLE_reader/BLE_reader.ino b/examples/BLE_reader/BLE_reader.ino index 055191a..7d27294 100644 --- a/examples/BLE_reader/BLE_reader.ino +++ b/examples/BLE_reader/BLE_reader.ino @@ -24,6 +24,6 @@ void setup() { void loop() { bead.clear(); - bead.setBloch_deg_smooth(bead.t_ble, bead.p_ble, bead.c_ble); + bead.setBloch_deg_smooth(bead.state.getTheta(), bead.state.getPhi(), bead.c_ble); bead.show(); } \ No newline at end of file diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino index 8fa517e..3d68615 100644 --- a/examples/IMU_reader/IMU_reader.ino +++ b/examples/IMU_reader/IMU_reader.ino @@ -25,6 +25,7 @@ void setup() { void loop() { bead.readIMU(); bead.clear(); - bead.setBloch_deg_smooth(bead.t_acc, bead.p_acc, color(255, 0, 255)); + bead.setBloch_deg_smooth(bead.state.getTheta(), bead.state.getPhi(), color(255, 0, 255)); bead.show(); + delay(5000); } \ No newline at end of file diff --git a/src/Qbead.h b/src/Qbead.h index f40e275..9ddf546 100644 --- a/src/Qbead.h +++ b/src/Qbead.h @@ -72,6 +72,17 @@ float sign(float x) { else return -1; } +void connect_callback(uint16_t conn_handle){ + // Get the reference to current connection + BLEConnection* connection = Bluefruit.Connection(conn_handle); + + char central_name[32] = { 0 }; + connection->getPeerName(central_name, sizeof(central_name)); + + Serial.print("Connected to "); // TODO take care of cases where Serial is not available + Serial.println(central_name); +} + // State Class Definition class State { private: @@ -103,7 +114,6 @@ class State { float getPhi() const; void printState(); - void Xgate(); }; State::State() : x(0), y(0), z(1) { @@ -200,11 +210,6 @@ void State::printState() { Serial.print(", phi = "); Serial.println(phi); } -void State::Xgate() { - theta = fmod(theta + 180.0, 360.0); - sphericalToCartesian(); -} - namespace Qbead { class Qbead { @@ -238,6 +243,7 @@ class Qbead { LSM6DS3 imu; Adafruit_NeoPixel pixels; + BLEService bleservice; BLECharacteristic blecharcol; BLECharacteristic blecharsph; @@ -256,8 +262,10 @@ class Qbead { State state; + uint32_t c_ble; // color + static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { - singletoninstance->state.setXYZ((data[0] / 255.0) * 2 - 1, (data[1] / 255.0) * 2 - 1, (data[2] / 255.0) * 2 - 1); + singletoninstance->c_ble = (data[2] << 16) | (data[1] << 8) | data[0]; } static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { @@ -277,10 +285,12 @@ class Qbead { state.setXYZ(0, 0, 1); // Ensure the state starts pointing along the z-axis Serial.println("qbead on XIAO BLE Sense + LSM6DS3 compiled on " __DATE__ " at " __TIME__); - if (!imu.begin()) { - Serial.println("IMU error"); + uint16_t imuResult = imu.begin(); + if (imuResult != 0) { + Serial.print("IMU error: "); + Serial.println(imuResult); } else { - Serial.println("IMU OK"); + Serial.println("IMU OK"); } Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0); From 23bc60c41db115d9f378c61604d27f5acf7efeaa Mon Sep 17 00:00:00 2001 From: nynzzz <91736289+nynzzz@users.noreply.github.com> Date: Tue, 15 Oct 2024 19:08:36 +0200 Subject: [PATCH 3/3] Changes as per last feedback --- examples/BLE_reader/BLE_reader.ino | 1 + examples/IMU_reader/IMU_reader.ino | 1 + src/Qbead.h | 303 +++++++++++++++-------------- 3 files changed, 160 insertions(+), 145 deletions(-) diff --git a/examples/BLE_reader/BLE_reader.ino b/examples/BLE_reader/BLE_reader.ino index 7d27294..ba75b8b 100644 --- a/examples/BLE_reader/BLE_reader.ino +++ b/examples/BLE_reader/BLE_reader.ino @@ -23,6 +23,7 @@ void setup() { } void loop() { + bead.state.setThetaPhi(bead.t_ble, bead.p_ble); bead.clear(); bead.setBloch_deg_smooth(bead.state.getTheta(), bead.state.getPhi(), bead.c_ble); bead.show(); diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino index 3d68615..babae84 100644 --- a/examples/IMU_reader/IMU_reader.ino +++ b/examples/IMU_reader/IMU_reader.ino @@ -24,6 +24,7 @@ void setup() { void loop() { bead.readIMU(); + bead.state.setXYZ(bead.x, bead.y, bead.z); bead.clear(); bead.setBloch_deg_smooth(bead.state.getTheta(), bead.state.getPhi(), color(255, 0, 255)); bead.show(); diff --git a/src/Qbead.h b/src/Qbead.h index 9ddf546..568d052 100644 --- a/src/Qbead.h +++ b/src/Qbead.h @@ -72,6 +72,23 @@ float sign(float x) { else return -1; } +// z = cos(t) +// x = cos(p)sin(t) +// y = sin(p)sin(t) +float phi(float x, float y, float z) { + float ll = x * x + y * y + z * z; + float l = sqrt(ll); + float phi = atan2(y, x); + return phi; +} + +float theta(float x, float y, float z) { + float ll = x * x + y * y + z * z; + float l = sqrt(ll); + float theta = acos(z / l); + return theta; +} + void connect_callback(uint16_t conn_handle){ // Get the reference to current connection BLEConnection* connection = Bluefruit.Connection(conn_handle); @@ -89,126 +106,104 @@ class State { float x, y, z; // Cartesian coordinates float theta, phi; // Spherical coordinates - void cartesianToSpherical(); - void sphericalToCartesian(); - -public: - State(); - State(float x_init, float y_init, float z_init); - State(float theta_init, float phi_init); - - void setX(float new_x); - void setY(float new_y); - void setZ(float new_z); - void setXYZ(float new_x, float new_y, float new_z); - - void setTheta(float new_theta); - void setPhi(float new_phi); - void setThetaPhi(float new_theta, float new_phi); - - float getX() const; - float getY() const; - float getZ() const; - - float getTheta() const; - float getPhi() const; - - void printState(); -}; - -State::State() : x(0), y(0), z(1) { - cartesianToSpherical(); -} + void cartesianToSpherical() { + float l = sqrt(x * x + y * y + z * z); + if (l == 0) l = 1; + theta = acos(z / l) * 180 / PI; + phi = atan2(y, x) * 180 / PI; + if (phi < 0) phi += 360; + } -State::State(float x_init, float y_init, float z_init) : x(x_init), y(y_init), z(z_init) { - cartesianToSpherical(); -} + void sphericalToCartesian() { + float r = 1; + x = r * sin(theta * PI / 180.0) * cos(phi * PI / 180.0); + y = r * sin(theta * PI / 180.0) * sin(phi * PI / 180.0); + z = r * cos(theta * PI / 180.0); + } -State::State(float theta_init, float phi_init) : theta(theta_init), phi(phi_init) { - sphericalToCartesian(); -} +public: + // Constructors + State() : x(0), y(0), z(1) { + cartesianToSpherical(); + } -void State::cartesianToSpherical() { - float l = sqrt(x * x + y * y + z * z); - if (l == 0) l = 1; - theta = acos(z / l) * 180 / PI; - phi = atan2(y, x) * 180 / PI; - if (phi < 0) phi += 360; -} + State(float x_init, float y_init, float z_init) : x(x_init), y(y_init), z(z_init) { + cartesianToSpherical(); + } -void State::sphericalToCartesian() { - float r = 1; - x = r * sin(theta * PI / 180.0) * cos(phi * PI / 180.0); - y = r * sin(theta * PI / 180.0) * sin(phi * PI / 180.0); - z = r * cos(theta * PI / 180.0); -} + State(float theta_init, float phi_init) : theta(theta_init), phi(phi_init) { + sphericalToCartesian(); + } -void State::setX(float new_x) { - x = new_x; - cartesianToSpherical(); -} + // Setters and getters + void setX(float new_x) { + x = new_x; + cartesianToSpherical(); + } -void State::setY(float new_y) { - y = new_y; - cartesianToSpherical(); -} + void setY(float new_y) { + y = new_y; + cartesianToSpherical(); + } -void State::setZ(float new_z) { - z = new_z; - cartesianToSpherical(); -} + void setZ(float new_z) { + z = new_z; + cartesianToSpherical(); + } -void State::setXYZ(float new_x, float new_y, float new_z) { - x = new_x; - y = new_y; - z = new_z; - cartesianToSpherical(); -} + void setXYZ(float new_x, float new_y, float new_z) { + x = new_x; + y = new_y; + z = new_z; + cartesianToSpherical(); + } -void State::setTheta(float new_theta) { - theta = new_theta; - sphericalToCartesian(); -} + void setTheta(float new_theta) { + theta = new_theta; + sphericalToCartesian(); + } -void State::setPhi(float new_phi) { - phi = new_phi; - sphericalToCartesian(); -} + void setPhi(float new_phi) { + phi = new_phi; + sphericalToCartesian(); + } -void State::setThetaPhi(float new_theta, float new_phi) { - theta = new_theta; - phi = new_phi; - sphericalToCartesian(); -} + void setThetaPhi(float new_theta, float new_phi) { + theta = new_theta; + phi = new_phi; + sphericalToCartesian(); + } -float State::getX() const { - return x; -} + float getX() const { + return x; + } -float State::getY() const { - return y; -} + float getY() const { + return y; + } -float State::getZ() const { - return z; -} + float getZ() const { + return z; + } -float State::getTheta() const { - return theta; -} + float getTheta() const { + return theta; + } -float State::getPhi() const { - return phi; -} + float getPhi() const { + return phi; + } -void State::printState() { - Serial.print("Cartesian: x = "); Serial.print(x); - Serial.print(", y = "); Serial.print(y); - Serial.print(", z = "); Serial.println(z); + // Method to print the state + void printState() { + Serial.print("Cartesian: x = "); Serial.print(x); + Serial.print(", y = "); Serial.print(y); + Serial.print(", z = "); Serial.println(z); - Serial.print("Spherical: theta = "); Serial.print(theta); - Serial.print(", phi = "); Serial.println(phi); -} + Serial.print("Spherical: theta = "); Serial.print(theta); + Serial.print(", phi = "); Serial.println(phi); + } +}; namespace Qbead { @@ -257,8 +252,11 @@ class Qbead { const uint8_t ix, iy, iz; const bool sx, sy, sz; float rbuffer[3]; - float rx, ry, rz; // filtered and raw acc, in units of g - float T_imu; // last update from the IMU + float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g + float t_acc, p_acc; // theta and phi according to gravity + float T_imu; // last update from the IMU + + float t_ble, p_ble; // BLE theta and phi State state; @@ -268,9 +266,9 @@ class Qbead { singletoninstance->c_ble = (data[2] << 16) | (data[1] << 8) | data[0]; } - static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { - singletoninstance->state.setTheta(data[0] * 180 / 255); - singletoninstance->state.setPhi(data[1] * 360 / 255); + static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len){ + singletoninstance->t_ble = data[0]*180/255; + singletoninstance->p_ble = data[1]*360/255; } void begin() { @@ -346,43 +344,6 @@ class Qbead { pixels.setBrightness(b); } - void readIMU() { - rbuffer[0] = imu.readFloatAccelX(); - rbuffer[1] = imu.readFloatAccelY(); - rbuffer[2] = imu.readFloatAccelZ(); - rx = (1 - 2 * sx) * rbuffer[ix]; - ry = (1 - 2 * sy) * rbuffer[iy]; - rz = (1 - 2 * sz) * rbuffer[iz]; - - float T_new = micros(); - float delta = T_new - T_imu; - T_imu = T_new; - const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable - if (delta > 100000) { - state.setXYZ(rx, ry, rz); - } else { - float d = delta / T; - float newX = d * rx + (1 - d) * state.getX(); - float newY = d * ry + (1 - d) * state.getY(); - float newZ = d * rz + (1 - d) * state.getZ(); - state.setXYZ(newX, newY, newZ); - } - - state.printState(); - - rbuffer[0] = state.getX(); - rbuffer[1] = state.getY(); - rbuffer[2] = state.getZ(); - blecharacc.write(rbuffer, 3*sizeof(float)); - for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) - { - if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) ) - { - blecharacc.notify(rbuffer, 3*sizeof(float)); - } - } - } - void setBloch_deg(float theta, float phi, uint32_t color) { if (theta < 0 || theta > 180 || phi < 0 || phi > 360) { return; @@ -397,7 +358,7 @@ class Qbead { int theta_int = theta_section + 0.5; theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range int phi_int = phi_leg + 0.5; - phi_int = phi_int > nlegs - 1 ? 0 : phi_int; + phi_int = phi_int % nlegs; setLegPixelColor(phi_int, theta_int, color); } } @@ -428,6 +389,58 @@ class Qbead { setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * gc, p * bc)); } + void readIMU() { + rbuffer[0] = imu.readFloatAccelX(); + rbuffer[1] = imu.readFloatAccelY(); + rbuffer[2] = imu.readFloatAccelZ(); + rx = (1-2*sx)*rbuffer[ix]; + ry = (1-2*sy)*rbuffer[iy]; + rz = (1-2*sz)*rbuffer[iz]; + + float T_new = micros(); + float delta = T_new - T_imu; + T_imu = T_new; + const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable + if (delta > 100000) { + x = rx; + y = ry; + z = rz; + } else { + float d = delta/T; + x = d*rx+(1-d)*x; + y = d*ry+(1-d)*y; + z = d*rz+(1-d)*z; + } + + t_acc = theta(x, y, z)*180/3.14159; + p_acc = phi(x, y, z)*180/3.14159; + if (p_acc<0) {p_acc+=360;}// to bring it to [0,360] range + + Serial.print(x); + Serial.print("\t"); + Serial.print(y); + Serial.print("\t"); + Serial.print(z); + Serial.print("\t-1\t1\t"); + Serial.print(t_acc); + Serial.print("\t"); + Serial.print(p_acc); + Serial.print("\t-360\t360\t"); + Serial.println(); + + rbuffer[0] = x; + rbuffer[1] = y; + rbuffer[2] = z; + blecharacc.write(rbuffer, 3*sizeof(float)); + for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) + { + if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) ) + { + blecharacc.notify(rbuffer, 3*sizeof(float)); + } + } + } + void startBLEadv(void) { // Advertising packet