JAKO_EXPORT int jakopter_land()
{
char * args[] = {land_arg};
set_cmd(HEAD_REF, args, 1);
int no_sq = 0;
int attempt = 0;
while (no_sq == 0 && attempt < NAVDATA_ATTEMPT) {
nanosleep(&cmd_wait, NULL);
no_sq = navdata_no_sq();
attempt++;
}
int init_no_sq = navdata_no_sq();
int emergency = 0;
attempt = 0;
while (jakopter_is_flying() && jakopter_height() > HEIGHT_THRESHOLD && !emergency) {
nanosleep(&cmd_wait, NULL);
no_sq = navdata_no_sq();
//emergency sent if no new data received
if (no_sq == init_no_sq && attempt >= NAVDATA_ATTEMPT)
emergency = jakopter_emergency();
attempt++;
}
set_cmd(NULL, NULL, 0);
return 0;
}
t_stat
ng_boot(int32 unit, DEVICE *dptr)
{
t_stat r;
set_cmd (0, "CPU 56K");
set_cmd (0, "NG TYPE=LOGO");
set_cmd (0, "PCLK ENABLED");
set_cmd (0, "KE ENABLED");
set_cmd (0, "RF ENABLED");
attach_cmd (0, "RF dummy");
sim_set_memory_load_file (BOOT_CODE_ARRAY, BOOT_CODE_SIZE);
r = load_cmd (0, BOOT_CODE_FILENAME);
sim_set_memory_load_file (NULL, 0);
cpu_set_boot (0400);
sim_printf ("List of 11LOGO commands:\n");
sim_printf (
"AND, BACK, BUTFIRST, BUTLAST, COUNT, CTF, DIFFERENCE, DISPLAY, DO,\n"
"EDIT, ELSE, EMPTYP, END, EQUAL, ERASETRACE, FIRST, FORWARD, FPRINT,\n"
"FPUT, GO, GREATER, HEADING, HERE, HIDETURTLE, HOME, IF, KILLDISPLAY,\n"
"LAMPOFF, LAMPON, LAST, LEFT, LESS, LEVEL, LIST, LISTP, LPUT, MAKE,\n"
"MOD, NEWSNAP, NUMBERP, OF, OUTPUT, PENDOWN, PENUP, PRINT, PRODUCT,\n"
"QUOTIENT, REQUEST, RIGHT, RUG, SENTENCE, SETHEADING, SETTURTLE, SETX,\n"
"SETXY, SETY, SHOW, SHOWTURTLE, SNAP, STARTDISPLAY, STF, STOP, SUM,\n"
"THEN, TO, TOOT, TRACE, TYPE, VERSION, WIPE, WIPECLEAN, WORD, WORDP,\n"
"XCOR, YCOR.\n\n");
sim_printf ("MIT AI memo 315 documents a later version of 11LOGO but may be helpful\n");
sim_printf ("in exploring the software. It can currently be found here:\n");
sim_printf ("https://dspace.mit.edu/handle/1721.1/6228\n\n");
sim_printf ("To get started with turtle graphics, type STARTDISPLAY.\n\n\n");
return r;
}
static SimpleSpiceUpdate *test_spice_create_update_copy_bits(uint32_t surface_id)
{
SimpleSpiceUpdate *update;
QXLDrawable *drawable;
int bw, bh;
QXLRect bbox = {
.left = 10,
.top = 0,
};
update = calloc(sizeof(*update), 1);
drawable = &update->drawable;
bw = g_primary_width/SINGLE_PART;
bh = 48;
bbox.right = bbox.left + bw;
bbox.bottom = bbox.top + bh;
//printf("allocated %p, %p\n", update, update->bitmap);
drawable->surface_id = surface_id;
drawable->bbox = bbox;
drawable->clip.type = SPICE_CLIP_TYPE_NONE;
drawable->effect = QXL_EFFECT_OPAQUE;
simple_set_release_info(&drawable->release_info, (intptr_t)update);
drawable->type = QXL_COPY_BITS;
drawable->surfaces_dest[0] = -1;
drawable->surfaces_dest[1] = -1;
drawable->surfaces_dest[2] = -1;
drawable->u.copy_bits.src_pos.x = 0;
drawable->u.copy_bits.src_pos.y = 0;
set_cmd(&update->ext, QXL_CMD_DRAW, (intptr_t)drawable);
return update;
}
static SimpleSurfaceCmd *create_surface(int surface_id, int width, int height, uint8_t *data)
{
SimpleSurfaceCmd *simple_cmd = calloc(sizeof(SimpleSurfaceCmd), 1);
QXLSurfaceCmd *surface_cmd = &simple_cmd->surface_cmd;
set_cmd(&simple_cmd->ext, QXL_CMD_SURFACE, (intptr_t)surface_cmd);
simple_set_release_info(&surface_cmd->release_info, (intptr_t)simple_cmd);
surface_cmd->type = QXL_SURFACE_CMD_CREATE;
surface_cmd->flags = 0; // ?
surface_cmd->surface_id = surface_id;
surface_cmd->u.surface_create.format = SPICE_SURFACE_FMT_32_xRGB;
surface_cmd->u.surface_create.width = width;
surface_cmd->u.surface_create.height = height;
surface_cmd->u.surface_create.stride = -width * 4;
surface_cmd->u.surface_create.data = (intptr_t)data;
return simple_cmd;
}
JAKO_EXPORT int jakopter_reinit()
{
if (set_cmd(HEAD_COM_WATCHDOG, NULL, 0) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
JAKO_EXPORT int jakopter_emergency()
{
char * args[] = {emergency_arg};
if (set_cmd(HEAD_REF, args, 1) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
int philips_write_session(
unsigned char lofp,
unsigned char onp,
unsigned char toctype,
struct philips_track_descriptor tda[],
unsigned short num )
{
int index;
unsigned short size;
scsi_cmd * Cmd;
struct philips_track_descriptor * pnt;
size = 0;
pnt = (struct philips_track_descriptor *)scsi_buf;
index = 0;
while( num-->0 )
{
size += tda[index].length;
memcpy( pnt, &tda[index], tda[index].length );
(char*)pnt += tda[index].length;
index++;
}
Cmd = set_cmd( 10, 0xED, 0, size, scsi_buf, size );
Cmd->Cmd[6] = ( (lofp&3) << 4 ) + ( (onp&1) << 3 ) + (toctype&7);
return scsi_out( Cmd );
}
JAKO_EXPORT int jakopter_move(float l_to_r, float b_to_f, float vertical_speed, float angular_speed)
{
//inverted in Parrot ARdrone Protocol
if (b_to_f != 0.0) {
b_to_f = -b_to_f;
}
char * args[5];
args[0] = "1";
char buf[INT_LEN];
snprintf(buf, INT_LEN, "%d", *((int *) &l_to_r));
args[1] = buf;
char buf2[INT_LEN];
snprintf(buf2, INT_LEN, "%d", *((int *) &b_to_f));
args[2] = buf2;
char buf3[INT_LEN];
snprintf(buf3, INT_LEN, "%d", *((int *) &vertical_speed));
args[3] = buf3;
char buf4[INT_LEN];
snprintf(buf4, INT_LEN, "%d", *((int *) &angular_speed));
args[4] = buf4;
if (set_cmd(HEAD_PCMD, args, 5) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
return 0;
}
void
ICR::pulser::command::PulserReceiver_cmd::update()
{
m_channel_char = m_pr->get_channel_char();
m_address = m_pr->get_address_char();
set_cmd();
}
void
ICR::pulser::command::master::set(const bool& is_master)
{
if (is_master) m_master=true;
else m_master=false;
set_cmd();
}
JAKO_EXPORT int jakopter_flat_trim()
{
if (jakopter_is_flying()) {
fprintf(stderr, "[*][cmd] Drone is flying, setting of frame of reference canceled.\n");
return -1;
}
if (set_cmd(HEAD_FTRIM, NULL, 0) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
int philips_read_track_info(
unsigned long Track,
struct track_info *Buffer,
unsigned short Len )
{
return scsi_in( set_cmd( 10, 0xE5, Track, Len, Buffer, Len ) );
}
int init_navdata_bootstrap()
{
int ret;
char * bootstrap_cmd[] = {"\"general:navdata_demo\"","\"TRUE\""};
if (set_cmd(HEAD_CONFIG, bootstrap_cmd, 2) < 0)
return -1;
ret = send_cmd();
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return ret;
}
int philips_write_track( int track_no, int copy, int raw, int aud,
int mode, int mix )
{
scsi_cmd * Cmd;
Cmd = set_cmd( 10, 0xE6, track_no&0xFF, 0, NULL, 0 );
Cmd->Cmd[6] = ((copy&0x03)<<4) | (raw?8:0) | (aud?4:0) | (mode&0x03);
if( mix ) Cmd->Cmd[9] |= 0x40;
return scsi_out( Cmd );
}
int config_ack()
{
int ret;
//5 to reset navdata mask
//Send ACK_CONTROL_MODE
char * ctrl_cmd[] = {"5","0"};
if (set_cmd(HEAD_CTRL, ctrl_cmd, 2) < 0)
return -1;
ret = send_cmd();
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return ret;
}
void
ICR::pulser::command::gain::set(const short& g)
{
const short actual_gain = g-m_measured;
if (actual_gain<m_min || actual_gain>m_max)
throw ICR::exception::requested_gain_out_of_bounds();
m_gain_char = (char) (actual_gain - m_min);
set_cmd();
};
int
bjnp_get_printer_status (const int port_type, const char *device_uri,
const int port_number, char *status)
{
/*
* get printer status
*/
struct BJNP_command cmd;
struct IDENTITY *id;
int resp_len;
int id_len;
char resp_buf[BJNP_RESP_MAX];
struct sockaddr_in addr;
if (port_type == BJNP)
{
if (port_number > num_printers)
return NO_PRINTER_FOUND;
else
memcpy(&addr, &list[port_number].addr, sizeof(struct sockaddr_in));
}
else
if(bjnp_get_address_for_named_printer(device_uri, &addr) != OK)
return NO_PRINTER_FOUND;
/* set defaults */
strcpy (status, "");
set_cmd (&cmd, CMD_UDP_GET_STATUS, 0, 0);
bjnp_hexdump (10, "Get printer status", (char *) &cmd,
sizeof (struct BJNP_command));
resp_len =
udp_command (&addr, (char *) &cmd, sizeof (struct BJNP_command),
resp_buf, BJNP_RESP_MAX);
if (resp_len <= sizeof (struct BJNP_command))
return -1;
bjnp_hexdump (10, "Printer status:", resp_buf, resp_len);
id = (struct IDENTITY *) resp_buf;
id_len = ntohs (id->id_len) - sizeof (id->id_len);
/* set status */
strncpy (status, id->id, id_len);
status[id_len] = '\0';
bjnp_debug (7, "Status = %s\n", status);
return 0;
}
JAKO_EXPORT int jakopter_calib()
{
if (!jakopter_is_flying()) {
fprintf(stderr, "[*][cmd] Drone isn't flying, calibration canceled.\n");
return -1;
}
// 0 is the magnetometer, the only we can calibrate with ARdrone 2.0
char * args[] = {"0"};
if (set_cmd(HEAD_CALIB, args, 1) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
JAKO_EXPORT int jakopter_stay()
{
char * args[5] = {"0","0","0","0","0"};
if (set_cmd(HEAD_PCMD, args, 5) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
return 0;
}
t_stat
ng_reset(DEVICE *dptr)
{
DEVICE *dptr2;
t_stat r;
if (dptr->flags & DEV_DIS) {
sim_cancel (dptr->units);
return auto_config ("NG", (dptr->flags & DEV_DIS) ? 0 : 1);;
}
dptr2 = find_dev ("VT");
if ((dptr2 != NULL) && !(dptr2->flags & DEV_DIS)) {
dptr->flags |= DEV_DIS;
return sim_messagef (SCPE_NOFNC, "NG and VT device can't both be enabled\n");
}
dptr2 = find_dev ("CH");
if ((dptr2 != NULL) && !(dptr2->flags & DEV_DIS)) {
dptr->flags |= DEV_DIS;
return sim_messagef (SCPE_ALATT, "NG device in conflict with CH.\n");
}
r = auto_config ("NG", (dptr->flags & DEV_DIS) ? 0 : 1);;
if (r != SCPE_OK) {
dptr->flags |= DEV_DIS;
return r;
}
if (!ng_inited && !ng_init(dptr, DEB_TRC))
return sim_messagef (SCPE_ALATT, "Display already in use.\n");
ng_inited = TRUE;
CLR_INT (NG);
ng_unit.wait = 100;
sim_activate (dptr->units, 1);
set_cmd (0, "DZ DISABLED"); /* Conflict with NG. */
set_cmd (0, "HK DISABLED"); /* Conflict with RF. */
vid_register_quit_callback (&ng_quit_callback);
return SCPE_OK;
}
/* Ecriture de la TOC après gravage en TAO */
int philips_fixation( int imm, int onp, int toc_type )
{
scsi_cmd * Cmd;
Cmd = set_cmd( 10, 0xE9, 0, (onp?8:0)|(toc_type&0x07), NULL, 0 );
if( imm )
Cmd->Cmd[1] |= 1;
else
Cmd->Timeout = 240*200; /* Timeout de 4 minutes */
return scsi_out( Cmd );
}
ICR::pulser::command::turn_off::turn_off(const char& addr, const enum channel::type& channel)
: DPR500_recv_cmd(),
m_cmd(),
m_address(addr)
{
if ( channel == channel::A)
m_channel_char = 0x41;
else
m_channel_char = 0x42;
set_cmd();
}
static SimpleSurfaceCmd *destroy_surface(int surface_id)
{
SimpleSurfaceCmd *simple_cmd = calloc(sizeof(SimpleSurfaceCmd), 1);
QXLSurfaceCmd *surface_cmd = &simple_cmd->surface_cmd;
set_cmd(&simple_cmd->ext, QXL_CMD_SURFACE, (intptr_t)surface_cmd);
simple_set_release_info(&surface_cmd->release_info, (intptr_t)simple_cmd);
surface_cmd->type = QXL_SURFACE_CMD_DESTROY;
surface_cmd->flags = 0; // ?
surface_cmd->surface_id = surface_id;
return simple_cmd;
}
JAKO_EXPORT int jakopter_switch_camera(unsigned int id)
{
if (id >= VID_CHANNELS) {
fprintf(stderr, "[*][cmd] Invalid id for the camera.\n");
return -1;
}
static char buf[INT_LEN+2];
snprintf(buf, INT_LEN, "\"%u\"", id);
char * args[] = {"\"video:video_channel\"", buf};
if (set_cmd(HEAD_CONFIG, args, 2) < 0)
return -1;
nanosleep(&cmd_wait, NULL);
// if (config_ack() < 0)
// return -1;
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
JAKO_EXPORT int jakopter_takeoff()
{
if (jakopter_flat_trim() < 0) {
fprintf(stderr, "[~][cmd] Can't establish frame of reference.\n");
return -1;
}
char * args[] = {takeoff_arg};
set_cmd(HEAD_REF, args, 1);
//set timeout
int no_sq = 0;
no_sq = navdata_no_sq();
int attempt = 0;
//wait navdata start
while(no_sq == 0 && attempt < NAVDATA_ATTEMPT) {
nanosleep(&cmd_wait, NULL);
no_sq = navdata_no_sq();
attempt++;
}
attempt = 0;
while(attempt < NAVDATA_ATTEMPT &&
(!jakopter_is_flying() || jakopter_height() < HEIGHT_THRESHOLD))
{
nanosleep(&cmd_wait, NULL);
attempt++;
}
if (set_cmd(NULL, NULL, 0) < 0)
return -1;
return 0;
}
void
ICR::pulser::command::voltage::set(const double& voltage)
{
//round value to nearest integer
if (voltage<m_min || voltage>m_max) throw ICR::exception::voltage_out_of_bounds();
short s = (short) (voltage+0.5 );
m_voltage[1] = s; //least significant
m_voltage[0] = s>>8; //most significant
// std::cout<<"v = "<<s<<std::endl;
// std::cout<<"v[0] = "<<(int) m_voltage[0]<<std::endl;
// std::cout<<"v[1] = "<<(int) m_voltage[1]<<std::endl;
set_cmd();
}
int main(){
char com[MAX];
pid_t pid;
while(1){
printf("$ ");
gets(com);
get_cmd(com);
if(strcmp(cmd[0],"cd")==0){
if(chdir(cmd[1])!=0) puts("cd error\n");
}else{
if(strcmp(cmd[0],"exit")==0) exit(0);
else exec_cmd();
}
set_cmd();
}
return 0;
}
/* Recherche de la première adresse écrivable */
int philips_first_writable_address( unsigned long * adr,
int track, /* Numéro de piste (0 si nouvelle piste) */
int raw, /* Mode raw */
int audio, /* Mode audio */
int mode ) /* Mode data (1 ou 2) */
{
unsigned char buf[6];
int ret;
scsi_cmd * Cmd;
Cmd = set_cmd( 10, 0xE2, 0, 6, buf, 6 );
Cmd->Cmd[2] = track&0x7F;
Cmd->Cmd[3] = (raw?8:0)|(audio?4:0)|(mode&3);
ret = scsi_in( Cmd );
if( !ret )
*adr = ((long)buf[1]<<24) | ((long)buf[2]<<16)
| ((long)buf[3]<<8) | ((long)buf[4]);
return ret;
}
ICR::pulser::command::gain::gain(const char& addr, const enum channel::type& channel,
const short& measured,
const short& min,
const short& max,
const short step
)
: DPR500_recv_cmd(),
m_measured(measured), m_min(min), m_max(max), m_step(step),
m_gain_char((short)(max-min)/2),
m_address(addr)
{
if (max<min) throw ICR::exception::incorrect_input_to_gain_command();
if ( channel == channel::A)
m_channel_char = 0x41;
else
m_channel_char = 0x42;
set_cmd();
}
static void posarg(int i, const char *arg) {
(void)i;
switch(i) {
case 0:
if( opt_verbosity ) {
fprintf( stderr, "Setting command: %s\n", arg );
}
if( 0 == strcasecmp(arg, "chmod") ) {
set_cmd( cmd_chmod );
} else if( 0 == strcasecmp(arg, "create") ||
0 == strcasecmp(arg, "mk") ||
0 == strcasecmp(arg, "make") )
{
set_cmd( cmd_create );
} else if( 0 == strcasecmp(arg, "unlink") ||
0 == strcasecmp(arg, "rm") ||
0 == strcasecmp(arg, "remove") )
{
set_cmd( cmd_unlink );
} else if( 0 == strcasecmp(arg, "dump") ) {
set_cmd( cmd_dump );
} else if( 0 == strcasecmp(arg, "file") ) {
set_cmd( cmd_file );
} else if( 0 == strcasecmp(arg, "search") ) {
set_cmd( cmd_search );
} else {
goto INVALID;
}
break;
case 1:
if( cmd_chmod == opt_command ) {
opt_mode = parse_mode( arg );
} else {
opt_chan_name = strdup( arg );
}
break;
case 2:
if( cmd_chmod == opt_command ) {
opt_chan_name = strdup( arg );
} else if (cmd_search == opt_command ) {
opt_domain = strdup(arg);
} else {
goto INVALID;
}
break;
default:
INVALID:
fprintf( stderr, "Invalid argument: %s\n", arg );
exit(EXIT_FAILURE);
}
}
static bool set_method(int m, int l)
{
if (m > 0)
{
if (!Packer::isValidCompressionMethod(m))
return false;
#if 1
// something like "--brute --lzma" should not disable "--brute"
if (!opt->all_methods)
#endif
{
opt->method = m;
opt->all_methods = false;
}
}
if (l > 0)
opt->level = l;
set_cmd(CMD_COMPRESS);
return true;
}
