Brunno Oliveira
GPS to Pulga
Diff: source/main.cpp
- Revision:
- 22:2c6161c96a76
- Parent:
- 21:6cf50085f9f3
- Child:
- 23:7f1c9c1a4c57
--- 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));