U_CFUNC void
ustr_initChars(struct UString *s, const char* source, int32_t length, UErrorCode *status)
{
int i = 0;
if (U_FAILURE(*status)) return;
s->fChars = 0;
s->fLength = s->fCapacity = 0;
if (length == -1) {
length = (int32_t)uprv_strlen(source);
}
if(s->fCapacity < length) {
ustr_resize(s, ALLOCATION(length), status);
if(U_FAILURE(*status)) return;
}
for (; i < length; i++)
{
UChar charToAppend;
u_charsToUChars(source+i, &charToAppend, 1);
ustr_ucat(s, charToAppend, status);
/*
#if U_CHARSET_FAMILY==U_ASCII_FAMILY
ustr_ucat(s, (UChar)(uint8_t)(source[i]), status);
#elif U_CHARSET_FAMILY==U_EBCDIC_FAMILY
ustr_ucat(s, (UChar)asciiFromEbcdic[(uint8_t)(*cs++)], status);
#else
# error U_CHARSET_FAMILY is not valid
#endif
*/
}
}
U_CFUNC void
ustr_setlen(struct UString *s,
int32_t len,
UErrorCode *status)
{
if(U_FAILURE(*status))
return;
if(s->fCapacity < (len + 1)) {
ustr_resize(s, ALLOCATION(len), status);
if(U_FAILURE(*status))
return;
}
s->fLength = len;
s->fChars[len] = 0x0000;
}
U_CFUNC void
ustr_uscat(struct UString *dst,
const UChar* src,int len,
UErrorCode *status)
{
if(U_FAILURE(*status))
return;
if(dst->fCapacity < (dst->fLength + len)) {
ustr_resize(dst, ALLOCATION(dst->fLength + len), status);
if(U_FAILURE(*status))
return;
}
uprv_memcpy(dst->fChars + dst->fLength, src,
sizeof(UChar) * len);
dst->fLength += len;
dst->fChars[dst->fLength] = 0x0000;
}
U_CFUNC void
ustr_ucat(struct UString *dst,
UChar c,
UErrorCode *status)
{
if(U_FAILURE(*status))
return;
if(dst->fCapacity < (dst->fLength + 1)) {
ustr_resize(dst, ALLOCATION(dst->fLength + 1), status);
if(U_FAILURE(*status))
return;
}
uprv_memcpy(dst->fChars + dst->fLength, &c,
sizeof(UChar) * 1);
dst->fLength += 1;
dst->fChars[dst->fLength] = 0x0000;
}
U_CFUNC void
ustr_cpy(struct UString *dst,
const struct UString *src,
UErrorCode *status)
{
if(U_FAILURE(*status) || dst == src)
return;
if(dst->fCapacity < src->fLength) {
ustr_resize(dst, ALLOCATION(src->fLength), status);
if(U_FAILURE(*status))
return;
}
if(src->fChars == NULL || dst->fChars == NULL){
return;
}
u_memcpy(dst->fChars, src->fChars, src->fLength);
dst->fLength = src->fLength;
dst->fChars[dst->fLength] = 0x0000;
}
U_CFUNC void
ustr_ncat(struct UString *dst,
const struct UString *src,
int32_t n,
UErrorCode *status)
{
if(U_FAILURE(*status) || dst == src)
return;
if(dst->fCapacity < (dst->fLength + n)) {
ustr_resize(dst, ALLOCATION(dst->fLength + n), status);
if(U_FAILURE(*status))
return;
}
uprv_memcpy(dst->fChars + dst->fLength, src->fChars,
sizeof(UChar) * n);
dst->fLength += src->fLength;
dst->fChars[dst->fLength] = 0x0000;
}
type_noeud_comment *validate_comments(type_noeud_comment *anchor_comment, GtkWidget * entry)
{
GtkTextIter start, end, iter;
GtkTextBuffer *buffer;
char *text;
gboolean is_not_the_end;
type_noeud_comment *pt_comment;
int longueur;
type_noeud_comment *pt_prec = NULL, *pt_fin = NULL;
pt_comment = anchor_comment;
buffer = gtk_text_view_get_buffer(GTK_TEXT_VIEW(entry));
gtk_text_buffer_get_line_count(buffer);
gtk_text_buffer_get_bounds(buffer, &start, &end);
iter = start;
do
{
is_not_the_end = gtk_text_iter_forward_line(&iter);
text = gtk_text_iter_get_text(&start, &iter);
start = iter;
if (pt_comment == NULL)
{
pt_comment = ALLOCATION(type_noeud_comment);
pt_comment->suiv = NULL;
if (anchor_comment == NULL)
{
anchor_comment = pt_comment;
}
else
{
pt_prec->suiv = pt_comment;
}
}
longueur = strlen(text) + 1;
if (longueur > TAILLE_CHAINE)
{
printf("WARNING: la chaine de char: %s a ete tronquee dans validate_comments car sa taille est superieure a %d\n", text, TAILLE_CHAINE);
longueur = TAILLE_CHAINE;
}
/* recopie dans le champs comment en rajoutant un % au debut s'il a ete oublie */
/* on ne fait rien pour les lignes vides */
if (test_comment(text) == 1 || empty_line(text) == 1)
{
memcpy(pt_comment->chaine, text, longueur * sizeof(char));
}
else
{
pt_comment->chaine[0] = '%';
memcpy(&(pt_comment->chaine[1]), text, longueur * sizeof(char));
}
pt_prec = pt_comment;
pt_comment = pt_comment->suiv;
g_free (text);
}
while (is_not_the_end == TRUE);
if (strlen(pt_prec->chaine) > 0 && pt_prec->chaine[strlen(pt_prec->chaine)-1] != '\n')
{
strcat(pt_prec->chaine, "\n");
}
/* Suppression des donnees du buffer */
gtk_text_buffer_get_start_iter(buffer, &start);
gtk_text_buffer_get_end_iter(buffer, &end);
gtk_text_buffer_delete(buffer, &start, &end);
/* Suppression du reste de la liste chainee des commentaire ==> utile si on a supprime des commentaires*/
pt_fin = pt_prec;
while (pt_comment != NULL)
{
pt_prec = pt_comment;
pt_comment = pt_comment->suiv;
free(pt_prec);
}
pt_fin->suiv = NULL;
return anchor_comment;
}
/**
* Main execution thread
*/
int unibo_allocation_thread_main(int argc, char *argv[])
{
warnx("[unibo_allocation] starting");
thread_running = true;
model = ALLOCATION(); //Init model!
int unibo_control_wrench_fd = orb_subscribe(ORB_ID(unibo_control_wrench));
//orb_set_interval(unibo_control_wrench_fd, 5); //1000 = 1Hz (ms)
/* advertise control wrench topic */
struct motor_output_s motor;
memset(&motor, 0, sizeof(motor));
int motor_pub_fd = orb_advertise(ORB_ID(motor_output), &motor);
/*
* |-----------------------------------------------------|
* | INIT VARIABLES! |
* |-----------------------------------------------------|
*/
// inizializzazione middle-layer
unsigned int module_ind;
unsigned int error_counter = 0;
unsigned int counter_warnx = 0;
unsigned int time_counter = 0;
//struct vehicle_attitude_s ahrs;
struct unibo_control_wrench_s wrench;
ALLOCATION_start();
ALLOCATION_control();
//int u2m[6] = {1,1,1,1,1,1};
//struct mr_config_struct curr_config=ConfigurationReader(1,u2m);
warnx("input logging to Simulink routine ..."); //TODO put configuration file instead of hardcode
warnx("uploading rotors data from configuration file ''%s'' ...",config_file_name);
config_file_handle=fopen(config_file_path,"r");
if(config_file_handle==NULL) {
// printf("%s\n",strerror(errno));
puts("no");
}else{
do{
fgets(file_line_string1,read_line_length,config_file_handle);
}while(strstr(file_line_string1,"<TAB")==NULL);
sscanf(file_line_string1,"<TAB,%u>",&tab_num);
do{
do{
fgets(file_line_string1,read_line_length,config_file_handle);
}while(strstr(file_line_string1,"<COL")==NULL);
}while(strstr(file_line_string1,"rotor")==NULL);
sscanf(file_line_string1,"<COL,%*s,%u,%*s>",&module_num);
//puts(file_line_string1);
for(module_ind=0;module_ind<module_num;module_ind++){
do{
fgets(file_line_string1,read_line_length,config_file_handle);
}while(strstr(file_line_string1,"<ROW")==NULL);
puts(file_line_string1);
sscanf(file_line_string1,"<ROW,%*u,%u,%u,%*s,%*s,%u,%u,%u>",&distance,&azimuth,&spin,&Ct,&Cq);
// Ct=0.0000115;
// warnx("read Cq: %u",Cq);
ALLOCATION_U.r[module_ind]=(float)distance/1000.0f;
ALLOCATION_U.psi[module_ind]=(float)azimuth;
ALLOCATION_U.s[module_ind]=(float)spin;
ALLOCATION_U.Ct[module_ind]=(double)Ct*1e-9;
ALLOCATION_U.Cq[module_ind]=(double)Cq*1e-12*100;
warnx("module %u distance: %4.2f m",module_ind+1,(double)ALLOCATION_U.r[module_ind]);
warnx("module %u azimuth: %1.0f deg",module_ind+1,(double)ALLOCATION_U.psi[module_ind]);
warnx("module %u spin direction: %1.0f",module_ind+1,(double)ALLOCATION_U.s[module_ind]);
warnx("module %u thrust coefficient: %.10f N/RPM2",module_ind+1,(double)ALLOCATION_U.Ct[module_ind]);
warnx("module %u torque coefficient: %.10f Nm/RPM2",module_ind+1,(double)ALLOCATION_U.Cq[module_ind]);
}
fclose(config_file_handle);
warnx("Done.");
}
// for(module_ind=0;module_ind<4;module_ind++){
// ALLOCATION_U.r[module_ind]=0.29;//curr_config.radius[module_ind]; //distanza dal baricentro
//// warnx("module %u: r=%u",module_ind+1,(int)ALLOCATION_U.r[module_ind]);
//// ALLOCATION_U.s[module_ind]=1;//curr_config.direction[module_ind]; //spin
//// warnx("module %u: s=%d",module_ind+1,(int)ALLOCATION_U.s[module_ind]);
//// warnx("module %u: Ct=%.5f",module_ind+1,(double)curr_config.thrust[module_ind]);
// ALLOCATION_U.Ct[module_ind]=0.000011500/1000*9.81;//curr_config.thrust[module_ind]; //coefficienti aerodinamici di spinta
//// warnx("module %u: Ct=%.5f",module_ind+1,(double)ALLOCATION_U.Ct[module_ind]);
// ALLOCATION_U.Cq[module_ind]=0.00000000055*100;//curr_config.torque[module_ind]; //coefficienti aerodinamici di momento
//// warnx("module %u: Cq=%.8f",module_ind+1,(double)ALLOCATION_U.Cq[module_ind]);
// }
// ALLOCATION_U.s[0]=-1;//curr_config.direction[module_ind]; //spin
// ALLOCATION_U.s[1]=1;//curr_config.direction[module_ind]; //spin
// ALLOCATION_U.s[2]=-1;//curr_config.direction[module_ind]; //spin
// ALLOCATION_U.s[3]=1;//curr_config.direction[module_ind]; //spin
// ALLOCATION_U.psi[0]=135;//azimuth
// ALLOCATION_U.psi[1]=-135;//azimuth
// ALLOCATION_U.psi[2]=-45;//azimuth
// ALLOCATION_U.psi[3]=45;//azimuth
// x
// 1 ^ 2
// \ | /
// \|/
// 0---->y
// / \
// / \
// 4 3
for(module_ind=4;module_ind<6;module_ind++){
ALLOCATION_U.r[module_ind]=0;//curr_config.radius[module_ind]; //distanza dal baricentro
// warnx("module %u: r=%u",module_ind+1,(int)ALLOCATION_U.r[module_ind]);
ALLOCATION_U.s[module_ind]=0;//curr_config.direction[module_ind]; //spin
// warnx("module %u: s=%d",module_ind+1,(int)ALLOCATION_U.s[module_ind]);
// warnx("module %u: Ct=%.5f",module_ind+1,(double)curr_config.thrust[module_ind]);
ALLOCATION_U.Ct[module_ind]=0;//curr_config.thrust[module_ind]; //coefficienti aerodinamici di spinta
// warnx("module %u: Ct=%.5f",module_ind+1,(double)ALLOCATION_U.Ct[module_ind]);
ALLOCATION_U.Cq[module_ind]=0;//curr_config.torque[module_ind]; //coefficienti aerodinamici di momento
// warnx("module %u: Cq=%.8f",module_ind+1,(double)ALLOCATION_U.Cq[module_ind]);
ALLOCATION_U.psi[module_ind]=0;
}
/* Bool for topics update */
// bool updated;
struct pollfd fds[] = {
{ .fd = unibo_control_wrench_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
*/
};