GPS to Pulga
Diff: source/main.cpp
- Revision:
- 22:2c6161c96a76
- Parent:
- 21:6cf50085f9f3
- Child:
- 23:7f1c9c1a4c57
diff -r 6cf50085f9f3 -r 2c6161c96a76 source/main.cpp --- a/source/main.cpp Wed Jul 10 17:55:38 2019 +0000 +++ b/source/main.cpp Thu Jul 18 13:42:17 2019 +0000 @@ -30,31 +30,68 @@ #include "ble/GattServer.h" #include "BLEProcess.h" #include "Si1133.h" +#include "BME280.h" +/* defines the axis for acc */ +#define ACC_NOOF_AXIS 3 +#define GYR_NOOF_AXIS 2 + +/* bmi160 slave address */ +#define BMI160_ADDR ((0x69)<<1) + +#define RAD_DEG 57.29577951 + +using mbed::callback; + +Si1133 sensor_light(P0_13, P0_15); +BME280 sensor_amb(P0_13, P0_15, 0x77 << 1) ; + +I2C i2c(p13, p15); -using mbed::callback; - Si1133 sensor_light(P0_13, P0_15); + int msg; + int sens; + uint32_t lux = 0; + uint32_t amb = 0; + uint32_t imux = 0; + uint32_t imuy = 0; + uint32_t imuz = 0; + float sensor_get = 0; + +int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555}; +int16_t gyr_sample_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555}; + +double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555}; +double gyr_result_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555}; + +double accel_ang_x, accel_ang_y; +double tiltx, tilty; +double tiltx_prev, tilty_prev; + +char i2c_reg_buffer[2] = {0}; /** * A My service that demonstrate the GattServer features. * - * The My service host three characteristics that model the current hour, - * led and count of the My. The value of the count characteristic is + * The My service host three characteristics that model the current amb, + * led and lux of the My. The value of the lux characteristic is * incremented automatically by the system. * * A client can subscribe to updates of the My characteristics and get * notified when one of the value is changed. Clients can also change value of - * the count, led and hour characteristric. + * the lux, led and amb characteristric. */ class MyService { typedef MyService Self; public: MyService() : - // _hour_char("485f4145-52b9-4644-af1f-7a6b9322490f", 0), - _led_char("0a924ca7-87cd-4699-a3bd-abdcd9cf126a", 0), - _count_char("8dd6a1b7-bc75-4741-8a26-264af75807de", 0), + _amb_char("00000000-000a-000a-a000-a0a000aa00aa", 0), + _lux_char("11111111-111b-111b-b111-b1b111bb11bb", 0), + _imux_char("22222222-222c-222c-c222-c2c222cc22cc", 0), + _imuy_char("33333333-333d-333d-d333-d3d333dd33dd", 0), + // _imuz_char("44444444-444e-444e-e444-e4e444ee44ee", 0), + _led_char("55555555-555f-555f-f555-f5f555ff55ff", 0), _My_service( /* uuid */ "51311102-030e-485f-b122-f8f381aa84ed", /* characteristics */ _My_characteristics, @@ -68,14 +105,20 @@ { // update internal pointers (value, descriptors and characteristics array) - // _My_characteristics[0] = &_hour_char; - _My_characteristics[1] = &_led_char; - _My_characteristics[0] = &_count_char; + _My_characteristics[4] = &_led_char; + // _My_characteristics[4] = &_imuz_char; + _My_characteristics[3] = &_imuy_char; + _My_characteristics[2] = &_imux_char; + _My_characteristics[1] = &_amb_char; + _My_characteristics[0] = &_lux_char; // setup authorization handlers - //_hour_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); + _amb_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); + _imux_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); + _imuy_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); + // _imuz_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); + _lux_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); _led_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); - _count_char.setWriteAuthorizationCallback(this, &Self::authorize_client_write); } @@ -112,12 +155,16 @@ // print the handles printf("My service registered\r\n"); printf("service handle: %u\r\n", _My_service.getHandle()); - //printf("\thour characteristic value handle %u\r\n", _hour_char.getValueHandle()); - // printf("\tled characteristic value handle %u\r\n", _led_char.getValueHandle()); - printf("\tcount characteristic value handle %u\r\n", _count_char.getValueHandle()); + printf("\tamb characteristic value handle %u\r\n", _amb_char.getValueHandle()); + printf("\timu characteristic value handle %u\r\n", _imux_char.getValueHandle()); + printf("\timu characteristic value handle %u\r\n", _imuy_char.getValueHandle()); + // printf("\timu characteristic value handle %u\r\n", _imuz_char.getValueHandle()); + printf("\tlux characteristic value handle %u\r\n", _lux_char.getValueHandle()); + printf("\tled characteristic value handle %u\r\n", _led_char.getValueHandle()); - _event_queue->call_every(1000 /* ms */, callback(this, &Self::increment_count)); - _event_queue->call_every(500 /* ms */, callback(this, &Self::blink)); + _event_queue->call_every(1741 /* ms */, callback(this, &Self::update_char)); + _event_queue->call_every(1621 /* ms */, callback(this, &Self::read_sensors)); + _event_queue->call_every(503 /* ms */, callback(this, &Self::blink)); } @@ -139,14 +186,19 @@ printf("data written:\r\n"); printf("\tconnection handle: %u\r\n", e->connHandle); printf("\tattribute handle: %u", e->handle); - //if (e->handle == _hour_char.getValueHandle()) { - // printf(" (hour characteristic)\r\n"); - // } else - if (e->handle == _led_char.getValueHandle()) { + if (e->handle == _amb_char.getValueHandle()) { + printf(" (amb characteristic)\r\n"); + } else if (e->handle == _led_char.getValueHandle()) { printf(" (led characteristic)\r\n"); _actuated_led = *(e->data); - } else if (e->handle == _count_char.getValueHandle()) { - printf(" (count characteristic)\r\n"); + } else if (e->handle == _lux_char.getValueHandle()) { + printf(" (lux characteristic)\r\n"); + } else if (e->handle == _imux_char.getValueHandle()) { + printf(" (imu_x characteristic)\r\n"); + } else if (e->handle == _imuy_char.getValueHandle()) { + printf(" (imu_y characteristic)\r\n"); + // } else if (e->handle == _imuz_char.getValueHandle()) { + // printf(" (imu_z characteristic)\r\n"); } else { printf("\r\n"); } @@ -170,13 +222,18 @@ printf("data read:\r\n"); printf("\tconnection handle: %u\r\n", e->connHandle); printf("\tattribute handle: %u", e->handle); - // if (e->handle == _hour_char.getValueHandle()) { - // printf(" (hour characteristic)\r\n"); - // } else - if (e->handle == _led_char.getValueHandle()) { + if (e->handle == _amb_char.getValueHandle()) { + printf(" (amb characteristic)\r\n"); + } else if (e->handle == _led_char.getValueHandle()) { printf(" (led characteristic)\r\n"); - } else if (e->handle == _count_char.getValueHandle()) { - printf(" (count characteristic)\r\n"); + } else if (e->handle == _lux_char.getValueHandle()) { + printf(" (lux characteristic)\r\n"); + } else if (e->handle == _imux_char.getValueHandle()) { + printf(" (imu_x characteristic)\r\n"); + } else if (e->handle == _imuy_char.getValueHandle()) { + printf(" (imu_y characteristic)\r\n"); + // } else if (e->handle == _imuz_char.getValueHandle()) { + // printf(" (imu_z characteristic)\r\n"); } else { printf("\r\n"); } @@ -236,51 +293,151 @@ return; } - //if ((e->data[0] >= 60) || - // ((e->data[0] >= 24) && (e->handle == _hour_char.getValueHandle()))) { - // printf("Error invalid data\r\n"); - // e->authorizationReply = AUTH_CALLBACK_REPLY_ATTERR_WRITE_NOT_PERMITTED; - // return; - // } e->authorizationReply = AUTH_CALLBACK_REPLY_SUCCESS; } /** - * Increment the count counter. + * Increment the lux luxer. */ - void increment_count(void) + + void read_sensors (void) { - uint8_t count = 0; - + int32_t float_to_32; + + + + if (sensor_light.open() && sens==0) { + float_to_32=0; + sensor_get = sensor_light.get_light_level(); + float_to_32 = sensor_get/10; + //float_to_32 = __rev( float_to_32 ); + float_to_32 = (float_to_32 < 0 ? 0 : float_to_32); + lux = (float_to_32 > 255 ? 255 : float_to_32); + sens=1; + printf("Lux = %f\n", sensor_get); + + } + if (sens==1) { + float_to_32=0; + sensor_amb.initialize(); + sensor_get = sensor_amb.getTemperature(); + float_to_32 = sensor_get*10 -103 ; + float_to_32 = (float_to_32 < 0 ? 0 : float_to_32); + amb = (float_to_32 > 255 ? 255 : float_to_32); + sens=2; + printf("Amb = %f\n", sensor_get); + } + if (sens==2) { + + i2c.frequency(20000); + + /*Le os Registradores do Acelerometro*/ + i2c_reg_buffer[0] = 0x12; + i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true); + i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false); + + /*Le os Registradores do Giroscopio*/ + i2c_reg_buffer[0] = 0x0C; + i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true); + i2c.read(BMI160_ADDR, (char *)&gyr_sample_buffer, sizeof(gyr_sample_buffer), false); + + /*Ajusta dados brutos Acelerometro em unidades de g */ + acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0); + acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0); + acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0); + + /*Ajusta dados Brutos do Giroscopio em unidades de deg/s */ + gyr_result_buffer[0] = (gyr_sample_buffer[0]/131.2); + gyr_result_buffer[1] = (gyr_sample_buffer[1]/131.2); + + /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/ + accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG; + accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG; + + /*Calcula os Angulos de Rotacao com valor do Giroscopio e aplica filtro complementar realizando a fusao*/ + tiltx = (0.98*(tiltx_prev+(gyr_result_buffer[0]*0.001)))+(0.02*(accel_ang_x)); + tilty = (0.98*(tilty_prev+(gyr_result_buffer[1]*0.001)))+(0.02*(accel_ang_y)); + - ble_error_t err = _count_char.get(*_server, count); + float_to_32 = tiltx * 100 + 127; + float_to_32 = (float_to_32 < 0 ? 0 : float_to_32); + imux = (float_to_32 > 255 ? 255 : float_to_32); + + float_to_32 = 0; + + float_to_32 = tilty * 100 + 127; + float_to_32 = (float_to_32 < 0 ? 0 : float_to_32); + imuy = (float_to_32 > 255 ? 255 : float_to_32); + //imuz = acc_result_buffer[2]; + + + + + + + /*Imprime os dados ACC pre-formatados*/ + printf("%.3f,%.3f\n\r",tiltx, tilty); + + sens=0; + + // update_char(); + + } + + //update_char(); + + } + + + void update_char(void) + { + if (msg==0) {ble_error_t err = _lux_char.set(*_server, lux); + printf ("lux sensor value: %d\n", lux); if (err) { - printf("read of the count value returned error %u\r\n", err); + printf("write of the lux value returned error %u\r\n", err); + return; + } + wait(0.1); + msg=1; } + + if (msg==1) {ble_error_t err = _amb_char.set(*_server, amb); + printf ("amb sensor value: %d\n", amb); + if (err) { + printf("write of the amb value returned error %u\r\n", err); return; } - //count ++; - - - if (sensor_light.open()) { - printf("Device detected!\n"); - //Print the current light level - printf("Lux = %.3f\n", (float)sensor_light.get_light_level()); - //Print the current UV index - printf("UV index = %.3f\n", (float)sensor_light.get_uv_index()); - //Sleep for 0.5 seconds - count= (uint8_t)sensor_light.get_light_level(); - - _actuated_led = !_actuated_led; + wait(0.1); + msg=2;} + + if (msg==2) {ble_error_t err = _imux_char.set(*_server, imux); + printf ("imu_x sensor value: %d\n", imux); + if (err) { + printf("write of the imu_x value returned error %u\r\n", err); + return; } - - err = _count_char.set(*_server, count); + wait(0.1); + msg=3;} + + if (msg==3) {ble_error_t err = _imuy_char.set(*_server, imuy); + printf ("imu_y sensor value: %d\n", imuy); if (err) { - printf("write of the count value returned error %u\r\n", err); + printf("write of the imu_y value returned error %u\r\n", err); return; - } - + } + wait(0.1); + msg=0;} + + // err = _imuz_char.set(*_server, imuz); + // printf ("imu_z sensor value: %d\n", imuz); + // if (err) { + // printf("write of the imu_z value returned error %u\r\n", err); + // return; + // } + + } + void blink() { _alive_led = !_alive_led; @@ -299,26 +456,8 @@ } } - /* * Increment the hour counter. - void increment_hour(void) - { - uint8_t hour = 0; - ble_error_t err = _hour_char.get(*_server, hour); - if (err) { - printf("read of the hour value returned error %u\r\n", err); - return; - } - hour = (hour + 1) % 24; - - err = _hour_char.set(*_server, hour); - if (err) { - printf("write of the hour value returned error %u\r\n", err); - return; - } - } -*/ private: /** * Helper that construct an event handler from a member function of this @@ -348,7 +487,7 @@ ReadWriteNotifyIndicateCharacteristic(const UUID & uuid, const T& initial_value) : GattCharacteristic( /* UUID */ uuid, - /* Initial value */ &_value, + /* Initial value */ (uint8_t *) &_value, /* Value size */ sizeof(_value), /* Value capacity */ sizeof(_value), /* Properties */ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | @@ -374,7 +513,7 @@ ble_error_t get(GattServer &server, T& dst) const { uint16_t value_length = sizeof(dst); - return server.read(getValueHandle(), &dst, &value_length); + return server.read(getValueHandle(), (uint8_t *) &dst, &value_length); } /** @@ -386,21 +525,81 @@ * locally or forwarded to subscribed clients. */ ble_error_t set( - GattServer &server, const uint8_t &value, bool local_only = false + GattServer &server, const uint8_t &value, bool local_only = false ) const { - return server.write(getValueHandle(), &value, sizeof(value), local_only); + return server.write(getValueHandle(), (uint8_t *) &value, sizeof(value), local_only); + } + + private: + uint32_t _value; + }; + + template<typename T> + class ReadWriteCharacteristic : public GattCharacteristic { + public: + /** + * Construct a characteristic that can be read or written and emit + * notification or indication. + * + * @param[in] uuid The UUID of the characteristic. + * @param[in] initial_value Initial value contained by the characteristic. + */ + ReadWriteCharacteristic(const UUID & uuid, const T& initial_value) : + GattCharacteristic( + /* UUID */ uuid, + /* Initial value */ (uint8_t *) &_value, + /* Value size */ sizeof(_value), + /* Value capacity */ sizeof(_value), + /* Properties */ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | + GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE , + /* Descriptors */ NULL, + /* Num descriptors */ 0, + /* variable len */ false + ), + _value(initial_value) { + } + + /** + * Get the value of this characteristic. + * + * @param[in] server GattServer instance that contain the characteristic + * value. + * @param[in] dst Variable that will receive the characteristic value. + * + * @return BLE_ERROR_NONE in case of success or an appropriate error code. + */ + ble_error_t get(GattServer &server, T& dst) const + { + uint16_t value_length = sizeof(dst); + return server.read(getValueHandle(), (uint8_t *) &dst, &value_length); + } + + /** + * Assign a new value to this characteristic. + * + * @param[in] server GattServer instance that will receive the new value. + * @param[in] value The new value to set. + * @param[in] local_only Flag that determine if the change should be kept + * locally or forwarded to subscribed clients. + */ + ble_error_t set( + GattServer &server, const uint8_t &value, bool local_only = false + ) const { + return server.write(getValueHandle(), (uint8_t *) &value, sizeof(value), local_only); } private: uint8_t _value; }; - //ReadWriteNotifyIndicateCharacteristic<uint8_t> _hour_char; - ReadWriteNotifyIndicateCharacteristic<uint8_t> _led_char; - ReadWriteNotifyIndicateCharacteristic<uint8_t> _count_char; - + ReadWriteNotifyIndicateCharacteristic<uint32_t> _amb_char; + ReadWriteCharacteristic<uint8_t> _led_char; + ReadWriteNotifyIndicateCharacteristic<uint32_t> _lux_char; + ReadWriteNotifyIndicateCharacteristic<uint32_t> _imux_char; + ReadWriteNotifyIndicateCharacteristic<uint32_t> _imuy_char; + // ReadWriteNotifyIndicateCharacteristic<uint32_t> _imuz_char; // list of the characteristics of the My service - GattCharacteristic* _My_characteristics[2]; + GattCharacteristic* _My_characteristics[5]; // demo service GattService _My_service; @@ -413,11 +612,57 @@ }; int main() { + + + printf("ambient sensor initialized"); + wait(1) ; BLE &ble_interface = BLE::Instance(); events::EventQueue event_queue; MyService demo_service; BLEProcess ble_process(event_queue, ble_interface); + i2c.frequency(20000); + + /*Reset BMI160*/ + i2c_reg_buffer[0] = 0x7E; + i2c_reg_buffer[1] = 0xB6; + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + wait_ms(200); + printf("BMI160 Resetado\n\r"); + + /*Habilita o Acelerometro*/ + i2c_reg_buffer[0] = 0x7E; + i2c_reg_buffer[1] = 0x11; //PMU Normal + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + printf("Acc Habilitado\n\r"); + + /*Habilita o Giroscopio*/ + i2c_reg_buffer[0] = 0x7E; + i2c_reg_buffer[1] = 0x15; //PMU Normal + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + printf("Gyr Habilitado\n\r"); + + /*Config o Data Rate ACC em 1600Hz*/ + i2c_reg_buffer[0] = 0x40; + i2c_reg_buffer[1] = 0x2C; + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + printf("Data Rate ACC Selecionado a 1600Hz\n\r"); + + /*Config o Data Rate GYR em 1600Hz*/ + i2c_reg_buffer[0] = 0x42; + i2c_reg_buffer[1] = 0x2C; + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + printf("Data Rate GYR Selecionado a 1600Hz\n\r"); + + /*Config o Range GYR em 250º/s*/ + i2c_reg_buffer[0] = 0x43; + i2c_reg_buffer[1] = 0x03; + i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); + printf("Range GYR Selecionado a 250deg/s\n\r"); + + wait(0.1); + + printf("BMI160 Configurado\n\r"); ble_process.on_init(callback(&demo_service, &MyService::start));