diff --git a/arduino_lib/devices/m5stack_tof_unit.h b/arduino_lib/devices/m5stack_tof_unit.h new file mode 100644 index 00000000..45b2e853 --- /dev/null +++ b/arduino_lib/devices/m5stack_tof_unit.h @@ -0,0 +1,150 @@ +/* + * From https://github.com/m5stack/M5Stack/blob/master/examples/Unit/ToF_VL53L0X/ToF_VL53L0X.ino + */ + +#include +#if defined(M5STACK_FIRE) +#include +#elif defined(M5STACK_CORE2) +#include +#endif + +#include + +#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0xc0 +#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0xc2 +#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50 +#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70 +#define VL53L0X_REG_SYSRANGE_START 0x00 +#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x13 +#define VL53L0X_REG_RESULT_RANGE_STATUS 0x14 +#define VL53L0X_I2C_address 0x29 // I2C VL53L0X_I2C_address + +byte gbuf[16]; + +uint16_t _bswap(byte b[]) { + // Big Endian unsigned short to little endian unsigned short + uint16_t val = ((b[0] << 8) & b[1]); + return val; +} + +uint16_t _makeuint16(int lsb, int msb) { + return ((msb & 0xFF) << 8) | (lsb & 0xFF); +} + +void _write_byte_data(byte data) { + Wire.beginTransmission(VL53L0X_I2C_address); + Wire.write(data); + Wire.endTransmission(); +} + +void _write_byte_data_at(byte reg, byte data) { + // write data word at VL53L0X_I2C_address and register + Wire.beginTransmission(VL53L0X_I2C_address); + Wire.write(reg); + Wire.write(data); + Wire.endTransmission(); +} + +void _write_word_data_at(byte reg, uint16_t data) { + // write data word at VL53L0X_I2C_address and register + byte b0 = (data & 0xFF); + byte b1 = ((data >> 8) && 0xFF); + + Wire.beginTransmission(VL53L0X_I2C_address); + Wire.write(reg); + Wire.write(b0); + Wire.write(b1); + Wire.endTransmission(); +} + +byte _read_byte_data() { + Wire.requestFrom(VL53L0X_I2C_address, 1); + while (Wire.available() < 1) delay(1); + byte b = Wire.read(); + return b; +} + +byte _read_byte_data_at(byte reg) { + // _write_byte_data((byte)0x00); + _write_byte_data(reg); + Wire.requestFrom(VL53L0X_I2C_address, 1); + while (Wire.available() < 1) delay(1); + byte b = Wire.read(); + return b; +} + +uint16_t _read_word_data_at(byte reg) { + _write_byte_data(reg); + Wire.requestFrom(VL53L0X_I2C_address, 2); + while (Wire.available() < 2) delay(1); + gbuf[0] = Wire.read(); + gbuf[1] = Wire.read(); + return _bswap(gbuf); +} + +void _read_block_data_at(byte reg, int sz) { + int i = 0; + _write_byte_data(reg); + Wire.requestFrom(VL53L0X_I2C_address, sz); + for (i = 0; i < sz; i++) { + while (Wire.available() < 1) delay(1); + gbuf[i] = Wire.read(); + } +} + +uint16_t _VL53L0X_decode_vcsel_period(short vcsel_period_reg) { + // Converts the encoded VCSEL period register value into the real + // period in PLL clocks + uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1; + return vcsel_period_pclks; +} + +bool init_m5stack_tof_unit() { + Wire.begin(); + return true; +} + +std::optional> get_m5stack_tof_unit_data() { + _write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01); + + byte val = 0; + int cnt = 0; + while (cnt < 100) { // 1 second waiting time max + delay(10); + val = _read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS); + if (val & 0x01) break; + cnt++; + } + if (val & 0x01) { + Serial.println("ready"); + } else { + Serial.println("not ready"); + return std::nullopt; + } + _read_block_data_at(0x14, 12); + uint16_t acnt = _makeuint16(gbuf[7], gbuf[6]); + uint16_t scnt = _makeuint16(gbuf[9], gbuf[8]); + uint16_t dist = _makeuint16(gbuf[11], gbuf[10]); + byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3); + return std::make_tuple(acnt, scnt, dist, DeviceRangeStatusInternal); +} + +// void loop() { +// _read_block_data_at(0x14, 12); +// uint16_t acnt = _makeuint16(gbuf[7], gbuf[6]); +// uint16_t scnt = _makeuint16(gbuf[9], gbuf[8]); +// uint16_t dist = _makeuint16(gbuf[11], gbuf[10]); +// byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3); +// M5.Lcd.fillRect(0, 35, 319, 239, BLACK); +// M5.Lcd.setCursor(0, 35, 4); +// M5.Lcd.print("ambient count: "); +// M5.Lcd.println(acnt); +// M5.Lcd.print("signal count: "); +// M5.Lcd.println(scnt); +// M5.Lcd.print("distance: "); +// M5.Lcd.println(dist); +// M5.Lcd.print("status: "); +// M5.Lcd.println(DeviceRangeStatusInternal); +// delay(1000); +// } \ No newline at end of file diff --git a/arduino_lib/sdp/sdp.h b/arduino_lib/sdp/sdp.h index d51aeb2d..5e79591b 100644 --- a/arduino_lib/sdp/sdp.h +++ b/arduino_lib/sdp/sdp.h @@ -307,7 +307,6 @@ void _meta_frame_broadcast_task(void *parameter) { for (;;) { vTaskDelay(pdMS_TO_TICKS(1000)); if (_sdp_interface_data_callbacks.size() == 0) { - Serial.println("hoge"); _broadcast_sdp_meta_packet(std::make_tuple("", "")); } else { for (auto &entry : _sdp_interface_data_callbacks) { diff --git a/sketchbooks/sdp_elevator_status_broadcaster/include/elevator_status.h b/sketchbooks/sdp_elevator_status_broadcaster/include/elevator_status.h index 00219823..eedc1ff6 100644 --- a/sketchbooks/sdp_elevator_status_broadcaster/include/elevator_status.h +++ b/sketchbooks/sdp_elevator_status_broadcaster/include/elevator_status.h @@ -2,6 +2,7 @@ #include +#include #include typedef struct @@ -10,41 +11,66 @@ typedef struct float floor_height; } ElevatorConfig; +enum ElevatorMovingStatus { + HALT, + UP_ACCEL, + UP_STABLE, + UP_DECEL, + DOWN_ACCEL, + DOWN_STABLE, + DOWN_DECEL +}; + +void print_elevator_config_vector(LGFX_Sprite &sprite, const std::vector &elevator_config, int32_t push_x, int32_t push_y); + /** * Calculate floor number from altitude * * @param altitude Altitude in meters + * @param reference_altitude Reference altitude in meters + * @param reference_floor Reference floor number * @param elevator_config Elevator configuration - * @param threshold Threshold for floor detection * @return Floor number + */ +std::optional calc_floor(float altitude, float reference_altitude, int32_t reference_floor, const std::vector &elevator_config); + +/** + * Calculate acceleration on gravity direction + * + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @return Acceleration on gravity direction + */ +float calc_accel_on_gravity(float accels_x, float accels_y, float accels_z, float gravity_x, float gravity_y, float gravity_z); + +/** + * WIthdraw gravity from acceleration * - * C++ version of code below + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @return Acceleration without gravity + */ +std::tuple withdraw_gravity(float accels_x, float accels_y, float accels_z, float gravity_x, float gravity_y, float gravity_z); + +/** + * Calculate next step moving status from acceleration * - * altitude_diff_list = [ - { - 'floor': key, - 'altitude_diff': - (entry['altitude'] - self.elevator_config[self.param_anchor_floor]['altitude']) - (self.state_altitude - self.param_anchor_altitude) - } - for key, entry in self.elevator_config.items()] - nearest_entry = min( - altitude_diff_list, - key=lambda x: math.fabs(x['altitude_diff']) - ) - self.state_current_floor = nearest_entry['floor'] + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @param current_status Current moving status + * @param moving_threshold Threshold for moving detection + * @return Moving status */ -std::optional calc_floor(float altitude, const std::vector &elevator_config) { - std::vector> altitude_diff_list; - for (uint8_t i = 0; i < elevator_config.size(); i++) { - altitude_diff_list.push_back(std::make_pair( - i, - (elevator_config[i].floor_height - elevator_config[0].floor_height) - (altitude - elevator_config[0].floor_height))); - } - auto nearest_entry = std::min_element( - altitude_diff_list.begin(), - altitude_diff_list.end(), - [](const std::pair &a, const std::pair &b) { - return std::fabs(a.second) < std::fabs(b.second); - }); - return nearest_entry->first; // floor number -} \ No newline at end of file +ElevatorMovingStatus calc_moving_status(float accel_on_gravity, ElevatorMovingStatus current_status, float moving_threshold); \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_status_broadcaster/include/lcd.h b/sketchbooks/sdp_elevator_status_broadcaster/include/lcd.h new file mode 100644 index 00000000..1dce6259 --- /dev/null +++ b/sketchbooks/sdp_elevator_status_broadcaster/include/lcd.h @@ -0,0 +1,4 @@ +#pragma once + +void init_lcd(); +void print_status(); \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_status_broadcaster/include/utils.h b/sketchbooks/sdp_elevator_status_broadcaster/include/utils.h new file mode 100644 index 00000000..aa5163cf --- /dev/null +++ b/sketchbooks/sdp_elevator_status_broadcaster/include/utils.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +std::tuple calc_gravity_direction(std::vector &accels_x, std::vector &accels_y, std::vector &accels_z) { + float gravity_x = 0.0; + float gravity_y = 0.0; + float gravity_z = 0.0; + for (auto &a : accels_x) { + gravity_x += a; + } + for (auto &a : accels_y) { + gravity_y += a; + } + for (auto &a : accels_z) { + gravity_z += a; + } + gravity_x /= accels_x.size(); + gravity_y /= accels_y.size(); + gravity_z /= accels_z.size(); + return std::make_tuple(gravity_x, gravity_y, gravity_z); +} diff --git a/sketchbooks/sdp_elevator_status_broadcaster/src/elevator_status.cpp b/sketchbooks/sdp_elevator_status_broadcaster/src/elevator_status.cpp new file mode 100644 index 00000000..7e598124 --- /dev/null +++ b/sketchbooks/sdp_elevator_status_broadcaster/src/elevator_status.cpp @@ -0,0 +1,173 @@ +#include "elevator_status.h" + +void print_elevator_config_vector(LGFX_Sprite &sprite, const std::vector &elevator_config, int32_t push_x, int32_t push_y) { + sprite.fillScreen(0xFFFFFF); + sprite.setCursor(0, 0); + sprite.printf("Elevator Config\n"); + for (auto &entry : elevator_config) { + sprite.printf("Floor: %d, Height: %.2f\n", entry.floor_num, entry.floor_height); + } + sprite.pushSprite(push_x, push_y); +} + +/** + * Calculate floor number from altitude + * + * @param altitude Altitude in meters + * @param elevator_config Elevator configuration + * @param threshold Threshold for floor detection + * @return Floor number + * + * C++ version of code below + * + * altitude_diff_list = [ + { + 'floor': key, + 'altitude_diff': + (entry['altitude'] - self.elevator_config[self.param_anchor_floor]['altitude']) - (self.state_altitude - self.param_anchor_altitude) + } + for key, entry in self.elevator_config.items()] + nearest_entry = min( + altitude_diff_list, + key=lambda x: math.fabs(x['altitude_diff']) + ) + self.state_current_floor = nearest_entry['floor'] + */ +std::optional calc_floor(float altitude, float reference_altitude, int32_t reference_floor, const std::vector &elevator_config) { + if (elevator_config.empty()) { + return std::nullopt; + } + int32_t candidate_floor = elevator_config[0].floor_num; + int reference_floor_index = -100; + for (int i = 0; i < elevator_config.size(); i++) { + if (elevator_config[i].floor_num == reference_floor) { + reference_floor_index = i; + break; + } + } + if (reference_floor_index == -100) { + return std::nullopt; + } + float candidate_diff = fabs((elevator_config[0].floor_height - elevator_config[reference_floor_index].floor_height) - (altitude - reference_altitude)); + for (int i = 1; i < elevator_config.size(); i++) { + float diff = fabs((elevator_config[i].floor_height - elevator_config[reference_floor_index].floor_height) - (altitude - reference_altitude)); + if (diff < candidate_diff) { + candidate_diff = diff; + candidate_floor = elevator_config[i].floor_num; + } + } + return candidate_floor; +} + +/** + * Calculate acceleration on gravity direction + * + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @return Acceleration on gravity direction + */ +float calc_accel_on_gravity(float accels_x, float accels_y, float accels_z, float gravity_x, float gravity_y, float gravity_z) { + return (accels_x * gravity_x + accels_y * gravity_y + accels_z * gravity_z) / sqrt(gravity_x * gravity_x + gravity_y * gravity_y + gravity_z * gravity_z); +} + +/** + * WIthdraw gravity from acceleration + * + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @return Acceleration without gravity + */ +std::tuple withdraw_gravity(float accels_x, float accels_y, float accels_z, float gravity_x, float gravity_y, float gravity_z) { + float accels_x_without_gravity = accels_x - gravity_x; + float accels_y_without_gravity = accels_y - gravity_y; + float accels_z_without_gravity = accels_z - gravity_z; + return std::make_tuple(accels_x_without_gravity, accels_y_without_gravity, accels_z_without_gravity); +} + +/** + * Calculate next step moving status from acceleration + * + * @param accels_x Acceleration in x-axis + * @param accels_y Acceleration in y-axis + * @param accels_z Acceleration in z-axis + * @param gravity_x Gravity in x-axis + * @param gravity_y Gravity in y-axis + * @param gravity_z Gravity in z-axis + * @param current_status Current moving status + * @param moving_threshold Threshold for moving detection + * @return Moving status + */ +ElevatorMovingStatus calc_moving_status(float accel_on_gravity, ElevatorMovingStatus current_status, float moving_threshold) { + switch (current_status) { + case HALT: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } + break; + case UP_ACCEL: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return UP_STABLE; + } + break; + case UP_STABLE: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return UP_STABLE; + } + break; + case UP_DECEL: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return UP_STABLE; + } + break; + case DOWN_ACCEL: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return DOWN_STABLE; + } + break; + case DOWN_STABLE: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return DOWN_STABLE; + } + break; + case DOWN_DECEL: + if (accel_on_gravity > moving_threshold) { + return UP_ACCEL; + } else if (accel_on_gravity < -moving_threshold) { + return DOWN_ACCEL; + } else { + return DOWN_STABLE; + } + break; + } + return HALT; +} \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_status_broadcaster/src/lcd.cpp b/sketchbooks/sdp_elevator_status_broadcaster/src/lcd.cpp new file mode 100644 index 00000000..fd538bc6 --- /dev/null +++ b/sketchbooks/sdp_elevator_status_broadcaster/src/lcd.cpp @@ -0,0 +1,81 @@ +#if 1 // defined(M5STACK_CORE2) +#include +#include +#elif defined(M5STACK_FIRE) +#include +#include +#endif + +#include +#include + +#include "elevator_status.h" + +extern LGFX lcd; +extern LGFX_Sprite sprite_header; +extern LGFX_Sprite sprite_status; +extern LGFX_Sprite sprite_info; + +extern float sensor_accX; +extern float sensor_accY; +extern float sensor_accZ; +extern float sensor_gyroX; +extern float sensor_gyroY; +extern float sensor_gyroZ; +extern float sensor_pitch; +extern float sensor_roll; +extern float sensor_yaw; +extern float sensor_temp_mpu; +extern float sensor_temp_dps; +extern float sensor_pressure; + +extern float altitude; +extern int32_t current_floor; +extern ElevatorMovingStatus current_status; + +extern float gravity_x; +extern float gravity_y; +extern float gravity_z; + +extern float accel_x_without_gravity; +extern float accel_y_without_gravity; +extern float accel_z_without_gravity; +extern float accel_on_gravity; + +void init_lcd() { + // LCD + lcd.init(); + lcd.setRotation(1); + lcd.setBrightness(128); + lcd.setColorDepth(24); + lcd.fillScreen(0xFFFFFF); + + sprite_header.createSprite(lcd.width(), lcd.height() / 3); + sprite_status.createSprite(lcd.width(), lcd.height() / 3); + sprite_info.createSprite(lcd.width(), lcd.height() / 3); + + sprite_header.fillScreen(0xFFFFFF); + sprite_header.setTextColor(0x000000); + sprite_header.setTextSize(1.5, 1.5); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setTextColor(0x000000); + sprite_info.fillScreen(0xFFFFFF); + sprite_info.setTextColor(0x000000); +} + +void print_status() { + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + // sprite_status.printf("Acc: %.2f, %.2f, %.2f\n", sensor_accX, sensor_accY, sensor_accZ); + // sprite_status.printf("Gyro: %.2f, %.2f, %.2f\n", sensor_gyroX, sensor_gyroY, sensor_gyroZ); + // sprite_status.printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", sensor_pitch, sensor_roll, sensor_yaw); + sprite_status.printf("Pressure: %.2f\n", sensor_pressure); + sprite_status.printf("Temp: %.2f, %.2f\n", sensor_temp_mpu, sensor_temp_dps); + sprite_status.printf("Altitude: %.2f\n", altitude); + sprite_status.printf("Gravity: %.2f, %.2f, %.2f\n", gravity_x, gravity_y, gravity_z); + sprite_status.printf("Accel without gravity: %.2f, %.2f, %.2f\n", accel_x_without_gravity, accel_y_without_gravity, accel_z_without_gravity); + sprite_status.printf("Accel on gravity: %.2f\n", accel_on_gravity); + sprite_status.printf("Floor: %d\n", current_floor); + sprite_status.printf("Status: %d\n", current_status); + sprite_status.pushSprite(0, lcd.height() / 3); +} \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_status_broadcaster/src/main.cpp b/sketchbooks/sdp_elevator_status_broadcaster/src/main.cpp index c3d027c3..145b5392 100644 --- a/sketchbooks/sdp_elevator_status_broadcaster/src/main.cpp +++ b/sketchbooks/sdp_elevator_status_broadcaster/src/main.cpp @@ -13,14 +13,17 @@ #include #include "elevator_status.h" +#include "lcd.h" #include "sdp/sdp.h" #include "smart_device_protocol/Packet.h" +#include "utils.h" #include "utils/calc_altitude.h" #include "utils/config_loader.h" -static LGFX lcd; -static LGFX_Sprite sprite_header(&lcd); -static LGFX_Sprite sprite_status(&lcd); +LGFX lcd; +LGFX_Sprite sprite_header(&lcd); +LGFX_Sprite sprite_status(&lcd); +LGFX_Sprite sprite_info(&lcd); Dps310 Dps310PressureSensor = Dps310(); @@ -38,11 +41,21 @@ float sensor_temp_mpu = 0.0F; float sensor_temp_dps = 0; float sensor_pressure = 0; -/* */ +/* Gravity */ +float gravity_x = 0.0F; +float gravity_y = 0.0F; +float gravity_z = 0.0F; + +/* Acceleration */ +float accel_x_without_gravity = 0.0F; +float accel_y_without_gravity = 0.0F; +float accel_z_without_gravity = 0.0F; +float accel_on_gravity = 0.0F; /* Elevator status */ float altitude = 0.; int32_t current_floor = 0; +ElevatorMovingStatus current_status = HALT; /* device */ uint8_t device_mac_address[6]; @@ -50,6 +63,11 @@ String device_name; /* Elevator config */ std::vector elevator_config; +float moving_threshold; +float moving_status_timeout = 10.0; +float initial_altitude; +int32_t initial_floor; +long last_moving_stamp; bool load_config_from_FS(fs::FS &fs, const String &filename) { StaticJsonDocument<1024> doc; @@ -57,15 +75,29 @@ bool load_config_from_FS(fs::FS &fs, const String &filename) { return false; } - if (not doc.containsKey("device_name") or not doc.containsKey("elevator_config")) { + if (not doc.containsKey("device_name") or + not doc.containsKey("elevator_config") or + not doc.containsKey("moving_status_threshold")) { sprite_status.println("Invalid config file"); - sprite_status.println("device_name and elevator_config are required"); + sprite_status.println("device_name and elevator_config and moving_status_threshold are required"); sprite_status.pushSprite(0, lcd.height() / 3); return false; } + if (doc.containsKey("moving_status_timeout")) { + moving_status_timeout = doc["moving_status_timeout"].as(); + } + if (doc.containsKey("initial_floor")) { + current_floor = doc["initial_floor"].as(); + initial_floor = current_floor; + } else { + current_floor = 7; + initial_floor = 7; + } + device_name = doc["device_name"].as(); JsonArray elevator_config_json = doc["elevator_config"].as(); + moving_threshold = doc["moving_status_threshold"].as(); for (auto itr = elevator_config_json.begin(); itr != elevator_config_json.end(); ++itr) { JsonObject e = *itr; if (e.containsKey("floor_num") and e.containsKey("floor_height")) { @@ -78,24 +110,6 @@ bool load_config_from_FS(fs::FS &fs, const String &filename) { return true; } -void init_lcd() { - // LCD - lcd.init(); - lcd.setRotation(1); - lcd.setBrightness(128); - lcd.setColorDepth(24); - lcd.fillScreen(0xFFFFFF); - - sprite_header.createSprite(lcd.width(), lcd.height() / 3); - sprite_status.createSprite(lcd.width(), lcd.height() / 3); - - sprite_header.fillScreen(0xFFFFFF); - sprite_header.setTextColor(0x000000); - sprite_header.setTextSize(1.5, 1.5); - sprite_status.fillScreen(0xFFFFFF); - sprite_status.setTextColor(0x000000); -} - void measure_sensors() { M5.IMU.getGyroData(&sensor_gyroX, &sensor_gyroY, &sensor_gyroZ); M5.IMU.getAccelData(&sensor_accX, &sensor_accY, &sensor_accZ); @@ -105,32 +119,45 @@ void measure_sensors() { Dps310PressureSensor.measureTempOnce(sensor_temp_dps); } -void calc_elevator_status() { - altitude = calc_altitude(sensor_pressure, sensor_temp_dps); +void postprocess_sensors() { + auto accels_without_gravity = withdraw_gravity(sensor_accX, sensor_accY, sensor_accZ, gravity_x, gravity_y, gravity_z); + accel_x_without_gravity = std::get<0>(accels_without_gravity); + accel_y_without_gravity = std::get<1>(accels_without_gravity); + accel_z_without_gravity = std::get<2>(accels_without_gravity); + accel_on_gravity = calc_accel_on_gravity(accel_x_without_gravity, accel_y_without_gravity, accel_z_without_gravity, gravity_x, gravity_y, gravity_z); } -std::tuple calc_gravity_direction(std::vector &accels_x, std::vector &accels_y, std::vector &accels_z) { - float gravity_x = 0.0; - float gravity_y = 0.0; - float gravity_z = 0.0; - for (auto &a : accels_x) { - gravity_x += a; +void calc_elevator_status() { + altitude = calc_altitude(sensor_pressure, sensor_temp_dps); + if (fabs(accel_on_gravity) < 0.1) { + if (millis() - last_moving_stamp > moving_status_timeout * 1000) { + current_status = HALT; + initial_floor = current_floor; + initial_altitude = altitude; + last_moving_stamp = millis(); + sprite_info.printf("Initial floor updated: %d\n", initial_floor); + sprite_info.printf("Initial altitude updated: %.2f\n", initial_altitude); + sprite_info.pushSprite(0, lcd.height() / 3 * 2); + } + } else { + last_moving_stamp = millis(); } - for (auto &a : accels_y) { - gravity_y += a; + auto floor = calc_floor(altitude, initial_altitude, initial_floor, elevator_config); + Serial.printf("altitude: %.2f, floor: %d from initial_altitude: %.2f, initial_floor: %d\n", altitude, floor.has_value() ? floor.value() : -1, initial_altitude, initial_floor); + // print_elevator_config_vector_serial(Serial, elevator_config); + Serial.println("Elevator Config"); + for (const auto &entry : elevator_config) { + Serial.printf(" Floor: %d, Height: %.2f\n", entry.floor_num, entry.floor_height); } - for (auto &a : accels_z) { - gravity_z += a; + if (floor.has_value()) { + current_floor = floor.value(); } - gravity_x /= accels_x.size(); - gravity_y /= accels_y.size(); - gravity_z /= accels_z.size(); - return std::make_tuple(gravity_x, gravity_y, gravity_z); + current_status = calc_moving_status(accel_on_gravity, current_status, moving_threshold); } void setup() { // Device Initialization - M5.begin(false, false, true, true); + M5.begin(false, true, true, true); Serial.begin(115200); M5.IMU.Init(); Dps310PressureSensor.begin(Wire, 0x77); @@ -141,40 +168,72 @@ void setup() { Serial.println("LCD initialized"); // Load config - // SD.begin(); - // SPIFFS.begin(); - // if (not load_config_from_FS(SD, "/config.json")) { - // Serial.println("Failed to load config from SD"); - // if (not load_config_from_FS(SPIFFS, "/config.json")) { - // Serial.println("Failed to load config from SPIFFS"); - // while (true) { - // delay(1000); - // } - // } - // } + SD.begin(); + SPIFFS.begin(); + if (not load_config_from_FS(SD, "/config.json")) { + Serial.println("Failed to load config from SD"); + if (not load_config_from_FS(SPIFFS, "/config.json")) { + Serial.println("Failed to load config from SPIFFS"); + sprite_status.println("Failed to load config"); + while (true) { + delay(1000); + } + } + } // Initialize ESP-NOW - init_sdp(device_mac_address, String("elevator_status")); + init_sdp(device_mac_address, device_name); // Print sprite_header.println("SDP ELV. STAT."); + sprite_header.printf("device: %s\n", device_name.c_str()); sprite_header.printf("MAC ADDR: %02X:%02X:%02X:%02X:%02X:%02X\n", device_mac_address[0], device_mac_address[1], device_mac_address[2], device_mac_address[3], device_mac_address[4], device_mac_address[5]); sprite_header.pushSprite(0, 0); + + // Show elevator config + print_elevator_config_vector(sprite_info, elevator_config, 0, lcd.height() / 3 * 2); + + // Initialization + std::vector accels_x; + std::vector accels_y; + std::vector accels_z; + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Gravity calibration will start in 5 second\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + delay(5000); + measure_sensors(); + postprocess_sensors(); + altitude = calc_altitude(sensor_pressure, sensor_temp_dps); + initial_altitude = altitude; + Serial.printf("Initial altitude: %.2f, Initial floor: %d\n", initial_altitude, initial_floor); + time_t duration_sec = 5.0; + long deadline = millis() + (long)(duration_sec * 1000); + while (millis() < deadline) { + measure_sensors(); + accels_x.push_back(sensor_accX); + accels_y.push_back(sensor_accY); + accels_z.push_back(sensor_accZ); + float left_sec = (deadline - millis()) / 1000.0; + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Gravity calibration running...\n"); + sprite_status.printf("%.2f secs remaining\n", left_sec); + sprite_status.pushSprite(0, lcd.height() / 3); + } + std::tie(gravity_x, gravity_y, gravity_z) = calc_gravity_direction(accels_x, accels_y, accels_z); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Gravity calibration finished\n"); + sprite_status.printf("Gravity: %.2f, %.2f, %.2f\n", gravity_x, gravity_y, gravity_z); + delay(1000); } void loop() { measure_sensors(); + postprocess_sensors(); calc_elevator_status(); - sprite_status.fillScreen(0xFFFFFF); - sprite_status.setCursor(0, 0); - sprite_status.printf("Acc: %.2f, %.2f, %.2f\n", sensor_accX, sensor_accY, sensor_accZ); - sprite_status.printf("Gyro: %.2f, %.2f, %.2f\n", sensor_gyroX, sensor_gyroY, sensor_gyroZ); - sprite_status.printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", sensor_pitch, sensor_roll, sensor_yaw); - sprite_status.printf("Pressure: %.2f\n", sensor_pressure); - sprite_status.printf("Temp: %.2f, %.2f\n", sensor_temp_mpu, sensor_temp_dps); - sprite_status.printf("Altitude: %.2f\n", altitude); - sprite_status.pushSprite(0, lcd.height() / 3); - Serial.println("loop"); + print_status(); } diff --git a/sketchbooks/sdp_switchbot_elevator_button/include/lcd.h b/sketchbooks/sdp_switchbot_elevator_button/include/lcd.h new file mode 100644 index 00000000..42ff451e --- /dev/null +++ b/sketchbooks/sdp_switchbot_elevator_button/include/lcd.h @@ -0,0 +1,3 @@ +#pragma once + +void init_lcd(); \ No newline at end of file diff --git a/sketchbooks/sdp_switchbot_elevator_button/platformio.ini b/sketchbooks/sdp_switchbot_elevator_button/platformio.ini index 45a6105e..9e41c1f1 100644 --- a/sketchbooks/sdp_switchbot_elevator_button/platformio.ini +++ b/sketchbooks/sdp_switchbot_elevator_button/platformio.ini @@ -16,6 +16,7 @@ lib_ldf_mode = deep lib_deps = m5stack/M5Stack@^0.4.3 bblanchon/ArduinoJson @ ^6.21.3 + lovyan03/LovyanGFX @ ^1.1.5 build_flags = -DM5STACK_FIRE -std=gnu++17 @@ -35,6 +36,7 @@ lib_ldf_mode = deep lib_deps = m5stack/M5Core2 bblanchon/ArduinoJson @ ^6.21.3 + lovyan03/LovyanGFX @ ^1.1.5 build_flags = -DM5STACK_CORE2 -std=gnu++17 diff --git a/sketchbooks/sdp_switchbot_elevator_button/src/lcd.cpp b/sketchbooks/sdp_switchbot_elevator_button/src/lcd.cpp new file mode 100644 index 00000000..206baac6 --- /dev/null +++ b/sketchbooks/sdp_switchbot_elevator_button/src/lcd.cpp @@ -0,0 +1,40 @@ +#include "lcd.h" + +#if defined(M5STACK_FIRE) +#include + +#include "m5stack_utils/m5stack.h" +#elif defined(M5STACK_CORE2) +#include + +#include "m5stack_utils/m5core2.h" +#endif + +#include +#include + +extern LGFX lcd; +extern LGFX_Sprite sprite_header; +extern LGFX_Sprite sprite_status; +extern LGFX_Sprite sprite_info; + +void init_lcd() { + // LCD + lcd.init(); + lcd.setRotation(1); + lcd.setBrightness(128); + lcd.setColorDepth(24); + lcd.fillScreen(0xFFFFFF); + + sprite_header.createSprite(lcd.width(), lcd.height() / 3); + sprite_status.createSprite(lcd.width(), lcd.height() / 3); + sprite_info.createSprite(lcd.width(), lcd.height() / 3); + + sprite_header.fillScreen(0xFFFFFF); + sprite_header.setTextColor(0x000000); + sprite_header.setTextSize(1.5, 1.5); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setTextColor(0x000000); + sprite_info.fillScreen(0xFFFFFF); + sprite_info.setTextColor(0x000000); +} \ No newline at end of file diff --git a/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp b/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp index 642490b8..5d5b9c6f 100644 --- a/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp +++ b/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp @@ -15,11 +15,22 @@ #include #include +#include +#include + +#include "devices/m5stack_tof_unit.h" #include "devices/uwb_module_util.h" #include "iot_com_util/iot_host_util.h" +#include "lcd.h" #include "sdp/sdp.h" #include "utils/config_loader.h" +// Display +LGFX lcd; +LGFX_Sprite sprite_header(&lcd); +LGFX_Sprite sprite_status(&lcd); +LGFX_Sprite sprite_info(&lcd); + // Device Name String device_name; @@ -27,10 +38,17 @@ String device_name; uint8_t mac_address[6] = {0}; // Interface +// Elevator Panel Control std::string packet_description_control = "Elevator call"; std::string serialization_format_control = "s"; SDPInterfaceDescription interface_description_control = std::make_tuple(packet_description_control, serialization_format_control); +// Door Open +std::string packet_description_door_open = "Door opening state"; +std::string serialization_format_door_open = "?"; +SDPInterfaceDescription interface_description_door_open = + std::make_tuple(packet_description_door_open, serialization_format_door_open); +std::vector body_door_open; // UWB int uwb_id = -1; @@ -46,8 +64,13 @@ String switchbot_secret = ""; String switchbot_upward_device_id = ""; String switchbot_downward_device_id = ""; +// Elevator Door Detection Parameters +uint16_t door_closed_distance = 300; +uint16_t door_closed_clearance = 100; + // Port configuration -M5StackSerialPortInfo port_info_m5atoms3 = M5StackSerialPortInfoList[PORT_A]; +// Port A is used for Wire +M5StackSerialPortInfo port_info_m5atoms3 = M5StackSerialPortInfoList[PORT_B]; M5StackSerialPortInfo port_info_uwb = M5StackSerialPortInfoList[PORT_C]; // Other @@ -71,6 +94,10 @@ bool load_config_from_FS(fs::FS& fs, String filename = "/config.json") { return false; } + if (doc.containsKey("door_closed_clearance")) { + door_closed_clearance = doc["door_closed_clearance"].as(); + } + device_name = doc["device_name"].as(); wifi_ssid = doc["wifi_ssid"].as(); wifi_password = doc["wifi_password"].as(); @@ -107,8 +134,10 @@ void callback_for_elevator_panel_control(const uint8_t* mac_address, const std:: void setup() { M5.begin(true, true, true, false); Serial.begin(115200); + // init_lcd(); - M5.Lcd.printf("SDP SWITCHBOT ELEVATOR PANEL\n"); + sprite_header.printf("SDP SWITCHBOT ELEVATOR PANEL\n"); + sprite_header.pushSprite(0, 0); // Load config from FS SPIFFS.begin(); @@ -116,12 +145,14 @@ void setup() { if (not load_config_from_FS(SD, "/config.json")) { if (not load_config_from_FS(SPIFFS, "/config.json")) { Serial.println("Failed to load config file"); - M5.lcd.printf("Failed to load config file\n"); + sprite_status.printf("Failed to load config file\n"); + sprite_status.pushSprite(0, lcd.height() / 3); while (true) { delay(1000); } } } + Serial.println("Config loaded!"); Serial1.begin(115200, SERIAL_8N1, port_info_uwb.rx, port_info_uwb.tx); Serial2.begin(115200, SERIAL_8N1, port_info_m5atoms3.rx, port_info_m5atoms3.tx); @@ -129,14 +160,16 @@ void setup() { // Initialization of SDP if (not init_sdp(mac_address, device_name.c_str())) { Serial.println("Failed to initialize SDP"); - M5.Lcd.printf("Failed to initialize SDP\n"); + sprite_status.printf("Failed to initialize SDP\n"); + sprite_status.pushSprite(0, lcd.height() / 3); while (true) { delay(1000); } } if (not register_sdp_interface_callback(interface_description_control, callback_for_elevator_panel_control)) { Serial.println("Failed to register callback for elevator panel control"); - M5.Lcd.printf("Failed to register callback for elevator panel control\n"); + sprite_status.printf("Failed to register callback for elevator panel control\n"); + sprite_status.pushSprite(0, lcd.height() / 3); while (true) { delay(1000); } @@ -144,11 +177,10 @@ void setup() { Serial.println("SDP Initialized!"); // Show device info - M5.Lcd.printf("Name: %s\n", device_name.c_str()); - M5.Lcd.printf("ADDR: %2x:%2x:%2x:%2x:%2x:%2x\n", mac_address[0], mac_address[1], mac_address[2], mac_address[3], - mac_address[4], mac_address[5]); - M5.Lcd.printf("SSID: %s\n", wifi_ssid.c_str()); - M5.Lcd.printf("PASS: %s\n", wifi_password.c_str()); + sprite_header.printf("Name: %s\n", device_name.c_str()); + sprite_header.printf("ADDR: %2x:%2x:%2x:%2x:%2x:%2x\n", mac_address[0], mac_address[1], mac_address[2], mac_address[3], + mac_address[4], mac_address[5]); + sprite_header.pushSprite(0, 0); // UWB module if (uwb_id >= 0) { @@ -156,15 +188,16 @@ void setup() { body_uwb.clear(); body_uwb.push_back(SDPData(uwb_id)); if (result) { - M5.Lcd.printf("UWB ID: %d\n", uwb_id); + sprite_header.printf("UWB ID: %d\n", uwb_id); } else { uwb_id = -1; - M5.Lcd.printf("UWB ID: Failed to initialize\n"); + sprite_header.printf("UWB ID: Failed to initialize\n"); } } else { bool result = resetUWB(Serial1); - M5.Lcd.printf("UWB ID: Not initialized\n"); + sprite_header.printf("UWB ID: Not initialized\n"); } + sprite_header.pushSprite(0, 0); // Wifi Configuration Serial.printf("Wifi Configuration\n"); @@ -187,13 +220,82 @@ void setup() { // Get device status ret = send_serial_command(String("") + "{\"command\":\"get_device_config\"}\n", 5000); Serial.printf("Response for get_device_status: %s\n", ret.c_str()); + + // ToF ranging unit + if (not init_m5stack_tof_unit()) { + Serial.println("Failed to initialize ToF unit"); + sprite_status.printf("Failed to initialize ToF unit\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + while (true) { + delay(1000); + } + } + Serial.println("ToF unit initialized!"); + + // Door detection parameter initialization + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Initialization of Door Detection Parameters will start in 3 seconds\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + delay(3000); + auto result = get_m5stack_tof_unit_data(); + if (not result.has_value()) { + Serial.println("Failed to get ToF data"); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Failed to get ToF data\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + while (true) { + delay(1000); + } + } else { + auto [acnt, scnt, dist, status] = result.value(); + door_closed_distance = dist; + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Door closed distance: %d\n", door_closed_distance); + sprite_status.pushSprite(0, lcd.height() / 3); + } + delay(3000); } void loop() { - delay(1000); + delay(100); Serial.println("Loop"); + auto result = get_m5stack_tof_unit_data(); + if (not result.has_value()) { + Serial.println("Failed to get ToF data"); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("Failed to get ToF data\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + } else { + auto [acnt, scnt, dist, status] = result.value(); + Serial.printf("acnt: %d, scnt: %d, dist: %d, status: %d\n", acnt, scnt, dist, status); + sprite_status.fillScreen(0xFFFFFF); + sprite_status.setCursor(0, 0); + sprite_status.printf("ambient count: %d\n", acnt); + sprite_status.printf("signal count: %d\n", scnt); + sprite_status.printf("distance: %d\n", dist); + sprite_status.printf("status: %d\n", status); + if (dist > door_closed_distance + door_closed_clearance or status != 11) { + Serial.println("Door is open"); + sprite_status.printf("Door is open\n"); + } else { + Serial.println("Door is closed"); + sprite_status.printf("Door is closed\n"); + } + body_door_open.clear(); + body_door_open.push_back(SDPData(dist > door_closed_distance + door_closed_clearance)); + if (not send_sdp_data_packet(packet_description_door_open, body_door_open)) { + Serial.printf("Failed to send SDP data packet\n"); + Serial.printf("packet description is %s\n", packet_description_door_open.c_str()); + sprite_status.printf("Failed to send SDP data packet\n"); + } + sprite_status.pushSprite(0, lcd.height() / 3); + } // Send SDP Data if (uwb_id >= 0) { if (not send_sdp_data_packet(packet_description_uwb, body_uwb)) { diff --git a/sketchbooks/smart_device_protocol_ros_interface/platformio.ini b/sketchbooks/smart_device_protocol_ros_interface/platformio.ini index 0b03e049..b6101a8e 100644 --- a/sketchbooks/smart_device_protocol_ros_interface/platformio.ini +++ b/sketchbooks/smart_device_protocol_ros_interface/platformio.ini @@ -79,7 +79,23 @@ build_flags = build_unflags = -std=gnu++11 [env:m5atom-lite] -platform = espressif32 +platform = espressif32@ ^6.0.1 +board = m5stack-atom +framework = arduino +lib_ldf_mode = deep +lib_deps = + m5stack/M5Atom@^0.1.1 + fastled/FastLED@^3.6.0 +monitor_speed = 115200 +build_flags = + -DM5ATOMLITE + -std=gnu++17 +build_unflags = + -std=gnu++11 + -DUSE_DISPLAY + +[env:m5atom-lite-new] +platform = espressif32@ ^6.9.0 board = m5stack-atom framework = arduino lib_ldf_mode = deep