/**
* Interrumpe ejecucion del programa. Dependiendo del valor del parametro
* que se le pase interrumpe mas o menos cosas.
* Q == 4 : interrupcion por muerte de gpsd, cierra todo menos gpsd
* Q == 3 : interrupcion por muerte del parser, cierra todo menos parser
* Q == 2 : interrupcion manual (ctrl-c), cierra todo y avisa que fue manual
* Q == 1 : interrupcion por muerte del sbusd, cierra todo menos sbusd
* Q == 0 : interrupcion estandar, cierra todo
* Q cualquier otro : igual que Q == 0.
*
* TODO QUE HACER CUANDO ALGO FALLA Y NECESITAMOS APAGAR LOS MOTORES!
* TODO SI SE CIERRA GPSD TRATAR DE ABRIRLO DENUEVO?
*/
void quit(int Q)
{
int retval;
if(Q == 2) {
err_log("interrumpido manualmente (ctrl-c)"); //TODO no funco bien en la beagle
} else
err_log("algo salio mal, cerrando...");
if(Q != 1) {
/// Terminar Demonio S-BUS
retval = system(KILL_SBUS);
//sprintf(str, "kill -SIGTERM %d", sbusd_child_pid);
//retval = system(str);
if (retval < 0)
err_log("Could not terminate sbusd!");
}
/* /// cerrar IO manager
retval = io_deinit(io);
if(retval != ERROR_OK)
err_log("Could not close IO correctly!");*/
/// Kernel Messeges Queue
uquad_kmsgq_deinit(kmsgq);
/// Log
close(log_fd);
#if !SIMULATE_GPS
if(Q != 4) {
/// cerrar conexiones con GPSD y terminarlo
retval = deinit_gps();
if(retval != ERROR_OK)
err_log("Could not close gps correctly!");
}
#endif // !SIMULATE_GPS
#if !DISABLE_UAVTALK
if(Q != 3) {
/// cerrar UAVTalk
retval = kill(uavtalk_child_pid, SIGKILL);
if(retval != ERROR_OK)
err_log("Could not close Parser correctly!");
}
#endif // !DISABLE_UAVTALK
exit(0);
}
portTASK_FUNCTION(ambilcepat, pvParameters ) {
vTaskDelay(500);
int loopambil=0;
#ifdef PAKAI_SHELL
printf(" Monita : Ambil cepat init !!\r\n");
#endif
#if defined(PAKAI_I2C) && defined(PAKAI_TSC)
unsigned char st_tsc=0;
char a;
int i;
int c=0;
if (setup_fma()) printf(" NO ack !\r\n");
else {
printf("Init TSC OK ack !!!\r\n");
st_tsc = 1;
}
#endif
#ifdef PAKAI_GPS
int hasil_gpsnya;
awal_gps();
#endif
#ifdef PAKAI_ADC
extern unsigned char status_adcnya;
#endif
#ifdef PAKAI_GPIO_DIMMER
//init_remang();
#endif
#ifdef PAKAI_PM
int almtSumber=0;
int sPM=0;
#ifdef AMBIL_PM
printf("Init ambil PM ..-ambilcepat-..!!!\r\n");
vTaskDelay(3000);
#endif
#endif
int waktunya=0;
char dataserial[50];
vTaskDelay(50);
for(;;) {
vTaskDelay(1);
#ifdef DATA_RANDOM
data_f[loopambil%10] = (float) ((rand() % 100));
//printf("%d: data: %.1f\r\n", loopambil%10, data_f[loopambil%10]);
#endif
loopambil++;
#ifdef PAKAI_GPS
if (serPollGPS())
{
hasil_gpsnya = proses_gps();
#if 0
info_gps(hasilGPS);
#endif
}
#endif
#ifdef PAKAI_PM
#ifdef AMBIL_PM // AMBIL_PM
sedot_pm();
#endif
#endif
#ifdef PAKAI_ADC
if (status_adcnya) {
proses_data_adc();
//printf("proses adc ... %d !!\r\n", status_adcnya);
#ifdef BOARD_KOMON_420_SAJA
hitung_datanya();
#endif
simpan_ke_data_f();
}
#endif
#ifdef BOARD_KOMON_KONTER
if (loopambil%20==0) { // 5x20 = 100
hitung_rpm();
}
data_frek_rpm();
#endif
#ifdef BOARD_KOMON_KONTER_3_0
if (loopambil%20==0) { // 5x20 = 100
hitung_rpm();
data_frek_rpm();
}
#endif
#ifdef PAKAI_I2C
#if 0
if (st_tsc) {
if (loopambil%500==0) { // 2*250: 500ms = 0.5 detik
printf("__detik: %3d\r\n", c++);
//baca_register_tsc();
#if 1
if (int_berganti() == 0)
{
printf("disentuh\r\n");
}
else
printf("HIGH\r\n");
//vSerialPutString(0, "HIGH\r\n");
#endif
}
}
#endif
#if 0
if (st_tsc) {
if (xSerialGetChar(1, &c, 0xFFFF ) == pdTRUE)
{
vSerialPutString(0, "Tombol ditekan = ");
xSerialPutChar( 0, (char ) c);
vSerialPutString(0, " \r\n");
if ( (char) c == 's')
{
printf(" Set\r\n");
/*
if (i2c_set_register(0x68, 1, 8))
{
out(" NO ACK !!\r\n");
}
else
out(" OK\r\n");
*/
if (setup_fma())
{
printf(" NO ack !\r\n");
}
else
printf(" OK ack !\r\n");
}
else
{
printf("====\r\n");
for (i=0; i<16; i++)
{
if (i == 8) printf("****\r\n");
if (i2c_read_register(0x68, (0x50 + i), &a))
{
printf(" Read failed !\r\n");
}
else
{
printf(" Read OK =");
a = a + '0';
printf("%c", (char) a);
printf(" \r\n");
}
}
printf("KEY \r\n");
if (i2c_read_register(0x68, 0x68, &a))
{
printf(" Read failed !\r\n");
}
else
{
printf(" Read OK =");
a = a + '0';
printf("%c", (char) a);
printf(" \r\n");
}
//a = read_key();
//a = a + '0';
//xSerialPutChar( 0, (char ) a);
}
}
}
#endif
#endif
#ifdef PAKAI_GPIO_DIMMER
//remangkan();
/*
loop_pwm++;
#ifdef DEBUG_PWM_GPIO
if (loop_pwm>500) {
loop_pwm=0;
blink_pwm=1-blink_pwm;
if (blink_pwm) {
FIO1SET = BIT(30);
} else {
FIO1CLR = BIT(30);
}
}
#endif
//*/
#endif
}
#ifdef PAKAI_GPS
deinit_gps();
#endif
}
