void cdc_rx_notify(uint8_t port) {
l("cdc_rx_notify [%d]", port);
uint8_t b = udi_cdc_getc();
if (b != 0x08) {
l("Protocol desync");
}
l("First byte ok");
uint32_t offset = 0;
do {
buffer[offset++] = b;
b = udi_cdc_getc();
l("-> 0x%02x", b);
} while (b & 0x80);
buffer[offset++] = b;
// Now we have enough to know the size
l("Length read, decoding...");
l("... 0x%02x 0x%02x", buffer[0], buffer[1]);
pb_istream_t istream = pb_istream_from_buffer(buffer + 1, USB_BUFFER_SIZE);
l("istream bytes_left before %d", istream.bytes_left);
uint64_t len = 0;
pb_decode_varint(&istream, &len);
l("message_length %d", (uint32_t)len);
l("offset %d", offset);
udi_cdc_read_buf(buffer + offset, len);
l("decode message");
istream = pb_istream_from_buffer(buffer + offset, len);
DonglePiRequest request = {0};
request.config.i2c.funcs.decode = handle_i2c_config_cb;
request.config.uart.funcs.decode = handle_uart_config_cb;
request.config.spi.funcs.decode = handle_spi_config_cb;
request.config.gpio.pins.funcs.decode = handle_gpio_pin_config_cb;
request.data.i2c.writes.funcs.decode = handle_i2c_write_cb;
if (!pb_decode(&istream, DonglePiRequest_fields, &request)) {
l("failed to decode the packet, wait for more data");
return;
}
l("Request #%d received", request.message_nb);
if (request.has_data && request.data.has_gpio) {
handle_gpio_write(request.data.gpio);
}
pb_ostream_t ostream = pb_ostream_from_buffer(buffer, USB_BUFFER_SIZE);
DonglePiResponse response = {};
response.message_nb = request.message_nb;
l("Create response for #%d", response.message_nb);
handle_gpio_read(&response);
pb_encode_delimited(&ostream, DonglePiResponse_fields, &response);
l("Write response nb_bytes = %d", ostream.bytes_written);
uint32_t wrote = udi_cdc_write_buf(buffer, ostream.bytes_written);
l("Done. wrote %d bytes", wrote);
}
int main(int argc, char** argv) {
if (argc != 2) {fprintf(stderr, "%s our_sk", argv[0]); return 1;}
unsigned char sk[crypto_box_SECRETKEYBYTES];
{
FILE* f = fopen(argv[1], "r");
if (f == NULL) {fprintf(stderr, "Cannot open our secret key file"); return 1;}
int len = fread(sk, 1, crypto_box_SECRETKEYBYTES, f);
fclose(f);
if (len != crypto_box_SECRETKEYBYTES) {fprintf(stderr, "Bad secret key file"); return 1;}
}
uint8_t buf[1<<16];
int len = fread(buf, 1, 1<<16, stdin);
pb_istream_t stream = pb_istream_from_buffer(buf, len);
Box box;
if (!pb_decode(&stream, Box_fields, &box)) return 1;
if (box.enc_algo != 1) return 1;
uint8_t n[crypto_box_NONCEBYTES];
for (int i=0; i<crypto_box_NONCEBYTES; ++i) n[i] = box.data.bytes[i];
for (int i=0; i<crypto_box_BOXZEROBYTES; ++i) box.data.bytes[i] = 0;
stream = pb_istream_from_buffer(box.sender.bytes, box.sender.size);
PublicKey pk;
if (!pb_decode(&stream, PublicKey_fields, &pk)) return 1;
PublicKeyData pkd;
stream = pb_istream_from_buffer(pk.publickey_msg.bytes,
pk.publickey_msg.size);
if (!pb_decode(&stream, PublicKeyData_fields, &pkd)) return 1;
int decrypted = 0;
for (int i=0; i<min(pkd.enc_keys_count,pkd.enc_algos_count); i++) {
if (pkd.enc_keys[i].size == crypto_box_PUBLICKEYBYTES
&& pkd.sig_algos[i] == 1) {
uint8_t* encpk = &pkd.enc_keys[i].bytes[0];
if (crypto_box_open(buf,box.data.bytes+8,box.data.size-8,n,encpk,sk)
== 0) {
decrypted = 1;
break;
}
}
}
for (int i=0; i<crypto_box_SECRETKEYBYTES; i++) sk[i] = 0;
if (decrypted) {
int got = fwrite(buf+crypto_box_ZEROBYTES, 1, box.data.size-8-crypto_box_ZEROBYTES, stdout);
assert(got == box.data.size-8-crypto_box_ZEROBYTES);
got = fwrite(box.sender.bytes, 1, box.sender.size, stderr);
assert(got == box.sender.size);
}
return !decrypted;
}
int main(int argc, char **argv)
{
uint8_t buffer[1024];
size_t count;
pb_istream_t stream;
/* Whether to expect the optional values or the default values. */
int mode = (argc > 1) ? atoi(argv[1]) : 0;
/* Read the data into buffer */
SET_BINARY_MODE(stdin);
count = fread(buffer, 1, sizeof(buffer), stdin);
/* Construct a pb_istream_t for reading from the buffer */
stream = pb_istream_from_buffer(buffer, count);
/* Decode and print out the stuff */
if (!check_alltypes(&stream, mode))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
// Validate the contents of a Resource proto. `id` is the intended resource id.
static bool validate_resource_pb(const uint8_t *resource_pb,
size_t resource_pb_size, size_t id) {
GPR_ASSERT(id < n_resources);
if (resource_pb == NULL) {
return false;
}
google_census_Resource vresource;
vresource.name.funcs.decode = &validate_string;
vresource.name.arg = resources[id];
vresource.description.funcs.decode = &validate_string;
vresource.description.arg = resources[id];
vresource.unit.numerator.funcs.decode = &validate_units;
vresource.unit.numerator.arg = resources[id];
vresource.unit.denominator.funcs.decode = &validate_units;
vresource.unit.denominator.arg = resources[id];
pb_istream_t stream =
pb_istream_from_buffer((uint8_t *)resource_pb, resource_pb_size);
if (!pb_decode(&stream, google_census_Resource_fields, &vresource)) {
return false;
}
// A Resource must have a name, a unit, with at least one numerator.
return (resources[id]->name != NULL && vresource.has_unit &&
resources[id]->n_numerators > 0);
}
bool decode_trace_context(google_trace_TraceContext *ctxt, uint8_t *buffer,
const size_t nbytes) {
// Create a stream that reads nbytes from the buffer.
pb_istream_t stream = pb_istream_from_buffer(buffer, nbytes);
// decode message
bool status = pb_decode(&stream, google_trace_TraceContext_fields, ctxt);
if (!status) {
gpr_log(GPR_DEBUG, "TraceContext decoding failed: %s",
PB_GET_ERROR(&stream));
return false;
}
// check fields
if (!ctxt->has_trace_id) {
gpr_log(GPR_DEBUG, "Invalid TraceContext: missing trace_id");
return false;
}
if (!ctxt->has_span_id) {
gpr_log(GPR_DEBUG, "Invalid TraceContext: missing span_id");
return false;
}
return true;
}
void ennoSystemMessageHandler(char* topic, char* payload, int length) {
Device_Header header;
pb_istream_t stream = pb_istream_from_buffer(payload, length);
if (pb_decode_delimited(&stream, Device_Header_fields, &header)) {
if (header.command == Device_Command_REGISTER_ACK) {
Device_RegistrationAck ack;
if (pb_decode_delimited(&stream, Device_RegistrationAck_fields, &ack)) {
if (ack.state == Device_RegistrationAckState_NEW_REGISTRATION) {
printf("Registered new device.\n");
registered = true;
}
else if (ack.state == Device_RegistrationAckState_ALREADY_REGISTERED) {
printf("Device has already registered.\n");
}
else if (ack.state == Device_RegistrationAckState_REGISTRATION_ERROR) {
printf("Error rigistering device.\n");
}
}
}
} else {
printf("Unable to decode system command.\n");
}
}
int main(int argc, char **argv)
{
uint8_t buffer[AnonymousOneOfMessage_size];
size_t count;
int option;
if (argc != 2)
{
fprintf(stderr, "Usage: decode_oneof [number]\n");
return 1;
}
option = atoi(argv[1]);
SET_BINARY_MODE(stdin);
count = fread(buffer, 1, sizeof(buffer), stdin);
if (!feof(stdin))
{
printf("Message does not fit in buffer\n");
return 1;
}
{
int status = 0;
pb_istream_t stream;
stream = pb_istream_from_buffer(buffer, count);
status = test_oneof_1(&stream, option);
if (status != 0)
return status;
}
return 0;
}
/** Handle a system command */
void handleSystemCommand(byte* payload, unsigned int length) {
Device_Header header;
pb_istream_t stream = pb_istream_from_buffer(payload, length);
// Read header to find what type of command follows.
if (pb_decode_delimited(&stream, Device_Header_fields, &header)) {
// Handle a registration acknowledgement.
if (header.command == Device_Command_REGISTER_ACK) {
Device_RegistrationAck ack;
if (pb_decode_delimited(&stream, Device_RegistrationAck_fields, &ack)) {
if (ack.state == Device_RegistrationAckState_NEW_REGISTRATION) {
baseEvents_log("Registered new device.");
registered = true;
} else if (ack.state == Device_RegistrationAckState_ALREADY_REGISTERED) {
baseEvents_log("Device was already registered.");
registered = true;
} else if (ack.state == Device_RegistrationAckState_REGISTRATION_ERROR) {
baseEvents_log("Error registering device.");
}
}
}
} else {
baseEvents_log("Unable to decode system command.");
}
}
int main()
{
uint8_t buffer[Person_size];
pb_istream_t stream;
size_t count;
/* Read the data into buffer */
SET_BINARY_MODE(stdin);
count = fread(buffer, 1, sizeof(buffer), stdin);
if (!feof(stdin))
{
printf("Message does not fit in buffer\n");
return 1;
}
/* Construct a pb_istream_t for reading from the buffer */
stream = pb_istream_from_buffer(buffer, count);
/* Decode and print out the stuff */
if (!print_person(&stream))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
bool NetDecode(
ENetPacket *packet, void *dest, const pb_field_t *fields)
{
pb_istream_t stream = pb_istream_from_buffer(
packet->data + NET_MSG_SIZE, packet->dataLength - NET_MSG_SIZE);
bool status = pb_decode(&stream, fields, dest);
CASSERT(status, "Failed to decode pb");
return status;
}
void msg_process(char type, uint16_t msg_id, const pb_field_t *fields, uint8_t *msg_raw, uint32_t msg_size)
{
static uint8_t msg_data[MSG_IN_SIZE];
pb_istream_t stream = pb_istream_from_buffer(msg_raw, msg_size);
bool status = pb_decode(&stream, fields, msg_data);
if (status) {
MessageProcessFunc(type, 'i', msg_id, msg_data);
} else {
fsm_sendFailure(FailureType_Failure_SyntaxError, stream.errmsg);
}
}
std::shared_ptr<Graph> ImportIRGraph(const std::string& serialized_graph,
std::vector<at::Tensor>& initializers) {
pb_istream_t istream = pb_istream_from_buffer(reinterpret_cast<const pb_byte_t *>(serialized_graph.data()), serialized_graph.size());
auto model = Reader<Model_>::read(&istream);
auto graph = buildGraph(model.graph, initializers);
return graph;
}
void msg_read_tiny(uint8_t *buf, int len)
{
if (len != 64) return;
if (buf[0] != '?' || buf[1] != '#' || buf[2] != '#') {
return;
}
uint16_t msg_id = (buf[3] << 8) + buf[4];
uint32_t msg_size = (buf[5] << 24) + (buf[6] << 16) + (buf[7] << 8) + buf[8];
if (msg_size > 64 || len - msg_size < 9) {
return;
}
const pb_field_t *fields = 0;
pb_istream_t stream = pb_istream_from_buffer(buf + 9, msg_size);
switch (msg_id) {
case MessageType_MessageType_PinMatrixAck:
fields = PinMatrixAck_fields;
break;
case MessageType_MessageType_ButtonAck:
fields = ButtonAck_fields;
break;
case MessageType_MessageType_PassphraseAck:
fields = PassphraseAck_fields;
break;
case MessageType_MessageType_Cancel:
fields = Cancel_fields;
break;
case MessageType_MessageType_Initialize:
fields = Initialize_fields;
break;
#if DEBUG_LINK
case MessageType_MessageType_DebugLinkDecision:
fields = DebugLinkDecision_fields;
break;
case MessageType_MessageType_DebugLinkGetState:
fields = DebugLinkGetState_fields;
break;
#endif
}
if (fields) {
bool status = pb_decode(&stream, fields, msg_tiny);
if (status) {
msg_tiny_id = msg_id;
} else {
fsm_sendFailure(FailureType_Failure_SyntaxError, stream.errmsg);
msg_tiny_id = 0xFFFF;
}
} else {
fsm_sendFailure(FailureType_Failure_UnexpectedMessage, "Unknown message");
msg_tiny_id = 0xFFFF;
}
}
int main(int argc, char** argv) {
int human = argc == 2 && argv[1][0] == '-' && argv[1][1] == 'h';
uint8_t buffer[256];
int len = fread(buffer, 1, sizeof(buffer), stdin);
pb_istream_t stream = pb_istream_from_buffer(buffer, len);
PublicKey pk;
if (!pb_decode(&stream, PublicKey_fields, &pk)) return 1;
PublicKeyData pkd;
stream = pb_istream_from_buffer(pk.publickey_msg.bytes,
pk.publickey_msg.size);
if (!pb_decode(&stream, PublicKeyData_fields, &pkd)) return 1;
for (int i=0; i<pkd.sig_keys_count; i++) {
if ( pk.sigs_count < i
|| pk.sigs[i].size < crypto_sign_BYTES
|| pkd.sig_keys[i].size < crypto_sign_PUBLICKEYBYTES
|| crypto_sign_detach_open(pk.publickey_msg.bytes,
pk.publickey_msg.size, pk.sigs[i].bytes, pkd.sig_keys[i].bytes)
!= 0) {
fprintf(stderr, "Invalid key\n");
return 2;
} else {
initialise(1344, 256, 128);
char* dgst = digest((char*)pkd.sig_keys[i].bytes,
pkd.sig_keys[i].size, 1);
for (int i=0; i<128/8; i+=2) {
if (i > 0 && human) {
fprintf(stdout, " ");
if (i == 8) fprintf(stdout, " ");
}
fprintf(stdout, "%X", (dgst[i]&0xF0)>>4);
fprintf(stdout, "%X", dgst[i]&0x0F);
fprintf(stdout, "%X", (dgst[i+1]&0xF0)>>4);
fprintf(stdout, "%X", dgst[i+1]&0x0F);
}
if (human) printf("\n");
else return 1;
dispose();
}
}
}
static bool checkreturn decode_callback_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter)
{
pb_callback_t *pCallback = (pb_callback_t*)iter->pData;
#ifdef PB_OLD_CALLBACK_STYLE
void *arg = pCallback->arg;
#else
void **arg = &(pCallback->arg);
#endif
if (pCallback->funcs.decode == NULL)
return pb_skip_field(stream, wire_type);
if (wire_type == PB_WT_STRING)
{
pb_istream_t substream;
if (!pb_make_string_substream(stream, &substream))
return false;
do
{
if (!pCallback->funcs.decode(&substream, iter->pos, arg))
PB_RETURN_ERROR(stream, "callback failed");
} while (substream.bytes_left);
pb_close_string_substream(stream, &substream);
return true;
}
else
{
/* Copy the single scalar value to stack.
* This is required so that we can limit the stream length,
* which in turn allows to use same callback for packed and
* not-packed fields. */
pb_istream_t substream;
uint8_t buffer[10];
size_t size = sizeof(buffer);
if (!read_raw_value(stream, wire_type, buffer, &size))
return false;
substream = pb_istream_from_buffer(buffer, size);
return pCallback->funcs.decode(&substream, iter->pos, arg);
}
}
/** Handle a command specific to the specification for this device */
void handleSpecificationCommand(byte* payload, unsigned int length) {
ArduinoCustom__Header header;
memset(buffer,0,300);
ArduinoCustom_testData testEvents;
pb_istream_t stream = pb_istream_from_buffer(payload, length);
if (pb_decode_delimited(&stream, ArduinoCustom__Header_fields, &header)) {
baseEvents_log("Decoded header for custom command.");
if (header.command == ArduinoCustom_Command_RGB_LED) {
if (pb_decode_delimited(&stream, ArduinoCustom_RGB_fields, &RGB_LED)) {
baseEvents_log("Command: RGB_LED set(h=%d, s=%d, b=%d)",
RGB_LED.rgbled_h, RGB_LED.rgbled_s, RGB_LED.rgbled_b);
hsb2rgb_led_open(RGB_LED.rgbled_h, RGB_LED.rgbled_s, RGB_LED.rgbled_b);
}
}
else if (header.command == ArduinoCustom_Command_DC_MOTOR) {
ArduinoCustom_DC_MOTOR dc_motor;
if (pb_decode_delimited(&stream, ArduinoCustom__Header_fields, &dc_motor)) {
baseEvents_log("Command: DC_MOTOR set: %d", dc_motor.motor_sw);
dc_motor_set(dc_motor.motor_sw);
}
}
else if (header.command == ArduinoCustom_Command_PING) {
ArduinoCustom_ping ping;
if (pb_decode_delimited(&stream, ArduinoCustom_ping_fields, &ping)) {
handlePing(ping, header.originator);
}
}
else if (header.command == ArduinoCustom_Command_TESTEVENTS) {
if (pb_decode_delimited(&stream, ArduinoCustom_testEvents_fields, &testEvents)) {
handleTestEvents(testEvents, header.originator);
}
}
else if (header.command == ArduinoCustom_Command_SERIALPRINTLN) {
ArduinoCustom_serialPrintln serialPrintln;
if (pb_decode_delimited(&stream, ArduinoCustom_serialPrintln_fields, &serialPrintln)) {
handleSerialPrintln(serialPrintln,header.originator);
}
}
else {
baseEvents_log("Unknown command.");
}
}
}
/**
* Message SSMsg decode function
* @param buffer
* @param buf_len
* @param ssmsg
* @return
*/
int ss_pb_decode(uint8_t *buffer,int buf_len,SSMsg *ssmsg)
{
bool status;
pb_istream_t stream = pb_istream_from_buffer(buffer, buf_len);
//ssmsg->devlist.funcs.decode=&ss_decode_devlist;
log_printf(LOG_VERBOSE,"SSMsg Decoding! \n");
status=pb_decode(&stream, SSMsg_fields, ssmsg);
if (!status)
{
log_printf(LOG_ERROR,"Decoding failed: %s\n", PB_GET_ERROR(&stream));
return -1;
}
return 1;
}
grpc_grpclb_initial_response *grpc_grpclb_initial_response_parse(
grpc_slice encoded_grpc_grpclb_response) {
pb_istream_t stream =
pb_istream_from_buffer(GRPC_SLICE_START_PTR(encoded_grpc_grpclb_response),
GRPC_SLICE_LENGTH(encoded_grpc_grpclb_response));
grpc_grpclb_response res;
memset(&res, 0, sizeof(grpc_grpclb_response));
if (!pb_decode(&stream, grpc_lb_v1_LoadBalanceResponse_fields, &res)) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
grpc_grpclb_initial_response *initial_res =
gpr_malloc(sizeof(grpc_grpclb_initial_response));
memcpy(initial_res, &res.initial_response,
sizeof(grpc_grpclb_initial_response));
return initial_res;
}
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);
}
int main()
{
TestResults results = {};
AllTypes message = {};
pb_istream_t istream = pb_istream_from_buffer((uint8_t*)input_data, sizeof(input_data));
results.success = pb_decode(&istream, AllTypes_fields, &message);
results.success &= (message.end == 1099);
results.has_stack_usage = true;
results.stack_usage = platform_stack_usage();
uint8_t buf[16];
pb_ostream_t ostream = pb_ostream_from_buffer(buf, sizeof(buf));
pb_encode(&ostream, TestResults_fields, &results);
platform_write(buf, ostream.bytes_written);
return 0;
}
void messageRxTask(void *pvParameters) {
BufferCntType bytesRead;
for (;;) {
bytesRead = usbReceiveFrame(gRxFrameBuffer, FRAME_BUFFER_MAX_LEN);
if (bytesRead > 0) {
pb_istream_t stream = pb_istream_from_buffer(gRxFrameBuffer, bytesRead);
const pb_field_t *type = decodeMotorMsgType(&stream);
if (type == MotorMsg_Data_fields) {
processDataMsg(&stream);
} else if (type == MotorMsg_Cmd_fields) {
processCmdMsg(&stream);
}
}
// Wait 10ms before we run another.
vTaskDelay(10 / portTICK_PERIOD_MS);
}
}
static void update_pbring(void *arg, long l)
{
pbring_inst_t *p = (pbring_inst_t *) arg;
ring_size_t cmdsize = record_next_size(&p->to_rt_rb);
if (cmdsize < 0) {
// command ring empty
*(p->underrun) += 1;
return;
}
const void *cmdbuffer = record_next(&p->to_rt_rb);
pb_istream_t stream = pb_istream_from_buffer((void *) cmdbuffer, cmdsize);
int retval;
if (!decode_msg(p, &stream)) {
// process command here
// prepare reply
tx.has_motstat = true;
tx.type = pb_ContainerType_MT_MOTSTATUS;
tx.note.funcs.encode = npb_encode_string;
tx.note.arg = "hi there!";
tx.motstat = (pb_MotionStatus) {
.commandEcho = rx.motcmd.command,
.commandNumEcho = rx.motcmd.commandNum,
.commandStatus = pb_cmd_status_t_EMCMOT_COMMAND_OK,
.has_carte_pos_fb = true,
.carte_pos_fb = {
.tran = {
.has_x = true,
.x = 42.0,
.has_y = true,
.y = 13.56,
.has_z = true,
.z = 27.12
},
.has_a = true,
.a = 3.14
}
};
grpc_grpclb_serverlist *grpc_grpclb_response_parse_serverlist(
grpc_slice encoded_grpc_grpclb_response) {
bool status;
decode_serverlist_arg arg;
pb_istream_t stream =
pb_istream_from_buffer(GRPC_SLICE_START_PTR(encoded_grpc_grpclb_response),
GRPC_SLICE_LENGTH(encoded_grpc_grpclb_response));
pb_istream_t stream_at_start = stream;
grpc_grpclb_response res;
memset(&res, 0, sizeof(grpc_grpclb_response));
memset(&arg, 0, sizeof(decode_serverlist_arg));
res.server_list.servers.funcs.decode = decode_serverlist;
res.server_list.servers.arg = &arg;
arg.first_pass = true;
status = pb_decode(&stream, grpc_lb_v1_LoadBalanceResponse_fields, &res);
if (!status) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
arg.first_pass = false;
status =
pb_decode(&stream_at_start, grpc_lb_v1_LoadBalanceResponse_fields, &res);
if (!status) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
grpc_grpclb_serverlist *sl = gpr_zalloc(sizeof(grpc_grpclb_serverlist));
sl->num_servers = arg.num_servers;
sl->servers = arg.servers;
if (res.server_list.has_expiration_interval) {
sl->expiration_interval = res.server_list.expiration_interval;
}
return sl;
}
void ennoCommandMessageHandler(char* topic, char* payload, int length){
ArduinoCustom__Header header;
pb_istream_t stream = pb_istream_from_buffer(payload, length);
if (pb_decode_delimited(&stream, ArduinoCustom__Header_fields, &header)) {
printf("Decoded header for custom command.\n");
if (header.command == ArduinoCustom_Command_PING) {
ArduinoCustom_ping ping;
if (pb_decode_delimited(&stream, ArduinoCustom_ping_fields, &ping)) {
handlePing(ping, header.originator);
}
} else if (header.command == ArduinoCustom_Command_TESTEVENTS) {
ArduinoCustom_testEvents testEvents;
if (pb_decode_delimited(&stream, ArduinoCustom_testEvents_fields, &testEvents)) {
handleTestEvents(testEvents, header.originator);
}
} else if (header.command == ArduinoCustom_Command_SERIALPRINTLN) {
ArduinoCustom_serialPrintln serialPrintln;
if (pb_decode_delimited(&stream, ArduinoCustom_serialPrintln_fields, &serialPrintln)) {
handleSerialPrintln(serialPrintln, header.originator);
}
}
}
}
void MotorController::recieveResponse()
{
/* Read response from the server */
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
memset(buffer, 0, sizeof(buffer));
/* read from the buffer */
n = (*sock_).readMessage(buffer); // blocks until data is read
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
bool status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
publishResponse(response);
ROS_INFO_STREAM_THROTTLE(log_period_, "Rate: " << 1 / response.dt_sec << "hz.");
}
// thread function, per-instance
// interpolates all joints of this instance
static int update(void *arg, const hal_funct_args_t *fa)
{
struct inst_data *ip = (struct inst_data *) arg;
double period = ((double) fa_period(fa)) * 1e-9;
int i;
if (segment_completed(ip, period)) {
// check for a new JointTrajectoryPoint
void *data;
ringsize_t size;
if (record_read(&ip->traj, (const void**)&data, &size) == 0) {
// protobuf-decode it
pb_istream_t stream = pb_istream_from_buffer(data, size);
pb_JointTrajectoryPoint rx = pb_JointTrajectoryPoint_init_zero;
if (!pb_decode(&stream, pb_JointTrajectoryPoint_fields, &rx)) {
rtapi_print_msg(RTAPI_MSG_ERR, "%s: pb_decode(JointTrajectoryPoint) failed: '%s'",
compname, PB_GET_ERROR(&stream));
} else {
// decode ok - start a new segment
double duration = *(ip->duration) = rx.time_from_start - ip->time_from_start;
// the very first point in the ringbuffer is not a segment.
// therefore we need to "jump" to these initial settings for the
// interpolator to calculate the correct path.
// for example, a path can start at position, velocity and acceleration
// who are non-zero. In a typical ROS message the first point has a
// duration of "0.0"
if (duration == 0.0) {
// set the start positions
// or try out to drop this point later on
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
*(jp->curr_pos) = *(jp->end_pos) = rx.positions[i];
*(jp->curr_vel) = *(jp->end_vel) = rx.velocities[i];
*(jp->curr_acc) = *(jp->end_acc) = rx.accelerations[i];
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
// so when we have read the first point, we need to discard everythin
// else and make sure we will read the second point, as to complete the
// first segment
} else {
generatePowers(*(ip->degree), duration, ip->powers);
ip->time_from_start = rx.time_from_start;
*(ip->progress) = 0.0;
for (i = 0; i < rx.positions_count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
double pos2 = *(jp->end_pos) = rx.positions[i];
double vel2 = *(jp->end_vel) = rx.velocities[i];
double acc2 = *(jp->end_acc) = rx.accelerations[i];
double pos1 = *(jp->curr_pos);
double vel1 = *(jp->curr_vel);
double acc1 = *(jp->curr_acc);
switch (*(ip->degree)) {
case 1:
jp->coeff[0] = pos1;
jp->coeff[1] = (pos2 - pos1) / duration;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 3:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*ip->powers[1] - vel2*ip->powers[1]) / ip->powers[2];
jp->coeff[3] = (2.0*pos1 - 2.0*pos2 + vel1*ip->powers[1] + vel2*ip->powers[1]) / ip->powers[3];
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 5:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = 0.5 * acc1;
jp->coeff[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*ip->powers[2] + acc2*ip->powers[2] -
12.0*vel1*ip->powers[1] - 8.0*vel2*ip->powers[1]) / (2.0*ip->powers[3]);
jp->coeff[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*ip->powers[2] - 2.0*acc2*ip->powers[2] +
16.0*vel1*ip->powers[1] + 14.0*vel2*ip->powers[1]) / (2.0*ip->powers[4]);
jp->coeff[5] = (-12.0*pos1 + 12.0*pos2 - acc1*ip->powers[2] + acc2*ip->powers[2] -
6.0*vel1*ip->powers[1] - 6.0*vel2*ip->powers[1]) / (2.0*ip->powers[5]);
break;
}
}
}
}
record_shift(&ip->traj); // consume record
} else {
// segment completed and no new point in ringbuffer
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = false;
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
}
}
*(ip->progress) += period;
generatePowers(*(ip->degree), *(ip->progress), ip->pnow);
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
interpolate_joint(ip, jp, *(ip->progress), 0);
}
return 0;
}
static ssize_t sacd_net_input_read(sacd_input_t dev, int pos, int blocks, void *buffer)
{
if (!dev)
{
return 0;
}
else
{
uint8_t output_buf[16];
ServerRequest request;
ServerResponse response;
pb_ostream_t output = pb_ostream_from_buffer(output_buf, sizeof(output_buf));
pb_istream_t input = pb_istream_from_socket(&dev->fd);
uint8_t zero = 0;
request.type = ServerRequest_Type_DISC_READ;
request.sector_offset = pos;
request.sector_count = blocks;
if (!pb_encode(&output, ServerRequest_fields, &request))
{
return 0;
}
/* We signal the end of request with a 0 tag. */
pb_write(&output, &zero, 1);
// write the output buffer to the opened socket
{
bool ret;
size_t written;
ret = (socket_send(&dev->fd, (char *) output_buf, output.bytes_written, &written, 0, 0) == IO_DONE && written == output.bytes_written);
if (!ret)
return 0;
}
#if 0
response.data.bytes = buffer;
{
size_t got;
uint8_t *buf_ptr = dev->input_buffer;
size_t buf_left = blocks * SACD_LSN_SIZE + 16;
input = pb_istream_from_buffer(dev->input_buffer, MAX_PROCESSING_BLOCK_SIZE * SACD_LSN_SIZE + 1024);
if (socket_recv(&dev->fd, (char *) buf_ptr, buf_left, &got, MSG_PARTIAL, 0) != IO_DONE)
return 0;
while(got > 0 && !pb_decode(&input, ServerResponse_fields, &response))
{
buf_ptr += got;
buf_left -= got;
if (socket_recv(&dev->fd, (char *) buf_ptr, buf_left, &got, MSG_PARTIAL, 0) != IO_DONE)
return 0;
input = pb_istream_from_buffer(dev->input_buffer, MAX_PROCESSING_BLOCK_SIZE * SACD_LSN_SIZE + 1024);
}
}
#else
response.data.bytes = buffer;
if (!pb_decode(&input, ServerResponse_fields, &response))
{
return 0;
}
#endif
if (response.type != ServerResponse_Type_DISC_READ)
{
return 0;
}
if (response.has_data)
{
return response.result;
}
}
return 0;
}
void MotorController::setPID()
{
ros::Rate rate(frequency_);
ROS_INFO_STREAM("Setting PID Values:"
<< "\n\t P => L: " << p_l_ << " R: " << p_r_ << "\n\t D => L: " << d_l_ << " R: " << d_r_
<< "\n\t I => L: " << i_l_ << " R: " << i_r_ << "\n\t Kv => L: " << kv_l_ << " R: " << kv_r_);
bool valid_values = false; // pid values have been set correctly
/* This is the buffer where we will store the request message. */
uint8_t requestbuffer[256];
size_t message_length;
bool status;
/* allocate space for the request message to the server */
RequestMessage request = RequestMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer));
/* indicate that pid fields will contain values */
request.has_p_l = true;
request.has_p_r = true;
request.has_i_l = true;
request.has_i_r = true;
request.has_d_l = true;
request.has_d_r = true;
request.has_kv_l = true;
request.has_kv_r = true;
/* fill in the message fields */
request.p_l = static_cast<float>(p_l_);
request.p_r = static_cast<float>(p_r_);
request.i_l = static_cast<float>(i_l_);
request.i_r = static_cast<float>(i_r_);
request.d_l = static_cast<float>(d_l_);
request.d_r = static_cast<float>(d_r_);
request.kv_l = static_cast<float>(kv_l_);
request.kv_r = static_cast<float>(kv_r_);
/* encode the protobuffer */
status = pb_encode(&ostream, RequestMessage_fields, &request);
message_length = ostream.bytes_written;
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream));
ros::shutdown();
}
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
// unsigned char responsebuffer[256];
/* Send PID values via ethernet and recieve response to ensure proper setting */
while (ros::ok() && !valid_values)
{
(*sock_).sendMessage(reinterpret_cast<char*>(requestbuffer), message_length);
memset(buffer, 0, sizeof(buffer));
n = (*sock_).readMessage(buffer); // blocks until data is read
// memcpy(responsebuffer, buffer, sizeof(responsebuffer));
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
valid_values = (response.p_l == static_cast<float>(p_l_)) && (response.p_r == static_cast<float>(p_r_)) &&
(response.i_l == static_cast<float>(i_l_)) && (response.i_r == static_cast<float>(i_r_)) &&
(response.d_l == static_cast<float>(d_l_)) && (response.d_r == static_cast<float>(d_r_)) &&
(response.kv_l == static_cast<float>(kv_l_)) && (response.kv_r == static_cast<float>(kv_r_));
rate.sleep();
}
ROS_INFO_ONCE("Sucessfully set all PID values");
}
/* Basic fields, nested submessages, extensions */
static bool test_TestMessage()
{
uint8_t buffer[256];
size_t msgsize;
/* Construct a message with various fields filled in */
{
TestMessage msg = TestMessage_init_zero;
pb_ostream_t stream;
fill_TestMessage(&msg);
stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
if (!pb_encode(&stream, TestMessage_fields, &msg))
{
fprintf(stderr, "Encode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
msgsize = stream.bytes_written;
}
/* Output encoded message for debug */
SET_BINARY_MODE(stdout);
fwrite(buffer, 1, msgsize, stdout);
/* Decode memory using dynamic allocation */
{
TestMessage msg = TestMessage_init_zero;
pb_istream_t stream;
SubMessage ext2_dest;
msg.extensions = &ext1;
ext1.type = &dynamic_ext;
ext1.dest = NULL;
ext1.next = &ext2;
ext2.type = &static_ext;
ext2.dest = &ext2_dest;
ext2.next = NULL;
stream = pb_istream_from_buffer(buffer, msgsize);
if (!pb_decode(&stream, TestMessage_fields, &msg))
{
fprintf(stderr, "Decode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
/* Make sure it encodes back to same data */
{
uint8_t buffer2[256];
pb_ostream_t ostream = pb_ostream_from_buffer(buffer2, sizeof(buffer2));
TEST(pb_encode(&ostream, TestMessage_fields, &msg));
TEST(ostream.bytes_written == msgsize);
TEST(memcmp(buffer, buffer2, msgsize) == 0);
}
/* Make sure that malloc counters work */
TEST(get_alloc_count() > 0);
/* Make sure that pb_release releases everything */
pb_release(TestMessage_fields, &msg);
TEST(get_alloc_count() == 0);
/* Check that double-free is a no-op */
pb_release(TestMessage_fields, &msg);
TEST(get_alloc_count() == 0);
}
return true;
}
static bool checkreturn decode_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter)
{
pb_decoder_t func = PB_DECODERS[PB_LTYPE(iter->current->type)];
switch (PB_HTYPE(iter->current->type))
{
case PB_HTYPE_REQUIRED:
return func(stream, iter->current, iter->pData);
case PB_HTYPE_OPTIONAL:
*(bool*)iter->pSize = true;
return func(stream, iter->current, iter->pData);
case PB_HTYPE_ARRAY:
if (wire_type == PB_WT_STRING
&& PB_LTYPE(iter->current->type) <= PB_LTYPE_LAST_PACKABLE)
{
/* Packed array */
bool status;
size_t *size = (size_t*)iter->pSize;
pb_istream_t substream;
if (!pb_make_string_substream(stream, &substream))
return false;
while (substream.bytes_left && *size < iter->current->array_size)
{
void *pItem = (uint8_t*)iter->pData + iter->current->data_size * (*size);
if (!func(&substream, iter->current, pItem))
return false;
(*size)++;
}
status = (substream.bytes_left == 0);
pb_close_string_substream(stream, &substream);
return status;
}
else
{
/* Repeated field */
size_t *size = (size_t*)iter->pSize;
void *pItem = (uint8_t*)iter->pData + iter->current->data_size * (*size);
if (*size >= iter->current->array_size)
PB_RETURN_ERROR(stream, "array overflow");
(*size)++;
return func(stream, iter->current, pItem);
}
case PB_HTYPE_CALLBACK:
{
pb_callback_t *pCallback = (pb_callback_t*)iter->pData;
if (pCallback->funcs.decode == NULL)
return pb_skip_field(stream, wire_type);
if (wire_type == PB_WT_STRING)
{
pb_istream_t substream;
if (!pb_make_string_substream(stream, &substream))
return false;
while (substream.bytes_left)
{
if (!pCallback->funcs.decode(&substream, iter->current, pCallback->arg))
PB_RETURN_ERROR(stream, "callback failed");
}
pb_close_string_substream(stream, &substream);
return true;
}
else
{
/* Copy the single scalar value to stack.
* This is required so that we can limit the stream length,
* which in turn allows to use same callback for packed and
* not-packed fields. */
pb_istream_t substream;
uint8_t buffer[10];
size_t size = sizeof(buffer);
if (!read_raw_value(stream, wire_type, buffer, &size))
return false;
substream = pb_istream_from_buffer(buffer, size);
return pCallback->funcs.decode(&substream, iter->current, pCallback->arg);
}
}
default:
PB_RETURN_ERROR(stream, "invalid field type");
}
}
