// 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);
}
