bool msg_write_common(char type, uint16_t msg_id, const void *msg_ptr)
{
const pb_field_t *fields = MessageFields(type, 'o', msg_id);
if (!fields) { // unknown message
return false;
}
pb_ostream_t sizestream = {0, 0, SIZE_MAX, 0, 0};
bool status = pb_encode(&sizestream, fields, msg_ptr);
if (!status) {
return false;
}
void (*append)(uint8_t);
bool (*pb_callback)(pb_ostream_t *, const uint8_t *, size_t);
if (type == 'n') {
append = msg_out_append;
pb_callback = pb_callback_out;
} else
#if DEBUG_LINK
if (type == 'd') {
append = msg_debug_out_append;
pb_callback = pb_debug_callback_out;
} else
#endif
{
return false;
}
uint32_t len = sizestream.bytes_written;
append('#');
append('#');
append((msg_id >> 8) & 0xFF);
append(msg_id & 0xFF);
append((len >> 24) & 0xFF);
append((len >> 16) & 0xFF);
append((len >> 8) & 0xFF);
append(len & 0xFF);
pb_ostream_t stream = {pb_callback, 0, SIZE_MAX, 0, 0};
status = pb_encode(&stream, fields, msg_ptr);
if (type == 'n') {
msg_out_pad();
}
#if DEBUG_LINK
else if (type == 'd') {
msg_debug_out_pad();
}
#endif
return status;
}
bool checkreturn pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct)
{
/* First calculate the message size using a non-writing substream. */
pb_ostream_t substream = PB_OSTREAM_SIZING;
size_t size;
bool status;
if (!pb_encode(&substream, fields, src_struct))
{
#ifndef PB_NO_ERRMSG
stream->errmsg = substream.errmsg;
#endif
return false;
}
size = substream.bytes_written;
if (!pb_encode_varint(stream, (uint64_t)size))
return false;
if (stream->callback == NULL)
return pb_write(stream, NULL, size); /* Just sizing */
if (stream->bytes_written + size > stream->max_size)
PB_RETURN_ERROR(stream, "stream full");
/* Use a substream to verify that a callback doesn't write more than
* what it did the first time. */
substream.callback = stream->callback;
substream.state = stream->state;
substream.max_size = size;
substream.bytes_written = 0;
#ifndef PB_NO_ERRMSG
substream.errmsg = NULL;
#endif
status = pb_encode(&substream, fields, src_struct);
stream->bytes_written += substream.bytes_written;
stream->state = substream.state;
#ifndef PB_NO_ERRMSG
stream->errmsg = substream.errmsg;
#endif
if (substream.bytes_written != size)
PB_RETURN_ERROR(stream, "submsg size changed");
return status;
}
void sendSensorValue(MotorMsg_Data_Param_Id dataId, MotorMsg_Unit unit, double value) {
pb_ostream_t stream = pb_ostream_from_buffer(gTxFrameBuffer, FRAME_BUFFER_MAX_LEN);
static MotorMsg_Data_Param param = MotorMsg_Data_Param_init_zero;
param.id = dataId;
param.unit = unit;
param.value = value;
static MotorMsg_Data motorData = MotorMsg_Data_init_zero;
motorData.action = MotorMsg_Data_Action_RESULT;
motorData.params.funcs.encode = &motordataEncoderCallback;
motorData.params.arg = ¶m;
static MotorMsg motorMsg = MotorMsg_init_zero;
memcpy(motorMsg.uuid, UUID, sizeof(motorMsg.uuid));
motorMsg.which_content = MotorMsg_motor_data_tag;
motorMsg.content.motor_data = motorData;
pb_encode(&stream, MotorMsg_fields, &motorMsg);
//pb_encode_tag_for_field(stream, field);
//pb_encode_submessage(&stream, MotorMsg_MotorData_fields, &motorData);
usbTransmitFrame((BufferStoragePtrType) gTxFrameBuffer, stream.bytes_written);
}
/**
* encode the gateway message and write the encoded buf to GATEWAY_MSG_PATH
* @param ssmsg
*/
void gwmsg_write(SSMsg *ssmsg)
{
uint8_t buffer[1024];
int msg_len;
FILE *fd;
int status;
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
status=pb_encode(&stream, SSMsg_fields, ssmsg);
msg_len=stream.bytes_written;
if (!status)
{
log_printf(LOG_ERROR,"GateWay Msg Encoding failed: %s\n", PB_GET_ERROR(&stream));
return ;
}
fd=fopen(GATEWAY_MSG_PATH,"wb");
if(fd==NULL)
{
log_printf(LOG_ERROR,"Can not open %s error!\n",GATEWAY_MSG_PATH);
}
fwrite(buffer,msg_len,1,fd);
fclose(fd);
}
void MotorController::sendRequest()
{
/* This is the buffer where we will store the request message. */
uint8_t requestbuffer[256];
/* 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 speed fields will contain values */
request.has_speed_l = true;
request.has_speed_r = true;
/* fill in the message fields */
request.speed_l = static_cast<float>(current_motor_command_.left_velocity);
request.speed_r = static_cast<float>(current_motor_command_.right_velocity);
/* encode the protobuffer */
bool status = pb_encode(&ostream, RequestMessage_fields, &request);
size_t message_length = ostream.bytes_written;
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream));
ros::shutdown();
}
/* Send the message strapped to a pigeon's leg! */
(*sock_).sendMessage(reinterpret_cast<char*>(requestbuffer), message_length);
}
grpc_slice grpc_grpclb_request_encode(const grpc_grpclb_request *request) {
size_t encoded_length;
pb_ostream_t sizestream;
pb_ostream_t outputstream;
grpc_slice slice;
memset(&sizestream, 0, sizeof(pb_ostream_t));
pb_encode(&sizestream, grpc_lb_v1_LoadBalanceRequest_fields, request);
encoded_length = sizestream.bytes_written;
slice = grpc_slice_malloc(encoded_length);
outputstream =
pb_ostream_from_buffer(GRPC_SLICE_START_PTR(slice), encoded_length);
GPR_ASSERT(pb_encode(&outputstream, grpc_lb_v1_LoadBalanceRequest_fields,
request) != 0);
return slice;
}
void send_debug_telemetry()
{
#ifdef SEND_TELEMETRY
shedBoat_Telemetry telemetryMessage = shedBoat_Telemetry_init_zero;
telemetryMessage.status = shedBoat_Telemetry_Status_STATIONARY;
telemetryMessage.has_debug = true;
telemetryMessage.debug.has_bearing_compensation = true;
telemetryMessage.debug.bearing_compensation = bearingCompensation * 1000;
telemetryMessage.debug.has_speed_over_ground_compensation = true;
telemetryMessage.debug.speed_over_ground_compensation = speedOverGroundCompensation * 1000;
telemetryMessage.debug.has_motor_1_throttle_compensation = true;
telemetryMessage.debug.motor_1_throttle_compensation = leftThrottle * 1000;
telemetryMessage.debug.has_motor_2_throttle_compensation = true;
telemetryMessage.debug.motor_2_throttle_compensation = rightThrottle * 1000;
uint8_t buffer[100];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
bool success = pb_encode(&stream, shedBoat_Telemetry_fields, &telemetryMessage);
if(success) {
send_xbee_packet(buffer, stream.bytes_written);
} else {
error("Failed to encode Proto Buffer");
DEBUG_OUTPUT("Failed to encode Proto Buffer /r/n");
}
#endif
}
// encode message struct in msg according to 'fields', and send off via rb
// return 0 on success or < 0 on failure; see rtmsg_errors_t
//
static int npb_send_msg(const void *msg, const pb_field_t *fields,
ringbuffer_t *rb, const hal_funct_args_t *fa)
{
void *buffer;
int retval;
size_t size;
// determine size
pb_ostream_t sstream = PB_OSTREAM_SIZING;
if (!pb_encode(&sstream, fields, msg)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: sizing pb_encode(): %s written=%zu\n",
fa_funct_name(fa), PB_GET_ERROR(&sstream), sstream.bytes_written);
return NBP_SIZE_FAIL;
}
size = sstream.bytes_written;
// preallocate memory in ringbuffer
if ((retval = record_write_begin(rb, (void **)&buffer, size)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: record_write_begin(%zu) failed: %d\n",
fa_funct_name(fa), size, retval);
return RB_RESERVE_FAIL;
}
// zero-copy encode directly into ringbuffer
pb_ostream_t rstream = pb_ostream_from_buffer(buffer, size);
if (!pb_encode(&rstream, fields, msg)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: pb_encode failed: %s, size=%zu written=%zu\n",
fa_funct_name(fa),
PB_GET_ERROR(&rstream),
size,
rstream.bytes_written);
return NBP_ENCODE_FAIL;
}
// send it off
if ((retval = record_write_end(rb, buffer, rstream.bytes_written))) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: record_write_end(%zu) failed: %d\n",
fa_funct_name(fa), size, retval);
return RB_WRITE_FAIL;
}
return 0;
}
int pc_pb_encode(uint8_t *buf, size_t len, size_t *written, const json_t *gprotos, const json_t *protos, json_t *msg) {
pb_ostream_t stream = pb_ostream_from_buffer(buf, len);
if (!pb_encode(&stream, gprotos, protos, msg)) {
fprintf(stderr, "pb_encode error\n");
return 0;
}
*written = stream.bytes_written;
return 1;
}
int pc_pb_encode(uint8_t *buf, size_t len, size_t *written, const pc_JSON *gprotos, const pc_JSON *protos, const pc_JSON *msg) {
/* TODO: const */
pb_ostream_t stream = pb_ostream_from_buffer(buf, len);
if (!pb_encode(&stream, gprotos, protos, (pc_JSON*)msg)) {
return 0;
}
*written = stream.bytes_written;
return 1;
}
bool pb_get_encoded_size(size_t *size, const pb_field_t fields[], const void *src_struct)
{
pb_ostream_t stream = PB_OSTREAM_SIZING;
if (!pb_encode(&stream, fields, src_struct))
return false;
*size = stream.bytes_written;
return true;
}
void gps_satellite_telemetry() {
DEBUG_OUTPUT("Time: %02d:%02d:%02d\r\n", NMEA::getHour(), NMEA::getMinute(), NMEA::getSecond());
DEBUG_OUTPUT("Satellites: %d\r\n", NMEA::getSatellites());
DEBUG_OUTPUT("Latitude: %0.5f\r\n", NMEA::getLatitude());
DEBUG_OUTPUT("Longitude: %0.5f\r\n", NMEA::getLongitude());
DEBUG_OUTPUT("Altitude: %0.2fm\r\n", NMEA::getAltitude());
DEBUG_OUTPUT("Speed: %0.2fkm/h\r\n", NMEA::getSpeed());
DEBUG_OUTPUT("GPS Bearing (Track made good): %0.2f degrees\r\n", NMEA::getBearing());
DEBUG_OUTPUT("Compass Bearing: %03.0f\r\n", bearing);
DEBUG_OUTPUT("Heading Delta to Waypoint: %03.0f\r\n", heading_delta(180.0, bearing));
DEBUG_OUTPUT("Distance to Next Nav %06.2fm\r\n",
distance_to_current_nav(degToRad((double)NMEA::getLatitude()), degToRad((double)NMEA::getLongitude()))
);
#ifdef SEND_TELEMETRY
shedBoat_Telemetry telemetryMessage = shedBoat_Telemetry_init_zero;
if(autopilotEngaged) {
telemetryMessage.status = shedBoat_Telemetry_Status_UNDERWAY_AUTOPILOT;
}
else {
telemetryMessage.status = shedBoat_Telemetry_Status_UNDERWAY_MANUAL;
}
telemetryMessage.has_location = true;
telemetryMessage.location.has_latitude = true;
telemetryMessage.location.latitude = NMEA::getLatitude();
telemetryMessage.location.has_longitude = true;
telemetryMessage.location.longitude = NMEA::getLongitude();
telemetryMessage.location.has_number_of_satellites_visible = true;
telemetryMessage.location.number_of_satellites_visible = NMEA::getSatellites();
telemetryMessage.location.has_true_heading = true;
telemetryMessage.location.true_heading = heading * 1000;
telemetryMessage.location.has_true_bearing = true;
telemetryMessage.location.true_bearing = bearing * 1000;
telemetryMessage.location.has_speed_over_ground = true;
telemetryMessage.location.speed_over_ground = NMEA::getSpeed() * 1000;
telemetryMessage.location.has_utc_seconds = true;
telemetryMessage.location.utc_seconds = NMEA::getSecond();
telemetryMessage.location.fix_quality = (_shedBoat_Location_Quality)NMEA::getFixQuality();
telemetryMessage.location.has_distance_to_waypoint = true;
telemetryMessage.location.distance_to_waypoint = (int32_t)distance_to_current_nav(degToRad((double)NMEA::getLatitude()), degToRad((double)NMEA::getLongitude()));
telemetryMessage.location.has_waypoint_number = true;
telemetryMessage.location.waypoint_number = get_nav_num();
uint8_t buffer[100];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
bool success = pb_encode(&stream, shedBoat_Telemetry_fields, &telemetryMessage);
if(success) {
send_xbee_packet(buffer, stream.bytes_written);
} else {
error("Failed to encode Proto Buffer");
}
#endif
}
/* Eecode submessage in __messages protos */
static int pb_encode_submessage(pb_ostream_t *stream, const pc_JSON *gprotos, const pc_JSON *protos, pc_JSON *value) {
/* First calculate the message size using a non-writing substream. */
pb_ostream_t substream = { 0, 0, 0, 0 };
size_t size;
int status;
if (!pb_encode(&substream, gprotos, protos, value)) {
return 0;
}
size = substream.bytes_written;
if (!pb_encode_varint(stream, (uint64_t) size)) {
return 0;
}
if (stream->callback == NULL )
return pb_write(stream, NULL, size); /* Just sizing */
if (stream->bytes_written + size > stream->max_size)
return 0;
/* Use a substream to verify that a callback doesn't write more than
* what it did the first time. */
substream.callback = stream->callback;
substream.state = stream->state;
substream.max_size = size;
substream.bytes_written = 0;
status = pb_encode(&substream, gprotos, protos, value);
stream->bytes_written += substream.bytes_written;
stream->state = substream.state;
if (substream.bytes_written != size) {
return 0;
}
return status;
}
bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_t *field, const void *src)
{
pb_ostream_t substream = {0};
size_t size;
bool status;
if (field->ptr == NULL)
return false;
if (!pb_encode(&substream, (pb_field_t*)field->ptr, src))
return false;
size = substream.bytes_written;
if (!pb_encode_varint(stream, size))
return false;
if (stream->callback == NULL)
return pb_write(stream, NULL, size); /* Just sizing */
if (stream->bytes_written + size > stream->max_size)
return false;
/* Use a substream to verify that a callback doesn't write more than
* what it did the first time. */
substream.callback = stream->callback;
substream.state = stream->state;
substream.max_size = size;
substream.bytes_written = 0;
status = pb_encode(&substream, (pb_field_t*)field->ptr, src);
stream->bytes_written += substream.bytes_written;
if (substream.bytes_written != size)
return false;
return status;
}
bool checkreturn pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct)
{
/* First calculate the message size using a non-writing substream. */
pb_ostream_t substream = {0,0,0,0};
size_t size;
bool status;
if (!pb_encode(&substream, fields, src_struct))
return false;
size = substream.bytes_written;
if (!pb_encode_varint(stream, (uint64_t)size))
return false;
if (stream->callback == NULL)
return pb_write(stream, NULL, size); /* Just sizing */
if (stream->bytes_written + size > stream->max_size)
return false;
/* Use a substream to verify that a callback doesn't write more than
* what it did the first time. */
substream.callback = stream->callback;
substream.state = stream->state;
substream.max_size = size;
substream.bytes_written = 0;
status = pb_encode(&substream, fields, src_struct);
stream->bytes_written += substream.bytes_written;
stream->state = substream.state;
if (substream.bytes_written != size)
return false;
return status;
}
/**
* close the SACD device and clean up.
*/
static int sacd_net_input_close(sacd_input_t dev)
{
if (!dev)
{
return 0;
}
else
{
ServerRequest request;
ServerResponse response;
pb_istream_t input = pb_istream_from_socket(&dev->fd);
pb_ostream_t output = pb_ostream_from_socket(&dev->fd);
uint8_t zero = 0;
request.type = ServerRequest_Type_DISC_CLOSE;
if (!pb_encode(&output, ServerRequest_fields, &request))
{
goto error;
}
pb_write(&output, &zero, 1);
if (!pb_decode(&input, ServerResponse_fields, &response))
{
goto error;
}
if (response.result == 0 || response.type != ServerResponse_Type_DISC_CLOSED)
{
goto error;
}
}
error:
if(dev)
{
socket_destroy(&dev->fd);
socket_close();
if (dev->input_buffer)
{
free(dev->input_buffer);
dev->input_buffer = 0;
}
free(dev);
dev = 0;
}
return 0;
}
int main()
{
int status = 0;
uint8_t buf[256];
SignedMsg msg1;
UnsignedMsg msg2;
pb_ostream_t s;
{
COMMENT("Test negative value of signed enum");
/* Negative value should take up the maximum size */
msg1.value = SignedEnum_SE_MIN;
s = pb_ostream_from_buffer(buf, sizeof(buf));
TEST(pb_encode(&s, SignedMsg_fields, &msg1));
TEST(s.bytes_written == SignedMsg_size);
COMMENT("Test positive value of signed enum");
/* Positive value should be smaller */
msg1.value = SignedEnum_SE_MAX;
s = pb_ostream_from_buffer(buf, sizeof(buf));
TEST(pb_encode(&s, SignedMsg_fields, &msg1));
TEST(s.bytes_written < SignedMsg_size);
}
{
COMMENT("Test positive value of unsigned enum");
/* This should take up the maximum size */
msg2.value = UnsignedEnum_UE_MAX;
s = pb_ostream_from_buffer(buf, sizeof(buf));
TEST(pb_encode(&s, UnsignedMsg_fields, &msg2));
TEST(s.bytes_written == UnsignedMsg_size);
}
return status;
}
int main()
{
/* Initialize the structure with constants */
Person person = {"Test Person 99", 99, true, "*****@*****.**",
1, {{"555-12345678", true, Person_PhoneType_MOBILE}}};
/* Prepare the stream, output goes directly to stdout */
pb_ostream_t stream = {&streamcallback, stdout, SIZE_MAX, 0};
/* Now encode it and check if we succeeded. */
if (pb_encode(&stream, Person_fields, &person))
return 0; /* Success */
else
return 1; /* Failure */
}
ENetPacket *NetEncode(const GameEventType e, const void *data)
{
uint8_t buffer[1024];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof buffer);
const pb_field_t *fields = GameEventGetEntry(e).Fields;
const bool status =
(data && fields) ? pb_encode(&stream, fields, data) : true;
CASSERT(status, "Failed to encode pb");
int msgId = (int)e;
ENetPacket *packet = enet_packet_create(
&msgId, NET_MSG_SIZE + stream.bytes_written,
ENET_PACKET_FLAG_RELIABLE);
memcpy(packet->data + NET_MSG_SIZE, buffer, stream.bytes_written);
return packet;
}
// This function assumes the TraceContext is valid.
size_t encode_trace_context(google_trace_TraceContext *ctxt, uint8_t *buffer,
const size_t buf_size) {
// Create a stream that will write to our buffer.
pb_ostream_t stream = pb_ostream_from_buffer(buffer, buf_size);
// encode message
bool status = pb_encode(&stream, google_trace_TraceContext_fields, ctxt);
if (!status) {
gpr_log(GPR_DEBUG, "TraceContext encoding failed: %s",
PB_GET_ERROR(&stream));
return 0;
}
return stream.bytes_written;
}
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 send_printer_status() {
uint8_t out[16];
uint8_t type;
pb_ostream_t stream = pb_ostream_from_buffer(out, sizeof(out));
bool status;
PrinterStatus record;
type = PRINTER_STATUS;
pb_write(&stream, &type, 1);
record.cardInserted = 1;
record.keyInserted = get_interlock_state();
record.overrideSwitch = get_interlock_state();
record.laserOn = g_laser_gating;
record.laserPowerFeedback = 42;
status = pb_encode(&stream, PrinterStatus_fields, &record);
if (status) {
serialio_write(out, stream.bytes_written);
}
}
void send_system_telemetry()
{
#ifdef SEND_TELEMETRY
shedBoat_Telemetry telemetryMessage = shedBoat_Telemetry_init_zero;
telemetryMessage.status = shedBoat_Telemetry_Status_STATIONARY;
telemetryMessage.motor_count = 2;
telemetryMessage.motor[0].motor_number = 1;
telemetryMessage.motor[0].has_is_alive = true;
telemetryMessage.motor[0].is_alive = leftMotor.isAlive();
telemetryMessage.motor[0].has_rpm = true;
telemetryMessage.motor[0].rpm = leftMotor.rpm();
telemetryMessage.motor[0].has_temperature = true;
telemetryMessage.motor[0].temperature = leftMotor.temperature();
telemetryMessage.motor[0].has_voltage = true;
telemetryMessage.motor[0].voltage = leftMotor.voltage();
telemetryMessage.motor[1].motor_number = 2;
telemetryMessage.motor[1].has_is_alive = true;
telemetryMessage.motor[1].is_alive = rightMotor.isAlive();
telemetryMessage.motor[1].has_rpm = true;
telemetryMessage.motor[1].rpm = rightMotor.rpm();
telemetryMessage.motor[1].has_temperature = true;
telemetryMessage.motor[1].temperature = rightMotor.temperature();
telemetryMessage.motor[1].has_voltage = true;
telemetryMessage.motor[1].voltage = rightMotor.voltage();
telemetryMessage.has_battery = false;
uint8_t buffer[100];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
bool success = pb_encode(&stream, shedBoat_Telemetry_fields, &telemetryMessage);
if(success) {
send_xbee_packet(buffer, stream.bytes_written);
} else {
error("Failed to encode Proto Buffer");
DEBUG_OUTPUT("Failed to encode Proto Buffer /r/n");
}
#endif
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
oca_led_toggle(oca_led_act);
CanMessage UsbMessage = CanMessage_init_default;
UsbMessage.DLC = rxmsg.DLC;
UsbMessage.RTR = rxmsg.DLC;
UsbMessage.IDE = rxmsg.IDE;
UsbMessage.ID = rxmsg.EID;
UsbMessage.Data1 = rxmsg.data32[0];
UsbMessage.Data1 = rxmsg.data32[1];
uint8_t buffer[30];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
pb_encode(&stream, CanMessage_fields, &UsbMessage);
/*if(serusbcfg.usbp->state == USB_ACTIVE)
{
if(SDU1.state == SDU_READY)
chnWrite(&SDU1, buffer, stream.bytes_written);
}*/
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
void handle_connection(int connfd)
{
ListFilesRequest request;
ListFilesResponse response;
pb_istream_t input = pb_istream_from_socket(connfd);
pb_ostream_t output = pb_ostream_from_socket(connfd);
DIR *directory;
if (!pb_decode(&input, ListFilesRequest_fields, &request))
{
printf("Decoding failed.\n");
return;
}
directory = opendir(request.path);
printf("Listing directory: %s\n", request.path);
if (directory == NULL)
{
perror("opendir");
response.has_path_error = true;
response.path_error = true;
response.file.funcs.encode = NULL;
}
else
{
response.has_path_error = false;
response.file.funcs.encode = &listdir_callback;
response.file.arg = directory;
}
if (!pb_encode(&output, ListFilesResponse_fields, &response))
{
printf("Encoding failed.\n");
}
}
static uint32_t sacd_net_input_total_sectors(sacd_input_t dev)
{
if (!dev)
{
return 0;
}
else
{
ServerRequest request;
ServerResponse response;
pb_istream_t input = pb_istream_from_socket(&dev->fd);
pb_ostream_t output = pb_ostream_from_socket(&dev->fd);
uint8_t zero = 0;
request.type = ServerRequest_Type_DISC_SIZE;
if (!pb_encode(&output, ServerRequest_fields, &request))
{
return 0;
}
/* We signal the end of request with a 0 tag. */
pb_write(&output, &zero, 1);
if (!pb_decode(&input, ServerResponse_fields, &response))
{
return 0;
}
if (response.type != ServerResponse_Type_DISC_SIZE)
{
return 0;
}
return (uint32_t) response.result;
}
}
int main(int argc, char *argv[])
{
uint8_t buffer[256];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
Dictionary dict = Dictionary_init_zero;
if (argc <= 1)
{
fprintf(stderr, "Usage: %s \"{'foobar': 1234, ...}\"\n", argv[0]);
return 1;
}
dict.dictItem.funcs.encode = encode_dictionary;
dict.dictItem.arg = argv[1];
if (!pb_encode(&stream, Dictionary_fields, &dict))
{
fprintf(stderr, "Encoding error: %s\n", PB_GET_ERROR(&stream));
return 1;
}
fwrite(buffer, 1, stream.bytes_written, stdout);
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");
}
/**
* initialize and open a SACD device or file.
*/
static sacd_input_t sacd_net_input_open(const char *target)
{
ServerRequest request;
ServerResponse response;
sacd_input_t dev = 0;
const char *err = 0;
t_timeout tm;
pb_istream_t input;
pb_ostream_t output;
uint8_t zero = 0;
/* Allocate the library structure */
dev = (sacd_input_t) calloc(sizeof(*dev), 1);
if (dev == NULL)
{
fprintf(stderr, "libsacdread: Could not allocate memory.\n");
return NULL;
}
dev->input_buffer = (uint8_t *) malloc(MAX_PROCESSING_BLOCK_SIZE * SACD_LSN_SIZE + 1024);
if (dev->input_buffer == NULL)
{
fprintf(stderr, "libsacdread: Could not allocate memory.\n");
goto error;
}
socket_open();
socket_create(&dev->fd, AF_INET, SOCK_STREAM, 0);
socket_setblocking(&dev->fd);
timeout_markstart(&tm);
err = inet_tryconnect(&dev->fd,
substr(target, 0, strchr(target, ':') - target),
atoi(strchr(target, ':') + 1), &tm);
if (err)
{
fprintf(stderr, "Failed to connect\n");
goto error;
}
socket_setblocking(&dev->fd);
input = pb_istream_from_socket(&dev->fd);
output = pb_ostream_from_socket(&dev->fd);
request.type = ServerRequest_Type_DISC_OPEN;
if (!pb_encode(&output, ServerRequest_fields, &request))
{
fprintf(stderr, "Failed to encode request\n");
goto error;
}
/* We signal the end of request with a 0 tag. */
pb_write(&output, &zero, 1);
if (!pb_decode(&input, ServerResponse_fields, &response))
{
fprintf(stderr, "Failed to decode response\n");
goto error;
}
if (response.result != 0 || response.type != ServerResponse_Type_DISC_OPENED)
{
fprintf(stderr, "Response result non-zero or disc opened\n");
goto error;
}
return dev;
error:
sacd_input_close(dev);
return 0;
}
