int main (void) {
//Do setup here
serial1_init_b(9600);
serial1_write_s("--reset--\n\r");
control_init();
serial1_write_s("--init--\n\r");
//Main program loop
while (1) {
_delay_ms(10);
control_update();
control = control_read_analog();
buttons = control_button_state();
// uint16_t buttons_changed = control_button_state_changed();
if (control.pitch != last_control.pitch
|| control.roll != last_control.roll
|| control.yaw != last_control.yaw
|| control.throttle != last_control.throttle) {
last_control = control;
sprintf(temp, "%g", control.pitch);
serial1_write_s(temp);
serial1_write_s(", ");
sprintf(temp, "%g", control.roll);
serial1_write_s(temp);
serial1_write_s(", ");
sprintf(temp, "%g", control.yaw);
serial1_write_s(temp);
serial1_write_s(", ");
sprintf(temp, "%g", control.throttle);
serial1_write_s(temp);
serial1_write_s("\n\r");
}
if (buttons != last_buttons) {
last_buttons = buttons;
if ((buttons & POWER)) serial1_write_s(">");
if ((buttons & MODE_STABLE)) serial1_write_s("X");
if ((buttons & MODE_SPORT)) serial1_write_s("O");
serial1_write_s("\n\r");
}
}
}
struct control *control_init(struct audio_device *dev, uint16_t uuid16)
{
struct control *control;
if (!g_dbus_register_interface(dev->conn, dev->path,
AUDIO_CONTROL_INTERFACE,
control_methods, control_signals, NULL,
dev, path_unregister))
return NULL;
DBG("Registered interface %s on path %s",
AUDIO_CONTROL_INTERFACE, dev->path);
control = g_new0(struct control, 1);
control->dev = dev;
control_update(control, uuid16);
if (!avctp_id)
avctp_id = avctp_add_state_cb(state_changed, NULL);
return control;
}
static void handle_uuid(const char *uuidstr, struct audio_device *device)
{
uuid_t uuid;
uint16_t uuid16;
if (bt_string2uuid(&uuid, uuidstr) < 0) {
error("%s not detected as an UUID-128", uuidstr);
return;
}
if (!sdp_uuid128_to_uuid(&uuid) && uuid.type != SDP_UUID16) {
error("Could not convert %s to a UUID-16", uuidstr);
return;
}
uuid16 = uuid.value.uuid16;
if (!server_is_enabled(&device->src, uuid16)) {
DBG("server not enabled for %s (0x%04x)", uuidstr, uuid16);
return;
}
switch (uuid16) {
case HEADSET_SVCLASS_ID:
DBG("Found Headset record");
if (device->headset)
headset_update(device, uuid16, uuidstr);
else
device->headset = headset_init(device, uuid16,
uuidstr);
break;
case HEADSET_AGW_SVCLASS_ID:
DBG("Found Headset AG record");
break;
case HANDSFREE_SVCLASS_ID:
DBG("Found Handsfree record");
if (device->headset)
headset_update(device, uuid16, uuidstr);
else
device->headset = headset_init(device, uuid16,
uuidstr);
break;
case HANDSFREE_AGW_SVCLASS_ID:
DBG("Found Handsfree AG record");
if (enabled.gateway && (device->gateway == NULL))
device->gateway = gateway_init(device);
break;
case AUDIO_SINK_SVCLASS_ID:
DBG("Found Audio Sink");
if (device->sink == NULL)
device->sink = sink_init(device);
break;
case AUDIO_SOURCE_SVCLASS_ID:
DBG("Found Audio Source");
if (device->source == NULL)
device->source = source_init(device);
break;
case AV_REMOTE_SVCLASS_ID:
case AV_REMOTE_TARGET_SVCLASS_ID:
DBG("Found AV %s", uuid16 == AV_REMOTE_SVCLASS_ID ?
"Remote" : "Target");
if (device->control)
control_update(device, uuid16);
else
device->control = control_init(device, uuid16);
/* SS_Bluetooth(sunjo4.kim) 2012.2.16 - add for a2dp connect fail */
//if (device->sink && sink_is_active(device))
// avrcp_connect(device);
/* SS_Bluetooth(sunjo4.kim) End */
break;
default:
DBG("Unrecognized UUID: 0x%04X", uuid16);
break;
}
}
