File content as of revision 22:2c6161c96a76:

/* mbed Microcontroller Library
 * Copyright (c) 2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
#include <events/mbed_events.h>
#include <stdio.h>
#include "mbed.h"

#include "platform/Callback.h"
#include "events/EventQueue.h"
#include "platform/NonCopyable.h"

#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/GattClient.h"
#include "ble/GapAdvertisingParams.h"
#include "ble/GapAdvertisingData.h"
#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);

        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 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 lux, led and amb characteristric.
 */
class MyService {
    typedef MyService Self;

public:
    MyService() :
        _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,
            /* numCharacteristics */ sizeof(_My_characteristics) /
                                     sizeof(_My_characteristics[0])
        ),
        _server(NULL),
        _event_queue(NULL),
        _alive_led(P1_13, 1),
        _actuated_led(P1_14,0)
        
    {
        // update internal pointers (value, descriptors and characteristics array)
        _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
        _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);
    }



    void start(BLE &ble_interface, events::EventQueue &event_queue)
    {
         if (_event_queue) {
            return;
        }

        _server = &ble_interface.gattServer();
        _event_queue = &event_queue;


        // register the service
        printf("Adding demo service\r\n");
        ble_error_t err = _server->addService(_My_service);

        if (err) {
            printf("Error %u during demo service registration.\r\n", err);
            return;
        }

        // read write handler
        _server->onDataSent(as_cb(&Self::when_data_sent));
        _server->onDataWritten(as_cb(&Self::when_data_written));
        _server->onDataRead(as_cb(&Self::when_data_read));

        // updates subscribtion handlers
        _server->onUpdatesEnabled(as_cb(&Self::when_update_enabled));
        _server->onUpdatesDisabled(as_cb(&Self::when_update_disabled));
        _server->onConfirmationReceived(as_cb(&Self::when_confirmation_received));

        // print the handles
        printf("My service registered\r\n");
        printf("service handle: %u\r\n", _My_service.getHandle());
        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(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));
        
    }

private:

    /**
     * Handler called when a notification or an indication has been sent.
     */
    void when_data_sent(unsigned count)
    {
        printf("sent %u updates\r\n", count);
    }

    /**
     * Handler called after an attribute has been written.
     */
    void when_data_written(const GattWriteCallbackParams *e)
    {
        printf("data written:\r\n");
        printf("\tconnection handle: %u\r\n", e->connHandle);
        printf("\tattribute handle: %u", e->handle);
        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 == _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");
        }
        printf("\twrite operation: %u\r\n", e->writeOp);
        printf("\toffset: %u\r\n", e->offset);
        printf("\tlength: %u\r\n", e->len);
        printf("\t data: ");

        for (size_t i = 0; i < e->len; ++i) {
            printf("%02X", e->data[i]);
        }

        printf("\r\n");
    }

    /**
     * Handler called after an attribute has been read.
     */
    void when_data_read(const GattReadCallbackParams *e)
    {
        printf("data read:\r\n");
        printf("\tconnection handle: %u\r\n", e->connHandle);
        printf("\tattribute handle: %u", e->handle);
        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 == _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");
        }
    }

    /**
     * Handler called after a client has subscribed to notification or indication.
     *
     * @param handle Handle of the characteristic value affected by the change.
     */
    void when_update_enabled(GattAttribute::Handle_t handle)
    {
        printf("update enabled on handle %d\r\n", handle);
    }

    /**
     * Handler called after a client has cancelled his subscription from
     * notification or indication.
     *
     * @param handle Handle of the characteristic value affected by the change.
     */
    void when_update_disabled(GattAttribute::Handle_t handle)
    {
        printf("update disabled on handle %d\r\n", handle);
    }

    /**
     * Handler called when an indication confirmation has been received.
     *
     * @param handle Handle of the characteristic value that has emitted the
     * indication.
     */
    void when_confirmation_received(GattAttribute::Handle_t handle)
    {
        printf("confirmation received on handle %d\r\n", handle);
    }

    /**
     * Handler called when a write request is received.
     *
     * This handler verify that the value submitted by the client is valid before
     * authorizing the operation.
     */
    void authorize_client_write(GattWriteAuthCallbackParams *e)
    {
        printf("characteristic %u write authorization\r\n", e->handle);

        if (e->offset != 0) {
            printf("Error invalid offset\r\n");
            e->authorizationReply = AUTH_CALLBACK_REPLY_ATTERR_INVALID_OFFSET;
            return;
        }

        if (e->len != 1) {
            printf("Error invalid len\r\n");
            e->authorizationReply = AUTH_CALLBACK_REPLY_ATTERR_INVALID_ATT_VAL_LENGTH;
            return;
        }


        e->authorizationReply = AUTH_CALLBACK_REPLY_SUCCESS;
    }

    /**
     * Increment the lux luxer.
     */
    
    void read_sensors (void)
    {
        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));
        

            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("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;
        }
        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;    
        }
        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 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;
    }

    /**
     * Change led status.
     */
    void change_led(void)
    {
        uint8_t led = 0;
        ble_error_t err = _led_char.get(*_server, led);
        if (err) {
            printf("read of the led value returned error %u\r\n", err);
            return;
        }
    }



private:
    /**
     * Helper that construct an event handler from a member function of this
     * instance.
     */
    template<typename Arg>
    FunctionPointerWithContext<Arg> as_cb(void (Self::*member)(Arg))
    {
        return makeFunctionPointer(this, member);
    }

    /**
     * Read, Write, Notify, Indicate  Characteristic declaration helper.
     *
     * @tparam T type of data held by the characteristic.
     */
    template<typename T>
    class ReadWriteNotifyIndicateCharacteristic : 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.
         */
        ReadWriteNotifyIndicateCharacteristic(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 |
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY |
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE,
                /* 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:
        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<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[5];

    // demo service
    GattService _My_service;

    GattServer* _server;
    events::EventQueue *_event_queue;
    
   DigitalOut _alive_led;
   DigitalOut _actuated_led;
};

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));


    // bind the event queue to the ble interface, initialize the interface
    // and start advertising
    ble_process.start();

    // Process the event queue.
    event_queue.dispatch_forever();

    return 0;
}