void *PapiKernel (void *Args) {
struct periodic_info info;
long long i=0;
long long tempvalues[3];
tempvalues[0]=0;
tempvalues[1]=0;
tempvalues[2]=0;
// 10ms interval
make_periodic (10000, &info);
while (1)
{
i+=10;
/* Do useful work */
PAPI_read(eventset, values);
fprintf(gnuplotfile,"%lld\t%lld\t%lld\n", i, values[1]-tempvalues[1], values[2]-tempvalues[2]);
tempvalues[1]=values[1];
tempvalues[2]=values[2];
wait_period (&info);
}
return NULL;
}
static void *periodic_thread(void *arg)
{
sigset_t alarm_sig;
noble_simulation * sim;
make_periodic(1000 * TIMING_CONST_MS, &alarm_sig);
while (1) {
sim = sim_sim();
sim_cycle();
count++;
if ((count & 2047) == 0)
{
printf("count is %ld\n", count);
}
if (sim->num == 0)
{
printf("new run at %ld\n", count);
sim_init(1,rand(),MAP_AREA,0);
}
wait_period(&alarm_sig);
}
return NULL;
}
void swing(uint8_t direction_flag){
if (direction_flag == 0){;
clear_dirpin();
PIDcalc(Left);
direction_flag = 1;
wait_period(.5);
swing(direction_flag);
}
else{
set_dirpin();
PIDcalc(Right);
direction_flag = 0;
wait_period(.5);
swing(direction_flag);
}
}
void* supervisor_thread_interact(void* arg)
{
/* Make this thread periodic */
struct periodic_info info;
char test[2u];
T_reception_state state;
char order_string[50u];
unsigned long timeStamp;
char date[50u];
unsigned char index;
#ifdef PRINT_TCP_DATA_RECEIVED
T_bool print = TRUE;
char frame_received[(RECV_BUFF_SIZE*3u) + 1u];
char byte_ascii[20u];
unsigned char index_frame = 0u;
#endif
make_periodic (INTERACT_TEMPO, &info);
LOG_WriteLevel(LOG_INFO, "supervisor : thread period set to %dus",INTERACT_TEMPO);
while((G_disconnected == FALSE) && (G_comm_lost == FALSE))
{
/* Share data with the supervisor */
#ifdef DEBUG_NAVDATA
LOG_WriteLevel(LOG_DEBUG, "supervisor : Nav: %x | Batt: %d | Ang: %f %f %f | Alt: %d | Speed: %d %d",
ATcommand_navdata()->ctrl_state,
(char)ATcommand_navdata()->vbat_flying_percentage,
(int)ATcommand_navdata()->theta,
(int)ATcommand_navdata()->phi,
(int)ATcommand_navdata()->psi
ATcommand_navdata()->altitude,
(int)ATcommand_navdata()->vx,
(int)ATcommand_navdata()->vy);
#endif
// Mission management
if((G_mission_started == TRUE) && (get_stateMission() == 0))
{
/* End of mission */
LOG_WriteLevel(LOG_INFO, "supervisor : end of mission detected");
/* Share the target square */
supervisor_sendData(TARGET_TCP);
/* Initialization */
G_square = 0u;
G_mission_started = FALSE;
}
// Others data
supervisor_sendData(NAVDATA_TCP);
supervisor_sendData(BATTERY_TCP);
supervisor_sendData(ANGLES_TCP);
supervisor_sendData(ALTITUDE_TCP);
supervisor_sendData(SPEEDS_TCP);
supervisor_sendData(MISSION_TCP);
/* Read an process orders from the supervisor */
memset((char *) &G_orders, 0, sizeof(G_orders));
state = socket_readPacket(G_comm_SPVSR->protocol, G_comm_SPVSR->client->id, &G_comm_SPVSR->client->parameters, &G_orders, sizeof(G_orders), NON_BLOCKING);
switch(state)
{
case RECEPTION_ERROR:
G_comm_lost = TRUE;
LOG_WriteLevel(LOG_WARN, "supervisor : communication lost");
break;
case PACKET_RECEIVED:
#ifdef PRINT_TCP_DATA_RECEIVED
print = TRUE;
#endif
/* Process the received command */
switch((T_TCP_data)G_orders[0u])
{
case TAKEOFF_TCP:
if(G_orders[2u] == 1u)
{
if(ATcommand_enoughBattery() == TRUE)
{
if(ATcommand_FlyingState() == FALSE)
{
/* Flat trim */
ATcommand_process(TRIM);
sleep(2u);
/* Take off */
ATcommand_process(TAKEOFF);
/* Wait the flying state */
while(ATcommand_FlyingState() != TRUE);
}
}
else
{
/* Not enough battery to takeoff */
ATcommand_process(CONFIGURATION_IDS);
ATcommand_process(LED_ANIMATION);
}
/* Update order to print */
strcpy(order_string, "TAKEOFF");
}
/* Landing */
else
{
if(ATcommand_FlyingState() == TRUE)
{
/* Landing */
ATcommand_process(LANDING);
/* Wait the landing state */
while(ATcommand_FlyingState() != FALSE);
}
/* Update order to print */
strcpy(order_string, "LANDING");
}
break;
/* incomplete */
case TARGET_TCP:
/* Do nothing for the moment */
G_square = G_orders[2u];
/* Start the mission */
G_mission_started = TRUE;
/* Update order to print */
sprintf(order_string, "REACH SQUARE %d", G_orders[2u]);
break;
case DECON_TCP:
if(G_orders[2u] == 1u)
{
G_disconnected = TRUE;
strcpy(order_string, "DISCONNECTION");
}
break;
case EMERGENCY_TCP:
ATcommand_process(EMERGENCY);
strcpy(order_string, "EMERGENCY");
break;
case TIME_TCP:
timeStamp = 0ul;
for(index = 0u; index < 4u; index++)
{
timeStamp |= ((unsigned long)G_orders[2u + index]) << (8u*(3u - index));
}
LOG_WriteLevel(LOG_DEBUG, "timeStamp = %x, %d", timeStamp, timeStamp);
/* Change the date */
sprintf(date, "date --set='@%d'", timeStamp);
system(date);
break;
default:
#ifdef PRINT_TCP_DATA_RECEIVED
print = FALSE;
#endif
break;
}
#ifdef PRINT_TCP_DATA_RECEIVED
if(print == TRUE)
{
/* printf */
sprintf(frame_received, "supervisor : bytes received: %x %x", G_orders[0u], G_orders[1u]);
index_frame = strlen(frame_received);
for(index = 0u; index < G_orders[1u]; index++)
{
sprintf(byte_ascii, " %x", G_orders[index + 2u]);
strcpy(&frame_received[index_frame], byte_ascii);
index_frame = strlen(frame_received);
}
LOG_WriteLevel(LOG_DEBUG, "%s -> %s order", frame_received, order_string);
}
else
{
LOG_WriteLevel(LOG_DEBUG, "supervisor : incorrect data received");
}
#endif
break;
case NO_PACKET_RECEIVED:
/* Do nothing */
break;
}
/* Wait until the next period is achieved */
wait_period (&info);
}
/* Inform the disconnection */
LOG_WriteLevel(LOG_INFO, "supervisor : disconnected");
/* Close */
supervisor_close();
/* Close this thread */
pthread_exit(NULL);
}
