File content as of revision 0:fccb789424fc:

#include "mbed.h"
#include <btstack/hci_cmds.h>
#include <btstack/run_loop.h>
#include <btstack/sdp_util.h>
#include "hci.h"
#include "l2cap.h"
#include "btstack_memory.h"
#include "remote_device_db.h"
#include "rfcomm.h"
extern "C" {
#include "sdp.h"
}
#include "config.h"
#include "debug.h"
#include "bd_addr.h"  // class bd_addr

#include "Wiimote.h"
#include "TB6612.h"

// ----- Wallbot I/O Setting ----- 
// Motor
TB6612 right(p21,p12,p11);
TB6612 left(p22,p14,p13);

#define WII_REMOTE_CONNECTION_SYNC    0        // 0:old Wii Remote 1:New Wii Remote
#define REPORTMODE_TT_BIT           0x00
#define WII_REMOTE_REPORT_MODE       0x31
#define MAX_SPEED 100

#define HEARTBEAT_PERIOD_MS     1000
#define INQUIRY_INTERVAL          15

Serial pc(USBTX, USBRX);
DigitalOut ActLED(LED1);



static bd_addr_t keyboard;
static int haveKeyboard = 0;
static char                     lineBuffer[80];
static short wii_remocon_channel_id=0;

int DirectMode( Wiimote* wii, int stat )
{
    
    if( wii->left )
    {
        right = -MAX_SPEED;
        left = MAX_SPEED;
    }
    else if( wii->right )
    {
        right = MAX_SPEED;
        left = -MAX_SPEED;
    }    
    else if( wii->up )
    {
        right = MAX_SPEED;
        left = MAX_SPEED;
    }
    else if( wii->down )
    {
        right = -MAX_SPEED;
        left = -MAX_SPEED;
    }
    else
    {
        right = 0;
        left = 0;
    }
    
    float factor = wii->wheel * 1.5f;
    
    if(factor > 100.0f )    factor = 100.0f; 
    if(factor < -100.0f )    factor = -100.0f;
    
    printf("%f\t%f\r\n",wii->wheel,factor);
    
    int left_factor = (int)((factor <= 0.0) ? 100.0 : 100.0 - factor);
    int right_factor = (int)((factor >= 0.0) ? 100.0 : 100.0 - (-factor));
    
    if( wii->one )
    {
        right = right_factor;
        left = left_factor;
    }
    if( wii->two )
    {
        right = -left_factor;
        left = -right_factor;
    }
    
    return(stat);
}

int wall_bot_remote(char *c)
{
    Wiimote wii;
    int ret = 0;
    
    wii.decode(c);
    
    ret = DirectMode( &wii ,ret );
    
    return(ret);
}

void wii_remote_report_mode(uint16_t channel,uint8_t tt,uint8_t mm)
{
    lineBuffer[0]=0xa2;
    lineBuffer[1]=0x12;
    lineBuffer[2]=tt;
    lineBuffer[3]=mm;
    l2cap_send_internal(channel,(uint8_t*)lineBuffer,4);
}

void wii_remote_request_status_report(uint16_t channel)
{
    lineBuffer[0]=0xa2;
    lineBuffer[1]=0x15;
    lineBuffer[2]=0x00;
    l2cap_send_internal(channel,(uint8_t*)lineBuffer,3);
}

void hid_process_packet(uint16_t channel,unsigned char *hid_report,uint16_t size)
{
    if(hid_report[0]!=0xa1){
        return;
    }

    if(hid_report[1]==0x30){
        wii_remote_report_mode(channel,REPORTMODE_TT_BIT,WII_REMOTE_REPORT_MODE);
        wii_remote_request_status_report(channel);
    }

    switch(hid_report[1]){
    case    0x30:                                    //0x30:Core Buttons
    case    0x31:                                    //0x31:Core Buttons and Accelerometer
    case    0x35:                                    //0x35:Core Buttons and Accelerometer with 16 Extension Bytes
        wall_bot_remote( (char*)&hid_report[2] );
        break;
    default:
        break;
    }
    return;
}

