char *
get_external_command(const char *prompt, char *input, int len)
{
// get/create the port
port_id port = get_command_port();
if (port < 0) {
fprintf(stderr, "Failed to create command port: %s\n", strerror(port));
return NULL;
}
while (true) {
// read a message
external_command_message message;
ssize_t bytesRead;
do {
int32 code;
bytesRead = read_port(port, &code, &message, sizeof(message));
} while (bytesRead == B_INTERRUPTED);
if (bytesRead < 0) {
fprintf(stderr, "Reading from port failed: %s\n",
strerror(bytesRead));
return NULL;
}
// get the len of the command
int commandLen = (char*)&message + bytesRead - message.command;
if (commandLen <= 1) {
fprintf(stderr, "No command given.\n");
continue;
}
if (commandLen > len) {
fprintf(stderr, "Command too long. Ignored.\n");
continue;
}
// copy the command
memcpy(input, message.command, commandLen);
input[len - 1] = '\0'; // always NULL-terminate
sReplyPort = message.reply_port;
return input;
}
}
static status_t
connection_thread(void*)
{
int32 code;
ssize_t ssizePort;
ssize_t ssizeRead;
HciConnection* conn = NULL;
// TODO: Keep this a static var
port_id fPort = find_port(BLUETOOTH_CONNECTION_SCHED_PORT);
if (fPort == B_NAME_NOT_FOUND)
{
panic("BT Connection port has been deleted");
}
while ((ssizePort = port_buffer_size(fPort)) != B_BAD_PORT_ID) {
if (ssizePort <= 0) {
debugf("Error %s\n", strerror(ssizePort));
snooze(500 * 1000);
continue;
}
if (ssizePort > (ssize_t) sizeof(conn)) {
debugf("Message too big %ld\n", ssizePort);
snooze(500 * 1000);
continue;
}
ssizeRead = read_port(fPort, &code, &conn, ssizePort);
if (ssizeRead != ssizePort) {
debugf("Missmatch size port=%ld read=%ld\n", ssizePort, ssizeRead);
snooze(500 * 1000);
continue;
}
purge_connection(conn);
}
return B_OK;
}
status_t
read_thread(void* _data)
{
port_id port = (port_id)_data;
printf("[%ld] read port...\n", find_thread(NULL));
while (true) {
ssize_t bytes = port_buffer_size(port);
printf("[%ld] buffer size %ld waiting\n", find_thread(NULL), bytes);
char buffer[256];
int32 code;
bytes = read_port(port, &code, buffer, sizeof(buffer));
printf("[%ld] read port result (code %lx): %s\n", find_thread(NULL),
code, strerror(bytes));
if (bytes >= 0)
break;
}
return B_OK;
}
static void key_update_port(Key3x4 *self)
{
int i,j;
char port_value;
char data=0xFF;
/* read bits 5..7 */
read_port(self, self->port_no, &port_value);
port_value&=0xE0;
/*
* If there is zero on bit (5..7) and corresponding key is pressed,
* some of bits (0..3) must be cleared. See schematics in doc.
*/
for (i=0; i<3; i++) {
if(((port_value>>(5+i)) & 1) == 0) {
for (j=0; j<4; j++) {
if (self->map[3*j+i]) {
data&=~(8>>j);
}
}
}
}
// Timer
gboolean timeout_callback(gpointer data)
{
static unsigned char dispatch = 0;
// Only do the settings if in xbee mode
if ((power_level >= 0) && (handle_api() == 0))
return TRUE;
// Every Time
read_port();
// One out of 4
if (dispatch > 2)
{
send_port();
dispatch = 0;
}
else
{
dispatch ++;
}
return TRUE;
}
static int32
connection_opener(void *_unused)
{
while(true) {
port_id port;
int32 msg;
ssize_t bytes = read_port(gStackPort, &msg, &port, sizeof(port_id));
if (bytes < B_OK)
return bytes;
if (msg == (int32) NET_STACK_NEW_CONNECTION) {
net_connection connection;
connection_cookie *cookie;
printf("incoming connection...\n");
if (init_connection(&connection, &cookie) == B_OK)
write_port(port, NET_STACK_NEW_CONNECTION, &connection, sizeof(net_connection));
} else
fprintf(stderr, "connection_opener: received unknown command: %lx (expected = %lx)\n",
msg, (int32) NET_STACK_NEW_CONNECTION);
}
return 0;
}
void
SyslogDaemon::Daemon()
{
char buffer[SYSLOG_MESSAGE_BUFFER_SIZE + 1];
syslog_message &message = *(syslog_message *)buffer;
int32 code;
while (true) {
ssize_t bytesRead = read_port(fPort, &code, &message, sizeof(buffer));
if (bytesRead == B_BAD_PORT_ID) {
// we've been quit
break;
}
// if we don't get what we want, ignore it
if (bytesRead < (ssize_t)sizeof(syslog_message) || code != SYSLOG_MESSAGE)
continue;
// add terminating null byte
message.message[bytesRead - sizeof(syslog_message)] = '\0';
if (!message.message[0]) {
// ignore empty messages
continue;
}
fHandlerLock.Lock();
for (int32 i = fHandlers.CountItems(); i-- > 0;) {
handler_func handle = (handler_func)fHandlers.ItemAt(i);
handle(message);
}
fHandlerLock.Unlock();
}
}
void nsToolkit::RunPump(void* arg)
{
int32 code;
char portname[64];
ThreadInterfaceData id;
ThreadInitInfo *info = (ThreadInitInfo*)arg;
PR_EnterMonitor(info->monitor);
gThreadState = PR_TRUE;
PR_Notify(info->monitor);
PR_ExitMonitor(info->monitor);
delete info;
// system wide unique names
int32 cookie = 0;
image_info iinfo;
char *leaf = NULL;
do {
if (get_next_image_info(0, &cookie, &iinfo) == B_OK &&
strlen(iinfo.name) > 0 &&
(leaf = strrchr(iinfo.name, '/')) != NULL)
{
leaf++;
PR_snprintf(portname, sizeof(portname), "event%lx",
(long unsigned) find_thread(leaf));
}
else
{
PR_snprintf(portname, sizeof(portname), "event%lx",
(long unsigned) find_thread(0));
}
} while(iinfo.type != B_APP_IMAGE);
port_id event = create_port(200, portname);
while(read_port(event, &code, &id, sizeof(id)) >= 0)
{
switch(code)
{
case WM_CALLMETHOD :
{
MethodInfo *mInfo = (MethodInfo *)id.data;
mInfo->Invoke();
if(id.waitingThread != 0)
resume_thread(id.waitingThread);
delete mInfo;
}
break;
case 'natv' : // native queue PLEvent
{
PREventQueue *queue = (PREventQueue *)id.data;
PR_ProcessPendingEvents(queue);
}
break;
default :
printf("nsToolkit::RunPump - UNKNOWN EVENT\n");
break;
}
}
}
int main(int argc, char *argv[]) {
progname = argv[0];
char *outfile = NULL;
int optc, optid;
int required_args = 1;
while ((optc = getopt_long(argc, argv, GETOPTS, long_opts, &optid)) != -1) {
switch (optc) {
case 0: {
if (long_opts[optid].flag == 0) {
show_help = 1;
}
break;
}
case 'f': {
required_args--;
outfile = strdup(optarg);
break;
}
default: {
show_help = 1;
break;
}
}
}
if (show_help || optind < argc || required_args > 0) {
showHelp();
return EXIT_FAILURE;
}
// Register Interrupt Handler
if (signal(SIGINT, sig_handler) == SIG_ERR) {
fprintf(stderr, "\ncan't catch SIGINT\n");
return EXIT_FAILURE;
}
// Open output file
FILE * output_file;
output_file = fopen(outfile, "w");
if (output_file == NULL) {
fprintf(stderr, "Unable to open file %s for writing!\n", outfile);
return EXIT_FAILURE;
}
free(outfile);
// Print header
fprintf(output_file, "Timestamp,");
for (int port = 1; port < 8; ++port) {
fprintf(output_file, "Ch%d milliamps,Ch%d Millivolts,Ch%d Milliwatts,", port, port, port);
}
fprintf(output_file, "\n");
struct timeval t_start, t_now;
// Main sampling loop
gettimeofday( &t_start, NULL );
while (!halted) {
// hack something shitty, CBA with unix timers.
usleep(100000); // 1/10th of a second, vaguely, ish
gettimeofday( &t_now, NULL );
fprintf(output_file, "%ld.%d,", (long int) (t_now.tv_sec - t_start.tv_sec), (int) t_now.tv_usec);
for (int port = 1; port < 8; ++port) {
reading_t result = read_port(port);
fprintf(output_file, "%d,%d,%d,", result.miliamps, result.milivolts, result.miliwatts);
}
fprintf(output_file, "\n");
}
// Tidy up
fclose(output_file);
printf("o\nExiting!\n");
return EXIT_SUCCESS;
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
float depthArr[10] = "00000000";
float velArr[10] = "00000000";
int i;
ros::init(argc, argv, "svp"); //inits the driver
ros::NodeHandle svpN; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher svpDepthMsg = svpN.advertise<std_msgs::Float32>("svpDepth", 100);
ros::Publisher svpVeloMsg = svpN.advertise<std_msgs::Float32>("svpVelocity", 100);
/*Sets up the message structures*/
std_msgs::Float32 svpDepth;
std_msgs::Float32 svpVelo;
ros::Rate loop_rate(3); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("SVP Driver Online");
while (ros::ok()){
if(read_port() != 0){ //if we read correctly
for (i = 0; i < 6; i++){
depthArr[i] = bufPos[i+1];
}
depth = strtod(depth, &dEnd);
depth -= SURFACE; //pressure - surface pressure = specific weight x depth
depth /= DENSITY;
svpDepth.data = depth; //dMsg = 1.01240;//
//The Velocity
for (i = 1; i < 10; i++){
velArr[i] = bufPos[i+7];
}
velocity = strtod(velo, &vEnd);
svpVelo.data = velocity;
svpDepthMsg.publish(svpDepth);
svpVeloMsg.publish(svpVelo);
}
else{
ROS_ERROR("Read no data");
}
/*Below here we publish our readings*/
/*Have a snooze*/
loop_rate.sleep();
}
ros::spin();
close(fd);
ROS_INFO("Shutting Down");
printf("Shutting Down\n");
return 0;
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
ros::init(argc, argv, "compass"); //inits the driver
ros::NodeHandle n; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher compassHeadingMsg = n.advertise<std_msgs::Float32>("compassHeading", 100);
ros::Publisher compassPitchMsg = n.advertise<std_msgs::Float32>("compassPitch", 100);
ros::Publisher compassRollMsg = n.advertise<std_msgs::Float32>("compassRoll", 100);
/*Sets up the message structures*/
std_msgs::Float32 compassHeading;
std_msgs::Float32 compassPitch;
std_msgs::Float32 compassRoll;
ros::Rate loop_rate(10); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("Compass Driver Online");
while (ros::ok()){
if(write_port()){ //if we send correctly
if(read_port() != 0){ //if we read correctly
parseBuffer(); //parse the buffer
//printf("H: %f P: %f R: %f\n",heading,pitch,roll);
/* Below here sets up the messages ready for transmission*/
compassHeading.data = heading;
compassPitch.data = pitch;
compassRoll.data = roll;
ROS_DEBUG("H: %.3f P: %.3f R: %.3f",heading,pitch,roll);
}
else{
ROS_ERROR("Read no data");
}
}
else{
ROS_ERROR("Failed to write");
}
/*Below here we publish our readings*/
compassHeadingMsg.publish(compassHeading);
compassRollMsg.publish(compassRoll);
compassPitchMsg.publish(compassPitch);
/*Have a snooze*/
loop_rate.sleep();
}
ros::spin();
close(fd);
ROS_INFO("Shutting Down");
printf("Shutting Down\n");
return 0;
}
int32
ColorField::_UpdateThread(void *data)
{
// initialization
ColorField *colorField = (ColorField *)data;
BLooper *looper = colorField->Looper();
if (looper)
looper->Lock();
BBitmap* bitmap = colorField->fBgBitmap[0];
BView* view = colorField->fBgView[0];
port_id port = colorField->fUpdatePort;
if (looper)
looper->Unlock();
// draw
float h, s, v, r, g, b;
int R, G, B;
int32 msg_code;
char msg_buffer;
while (true) {
port_info info;
do {
read_port(port, &msg_code, &msg_buffer, sizeof(msg_buffer));
get_port_info(port, &info);
} while (info.queue_count);
if (looper)
looper->Lock();
uint colormode = colorField->fColorMode;
float fixedvalue = colorField->fFixedValue;
if (looper)
looper->Unlock();
bitmap->Lock();
view->BeginLineArray(256 * 256);
switch (colormode)
{
case R_SELECTED:
{
R = round(fixedvalue * 255);
for (int G = 0; G < 256; ++G)
for (int B = 0; B < 256; ++B)
DrawColorPoint(BPoint(G, 255.0 - B), R, G, B);
break;
}
case G_SELECTED:
{
G = round(fixedvalue * 255);
for (int R = 0; R < 256; ++R)
for (int B = 0; B < 256; ++B)
DrawColorPoint(BPoint(R, 255.0 - B), R, G, B);
break;
}
case B_SELECTED:
{
B = round(fixedvalue * 255);
for (int R = 0; R < 256; ++R)
for (int G = 0; G < 256; ++G)
DrawColorPoint(BPoint(R, 255.0 - G), R, G, B);
break;
}
case H_SELECTED:
{
h = fixedvalue;
for (int x = 0; x < 256; ++x) {
s = (float)x / 255.0;
for (int y = 0; y < 256; ++y) {
v = (float)y / 255.0;
HSV_to_RGB(h, s, v, r, g, b);
DrawColorPoint(BPoint(x, 255.0 - y), round(r * 255.0),
round(g * 255.0), round(b * 255.0));
}
}
break;
}
case S_SELECTED:
{
s = fixedvalue;
for (int x = 0; x < 256; ++x) {
h = 6.0 / 255 * x;
for (int y = 0; y<256; ++y) {
v = (float)y / 255.0;
HSV_to_RGB(h, s, v, r, g, b);
DrawColorPoint(BPoint(x, 255.0 - y), round(r * 255.0),
round(g * 255.0), round(b * 255.0));
}
}
break;
}
case V_SELECTED:
{
v = fixedvalue;
for (int x = 0; x < 256; ++x) {
h = 6.0 / 255 * x;
for (int y = 0; y < 256; ++y) {
s = (float)y / 255.0;
HSV_to_RGB(h, s, v, r, g, b);
DrawColorPoint(BPoint(x, 255.0 - y), round(r * 255.0),
round(g * 255.0), round(b * 255.0));
}
}
break;
}
}
view->EndLineArray();
view->Sync();
bitmap->Unlock();
looper = colorField->Looper();
if (looper && looper->Lock()) {
colorField->Update(2);
looper->Unlock();
}
}
return 0;
}
// _UpdateThread
status_t
ColorSlider::_UpdateThread(void* data)
{
// initializing
ColorSlider* colorSlider = (ColorSlider*)data;
bool looperLocked = colorSlider->LockLooper();
port_id port = colorSlider->fUpdatePort;
orientation orient = colorSlider->fOrientation;
if (looperLocked)
colorSlider->UnlockLooper();
float h, s, v, r, g, b;
int R, G, B;
// drawing
int32 msg_code;
char msg_buffer;
while (true) {
port_info info;
do {
read_port(port, &msg_code, &msg_buffer, sizeof(msg_buffer));
get_port_info(port, &info);
} while (info.queue_count);
if (colorSlider->LockLooper()) {
uint colormode = colorSlider->fMode;
float fixedvalue1 = colorSlider->fFixedValue1;
float fixedvalue2 = colorSlider->fFixedValue2;
BBitmap* bitmap = colorSlider->fBgBitmap;
BView* view = colorSlider->fBgView;
bitmap->Lock();
colorSlider->UnlockLooper();
view->BeginLineArray(256);
switch (colormode) {
case R_SELECTED: {
G = round(fixedvalue1 * 255);
B = round(fixedvalue2 * 255);
for (int R = 0; R < 256; ++R) {
_DrawColorLineY( view, R, R, G, B );
}
}; break;
case G_SELECTED: {
R = round(fixedvalue1 * 255);
B = round(fixedvalue2 * 255);
for (int G = 0; G < 256; ++G) {
_DrawColorLineY( view, G, R, G, B );
}
}; break;
case B_SELECTED: {
R = round(fixedvalue1 * 255);
G = round(fixedvalue2 * 255);
for (int B = 0; B < 256; ++B) {
_DrawColorLineY( view, B, R, G, B );
}
}; break;
case H_SELECTED: {
s = 1.0;//fixedvalue1;
v = 1.0;//fixedvalue2;
if (orient == B_VERTICAL) {
for (int y = 0; y < 256; ++y) {
HSV_to_RGB( (float)y*6.0/255.0, s, v, r, g, b );
_DrawColorLineY( view, y, r*255, g*255, b*255 );
}
} else {
for (int x = 0; x < 256; ++x) {
HSV_to_RGB( (float)x*6.0/255.0, s, v, r, g, b );
_DrawColorLineX( view, x, r*255, g*255, b*255 );
}
}
}; break;
case S_SELECTED: {
h = fixedvalue1;
v = 1.0;//fixedvalue2;
for (int y = 0; y < 256; ++y) {
HSV_to_RGB( h, (float)y/255, v, r, g, b );
_DrawColorLineY( view, y, r*255, g*255, b*255 );
}
}; break;
case V_SELECTED: {
h = fixedvalue1;
s = 1.0;//fixedvalue2;
for (int y = 0; y < 256; ++y) {
HSV_to_RGB( h, s, (float)y/255, r, g, b );
_DrawColorLineY( view, y, r*255, g*255, b*255 );
}
}; break;
}
view->EndLineArray();
view->Sync();
bitmap->Unlock();
if (colorSlider->LockLooper()) {
colorSlider->Update(1);
colorSlider->UnlockLooper();
}
}
}
return B_OK;
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
ros::init(argc, argv, "picsubnet"); //inits the driver
ros::NodeHandle picsubnetN; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher pubReedSwitch = picsubnetN.advertise<std_msgs::Char>("reed_switch", 100);
ros::Publisher pubBTShutdown = picsubnetN.advertise<std_msgs::Char>("bt_shutdown", 100);
ros::Publisher pubBatteryVoltage1 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_1", 100);
ros::Publisher pubBatteryVoltage2 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_2", 100);
ros::Publisher pubAmbient = picsubnetN.advertise<std_msgs::Float32>("ambient_temperature", 100);
ros::Publisher pubMotor1 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_1", 100);
ros::Publisher pubMotor2 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_2", 100);
ros::Publisher pubFitPC = picsubnetN.advertise<std_msgs::Float32>("fitpc_temperature", 100);
ros::Publisher pubRouter = picsubnetN.advertise<std_msgs::Float32>("router_temperature", 100);
ros::Publisher pubRoboard = picsubnetN.advertise<std_msgs::Float32>("roboard_temperature", 100);
ros::Rate loop_rate(15); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
ROS_ERROR("FAILED TO CONNECT");
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("PIC SUBNET ONLINE");
while (ros::ok()){
//All on/All off
led_test++;
if (led_test == 1)
write_port((unsigned char *)"$1N", 4);
else if (led_test == 2)
write_port((unsigned char *)"$2N", 4);
else if (led_test == 3)
write_port((unsigned char *)"$3N", 4);
else if (led_test == 4)
write_port((unsigned char *)"$4N", 4);
else if (led_test == 5)
write_port((unsigned char *)"$5N", 4);
else if (led_test == 6)
write_port((unsigned char *)"$6N", 4);
else if (led_test == 7)
write_port((unsigned char *)"$7N", 4);
else if (led_test == 8)
write_port((unsigned char *)"$1F", 4);
else if (led_test == 9)
write_port((unsigned char *)"$2F", 4);
else if (led_test == 10)
write_port((unsigned char *)"$3F", 4);
else if (led_test == 11)
write_port((unsigned char *)"$4F", 4);
else if (led_test == 12)
write_port((unsigned char *)"$5F", 4);
else if (led_test == 13)
write_port((unsigned char *)"$6F", 4);
else if (led_test == 14)
{
write_port((unsigned char *)"$7F", 4);
led_test = 0;
}
// Read data from the port
if (read_port() == 1)
{
//if (bt_shutdown.data == (uint8_t)1) ROS_ERROR("BLUETOOTH SHUTDOWN ACTIVATED");
//if (reed_status.data == (uint8_t)-1) ROS_WARN("REED SWITCH - NO RESPONSE");
//There should only ever be 2 batteries connected...
int battery_counter = 0;
if (battery_voltage1.data != (float)-1.0)
{
battery1_publish_data.data = battery_voltage1.data;
battery_counter++;
}
if (battery_voltage2.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage2.data;
else
battery2_publish_data.data = battery_voltage2.data;
battery_counter++;
}
if (battery_voltage3.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage3.data;
else if (battery_counter == 1)
battery2_publish_data.data = battery_voltage3.data;
//else
//ROS_WARN("More than 2 batteries detected...");
battery_counter++;
}
if (battery_voltage4.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage4.data;
else if (battery_counter == 1)
battery2_publish_data.data = battery_voltage4.data;
//else
//ROS_WARN("More than 2 batteries detected...");
battery_counter++;
}
if (battery_counter < 2)
;
//ROS_WARN("Only %d Batteries found", battery_counter);
else
{
pubBatteryVoltage1.publish(battery1_publish_data);
pubBatteryVoltage2.publish(battery2_publish_data);
}
//Publish new data
pubReedSwitch.publish(reed_status);
pubBTShutdown.publish(bt_shutdown);
pubAmbient.publish(ambient_temperature);
pubMotor1.publish(motor1_temperature);
pubMotor2.publish(motor2_temperature);
pubFitPC.publish(fitpc_temperature);
pubRouter.publish(router_temperature);
pubRoboard.publish(roboard_temperature);
}
/*Have a snooze*/
loop_rate.sleep();
}
close(fd);
ROS_INFO("Shutting Down");
return 0;
}
void timer_install() {
uint8_t current_mask = read_port(0x21);
write_port(0x21 , current_mask & TIMER_INTERRUPT_MASK);
}
long _recv_img_file(QSP_ARG_DECL Port *mpp, /* char **bufp */ Packet *pkp )
{
long len;
Image_File *old_ifp, *new_ifp;
char namebuf[LLEN];
Image_File imgf, *ifp;
long code;
ifp=(&imgf);
len=get_port_int32(mpp);
if( len <= 0 ) goto error_return;
#ifdef QUIP_DEBUG
if( debug & debug_data ){
sprintf(ERROR_STRING,
"recv_file: want %ld name bytes",len);
advise(ERROR_STRING);
}
#endif /* QUIP_DEBUG */
if( (ifp->if_nfrms = get_port_int32(mpp)) == BAD_PORT_LONG ||
(ifp->if_ftp = filetype_for_code( QSP_ARG (filetype_code) get_port_int32(mpp))) == NULL ||
(ifp->if_flags = (short) get_port_int32(mpp)) == (short)BAD_PORT_LONG ){
warn("error getting image file data");
goto error_return;
}
if( len > LLEN ){
warn("more than LLEN name chars!?");
goto error_return;
}
if( read_port(mpp,namebuf,len) != len ){
warn("recv_file: error reading data object name");
goto error_return;
}
/* where does the string get null-terminated? */
if( (long)strlen( namebuf ) != len-1 ){
u_int i;
sprintf(ERROR_STRING,"name length %ld, expected %ld",
(long)strlen(namebuf), len-1);
advise(ERROR_STRING);
sprintf(ERROR_STRING,"name: \"%s\"",namebuf);
advise(ERROR_STRING);
for(i=0;i<strlen(namebuf);i++){
sprintf(ERROR_STRING,"name[%d] = '%c' (0%o)",
i,namebuf[i],namebuf[i]);
advise(ERROR_STRING);
}
error1("choked");
}
old_ifp=img_file_of(namebuf);
if( old_ifp != NULL ){
del_img_file(old_ifp);
rls_str((char *)old_ifp->if_name); // BUG? release name here or not?
old_ifp = NULL;
}
new_ifp=new_img_file(namebuf);
if( new_ifp==NULL ){
sprintf(ERROR_STRING,
"recv_file: couldn't create file struct \"%s\"",
namebuf);
warn(ERROR_STRING);
goto error_return;
}
new_ifp->if_nfrms = imgf.if_nfrms;
new_ifp->if_ftp = filetype_for_code(QSP_ARG IFT_NETWORK);
new_ifp->if_flags = imgf.if_flags;
new_ifp->if_dp = NULL; /* BUG should receive? */
new_ifp->if_pathname = new_ifp->if_name;
code = get_port_int32(mpp);
if( code == -1 )
error1("error port code received!?");
if( code != P_DATA ){
sprintf(ERROR_STRING,
"recv_file: expected data object packet to complete transmission of file %s!?",
new_ifp->if_name);
error1(ERROR_STRING);
}
// the cast generates a compiler warning???
if( recv_obj(mpp, pkp ) != sizeof(Data_Obj) ){
warn("Error receiving data object!?");
goto error_return;
}
// The packet returns the dp in pk_extra...
return sizeof(*new_ifp); // BUG - what size should we return???
error_return:
return -1;
}
static void PREFIX186(_insb)(void) /* Opcode 0x6c */
{
ICOUNT -= cycles.ins8;
PutMemB(ES,I.regs.w[DI],read_port(I.regs.w[DX]));
I.regs.w[DI] += I.DirVal;
}
void nosound(){
uint8_t tmp = read_port(0x61) & 0xFC;
write_port(0x61, tmp);
}
// _UpdateThread
int32
ColorField::_UpdateThread(void* data)
{
// initializing
ColorField* colorField = (ColorField *)data;
bool looperLocked = colorField->LockLooper();
BBitmap* bitmap = colorField->fBgBitmap[0];
port_id port = colorField->fUpdatePort;
orientation orient = colorField->fOrientation;
if (looperLocked)
colorField->UnlockLooper();
float h, s, v, r, g, b;
int R, G, B;
// drawing
int32 msg_code;
char msg_buffer;
while (true) {
port_info info;
do {
read_port(port, &msg_code, &msg_buffer, sizeof(msg_buffer));
get_port_info(port, &info);
} while (info.queue_count);
if (colorField->LockLooper()) {
uint colormode = colorField->fMode;
float fixedvalue = colorField->fFixedValue;
int width = (int)colorField->Width();
int height = (int)colorField->Height();
colorField->UnlockLooper();
bitmap->Lock();
//bigtime_t now = system_time();
uint8* bits = (uint8*)bitmap->Bits();
uint32 bpr = bitmap->BytesPerRow();
// offset 2 pixels from top and left
bits += 2 * 4 + 2 * bpr;
switch (colormode) {
case R_SELECTED: {
R = round(fixedvalue * 255);
for (int y = height; y >= 0; y--) {
uint8* bitsHandle = bits;
int B = y / height * 255;
for (int x = 0; x <= width; ++x) {
int G = x / width * 255;
set_bits(bitsHandle, R, G, B);
bitsHandle += 4;
}
bits += bpr;
}
}; break;
case G_SELECTED: {
G = round(fixedvalue * 255);
for (int y = height; y >= 0; y--) {
uint8* bitsHandle = bits;
int B = y / height * 255;
for (int x = 0; x <= width; ++x) {
int R = x / width * 255;
set_bits(bitsHandle, R, G, B);
bitsHandle += 4;
}
bits += bpr;
}
}; break;
case B_SELECTED: {
B = round(fixedvalue * 255);
for (int y = height; y >= 0; y--) {
uint8* bitsHandle = bits;
int G = y / height * 255;
for (int x = 0; x <= width; ++x) {
int R = x / width * 255;
set_bits(bitsHandle, R, G, B);
bitsHandle += 4;
}
bits += bpr;
}
}; break;
case H_SELECTED: {
h = fixedvalue;
if (orient == B_VERTICAL) {
for (int y = 0; y <= height; ++y) {
v = (float)(height - y) / height;
uint8* bitsHandle = bits;
for (int x = 0; x <= width; ++x) {
s = (float)x / width;
HSV_to_RGB( h, s, v, r, g, b );
set_bits(bitsHandle, round(r * 255.0), round(g * 255.0), round(b * 255.0));
bitsHandle += 4;
}
bits += bpr;
}
} else {
for (int y = 0; y <= height; ++y) {
s = (float)y / height;
uint8* bitsHandle = bits;
for (int x = 0; x <= width; ++x) {
v = (float)(width - x) / width;
HSV_to_RGB( h, s, v, r, g, b );
set_bits(bitsHandle, round(r * 255.0), round(g * 255.0), round(b * 255.0));
bitsHandle += 4;
}
bits += bpr;
}
}
}; break;
case S_SELECTED: {
s = fixedvalue;
for (int y = 0; y <= height; ++y) {
v = (float)(height - y) / height;
uint8* bitsHandle = bits;
for (int x = 0; x <= width; ++x) {
h = 6.0 / width * x;
HSV_to_RGB( h, s, v, r, g, b );
set_bits(bitsHandle, round(r * 255.0), round(g * 255.0), round(b * 255.0));
bitsHandle += 4;
}
bits += bpr;
}
}; break;
case V_SELECTED: {
v = fixedvalue;
for (int y = 0; y <= height; ++y) {
s = (float)(height - y) / height;
uint8* bitsHandle = bits;
for (int x = 0; x <= width; ++x) {
h = 6.0 / width * x;
HSV_to_RGB( h, s, v, r, g, b );
set_bits(bitsHandle, round(r * 255.0), round(g * 255.0), round(b * 255.0));
bitsHandle += 4;
}
bits += bpr;
}
}; break;
}
//printf("color field update: %lld\n", system_time() - now);
bitmap->Unlock();
if (colorField->LockLooper()) {
colorField->Update(2);
colorField->UnlockLooper();
}
}
}
}
void* console_thread(void* ptr) {
read_port();
}
int main()
{
printf("Hi, welcome to the program\n");
read_port();
return 0;
}
/* delay for start/stop action */
void delay_spi(int n)
{
while(n--)
read_port(HIGH_PORT);
}
/****************************************************************
SeaNet General Packet Format is;
* '@' : Message Header = '@' (0x40).
* HHHH : Hex Length of whole binary packet (excluding LF Terminator).
* BB : Binary Word of above Hex Length.
* SID : Packet Source Identification (Tx Node number 0 - 255).
* DID : Packet Destination Identification (Rx Node number 0 -255).
* COUNT : Byte Count of attached message that follows this byte.
* MSG : Command / Reply Message (i.e. 'Alive' command, 'Data Reply' message).
* TERM : Message Terminator = Line Feed (0Ah).
*
* Returns -1 if failed, 1 if correct first time, 2 if it had to
* stich packets
*
****************************************************************/
int sortPacket(void)
{
int packetFlag = 0,
leFlag = 0,
buffLen, //length of the recieved buffer, output from read_port()
i, //counter
temp[263],
msgLen,
binFlag;
//How long was the msg, according to read() ?
buffLen = read_port();
//printf("buffLen = %d\n", buffLen);
rBptr = &returnBuffer[0]; //set pointer for recieved data
//Store all packets into temp[]
for( i = 0; i < buffLen; i++ )
{
leFlag = temp[i];
temp[i] = getU8();
if(i >= 1 && temp[i] == 0x40 && temp[i-1] == 0x0a)
{
buffLen = i;
}
}
//Store temp stuff into right place
header = temp[0];
hLength = getU32(temp[1], temp[2], temp[3], temp[4]);
bLength = getU16(temp[6], temp[5]);
sID = temp[7];
dID = temp[8];
byteCount = temp[9];
term = temp[buffLen-1];
//Clear msg buffer first
for(i = 0; i < 263; i++)
msg[i] = NULL;
msgLen = 0;
//Store the msg, works for varying lengths
for( i = 10; i < (buffLen-2); i ++)
{
msg[(i-10)] = temp[i];
msgLen ++;
}
//What is prinf?
if(header == '@' && term == 10 && buffLen == bLength + 6)
{
//Prinf the packet
//printf("<< ");
//for(i = 0; i < buffLen; i ++)
// printf("%x : ", temp[i]);
//printf("\n");
packetFlag = 1;
if(msg[0] == mtHeadData)
{
// if(byteCount == 0)
// printf("Single Packet\n");
// else
// printf("Multi Packet\n");
binFlag = 0;
//printf("\nBearing: %f\n Bins: ", (float) getU16(temp[41], temp[40]) / 17.775 );
for(i = 44; i < buffLen-1; i++ )
{
tempBinArray[i-44] = temp[i];
//printf("%d, ", temp[i]);
if(temp[i] >= THRESHOLD && binFlag == 0)
{
bins = i-44;
binFlag = 1;
}
}
//printf("%d\n", bins * 6);
bearing = getU16(temp[41], temp[40]);
}
//printf("%d, %d\n", byteCount, msgLen+1);
}
else
{
packetFlag = -1;
}
return packetFlag;
}
static NTSTATUS ioctl(PDEVICE_OBJECT devobj, PIRP irp)
{
NTSTATUS status = STATUS_NOT_IMPLEMENTED;
PIO_STACK_LOCATION irpsp;
PFILE_OBJECT fileobj;
struct eppflex *s = devobj->DeviceExtension;
struct eppflexfile *fs;
irpsp = IoGetCurrentIrpStackLocation(irp);
fileobj = irpsp->FileObject;
fs = fileobj->FsContext;
VLOG(printk("eppflex: ioctl\n"));
if (!fs) {
status = irp->IoStatus.Status = STATUS_NOT_IMPLEMENTED;
IoCompleteRequest(irp, IO_NO_INCREMENT);
return status;
}
switch (irpsp->Parameters.DeviceIoControl.IoControlCode) {
case IOCTL_EPPFLEX_READ_DATA:
return read_port(devobj, irp, fs->info.Controller);
case IOCTL_EPPFLEX_WRITE_DATA:
return write_port(devobj, irp, fs->info.Controller);
case IOCTL_EPPFLEX_READ_STATUS:
return read_port(devobj, irp, fs->info.Controller + 1);
case IOCTL_EPPFLEX_WRITE_STATUS:
return write_port(devobj, irp, fs->info.Controller + 1);
case IOCTL_EPPFLEX_READ_CONTROL:
return read_port(devobj, irp, fs->info.Controller + 2);
case IOCTL_EPPFLEX_WRITE_CONTROL:
return write_port(devobj, irp, fs->info.Controller + 2);
case IOCTL_EPPFLEX_FROB_CONTROL:
return frob_port(devobj, irp, fs->info.Controller + 2);
case IOCTL_EPPFLEX_READ_ECONTROL:
return read_port(devobj, irp, fs->pnpinfo.EcpController + 2);
case IOCTL_EPPFLEX_WRITE_ECONTROL:
return write_port(devobj, irp, fs->pnpinfo.EcpController + 2);
case IOCTL_EPPFLEX_FROB_ECONTROL:
return frob_port(devobj, irp, fs->pnpinfo.EcpController + 2);
case IOCTL_EPPFLEX_READ_CONFIGA:
return read_port(devobj, irp, fs->pnpinfo.EcpController);
case IOCTL_EPPFLEX_WRITE_CONFIGA:
return write_port(devobj, irp, fs->pnpinfo.EcpController);
case IOCTL_EPPFLEX_READ_CONFIGB:
return read_port(devobj, irp, fs->pnpinfo.EcpController + 1);
case IOCTL_EPPFLEX_WRITE_CONFIGB:
return write_port(devobj, irp, fs->pnpinfo.EcpController + 1);
case IOCTL_EPPFLEX_READ_EPPADDR:
return read_port(devobj, irp, fs->info.Controller + 3);
case IOCTL_EPPFLEX_WRITE_EPPADDR:
return write_port(devobj, irp, fs->info.Controller + 3);
case IOCTL_EPPFLEX_READ_EPPDATA:
return read_port(devobj, irp, fs->info.Controller + 4);
case IOCTL_EPPFLEX_WRITE_EPPDATA:
return write_port(devobj, irp, fs->info.Controller + 4);
}
irp->IoStatus.Status = status;
IoCompleteRequest(irp, IO_NO_INCREMENT);
return status;
}
/* attribute callback handler (read) */
static ssize_t get_port1_handler(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_port(dev, attr, buf, 1, CYPRESS_READ_PORT_ID1);
}
int main(void)
{
return read_port();
}
int main() {
create_DI_channel(0);
create_DI_channel(1);
create_DI_channel(2);
create_DI_channel(3);
create_DO_channel(4);
create_DO_channel(5);
bool run = true;
int bit = 0;
while(run){
if (_kbhit())
if(_getch()==27){
exit(1);
}
uInt8 p4 = read_port(4);
bit++;
Sleep(100);
if(get_bit(7, p4)){
printf("\n1");
}
if (!get_bit(7, p4)) {
printf("\n0");
}
if(bit>=10){
if(!get_bit(7, p4)){
p4 = set_bit(p4, 7, true);
write_port(4, p4);
} else {
if (get_bit(7, p4)) {
p4 = set_bit(p4, 7, false);
write_port(4, p4);
}
}
bit = 0;
}
}
/*time_t t = time(0); //now time
for (int i = 0; i < 5; i++) {
for (int j = 0; j < 10; j++) {
matrix[i][j].cheio = false;
matrix[i][j].valor = 0;
}
}
calibrate();
task_storage_services();
write_port(4, 0); //para todos os motores como precaução
close_channels();*/
_getch();
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
static int port = -1;
int cmd_len, arg_len;
int port_open, arg_given;
char *cmd, *arg;
// Checks
if (nrhs < 1) mexErrMsgTxt("One argument required.");
if (nrhs > 2) mexErrMsgTxt("Too many arguments given.");
if (nlhs > 1) mexErrMsgTxt("Too many outputs.");
if (!mxIsChar(prhs[0])) mexErrMsgTxt("Input must be a string.");
if (mxGetM(prhs[0]) != 1) mexErrMsgTxt("Input must be a row vector.");
// Get command
cmd = mxArrayToString(prhs[0]);
cmd_len = mxGetN(prhs[0]);
// Get argument
if (nrhs == 2) {
arg = mxArrayToString(prhs[1]);
arg_len = mxGetN(prhs[1]);
}
// Last inits
// out = mxCalloc(BUFFER_SIZE, sizeof(char));
port_open = (port != -1);
arg_given = (nrhs == 2);
// Check command
if (strcmp("open", cmd) == 0) {
// Open port
if (!arg_given) mexErrMsgTxt("Second argument required.");
port = open_port(arg);
// Check if port is opened
if (port == -1) {
// Return failure
plhs[0] = mxCreateDoubleScalar(0);
} else {
// Return success
plhs[0] = mxCreateDoubleScalar(1);
}
} else if (strcmp("close", cmd) == 0) {
// Close port
if (arg_given) mexErrMsgTxt("Second argument unexpected.");
if (port != -1) {
close_port(port);
port = -1;
}
// Return success
plhs[0] = mxCreateDoubleScalar(1);
} else if (strcmp("transmit", cmd) == 0) {
// Transmit and receive data
if (!arg_given) mexErrMsgTxt("Second argument required.");
if (port == -1) mexErrMsgTxt("No port openend.");
// Write port
write_port(port, arg);
// Read data
char *ret = mxCalloc(BUFFER_SIZE, sizeof(char));
read_port(port, ret, 4); // ASCII 4 is end-of-transmission character
// Return data read
plhs[0] = mxCreateString(ret);
mxFree(ret);
}
// Return value and free variables
mxFree(cmd);
if (arg_given) mxFree(arg);
}
void *recv_thread()
{
read_port();
pthread_exit(NULL);
}
static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
#endif
{
_irqL irqL;
uint isevt, *pbuf;
struct recv_buf *precvbuf = (struct recv_buf *)purb->context;
_adapter *padapter =(_adapter *)precvbuf->adapter;
struct recv_priv *precvpriv = &padapter->recvpriv;
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete!!!\n"));
//Useless for linux usb driver.
//2010-03-10 by Thomas
//_enter_critical(&precvpriv->lock, &irqL);
//precvbuf->irp_pending=_FALSE;
//precvpriv->rx_pending_cnt --;
//_exit_critical(&precvpriv->lock, &irqL);
//if(precvpriv->rx_pending_cnt== 0)
//{
// RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete: rx_pending_cnt== 0, set allrxreturnevt!\n"));
// _up_sema(&precvpriv->allrxreturnevt);
//}
if(padapter->bSurpriseRemoved ||padapter->bDriverStopped)
{
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n", padapter->bDriverStopped, padapter->bSurpriseRemoved));
goto exit;
}
if(purb->status==0)//SUCCESS
{
if((purb->actual_length>(MAX_RECVBUF_SZ)) || (purb->actual_length < RXDESC_SIZE))
{
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete: (purb->actual_length > MAX_RECVBUF_SZ) || (purb->actual_length < RXDESC_SIZE)\n"));
precvbuf->reuse = _TRUE;
read_port(padapter, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
}
else
{
precvbuf->transfer_len = purb->actual_length;
pbuf = (uint*)precvbuf->pbuf;
if((isevt = le32_to_cpu(*(pbuf+1))&0x1ff) == 0x1ff)
{
//_irqL irqL;
//_enter_critical( &padapter->lockRxFF0Filter, &irqL );
//if ( padapter->blnEnableRxFF0Filter )
//{
// padapter->blnEnableRxFF0Filter = 0;
//}
//_exit_critical( &padapter->lockRxFF0Filter, &irqL );
//MSG_8712("usb_read_port_complete():rxcmd_event_hdl\n");
rxcmd_event_hdl(padapter, pbuf);//rx c2h events
precvbuf->reuse = _TRUE;
read_port(padapter, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
}
else
{
#ifdef CONFIG_RECV_TASKLET
_pkt *pskb = precvbuf->pskb;
skb_put(pskb, purb->actual_length);
skb_queue_tail(&precvpriv->rx_skb_queue, pskb);
tasklet_hi_schedule(&precvpriv->recv_tasklet);
precvbuf->pskb = NULL;
precvbuf->reuse = _FALSE;
read_port(padapter, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
#else
if(recvbuf2recvframe(padapter, precvbuf)==_FAIL)//rx packets
{
precvbuf->reuse = _TRUE;
read_port(padapter, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
}
#endif
}
}
}
else
{
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete : purb->status(%d) != 0 \n", purb->status));
printk( "[%s] purb->status(%d) != 0\n", __FUNCTION__, purb->status );
switch(purb->status) {
case -EINVAL:
case -EPIPE:
case -ENODEV:
case -ESHUTDOWN:
//padapter->bSurpriseRemoved=_TRUE;
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete:bSurpriseRemoved=TRUE\n"));
case -ENOENT:
padapter->bDriverStopped=_TRUE;
RT_TRACE(_module_hci_ops_os_c_,_drv_err_,("usb_read_port_complete:bDriverStopped=TRUE\n"));
break;
case -EPROTO:
precvbuf->reuse = _TRUE;
read_port(padapter, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
break;
case -EINPROGRESS:
printk("ERROR: URB IS IN PROGRESS!/n");
break;
default:
break;
}
}
exit:
_func_exit_;
}