void process_command()
{
DEBUG_PRINTLN("process_command");
DEBUG_PRINTLN2("command ", command.command);
DEBUG_PRINTLN2H("com.addr ", command.node_id.addr);
DEBUG_PRINTLN2H("this.addr ", THIS_NODE);
answer.node_id.addr = THIS_NODE;
answer.command = command.command;
answer.number = command.number;
answer.answer = 0;
if (command.node_id.addr == THIS_NODE ||
command.node_id.addr == 0)
{
switch(command.command)
{
case ECommand_LIGHT_ON:
{
DEBUG_PRINTLN("ECommand_LIGHT_ON");
//digitalWrite(ledPin, HIGH);
// digitalWrite(ledPin, LOW);
current_color = 7;
set_led(true);
answer.answer = 1;
break;
}
case ECommand_LIGHT_OFF:
{
DEBUG_PRINTLN("ECommand_LIGHT_OFF");
// digitalWrite(ledPin, LOW);
// digitalWrite(ledPin, HIGH);
set_led(false);
answer.answer = 2;
break;
}
case ECommand_BEACON:
{
// answer is ready for beacon, no processing needed
DEBUG_PRINTLN("ECommand_BEACON");
break;
}
default:
{
answer.answer = -1;
}
}
send_package(SimpleAnswer_fields, &answer, packet_xb_writer);
}
}
extern "C" int8_t fsm_parse(fsm const* f, unsigned long timeout, uint8_t parse_error)
{
DEBUG_PRINTLN("fsm_parse");
uint8_t state = 0;
uint8_t newstate = 0;
int8_t action;
unsigned long begin_time = millis();
while (1)
{
if (f->gen->avail(f->gen->avail_param))
{
char c = f->gen->next(f->gen->next_param);
DEBUG_PRINTLN2("c = ", c);
uint8_t cls = get_class(f, c);
DEBUG_PRINTLN2("cls = ", cls);
DEBUG_PRINTLN2("state = ", (int)newstate);
if (cls != f->classes_count)
newstate = f->table[state * f->states_count + cls];
else
newstate = -1;
DEBUG_PRINTLN2("newstate = ", (int)newstate);
if (-1 == newstate)
{
newstate = 0;
if (parse_error)
return FSM_PARSE_ERR;
}
action = get_action(f, state, newstate);
DEBUG_PRINTLN2("action = ", action);
if (action != FSM_NOTHING)
{
DEBUG_PRINTLN("WTF");
return action;
}
if (millis() - begin_time > timeout)
return FSM_TIMEOUT;
state = newstate;
}
}
}
static void set_led(bool on)
{
#define ON(color) ((current_color & color) ? LOW : HIGH)
#define OFF(color) HIGH
#define SWITCH(on, color) (on ? ON(color) : OFF(color))
DEBUG_PRINTLN2("set_led ", on);
DEBUG_PRINTLN2("color ", current_color);
current_enabled = on;
digitalWrite(RED_LED, SWITCH(on, 4));
digitalWrite(GREEN_LED, SWITCH(on, 2));
digitalWrite(BLUE_LED, SWITCH(on, 1));
#undef SWITCH
#undef OFF
#undef ON
}
void process_ZB_TX_STATUS_RESPONSE()
{
DEBUG_PRINTLN("process_ZB_TX_STATUS_RESPONSE");
sent = true;
sent_ok = mrf.get_txinfo()->tx_ok;
DEBUG_PRINTLN2("tx ", sent_ok ? "ok" : "err");
DEBUG_PRINTLN2H("rc ", mrf.get_txinfo()->retries);
}
void process_position_notify()
{
DEBUG_PRINTLN("process_position_notify");
DEBUG_PRINTLN2H("pos_notify.addr ", pos_notify.node_id.addr);
DEBUG_PRINTLN2("pos_notify.x ", pos_notify.x);
DEBUG_PRINTLN2("pos_notify.y ", pos_notify.y);
uint8_t x = pos_notify.x;
uint8_t y = pos_notify.y;
for (uint8_t i = 0; i < sizeof(colors) / sizeof(colors[0]); ++i)
{
ColorRect& r = colors[i];
if (r.l <= x && x <= r.r &&
r.t <= y && y <= r.b)
{
current_color = r.color;
set_led(current_enabled);
DEBUG_PRINTLN2("set color ", current_color);
break;
}
}
}
void process_ZB_RX_RESPONSE()
{
uint8_t* data = mrf.get_rxinfo()->rx_data;
uint8_t const length = mrf.rx_datalength();
DEBUG_PRINTLN2("process_ZB_RX_RESPONSE length = ", length);
for (int i =0;i < length; ++i)
{
Serial.print(data[i], HEX);
Serial.print(' ');
}
DEBUG_PRINTLN(' ');
send_dst_addr = mrf.get_rxinfo()->src_addr16;
pb_istream_t istream = pb_istream_from_buffer(data, length);
process_incoming_packet(&istream);
}
bool process_incoming_packet(pb_istream_t* isrt)
{
bool status = false;
const pb_field_t *type = decode_unionmessage_type(isrt, MessagePackage_fields);
if (SimpleCommand_fields == type)
{
status = decode_unionmessage_contents(isrt, SimpleCommand_fields, &command);
if (status)
process_command();
}
else if (SimpleAnswer_fields == type)
{
status = decode_unionmessage_contents(isrt, SimpleAnswer_fields, &answer);
if (status)
process_answer();
}
else if (DebugPrint_fields == type)
{
status = decode_unionmessage_contents(isrt, DebugPrint_fields, &debug_print_message);
if (status)
process_debug_print_package();
}
else if (PositionNotify_fields == type)
{
status = decode_unionmessage_contents(isrt, PositionNotify_fields, &pos_notify);
if (status)
process_position_notify();
}
else
{
// Serial.println("Unknown package");
// ERROR_BLINK(9);
}
if (!status)
{
DEBUG_PRINTLN2("process_incoming_packet error ", PB_GET_ERROR(isrt));
}
return status;
}
bool packet_xb_writer(uint8_t* data, uint8_t size)
{
DEBUG_PRINTLN2("packet_xb_writer", (int)size);
sent = false;
sent_ok = false;
const char* pd = (char*)data;
mrf.send16(send_dst_addr, pd, size);
unsigned long now = millis();
do
{
DEBUG_PRINTLN("packet_xb_writer waiting");
process_mrf_packets();
} while (!sent);
DEBUG_PRINTLN("packet_xb_writer done");
return sent_ok;
}
/**************************************************************************
*
* Handles low level ACI events, and passes them up to an application
* level callback when appropriate
*
**************************************************************************/
void GoosciBleGatt::pollACI() {
static bool setup_required = false;
// We enter the if statement only when there is a ACI event
// available to be processed
if (lib_aci_event_get(&aci_state, &aci_data)) {
aci_evt_t *aci_evt;
aci_evt = &aci_data.evt;
// DEBUG_PRINTLN(F("pollACI"));
// DEBUG_PRINT("evt opcode: ");
// DEBUG_PRINTLN2(aci_evt->evt_opcode, HEX);
// DEBUG_PRINT(F("State Total credit: "));
// DEBUG_PRINTLN(aci_state.data_credit_total);
// DEBUG_PRINT(F("State Available credit: "));
// DEBUG_PRINTLN(aci_state.data_credit_available);
// DEBUG_PRINT("Event Available credit: ");
// DEBUG_PRINTLN(aci_evt->params.device_started.credit_available);
switch (aci_evt->evt_opcode) {
case ACI_EVT_DEVICE_STARTED: {
// DEBUG_PRINTLN("STARTED");
aci_state.data_credit_total =
aci_evt->params.device_started.credit_available;
switch (aci_evt->params.device_started.device_mode) {
case ACI_DEVICE_SETUP: {
// DEBUG_PRINTLN("SETUP");
aci_state.device_state = ACI_DEVICE_SETUP;
setup_required = true;
break;
}
case ACI_DEVICE_STANDBY: {
// DEBUG_PRINTLN("STANDBY");
aci_state.device_state = ACI_DEVICE_STANDBY;
// sleep_to_wakeup_timeout = 30;
if (aci_evt->params.device_started.hw_error) {
// Magic number used to make sure the HW error
// event is handled correctly.
delay(20);
} else {
get_address();
// DEBUG_PRINTLN("lib_aci_connect");
lib_aci_connect(adTimeout, adInterval);
}
break;
}
case ACI_DEVICE_INVALID: {
DEBUG_PRINTLN(F("Evt Device Invalid"));
break;
}
case ACI_DEVICE_TEST: {
break;
}
case ACI_DEVICE_SLEEP: {
break;
}
}
break; // case ACI_EVT_DEVICE_STARTED:
}
case ACI_EVT_CMD_RSP: {
DEBUG_PRINTLN("ACI_EVT_CMD_RSP");
// If an ACI command response event comes with an error -> stop
if (ACI_STATUS_SUCCESS != aci_evt->params.cmd_rsp.cmd_status) {
// ACI ReadDynamicData and ACI WriteDynamicData
// will have status codes of
// TRANSACTION_CONTINUE and TRANSACTION_COMPLETE
// all other ACI commands will have status code of
// ACI_STATUS_SCUCCESS for a successful command
DEBUG_PRINT(F("ACI Command 0x"));
DEBUG_PRINTLN2(aci_evt->params.cmd_rsp.cmd_opcode, HEX);
DEBUG_PRINT(F("Evt Cmd response: Error. "));
DEBUG_PRINTLN2(aci_evt->params.cmd_rsp.cmd_status, HEX);
}
if (ACI_CMD_GET_DEVICE_ADDRESS == aci_evt->params.cmd_rsp.cmd_opcode) {
// If this is a response to get device address, save the address
addrReceived = true;
// DEBUG_PRINT(F("Get device address response: "));
for (int i = BTLE_DEVICE_ADDRESS_SIZE - 1; i >= 0; --i) {
deviceAddress[i] = aci_evt->params.cmd_rsp.params.get_device_address
.bd_addr_own[i];
// DEBUG_PRINT2(deviceAddress[i], HEX);
// DEBUG_PRINT(F(":"));
}
// DEBUG_PRINTLN(F(""));
} else if (ACI_CMD_OPEN_ADV_PIPE ==
aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN(
F("Open advertising pipe response, setting service data."));
lib_aci_set_local_data(
&aci_state, PIPE_DEVICE_INFORMATION_MODEL_NUMBER_STRING_BROADCAST,
(unsigned char *)longName, 8);
DEBUG_PRINT(F("Advertising starting for "));
DEBUG_PRINTLN(deviceName);
lib_aci_connect(adTimeout, adInterval);
} else if (ACI_CMD_GET_DEVICE_VERSION == aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN("ACI_CMD_GET_DEVICE_VERSION");
} else if (ACI_CMD_SET_LOCAL_DATA == aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN("ACI_CMD_SET_LOCAL_DATA");
} else if (ACI_CMD_CONNECT == aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN("ACI_CMD_CONNECT");
} else if (ACI_CMD_DISCONNECT == aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN("ACI_CMD_DISCONNECT");
} else if (ACI_CMD_CHANGE_TIMING == aci_evt->params.cmd_rsp.cmd_opcode) {
DEBUG_PRINTLN("ACI_CMD_CHANGE_TIMING");
} else {
// print command
DEBUG_PRINT(F("Unexpected ACI Command 0x"));
DEBUG_PRINTLN2(aci_evt->params.cmd_rsp.cmd_opcode, HEX);
}
break;
}
case ACI_EVT_CONNECTED: {
// The nRF8001 is now connected to the peer device.
DEBUG_PRINTLN(F("Evt Connected"));
aci_state.data_credit_available = aci_state.data_credit_total;
timing_change_done = false;
break;
}
case ACI_EVT_PIPE_STATUS: {
DEBUG_PRINTLN(F("Evt Pipe Status: "));
// DEBUG_PRINT2((int) aci_evt->params.pipe_status.pipes_open_bitmap, HEX);
// DEBUG_PRINT(" ");
// DEBUG_PRINTLN2((int) aci_evt->params.pipe_status.pipes_closed_bitmap, HEX);
if (lib_aci_is_pipe_available(&aci_state, PIPE_GOOSCI_SENSOR_VALUE_TX) && !timing_change_done) {
lib_aci_change_timing(6, 6, 0, 600); // Low-latency parameters
timing_change_done = true;
}
break;
}
case ACI_EVT_TIMING: {
// DEBUG_PRINT("ACI_EVT_TIMING: ");
// DEBUG_PRINT(aci_evt->params.timing.conn_rf_interval);
// DEBUG_PRINT(" ");
// DEBUG_PRINT(aci_evt->params.timing.conn_slave_rf_latency);
// DEBUG_PRINT(" ");
// DEBUG_PRINT(aci_evt->params.timing.conn_rf_timeout);
// DEBUG_PRINT(" ");
// DEBUG_PRINTLN(aci_evt->params.timing.conn_rf_interval);
break;
}
case ACI_EVT_DISCONNECTED: {
// Advertise again if the advertising timed out.
DEBUG_PRINTLN(F("Evt Disconnected."));
// TODO(dek): figure out why the transition to using credits
// broke disconnection (packets are still transmitted).
// Setting the credits to 0 was an experiment but it didn't work.
// aci_state.data_credit_available = 0;
lib_aci_connect(adTimeout, adInterval);
timing_change_done = false;
break;
}
case ACI_EVT_DATA_RECEIVED: {
DEBUG_PRINTLN("ACI_EVT_DATA_RECEIVED");
if (aci_evt->params.data_received.rx_data.pipe_number == PIPE_GOOSCI_SENSOR_CONFIGURATION_RX_ACK_AUTO) {
int8_t packet_length = aci_evt->len;
handle(aci_evt->params.data_received.rx_data.aci_data, aci_evt->len);
} else {
DEBUG_PRINT(F(" Data(Hex) : "));
for (int i = 0; i < aci_evt->len - 2; i++) {
DEBUG_PRINT2(aci_evt->params.data_received.rx_data.aci_data[i], HEX);
DEBUG_PRINT(F(" "));
}
DEBUG_PRINTLN(F(""));
}
break;
}
case ACI_EVT_DATA_CREDIT: {
// DEBUG_PRINTLN(F("Evt Credit: Peer has received our send"));
aci_state.data_credit_available = aci_state.data_credit_available + aci_evt->params.data_credit.credit;
break;
}
case ACI_EVT_PIPE_ERROR: {
// See the appendix in the nRF8001
// Product Specication for details on the error codes
DEBUG_PRINT(F("ACI Evt Pipe Error: Pipe #:"));
DEBUG_PRINT2(aci_evt->params.pipe_error.pipe_number, DEC);
DEBUG_PRINT(F(" Pipe Error Code: 0x"));
DEBUG_PRINTLN2(aci_evt->params.pipe_error.error_code, HEX);
// Increment the credit available as the data packet was not sent.
// The pipe error also represents the Attribute protocol
// Error Response sent from the peer and that should not be counted
// for the credit.
if (ACI_STATUS_ERROR_PEER_ATT_ERROR !=
aci_evt->params.pipe_error.error_code) {
aci_state.data_credit_available++;
}
break;
}
case ACI_EVT_DATA_ACK: {
// DEBUG_PRINTLN(F("ACK"));
break;
}
case ACI_EVT_HW_ERROR: {
DEBUG_PRINTLN(F("HW error: "));
DEBUG_PRINTLN2(aci_evt->params.hw_error.line_num, DEC);
for (int counter = 0; counter <= (aci_evt->len - 3); counter++) {
DEBUG_PRINT(aci_evt->params.hw_error.file_name[counter]);
}
DEBUG_PRINTLN();
initLocalData();
lib_aci_connect(adTimeout, adInterval);
break;
}
default: {
DEBUG_PRINT(F("Evt Opcode 0x"));
DEBUG_PRINT2(aci_evt->evt_opcode, HEX);
DEBUG_PRINTLN(F(" unhandled"));
break;
}
}
}
// setup_required is set to true when the device starts
// up and enters setup mode.
// It indicates that do_aci_setup() should be called.
// The flag should be cleared if do_aci_setup() returns
// ACI_STATUS_TRANSACTION_COMPLETE.
if (setup_required) {
int result = do_aci_setup(&aci_state);
if (result != SETUP_SUCCESS ) {
switch(result) {
case SETUP_FAIL_COMMAND_QUEUE_NOT_EMPTY:
DEBUG_PRINTLN("SETUP_FAIL_COMMAND_QUEUE_NOT_EMPTY");
break;
case SETUP_FAIL_EVENT_QUEUE_NOT_EMPTY:
DEBUG_PRINTLN("SETUP_EVENT_COMMAND_QUEUE_NOT_EMPTY");
break;
case SETUP_FAIL_NOT_SETUP_EVENT:
DEBUG_PRINTLN("SETUP_FAIL_NOT_SETUP_EVENT");
break;
case SETUP_FAIL_NOT_COMMAND_RESPONSE:
DEBUG_PRINTLN("SETUP_FAIL_NOT_COMMAND_RESPONSE");
break;
}
} else {
setup_required = false;
}
}
}
