diff --git a/Makefile b/Makefile index 0786d2c..cdd9ef3 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,9 @@ # The name of the LF application inside "./src" to build/run/flash etc. -LF_MAIN ?= HelloUc +LF_MAIN ?= Sensor + +# so i2c and printf support for floats is compiled into the riot kernel +USEMODULE += periph_i2c +USEMODULE += printf_float # Increase the default stack-size CFLAGS += -DTHREAD_STACKSIZE_MAIN=4096 diff --git a/reference/Sensor.lf b/reference/Sensor.lf index 9df56c8..3eec40f 100644 --- a/reference/Sensor.lf +++ b/reference/Sensor.lf @@ -25,9 +25,7 @@ main reactor { reaction(lsm6ds33.orientation) {= float total_angle_x = lsm6ds33.orientation->value.x; printf("total angle x: %.6f\n", total_angle_x); - uint32_t time_speed_up = (ORIENTATION_TO_TIME / (total_angle_x + OFFSET_ANGLE)); - printf("time delay: %i\n", time_speed_up); - self->blinking_speed = MSEC(time_speed_up); + self->blinking_speed = MSEC((int)(total_angle_x + 10) * 100); =} reaction (blink) -> led.toggle, blink {= diff --git a/src/HelloUc.lf b/src/HelloUc.lf index cd9b54d..8a74083 100644 --- a/src/HelloUc.lf +++ b/src/HelloUc.lf @@ -6,10 +6,8 @@ main reactor { led = new LedController(); timer t(10msec, 500msec) - - /* - ========================= TODO =========================== - Here use the led.state_state to initialize the LED and the another - reaction to toggle the LED every periodically. - */ + + reaction(t) -> led.toggle {= + lf_set(led.toggle, 0); + =} } diff --git a/src/LSM6DS33.lf b/src/LSM6DS33.lf index a2fa11c..f9c0a65 100644 --- a/src/LSM6DS33.lf +++ b/src/LSM6DS33.lf @@ -13,7 +13,12 @@ typedef struct Vector Vector; /* I2C Configuration */ #define I2C_BUS I2C_DEV(0) // Usually I2C_DEV(0) in RIOT -#define SENSOR_ADDR 0x6A // SA0 pulled to GND + +// Sadly, three version of the sensor have different addresses: DS33/DS3/DSOX +#define SENSOR_ADDR1 0x69 +#define SENSOR_ADDR2 0x6A +#define SENSOR_ADDR3 0x6C + #define I2C_FLAG_NONE 0 /* LSM6DS33 Register Map */ @@ -45,6 +50,8 @@ reactor LSM6DS33 { state counter: int; + state sensor_addr: uint8_t; + reaction(startup) {= printf("initializing sensor\n"); uint8_t whoami; @@ -57,45 +64,57 @@ reactor LSM6DS33 { i2c_acquire(I2C_BUS); // 2. Verify connection (WHO_AM_I) - if (i2c_read_reg(I2C_BUS, SENSOR_ADDR, WHO_AM_I, &whoami, I2C_FLAG_NONE) < 0) { - i2c_release(I2C_BUS); - printf("failure releasing i2c\n"); - return; + uint8_t addrs[] = {0x6A, 0x6B}; + int found = 0; + + for (int i = 0; i < 2; i++) { + if (i2c_read_reg(I2C_BUS, addrs[i], WHO_AM_I, &whoami, I2C_FLAG_NONE) >= 0) { + if (whoami == SENSOR_ADDR1 || whoami == SENSOR_ADDR2 || whoami == SENSOR_ADDR2) { // DS33/DS3/DSOX variants + self->sensor_addr = addrs[i]; + found = 1; + break; + } + } } - if (whoami != 0x69) { + if (!found) { i2c_release(I2C_BUS); - printf("failure wrong device\n"); + printf("failure: device not found\n"); return; // Wrong device } // 3. Software Reset - i2c_write_reg(I2C_BUS, SENSOR_ADDR, CTRL3_C, 0x05, I2C_FLAG_NONE); + i2c_write_reg(I2C_BUS, self->sensor_addr, CTRL3_C, 0x05, I2C_FLAG_NONE); // 4. Configure Accel (208 Hz, +/- 2g) - i2c_write_reg(I2C_BUS, SENSOR_ADDR, CTRL1_XL, 0x40, I2C_FLAG_NONE); + i2c_write_reg(I2C_BUS, self->sensor_addr, CTRL1_XL, 0x40, I2C_FLAG_NONE); // 5. Configure Gyro (208 Hz, 245 dps) - i2c_write_reg(I2C_BUS, SENSOR_ADDR, CTRL2_G, 0x50, I2C_FLAG_NONE); + i2c_write_reg(I2C_BUS, self->sensor_addr, CTRL2_G, 0x50, I2C_FLAG_NONE); // 6. Enable Block Data Update and Auto-increment (IF_INC) - i2c_write_reg(I2C_BUS, SENSOR_ADDR, CTRL3_C, 0x44, I2C_FLAG_NONE); + i2c_write_reg(I2C_BUS, self->sensor_addr, CTRL3_C, 0x44, I2C_FLAG_NONE); i2c_release(I2C_BUS); - printf("successfull sensor initialization\n"); + printf("successful sensor initialization\n"); =} reaction(t) -> orientation {= uint8_t buf[6]; int16_t raw_x, raw_y, raw_z; - /* ========================= TODO =========================== Here check the RIOT manual and acquire the I2C bus, read the 6 gyro data bytes, and finally release the bus */ - + i2c_acquire(I2C_BUS); + if (i2c_read_regs(I2C_BUS, self->sensor_addr, GYRO_OUTX_L, buf, 6, I2C_FLAG_NONE) < 0) { + i2c_release(I2C_BUS); + printf("failed to read gyro data\n"); + return; + } + i2c_release(I2C_BUS); // Reconstruct 16-bit signed integers (Little Endian) raw_x = (int16_t)(buf[0] | (buf[1] << 8)); @@ -133,8 +152,8 @@ reactor LSM6DS33 { self->angle_y += gy * DT; self->angle_z += gz * DT; - printf("diff: %.6f, %.6f, %.6f\n", gx * DT, gy * DT, gz * DT); - printf("total: %.6f, %.6f, %.6f\n", self->angle_x, self->angle_y, self->angle_z); + // printf("\rdiff: %.6f, %.6f, %.6f", gx * DT, gy * DT, gz * DT); + // printf("\ttotal: %.6f, %.6f, %.6f", self->angle_x, self->angle_y, self->angle_z); struct Vector vec; vec.x = self->angle_x; diff --git a/src/LedController.lf b/src/LedController.lf index 7f94b28..1e8fe79 100644 --- a/src/LedController.lf +++ b/src/LedController.lf @@ -8,6 +8,17 @@ reactor LedController { input toggle: int; input set_state: bool; + reaction(toggle) {= + LED0_TOGGLE; + =} + + reaction(set_state) {= + if (set_state->value) { + LED0_ON; + } else { + LED0_OFF; + } + =} /* ========================= TODO =========================== Implement the reactions to toggle and set_state. You can use the diff --git a/src/Sensor.lf b/src/Sensor.lf index 84f646c..48e1d90 100644 --- a/src/Sensor.lf +++ b/src/Sensor.lf @@ -17,7 +17,7 @@ main reactor { reaction(startup) -> led.set_state, blink {= lf_set(led.set_state, true); - self->blinking_speed = SEC(10); + self->blinking_speed = SEC(3); lf_schedule(blink, SEC(1), 0); =} @@ -28,7 +28,13 @@ main reactor { and OFFSET_ANGLE macros defined in the preamble to calculate the blinking speed. Finally, set the blinking_speed state variable to the new value. */ - + reaction(lsm6ds33.orientation) {= + float total_angle_x = lsm6ds33.orientation->value.x; + printf("total angle x: %.6f\n", total_angle_x); + // float period = (4 * 3.14159f * 1000) / (total_angle_x + 4 * 3.1459f); + //printf("period: %.6f\n", period); + // self->blinking_speed = MSEC((int)period); + =} /* ========================= TODO =========================== @@ -37,4 +43,10 @@ main reactor { Optionally check if the blinking_speed is zero and set it to a default value to avoid rescheduling with zero delay. */ + + reaction(blink) -> led.toggle, blink {= + printf("blinking speed: " PRINTF_TIME "\n", self->blinking_speed); + lf_set(led.toggle, 0); + lf_schedule(blink, self->blinking_speed, 0); + =} }