// prediction with P(x_t|y_0, ..., y_t) double HMM::average_entropy(int nseq, int* iseq, double* phi, double* c, double* Z, int* count, bool verbose /* = false */) { double p_sumlog = 0; *count = 0; for(int i = 0; i < nseq; i++) { if(verbose) print_update(i, nseq, 20); int iseq_ = iseq[i]; int sl = o->slen[iseq_]; filter(iseq_, phi, c, Z); for(int t = 0; t < sl - 1; t++) { uint32_t y_next = o->y[o->sind[iseq_] + t + 1]; // phi + t*hs represents P(x_t|y_0, ..., y_t) double p = prob_next_obs(phi + t*hs, y_next); p_sumlog += log2(p); (*count)++; } } if(verbose) std::cout << std::endl; double entropy = - p_sumlog; return entropy; }
void emergency_stop(char* errMsgLine1, char* errMsgLine2) { motor_set_speed(NXT_PORT_A,0,1); motor_set_speed(NXT_PORT_B,0,1); motor_set_speed(NXT_PORT_C,0,1); print_clear_display(); print_str(0,1,errMsgLine1); print_str(0,2,errMsgLine2); int i; for(i = 0; i < 3; i++) { print_str(6,4,"ERROR"); print_update(); ecrobot_set_light_sensor_active(NXT_PORT_S1); ecrobot_set_light_sensor_active(NXT_PORT_S2); ecrobot_set_light_sensor_active(NXT_PORT_S3); ecrobot_sound_tone(1000,500,15); systick_wait_ms(750); print_clear_line(4); print_update(); ecrobot_set_light_sensor_inactive(NXT_PORT_S1); ecrobot_set_light_sensor_inactive(NXT_PORT_S2); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); ecrobot_sound_tone(500,500,15); systick_wait_ms(750); } motor_set_speed(NXT_PORT_A,0,0); motor_set_speed(NXT_PORT_B,0,0); motor_set_speed(NXT_PORT_C,0,0); while(true) { print_str(6,4,"ERROR"); print_update(); ecrobot_set_light_sensor_active(NXT_PORT_S1); ecrobot_set_light_sensor_active(NXT_PORT_S2); ecrobot_set_light_sensor_active(NXT_PORT_S3); systick_wait_ms(750); print_clear_line(4); print_update(); ecrobot_set_light_sensor_inactive(NXT_PORT_S1); ecrobot_set_light_sensor_inactive(NXT_PORT_S2); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); systick_wait_ms(750); } }
/* * Enable CMCI (Corrected Machine Check Interrupt) for available MCE banks * on this CPU. Use the algorithm recommended in the SDM to discover shared * banks. */ static void cmci_discover(int banks, int boot) { unsigned long *owned = (void *)&__get_cpu_var(mce_banks_owned); unsigned long flags; int hdr = 0; int i; spin_lock_irqsave(&cmci_discover_lock, flags); for (i = 0; i < banks; i++) { u64 val; if (test_bit(i, owned)) continue; rdmsrl(MSR_IA32_MCx_CTL2(i), val); /* Already owned by someone else? */ if (val & CMCI_EN) { if (test_and_clear_bit(i, owned) || boot) print_update("SHD", &hdr, i); __clear_bit(i, __get_cpu_var(mce_poll_banks)); continue; } val |= CMCI_EN | CMCI_THRESHOLD; wrmsrl(MSR_IA32_MCx_CTL2(i), val); rdmsrl(MSR_IA32_MCx_CTL2(i), val); /* Did the enable bit stick? -- the bank supports CMCI */ if (val & CMCI_EN) { if (!test_and_set_bit(i, owned) || boot) print_update("CMCI", &hdr, i); __clear_bit(i, __get_cpu_var(mce_poll_banks)); } else { WARN_ON(!test_bit(i, __get_cpu_var(mce_poll_banks))); } } spin_unlock_irqrestore(&cmci_discover_lock, flags); if (hdr) printk(KERN_CONT "\n"); }
static void print_play_verbose_info (const unsigned char * buf, ssize_t l, void * metadata) { /* * Display a rough recording level meter, and the elapsed time. */ verbose_values_t * val = (verbose_values_t *)metadata; val->secs += l/val->constant; if (val->secs < val->next_sec) return; val->next_sec += PLAY_UPDATE_INTERVAL/1000; /* * This check is done to ensure an update at the end of the playback. * Note that some files lie about total time, so the second condition is * necessary so that updates will still be constricted by PLAY_UPDATE_INTERVAL. */ if ((val->next_sec > val->tsecs) && (val->secs < val->tsecs)) val->next_sec = val->tsecs; print_update (get_db_level (buf, l, val->format), val->secs, val->tstring); return; }
double HMM::average_entropy(int nseq, int* iseq, uint32_t* x_est, double* T1, int* T2, double* aux, int* count, bool verbose /* = false */) { // DEBUG // sanity_check_parameters(); double logprob; double p_sumlog = 0; *count = 0; for(int i = 0; i < nseq; i++) { if(verbose) print_update(i, nseq, 20); int iseq_ = iseq[i]; int sl = o->slen[iseq_]; for(int t = 0; t < sl - 1; t++) { uint32_t y_next = o->y[o->sind[iseq_] + t + 1]; // if prediction with last state by viterbi viterbi_update(o->y + o->sind[iseq_], T1, T2, aux, t, t+1); viterbi_backtrack(t+1, T1, T2, x_est, &logprob); double p = prob_next_obs(x_est[t], y_next); p_sumlog += log2(p); (*count)++; } } if(verbose) std::cout << std::endl; double entropy = - p_sumlog; return entropy; }
/******************************************************************* * MAIN() *******************************************************************/ int main(void) { long lEEPROMRetStatus; uint16_t i=0; uint8_t halted_latch = 0; // Set the clocking to run at 80 MHz from the PLL. // (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't // write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we // could slow the clock down when writing to FLASH but then we need to find out how long // it takes for the clock to stabilize. This is on at the bottom of my list of things to do // for now) SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); // Initialize the device pinout. SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ); // Enable processor interrupts. IntMasterEnable(); // Setup the UART's my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // command_handler_init overwrites the baud rate. We still need to configure the pins though my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // Enable the command handler command_handler_init(); // We set the baud in here // Start the timers my_timer0_init(); my_timer1_init(); i2c_init(); motor_init(); qei_init(); gyro_init(); accel_init(); led_init(); //rc_radio_init(); //setupBluetooth(); // Initialize the EEPROM emulation region. lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE); if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14); #if 0 // If ever we wanted to write some parameters to FLASH without the HMI // we could do it here. SoftEEPROMWriteDouble(kP_ID, 10.00); SoftEEPROMWriteDouble(kI_ID, 10.00); SoftEEPROMWriteDouble(kD_ID, 10.00); SoftEEPROMWriteDouble(ANG_ID, 0.0); SoftEEPROMWriteDouble(COMPC_ID, 0.99); #endif kP = SoftEEPROMReadDouble(kP_ID); kI = SoftEEPROMReadDouble(kI_ID); kD = SoftEEPROMReadDouble(kD_ID); commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID); COMP_C = SoftEEPROMReadDouble(COMPC_ID); pid_init(kP, kI, kD, &pid_ang); motor_controller_init(20, 100, 10, &mot_left); motor_controller_init(20, 100, 10, &mot_right); //pid_init(0.0, 0.0, 0.0, &pid_pos_left); //pid_init(0.0, 0.0, 0.0, &pid_pos_right); //UART0Send("Hello World!\n", 13); // Tell the HMI what the initial parameters are. print_params(1); while(1) { delta_t = myTimerValueGet(); myTimerZero(); sum_delta_t += delta_t; // Read our sensors accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true); gyro_get_y_cal(&gyro_y, false); // Calculate the pitch angle with the accelerometer only R = sqrt(pow(accel_x, 2) + pow(accel_z, 2)); accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang; //accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0); gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t); // Kalman filter //filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t)); filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang); // Skip the rest of the process until the angle stabilizes if(i < 250) { i++; continue; } // Tell the HMI what's going on every 100ms if(sum_delta_t >= 1000) { print_update(1); print_debug(0); //print_control_surfaces(0); led_toggle(); //print_angle(); sum_delta_t = 0; } // See if the HMI has anything to say command_handler(); //continue; // If we are leaning more than +/- FALL_ANG deg off center it's hopeless. // Turn off the motors in hopes of some damage control if( abs(filtered_ang) > FALL_ANG ) { if(halted_latch) continue; stop_motors(); halted_latch = 1; continue; } halted_latch = 0; motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang); motor_left = motor_right = motor_val; drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain); } }
int main(int argc, char **argv) { int c; bool_t verbose = FALSE; bool_t headeronly = FALSE; uint32_t entry = 0; krb5_context context; kadm5_config_params params; kdb_log_context *log_ctx; kdb_hlog_t *ulog = NULL; (void) setlocale(LC_ALL, ""); #if !defined(TEXT_DOMAIN) #define TEXT_DOMAIN "SYS_TEST" #endif /* TEXT_DOMAIN */ (void) textdomain(TEXT_DOMAIN); if (geteuid() != (uid_t)0) { (void) fprintf(stderr, gettext("kproplog must be run as root\n\n")); exit(1); } progname = argv[0]; while ((c = getopt(argc, argv, "vhe:")) != -1) { switch (c) { case 'h': headeronly = TRUE; break; case 'e': entry = atoi(optarg); break; case 'v': verbose = TRUE; break; default: usage(); } } if (krb5_init_context(&context)) { (void) fprintf(stderr, gettext("Unable to initialize Kerberos\n\n")); exit(1); } (void) memset((char *)¶ms, 0, sizeof (params)); if (kadm5_get_config_params(context, NULL, NULL, ¶ms, ¶ms)) { (void) fprintf(stderr, gettext("Couldn't read database_name\n\n")); exit(1); } (void) printf(gettext("\nKerberos update log (%s.ulog)\n"), params.dbname); if (ulog_map(context, ¶ms, FKPROPLOG)) { (void) fprintf(stderr, gettext("Unable to map log file " "%s.ulog\n\n"), params.dbname); exit(1); } log_ctx = context->kdblog_context; if (log_ctx) ulog = log_ctx->ulog; else { (void) fprintf(stderr, gettext("Unable to map log file " "%s.ulog\n\n"), params.dbname); exit(1); } if (ulog->kdb_hmagic != KDB_HMAGIC) { (void) fprintf(stderr, gettext("Corrupt header log, exiting\n\n")); exit(1); } (void) printf(gettext("Update log dump :\n")); (void) printf(gettext("\tLog version # : %u\n"), ulog->db_version_num); (void) printf(gettext("\tLog state : ")); switch (ulog->kdb_state) { case KDB_STABLE: (void) printf(gettext("Stable\n")); break; case KDB_UNSTABLE: (void) printf(gettext("Unstable\n")); break; case KDB_CORRUPT: (void) printf(gettext("Corrupt\n")); break; default: (void) printf(gettext("Unknown state: %d\n"), ulog->kdb_state); break; } (void) printf(gettext("\tEntry block size : %u\n"), ulog->kdb_block); (void) printf(gettext("\tNumber of entries : %u\n"), ulog->kdb_num); if (ulog->kdb_last_sno == 0) (void) printf(gettext("\tLast serial # : None\n")); else { if (ulog->kdb_first_sno == 0) (void) printf(gettext("\tFirst serial # : None\n")); else { (void) printf(gettext("\tFirst serial # : ")); (void) printf("%u\n", ulog->kdb_first_sno); } (void) printf(gettext("\tLast serial # : ")); (void) printf("%u\n", ulog->kdb_last_sno); } if (ulog->kdb_last_time.seconds == 0L) { (void) printf(gettext("\tLast time stamp : None\n")); } else { if (ulog->kdb_first_time.seconds == 0L) (void) printf(gettext("\tFirst time stamp : None\n")); else { (void) printf(gettext("\tFirst time stamp : %s"), ctime((time_t *) &(ulog->kdb_first_time.seconds))); } (void) printf(gettext("\tLast time stamp : %s\n"), ctime((time_t *)&(ulog->kdb_last_time.seconds))); } if ((!headeronly) && ulog->kdb_num) { print_update(ulog, entry, verbose); } (void) printf("\n"); return (0); }