static void l2cap_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
    if (packet_type == HCI_EVENT_PACKET){
        switch(packet[0]){
        case L2CAP_EVENT_INCOMING_CONNECTION:
            log_info("L2CAP_EVENT_INCOMING_CONNECTION\n");
            l2cap_accept_connection_internal(channel);
            break;
        case L2CAP_EVENT_CHANNEL_OPENED:
            if (packet[2]) {
                log_info("Connection failed\n");
                return;
            }
            log_info("interrupt channel connected\n");
            wii_remocon_channel_id=channel;
             break;
        case L2CAP_EVENT_CHANNEL_CLOSED:
            wii_remocon_channel_id=0;
        //    use_extension=0;
            haveKeyboard = 0;
            hci_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, INQUIRY_INTERVAL, 0);
            break;
        default:
            break;
        }    
    }
    if (packet_type == L2CAP_DATA_PACKET){
        hid_process_packet(channel,packet,size);
    }
}

//static uint16_t control_channel=0;
static void l2cap_control_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
    if (packet_type == HCI_EVENT_PACKET){
        switch(packet[0]){
        case L2CAP_EVENT_INCOMING_CONNECTION:
            log_info("L2CAP_EVENT_INCOMING_CONNECTION\n");
            l2cap_accept_connection_internal(channel);
            break;
        case L2CAP_EVENT_CHANNEL_OPENED:
            if (packet[2]) {
               // xputs("control Connection failed\n");
                return;
            }
          //  xputs("control channel connected\n");
            l2cap_create_channel_internal(NULL, l2cap_packet_handler, keyboard, PSM_HID_INTERRUPT, 150);
            //control_channel=channel;
            break;
        case L2CAP_EVENT_CHANNEL_CLOSED:
            log_info("L2CAP_CHANNEL_CLOSED\n");
            //control_channel=0;
            break;
        case L2CAP_EVENT_CREDITS:
            break;
        default:
            log_info("l2cap:unknown(%02x)\n",packet[0]);
            break;
        }    
    }
}

// Bluetooth logic
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
    bd_addr_t event_addr;
    
    switch (packet_type) {
        case HCI_EVENT_PACKET:
            switch (packet[0]) {
                case BTSTACK_EVENT_STATE:
                    // bt stack activated, get started - set local name
                    if (packet[2] == HCI_STATE_WORKING) {
                        hci_send_cmd(&hci_write_local_name, "mbed");
                    }
                    break;
                
                case HCI_EVENT_COMMAND_COMPLETE:
                    if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
                        bt_flip_addr(event_addr, &packet[6]);
                        log_info("BD-ADDR: %s\n", bd_addr_to_str(event_addr));
                        break;
                    }
                    if (COMMAND_COMPLETE_EVENT(packet, hci_write_local_name)){
//                        hci_discoverable_control(1);
//                        hci_send_cmd(&hci_write_authentication_enable, 1);
                        hci_send_cmd(&hci_write_authentication_enable, 0);
                        break;
                    }
                    if (COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable)){
                    inquiry:

                        log_info("Inquiry\n");
                        hci_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, INQUIRY_INTERVAL, 0);

                        break;
                    }
                    if (COMMAND_COMPLETE_EVENT(packet, hci_inquiry_cancel) ) {
                        //l2cap_create_channel_internal(NULL, l2cap_packet_handler, keyboard, PSM_HID_INTERRUPT, 150);
                        l2cap_create_channel_internal(NULL, l2cap_control_packet_handler, keyboard, PSM_HID_CONTROL, 150);
                        break;
                    }
                    break;
                case HCI_EVENT_INQUIRY_RESULT:
                    
                    // ignore further results
                    if (haveKeyboard) break;
                    
                    // ignore none wii remocon
                    if(
                        !(packet[12]==0x04 && packet[13]==0x25 && packet[14]==0x00)
                    &&    !(packet[12]==0x08 && packet[13]==0x05 && packet[14]==0x00)    //RVL-CNT-01-TR
                    ){
                        break;
                    }

                    // flip addr
                    bt_flip_addr(keyboard, &packet[3]);
                    
                    // show
                    log_info("found wii remocon:%s\n",bd_addr_to_str(keyboard));

                    haveKeyboard = 1;
                    hci_send_cmd(&hci_inquiry_cancel);
                    break;
                    
                case HCI_EVENT_INQUIRY_COMPLETE:
                    log_info("No wii remocon\n");
                    goto inquiry;
                    //break;
                case HCI_EVENT_LINK_KEY_REQUEST:
                    // deny link key request
                    log_info("Link key request\n");
                    bt_flip_addr(event_addr, &packet[2]);
                    hci_send_cmd(&hci_link_key_request_negative_reply, &event_addr);
                    break;
                    
                case HCI_EVENT_PIN_CODE_REQUEST:
                    // inform about pin code request
                    bt_flip_addr(event_addr, &packet[2]);
                    //wii remocon
