#include "vrpn_Analog_Output.h" #include <stdio.h> vrpn_Analog_Output::vrpn_Analog_Output(const char* name, vrpn_Connection* c) : vrpn_BaseClass(name, c) , o_num_channel(0) { int i; // Call the base class' init routine vrpn_BaseClass::init(); // Set the time to 0 just to have something there. o_timestamp.tv_usec = o_timestamp.tv_sec = 0; // Initialize the values in the channels, // gets rid of uninitialized memory read error in Purify // and makes sure any initial value change gets reported. for (i = 0; i < vrpn_CHANNEL_MAX; i++) { o_channel[i] = 0; } } int vrpn_Analog_Output::register_types(void) { request_m_id = d_connection->register_message_type( "vrpn_Analog_Output Change_request"); request_channels_m_id = d_connection->register_message_type( "vrpn_Analog_Output Change_Channels_request"); report_num_channels_m_id = d_connection->register_message_type( "vrpn_Analog_Output Num_Channels_report"); got_connection_m_id = d_connection->register_message_type(vrpn_got_connection); if ((request_m_id == -1) || (request_channels_m_id == -1) || (report_num_channels_m_id == -1) || (got_connection_m_id == -1)) { return -1; } else { return 0; } } void vrpn_Analog_Output::o_print(void) { printf("Analog_Output Report: "); for (vrpn_int32 i = 0; i < o_num_channel; i++) { // printf("Channel[%d]= %f\t", i, o_channel[i]); printf("%f\t", o_channel[i]); } printf("\n"); } vrpn_Analog_Output_Server::vrpn_Analog_Output_Server(const char* name, vrpn_Connection* c, vrpn_int32 numChannels) : vrpn_Analog_Output(name, c) { this->setNumChannels(numChannels); // Check if we have a connection if (d_connection == NULL) { fprintf(stderr, "vrpn_Analog_Output: Can't get connection!\n"); } // Register a handler for the request channel change message if (register_autodeleted_handler(request_m_id, handle_request_message, this, d_sender_id)) { fprintf(stderr, "vrpn_Analog_Output_Server: can't register change " "channel request handler\n"); d_connection = NULL; } // Register a handler for the request channels change message if (register_autodeleted_handler(request_channels_m_id, handle_request_channels_message, this, d_sender_id)) { fprintf(stderr, "vrpn_Analog_Output_Server: can't register change " "channels request handler\n"); d_connection = NULL; } // Register a handler for vrpn_got_connection, so we can tell the // client how many channels are active if (register_autodeleted_handler(got_connection_m_id, handle_got_connection, this)) { fprintf(stderr, "vrpn_Analog_Output_Server: can't register new " "connection handler\n"); d_connection = NULL; } } // virtual vrpn_Analog_Output_Server::~vrpn_Analog_Output_Server(void) {} vrpn_int32 vrpn_Analog_Output_Server::setNumChannels(vrpn_int32 sizeRequested) { if (sizeRequested < 0) sizeRequested = 0; if (sizeRequested > vrpn_CHANNEL_MAX) sizeRequested = vrpn_CHANNEL_MAX; o_num_channel = sizeRequested; return o_num_channel; } /* static */ int vrpn_Analog_Output_Server::handle_request_message(void* userdata, vrpn_HANDLERPARAM p) { const char* bufptr = p.buffer; vrpn_int32 chan_num; vrpn_int32 pad; vrpn_float64 value; vrpn_Analog_Output_Server* me = (vrpn_Analog_Output_Server*)userdata; // Read the parameters from the buffer vrpn_unbuffer(&bufptr, &chan_num); vrpn_unbuffer(&bufptr, &pad); vrpn_unbuffer(&bufptr, &value); // Set the appropriate value, if the channel number is in the // range of the ones we have. if ((chan_num < 0) || (chan_num >= me->o_num_channel)) { fprintf(stderr, "vrpn_Analog_Output_Server::handle_request_message(): " "Index out of bounds\n"); char msg[1024]; sprintf(msg, "Error: (handle_request_message): channel %d is not " "active. Squelching.", chan_num); me->send_text_message(msg, p.msg_time, vrpn_TEXT_ERROR); return 0; } me->o_channel[chan_num] = value; return 0; } /* static */ int vrpn_Analog_Output_Server::handle_request_channels_message( void* userdata, vrpn_HANDLERPARAM p) { int i; const char* bufptr = p.buffer; vrpn_int32 num; vrpn_int32 pad; vrpn_Analog_Output_Server* me = (vrpn_Analog_Output_Server*)userdata; // Read the values from the buffer vrpn_unbuffer(&bufptr, &num); vrpn_unbuffer(&bufptr, &pad); if (num > me->o_num_channel) { char msg[1024]; sprintf(msg, "Error: (handle_request_channels_message): channels " "above %d not active; " "bad request up to channel %d. Squelching.", me->o_num_channel, num); me->send_text_message(msg, p.msg_time, vrpn_TEXT_ERROR); num = me->o_num_channel; } if (num < 0) { char msg[1024]; sprintf(msg, "Error: (handle_request_channels_message): invalid " "channel %d. Squelching.", num); me->send_text_message(msg, p.msg_time, vrpn_TEXT_ERROR); return 0; } for (i = 0; i < num; i++) { vrpn_unbuffer(&bufptr, &(me->o_channel[i])); } return 0; } /* static */ int vrpn_Analog_Output_Server::handle_got_connection(void* userdata, vrpn_HANDLERPARAM) { vrpn_Analog_Output_Server* me = (vrpn_Analog_Output_Server*)userdata; if (me->report_num_channels() == false) { fprintf(stderr, "Error: failed sending active channels to client.\n"); } return 0; } bool vrpn_Analog_Output_Server::report_num_channels( vrpn_uint32 class_of_service) { char msgbuf[sizeof(vrpn_int32)]; vrpn_int32 len = sizeof(vrpn_int32); ; encode_num_channels_to(msgbuf, this->o_num_channel); vrpn_gettimeofday(&o_timestamp, NULL); if (d_connection && d_connection->pack_message(len, o_timestamp, report_num_channels_m_id, d_sender_id, msgbuf, class_of_service)) { fprintf(stderr, "vrpn_Analog_Output_Server (report_num_channels): " "cannot write message: tossing\n"); return false; } return true; } vrpn_int32 vrpn_Analog_Output_Server::encode_num_channels_to(char* buf, vrpn_int32 num) { // Message includes: int32 number of active channels int buflen = sizeof(vrpn_int32); vrpn_buffer(&buf, &buflen, num); return sizeof(vrpn_int32); } vrpn_Analog_Output_Callback_Server::vrpn_Analog_Output_Callback_Server( const char* name, vrpn_Connection* c, vrpn_int32 numChannels) : vrpn_Analog_Output_Server(name, c, numChannels) { // Register a handler for the request channel change message. This will go // in the list AFTER the one for the base class, so the values will already // have been filled in. So we just need to call the user-level callbacks // and pass them a pointer to the data values. if (register_autodeleted_handler(request_m_id, handle_change_message, this, d_sender_id)) { fprintf(stderr, "vrpn_Analog_Output_Callback_Server: can't register " "change channel request handler\n"); d_connection = NULL; } // Register a handler for the request channels change message. This will go // in the list AFTER the one for the base class, so the values will already // have been filled in. So we just need to call the user-level callbacks // and pass them a pointer to the data values. if (register_autodeleted_handler( request_channels_m_id, handle_change_message, this, d_sender_id)) { fprintf(stderr, "vrpn_Analog_Output_Callback_Server: can't register " "change channels request handler\n"); d_connection = NULL; } } // virtual vrpn_Analog_Output_Callback_Server::~vrpn_Analog_Output_Callback_Server(void) {} /* static */ // This is inserted into the list AFTER the int vrpn_Analog_Output_Callback_Server::handle_change_message( void* userdata, vrpn_HANDLERPARAM p) { vrpn_Analog_Output_Callback_Server* me = (vrpn_Analog_Output_Callback_Server*)userdata; vrpn_ANALOGOUTPUTCB callback_data; callback_data.msg_time = p.msg_time; callback_data.num_channel = me->getNumChannels(); callback_data.channel = me->o_channels(); me->d_callback_list.call_handlers(callback_data); return 0; } vrpn_Analog_Output_Remote::vrpn_Analog_Output_Remote(const char* name, vrpn_Connection* c) : vrpn_Analog_Output(name, c) { vrpn_int32 i; o_num_channel = vrpn_CHANNEL_MAX; for (i = 0; i < vrpn_CHANNEL_MAX; i++) { o_channel[i] = 0; } vrpn_gettimeofday(&o_timestamp, NULL); // Register a handler for the report number of active channels message if (register_autodeleted_handler(report_num_channels_m_id, handle_report_num_channels, this, d_sender_id)) { fprintf(stderr, "vrpn_Analog_Output_Remote: can't register active " "channel report handler\n"); d_connection = NULL; } } // virtual vrpn_Analog_Output_Remote::~vrpn_Analog_Output_Remote(void) {} void vrpn_Analog_Output_Remote::mainloop() { if (d_connection) { d_connection->mainloop(); client_mainloop(); } } /* static */ int vrpn_Analog_Output_Remote::handle_report_num_channels(void* userdata, vrpn_HANDLERPARAM p) { const char* bufptr = p.buffer; vrpn_int32 num; vrpn_Analog_Output_Remote* me = (vrpn_Analog_Output_Remote*)userdata; // Read the parameters from the buffer vrpn_unbuffer(&bufptr, &num); if (num >= 0 && num <= vrpn_CHANNEL_MAX) { me->o_num_channel = num; } else { fprintf( stderr, "vrpn_Analog_Output_Remote::handle_report_num_channels_message: " "Someone sent us a bogus number of channels: %d.\n", num); } return 0; } bool vrpn_Analog_Output_Remote::request_change_channel_value( unsigned int chan, vrpn_float64 val, vrpn_uint32 class_of_service) { // msgbuf must be float64-aligned! vrpn_float64 fbuf[2]; char* msgbuf = (char*)fbuf; vrpn_int32 len; vrpn_gettimeofday(&o_timestamp, NULL); len = encode_change_to(msgbuf, chan, val); if (d_connection && d_connection->pack_message(len, o_timestamp, request_m_id, d_sender_id, msgbuf, class_of_service)) { fprintf(stderr, "vrpn_Analog_Output_Remote: cannot write message: tossing\n"); return false; } return true; } bool vrpn_Analog_Output_Remote::request_change_channels( int num, vrpn_float64* vals, vrpn_uint32 class_of_service) { if (num < 0 || num > vrpn_CHANNEL_MAX) { fprintf(stderr, "vrpn_Analog_Output_Remote: cannot change channels: " "number of channels out of range\n"); return false; } vrpn_float64 fbuf[1 + vrpn_CHANNEL_MAX]; char* msgbuf = (char*)fbuf; vrpn_int32 len; vrpn_gettimeofday(&o_timestamp, NULL); len = encode_change_channels_to(msgbuf, num, vals); if (d_connection && d_connection->pack_message(len, o_timestamp, request_channels_m_id, d_sender_id, msgbuf, class_of_service)) { fprintf(stderr, "vrpn_Analog_Output_Remote: cannot write message: tossing\n"); return false; } return true; } vrpn_int32 vrpn_Analog_Output_Remote::encode_change_to(char* buf, vrpn_int32 chan, vrpn_float64 val) { // Message includes: int32 channel_number, int32 padding, float64 // request_value // Byte order of each needs to be reversed to match network standard vrpn_int32 pad = 0; int buflen = 2 * sizeof(vrpn_int32) + sizeof(vrpn_float64); vrpn_buffer(&buf, &buflen, chan); vrpn_buffer(&buf, &buflen, pad); vrpn_buffer(&buf, &buflen, val); return 2 * sizeof(vrpn_int32) + sizeof(vrpn_float64); } vrpn_int32 vrpn_Analog_Output_Remote::encode_change_channels_to(char* buf, vrpn_int32 num, vrpn_float64* vals) { int i; vrpn_int32 pad = 0; int buflen = 2 * sizeof(vrpn_int32) + num * sizeof(vrpn_float64); vrpn_buffer(&buf, &buflen, num); vrpn_buffer(&buf, &buflen, pad); for (i = 0; i < num; i++) { vrpn_buffer(&buf, &buflen, vals[i]); } return 2 * sizeof(vrpn_int32) + num * sizeof(vrpn_float64); }