//UART RX not empty interrupt
//adds data to the struct, until struct is full. Then calculates uvw, and sets pwm
//TODO: chekcsum
void USART2_IRQHandler(){
USART_ClearITPendingBit(USART2, USART_IT_RXNE);
buf = USART_ReceiveData(USART2);
if(buf == 0x155){ //start condition
datapos = 0;
uartsend = 1;
//GPIOC->BSRR = (GPIOC->ODR ^ GPIO_Pin_2) | (GPIO_Pin_2 << 16);//green
}else if(datapos >= 0 && datapos < sizeof(to_hv_t)){
((uint8_t*)&to_hv)[datapos++] = (uint8_t)buf;
}
if(datapos == sizeof(to_hv_t)){//all data received
datapos = -1;
float ua = TOFLOAT(to_hv.a);
float ub = TOFLOAT(to_hv.b);
float u = ua; // inverse clarke
float v = - ua / 2.0 + ub / 2.0 * SQRT3;
float w = - ua / 2.0 - ub / 2.0 * SQRT3;
//TODO: SVM, clamping
u += volt / 2.0;
v += volt / 2.0;
w += volt / 2.0;
if(u < v){
if(u < w){
v -= u;
w -= u;
u = 0.0;
}
else{
u -= w;
v -= w;
w = 0.0;
}
}
else{
if(v < w){
u -= v;
w -= v;
v = 0.0;
}
else{
u -= w;
v -= w;
w = 0.0;
}
}
PWM_U = CLAMP(u/volt*res, 0, res * 0.95);
PWM_V = CLAMP(v/volt*res, 0, res * 0.95);
PWM_W = CLAMP(w/volt*res, 0, res * 0.95);
timeout = 0; //reset timeout
//GPIOC->BSRR = (GPIOC->ODR ^ GPIO_Pin_0) | (GPIO_Pin_0 << 16);//toggle red led
}
}
elem Verify_SimpleStructReturnTest(elem num)
{
elem t;
int i;
t=TyObj_CloneNull();
TyObj_SetSlot(t, SYM("times10"), FLONUM(TOFLOAT(num)*10));
TyObj_SetSlot(t, SYM("times100"), FLONUM(TOFLOAT(num)*100));
TyObj_SetSlot(t, SYM("times1000"), FLONUM(TOFLOAT(num)*1000));
return(t);
}
void LinkPromLink::realize() {
ModulePromGroup* s = link->project->get(link->src);
if(!s) { ERROR("Unknown module : " << link->src); return;}
ModulePromGroup* d = link->project->get(link->dst);
if(!d) { ERROR("Unknown module : " << link->dst); return;}
connect(s,d);
if(link->type == "") add_class("seq");
if(link->type == "-") add_class("sync");
if(link->type == "0") add_class("clock");
if(link->type == "/") add_class("async");
if(link->properties.get("bezier_x2")) component->b->x2 = TOFLOAT(link->properties.get_as_string("bezier_x2"));
if(link->properties.get("bezier_y2")) component->b->y2 = TOFLOAT(link->properties.get_as_string("bezier_y2"));
if(link->properties.get("bezier_x3")) component->b->x3 = TOFLOAT(link->properties.get_as_string("bezier_x3"));
if(link->properties.get("bezier_y3")) component->b->y3 = TOFLOAT(link->properties.get_as_string("bezier_y3"));
if(!link->properties.get("bezier_x2") && src==dst) component->b->set(120,0,0,-120);
}
void LinkPromLink::realize() {
ModulePromGroup* s = link->project->get(link->src);
if(!s) { ERROR("Unknown module : " << link->src); return;}
ModulePromGroup* d = link->project->get(link->dst);
if(!d) { ERROR("Unknown module : " << link->dst); return;}
connect(s,d);
if(link->is_type_algo()) add_class("algo");
else {
if(link->type == No_l_1_a || link->type == No_l_1_t) add_class("one_to_all");
if(link->type == No_l_1_1_modif || link->type == No_l_1_1_non_modif) add_class("one_to_one");
if(link->type == No_l_neuro_mod) add_class("neuromod");
}
if(link->secondary > 0) add_class("secondary");
if(link->annotations.get("bezier_x2")) component->b->x2 = TOFLOAT(link->annotations.get_as_string("bezier_x2"));
if(link->annotations.get("bezier_y2")) component->b->y2 = TOFLOAT(link->annotations.get_as_string("bezier_y2"));
if(link->annotations.get("bezier_x3")) component->b->x3 = TOFLOAT(link->annotations.get_as_string("bezier_x3"));
if(link->annotations.get("bezier_y3")) component->b->y3 = TOFLOAT(link->annotations.get_as_string("bezier_y3"));
if(!link->annotations.get("bezier_x2") && src==dst) component->b->set(120,0,0,-120);
}
/*
** 'restore_retparm' is called when a 'return parameter' block is found on the
** stack. It saves the value currently in the parameter at the address stored as
** the return parameter address and then returns the local variable to its
** correct value
*/
void restore_retparm(int32 parmcount) {
stack_retparm *p;
int32 vartype, intvalue;
float64 floatvalue;
basicstring stringvalue;
p = basicvars.stacktop.retparmsp; /* Not needed, but the code is unreadable otherwise */
#ifdef DEBUG
if (basicvars.debug_flags.stack) fprintf(stderr, "Restoring RETURN variable at %p from %p, return dest=%p\n",
p->savedetails.address.intaddr, p, p->retdetails.address.intaddr);
#endif
basicvars.stacktop.retparmsp++;
switch (p->savedetails.typeinfo & PARMTYPEMASK) { /* Fetch value from local variable and restore local var */
case VAR_INTWORD: /* Integer variable */
intvalue = *p->savedetails.address.intaddr; /* Fetch current value of local variable */
*p->savedetails.address.intaddr = p->value.savedint; /* Restore local variable to its old value */
vartype = VAR_INTWORD;
break;
case VAR_FLOAT: /* Floating point variable */
floatvalue = *p->savedetails.address.floataddr;
*p->savedetails.address.floataddr = p->value.savedfloat;
vartype = VAR_FLOAT;
break;
case VAR_STRINGDOL: /* String variable */
stringvalue = *p->savedetails.address.straddr;
*p->savedetails.address.straddr = p->value.savedstring;
vartype = VAR_STRINGDOL;
break;
case VAR_INTBYTEPTR: /* Indirect byte integer variable */
intvalue = basicvars.offbase[p->savedetails.address.offset];
basicvars.offbase[p->savedetails.address.offset] = p->value.savedint;
vartype = VAR_INTWORD;
break;
case VAR_INTWORDPTR: /* Indirect word integer variable */
intvalue = get_integer(p->savedetails.address.offset);
store_integer(p->savedetails.address.offset, p->value.savedint);
vartype = VAR_INTWORD;
break;
case VAR_FLOATPTR: /* Indirect floating point variable */
floatvalue = get_float(p->savedetails.address.offset);
store_float(p->savedetails.address.offset, p->value.savedfloat);
vartype = VAR_FLOAT;
break;
case VAR_DOLSTRPTR: /* Indirect string variable */
intvalue = stringvalue.stringlen = get_stringlen(p->savedetails.address.offset);
stringvalue.stringaddr = alloc_string(intvalue);
if (intvalue>0) memmove(stringvalue.stringaddr, &basicvars.offbase[p->savedetails.address.offset], intvalue);
memmove(&basicvars.offbase[p->savedetails.address.offset], p->value.savedstring.stringaddr, p->value.savedstring.stringlen);
free_string(p->value.savedstring); /* Discard saved copy of original '$ string' */
vartype = VAR_DOLSTRPTR;
break;
case VAR_INTARRAY: case VAR_FLOATARRAY: case VAR_STRARRAY: /* Array - Do nothing */
break;
default:
error(ERR_BROKEN, __LINE__, "stack");
}
/* Now restore the next parameter */
parmcount--;
if (parmcount>0) { /* There are still some parameters to do */
if (basicvars.stacktop.intsp->itemtype==STACK_LOCAL)
restore(parmcount);
else { /* Must be a return parameter */
restore_retparm(parmcount);
}
}
/* Now we can store the returned value in original variable */
switch (p->retdetails.typeinfo) {
case VAR_INTWORD:
*p->retdetails.address.intaddr = vartype==VAR_INTWORD ? intvalue : TOINT(floatvalue);
break;
case VAR_FLOAT:
*p->retdetails.address.floataddr = vartype==VAR_INTWORD ? TOFLOAT(intvalue) : floatvalue;
break;
case VAR_STRINGDOL:
free_string(*p->retdetails.address.straddr);
*p->retdetails.address.straddr = stringvalue;
break;
case VAR_INTBYTEPTR:
basicvars.offbase[p->retdetails.address.offset] = vartype==VAR_INTWORD ? intvalue : TOINT(floatvalue);
break;
case VAR_INTWORDPTR:
store_integer(p->retdetails.address.offset, vartype==VAR_INTWORD ? intvalue : TOINT(floatvalue));
break;
case VAR_FLOATPTR:
store_float(p->retdetails.address.offset, vartype==VAR_INTWORD ? TOFLOAT(intvalue) : floatvalue);
break;
case VAR_DOLSTRPTR:
if (stringvalue.stringlen>0) memmove(&basicvars.offbase[p->retdetails.address.offset], stringvalue.stringaddr, stringvalue.stringlen);
if (vartype==VAR_STRINGDOL) { /* Local var was a normal string variable */
basicvars.offbase[p->retdetails.address.offset+stringvalue.stringlen] = CR; /* So add a 'CR' at the end of the string */
}
free_string(stringvalue);
break;
case VAR_INTARRAY: case VAR_FLOATARRAY: case VAR_STRARRAY: /* 'RETURN' dest is array - Do nothing */
break;
default:
error(ERR_BROKEN, __LINE__, "stack");
}
}
elem XmlRpc_EncodeValue(elem val)
{
char buf[256];
int i;
double x;
elem t;
char *s, *s2;
if(ELEM_STRINGP(val))
{
t=val;
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("string"), t);
return(t);
}
if(ELEM_FIXNUMP(val))
{
i=TOINT(val);
sprintf(buf, "%d", i);
t=STRING(buf);
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("i4"), t);
return(t);
}
if(ELEM_FLONUMP(val))
{
x=TOFLOAT(val);
sprintf(buf, "%g", x);
t=STRING(buf);
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("double"), t);
return(t);
}
if(ELEM_CONSP(val))
{
if(CAR(val)==SYM("date-time:"))
{
t=XmlRpc_EncodeDate(val);
return(t);
}
t=XmlRpc_EncodeArray(val);
return(t);
}
if(ELEM_ENVOBJP(val))
{
t=XmlRpc_EncodeStruct(val);
return(t);
}
if(ELEM_BYTEVECTORP(val))
{
s=TyFcn_ByteVectorBody(val);
i=VECTOR_LEN(val);
s2=kalloc(((i*4)/3)+5);
HttpNode_EncodeMime(s2, s, i);
kprint("send mime %d->%d\n", i, (i*4)/3);
t=STRING(s2);
kfree(s2);
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("base64"), t);
return(t);
}
if(val==MISC_TRUE)
{
t=STRING("1");
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("boolean"), t);
return(t);
}
if(val==MISC_FALSE)
{
t=STRING("0");
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("boolean"), t);
return(t);
}
if(val==MISC_NULL)
{
t=STRING("$null");
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("boolean"), t);
return(t);
}
t=STRING("$undefined");
t=CONS(t, MISC_EOL);
t=CONS(MISC_EOL, t);
t=CONS(SYM("string"), t);
return(t);
}