#if    WII_REMOTE_CONNECTION_SYNC
                    bt_flip_addr(packet+2, addr_global);
#endif
                    log_info("Pin code request - using %02x%02x%02x%02x%02x%02x\n",packet[2],packet[3],packet[4],packet[5],packet[6],packet[7]);
                    memset(packet+2+6,0x00,10);
                    hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 6, &packet[2]);
                    break;
                case HCI_EVENT_CONNECTION_COMPLETE:
                    //log_info("connection complete: status=%d\n",packet[2]);
                    if(packet[2]){
                        haveKeyboard=0;
                        goto inquiry;
                    }
                    break;
                default:
                    break;
            }
            break;
        default:
            break;
    }
}

static void  heartbeat_handler(struct timer *ts){
    static int counter = 0;
    counter++;

    if(wii_remocon_channel_id){
        lineBuffer[0]=0xa2;
        lineBuffer[1]=0x11;
        lineBuffer[2]=0x10<<(counter%4);        //LED Player1
        if(l2cap_send_internal(wii_remocon_channel_id,(uint8_t*)lineBuffer,3)<0){
            //error control init
        }
       ActLED = !ActLED;
    }
    run_loop_set_timer(ts, HEARTBEAT_PERIOD_MS);
    run_loop_add_timer(ts);
} 

// main
int main(void)
{
    pc.baud(921600);
    log_info("%s\n", __FILE__);

    // init LEDs
    ActLED = 0;
    
    /// GET STARTED with BTstack ///
    btstack_memory_init();
    run_loop_init(RUN_LOOP_EMBEDDED);
    
    // init HCI
    hci_transport_t* transport = hci_transport_usb_instance();
    remote_device_db_t * remote_db = (remote_device_db_t *) &remote_device_db_memory;
    hci_init(transport, NULL, NULL, remote_db);
    
    // init L2CAP
    l2cap_init();
    l2cap_register_packet_handler(packet_handler);
    
#if 0
    // init RFCOMM
    rfcomm_init();
    rfcomm_register_packet_handler(packet_handler);
    rfcomm_register_service_internal(NULL, rfcomm_channel_nr, 100);  // reserved channel, mtu=100


    // init SDP, create record for SPP and register with SDP
    sdp_init();
    memset(spp_service_buffer, 0, sizeof(spp_service_buffer));
    service_record_item_t * service_record_item = (service_record_item_t *) spp_service_buffer;
    sdp_create_spp_service( (uint8_t*) &service_record_item->service_record, 1, "loopback");
    log_info("SDP service buffer size: %u\n\r", (uint16_t) (sizeof(service_record_item_t) + de_get_len((uint8_t*) &service_record_item->service_record)));
    sdp_register_service_internal(NULL, service_record_item);
#endif
    
    // set one-shot timer
    timer_source_t heartbeat;
    heartbeat.process = &heartbeat_handler;
    run_loop_set_timer(&heartbeat, HEARTBEAT_PERIOD_MS);
    run_loop_add_timer(&heartbeat);
    
    log_info("SPP loopback demo...\n\r");

     // turn on!
    hci_power_control(HCI_POWER_ON);

    // go!
    run_loop_execute();    

    return 0;
}