void update_behavior(vector<float> &k, Creature* c,bool good=true)
{
if (novelty_function==NF_COGSAMPSQ)
{
dVector3& o_com= ((Biped*)c)->orig_com;
dVector3& c_com= ((Biped*)c)->curr_com;
dVector3 com;
dVector3 delta;
c->CenterOfMass(com);
calculate_delta(o_com,com,delta);
calculate_power(delta,2);
if (good)
{
k.push_back(delta[0]);
k.push_back(delta[1]);
}
else
{
k.push_back(0.0);
k.push_back(0.0);
}
}
}
void collect_gen_word (int ptr, int length, int cp, struct pcp_vars *pcp)
{
register int *y = y_address;
register int gen;
register int exp = y[ptr + 1];
register int entry;
register int gen_val;
for (entry = 2; entry <= length; ++entry) {
gen = y[ptr + entry];
/* check for illegal defining generators */
if (abs (gen) > pcp->ndgen || gen == 0)
report_error (0, gen, 0);
gen_val = y[pcp->dgen + gen];
if (gen < 0 && gen_val == 1)
report_error (0, gen, 0);
collect (gen_val, cp, pcp);
}
calculate_power (exp, cp + 2 * pcp->lastg + 1, cp, pcp);
}
int main(void)
{
uint8_t i;
cli();
MCUSR = 0;
wdt_disable();
//TODO replace by an ana comp polling loop
_delay_ms(10);
setup_datastructs();
setup_led();
setup_ar_uart();
setup_adc();
setup_pulse_input();
setup_analog_comparator();
setup_timer0();
setup_timer1();
// initialize the CTRL buffers
ctrlInit();
// initialize the SPI in slave mode
setup_spi(SPI_MODE_0, SPI_MSB, SPI_INTERRUPT, SPI_SLAVE);
// initialize the Si4421/RFM12 radio and buffers
rfm12_init();
// the clk/8 fuse bit is set
clock_prescale_set(clock_div_1);
FLAG_CLR_ICF1();
sei();
for(;;) {
if (spi_status & SPI_NEW_CTRL_MSG) {
ctrlDecode();
spi_status &= ~SPI_NEW_CTRL_MSG;
}
for (i = 0; i < max_analog_sensors; i++) {
if (state[i].flags & STATE_POWER_CALC) {
calculate_power(&state[i]);
state[i].flags &= ~STATE_POWER_CALC;
state[i].flags |= STATE_POWER;
}
}
rfm12_tick();
}
return 0;
}
int main(void)
{
uint8_t i;
cli();
// RS-485: Configure PD5=DE as output pin with low as default
DDRD |= (1<<DDD5);
// set high to transmit
//PORTD |= (1<<PD5);
setup_datastructs();
setup_led();
setup_adc();
setup_pulse_input();
setup_analog_comparator();
setup_timer1();
// initialize the CTRL buffers
ctrlInit();
// initialize the UART hardware and buffers
uartInit();
// initialize the SPI in slave mode
setup_spi(SPI_MODE_2, SPI_MSB, SPI_INTERRUPT, SPI_SLAVE);
sei();
for(;;) {
if (spi_status & SPI_NEW_CTRL_MSG) {
ctrlDecode();
spi_status &= ~SPI_NEW_CTRL_MSG;
}
for (i = 0; i < MAX_ANALOG_SENSORS; i++) {
if (state[i].flags & STATE_POWER_CALC) {
calculate_power(&state[i]);
state[i].flags &= ~STATE_POWER_CALC;
state[i].flags |= STATE_POWER;
}
}
}
return 0;
}
void invert_word(int ptr, int cp, struct pcp_vars *pcp)
{
register int *y = y_address;
register int gen;
register int exp;
register int length = y[ptr];
for (; length > 1; --length) {
gen = y[ptr + length];
if (gen < 0)
collect(-gen, cp, pcp);
else
invert_generator(gen, 1, cp, pcp);
}
exp = y[ptr + 1];
if (exp != 1)
calculate_power(exp, ptr, cp, pcp);
}
static int generate_P_state_entries(int core, int cores_per_package)
{
int len, len_pss;
int ratio_min, ratio_max, ratio_turbo, ratio_step, ratio_range_2;
int coord_type, power_max, power_unit, num_entries;
int ratio, power, clock, clock_max;
int vid, vid_turbo, vid_min, vid_max, vid_range_2;
u32 control_status;
const struct pattrs *pattrs = pattrs_get();
msr_t msr;
/* Inputs from CPU attributes */
ratio_max = pattrs->iacore_ratios[IACORE_MAX];
ratio_min = pattrs->iacore_ratios[IACORE_LFM];
vid_max = pattrs->iacore_vids[IACORE_MAX];
vid_min = pattrs->iacore_vids[IACORE_LFM];
/* Hardware coordination of P-states */
coord_type = HW_ALL;
/* Max Non-Turbo Frequency */
clock_max = (ratio_max * pattrs->bclk_khz) / 1000;
/* Calculate CPU TDP in mW */
msr = rdmsr(MSR_PKG_POWER_SKU_UNIT);
power_unit = 1 << (msr.lo & 0xf);
msr = rdmsr(MSR_PKG_POWER_LIMIT);
power_max = ((msr.lo & 0x7fff) / power_unit) * 1000;
/* Write _PCT indicating use of FFixedHW */
len = acpigen_write_empty_PCT();
/* Write _PPC with NVS specified limit on supported P-state */
len += acpigen_write_PPC_NVS();
/* Write PSD indicating configured coordination type */
len += acpigen_write_PSD_package(core, 1, coord_type);
/* Add P-state entries in _PSS table */
len += acpigen_write_name("_PSS");
/* Determine ratio points */
ratio_step = 1;
num_entries = (ratio_max - ratio_min) / ratio_step;
while (num_entries > 15) { /* ACPI max is 15 ratios */
ratio_step <<= 1;
num_entries >>= 1;
}
/* P[T] is Turbo state if enabled */
if (get_turbo_state() == TURBO_ENABLED) {
/* _PSS package count including Turbo */
len_pss = acpigen_write_package(num_entries + 2);
ratio_turbo = pattrs->iacore_ratios[IACORE_TURBO];
vid_turbo = pattrs->iacore_vids[IACORE_TURBO];
control_status = (ratio_turbo << 8) | vid_turbo;
/* Add entry for Turbo ratio */
len_pss += acpigen_write_PSS_package(
clock_max + 1, /*MHz*/
power_max, /*mW*/
10, /*lat1*/
10, /*lat2*/
control_status, /*control*/
control_status); /*status*/
} else {
/* _PSS package count without Turbo */
len_pss = acpigen_write_package(num_entries + 1);
ratio_turbo = ratio_max;
vid_turbo = vid_max;
}
/* First regular entry is max non-turbo ratio */
control_status = (ratio_max << 8) | vid_max;
len_pss += acpigen_write_PSS_package(
clock_max, /*MHz*/
power_max, /*mW*/
10, /*lat1*/
10, /*lat2*/
control_status, /*control */
control_status); /*status*/
/* Set up ratio and vid ranges for VID calculation */
ratio_range_2 = (ratio_turbo - ratio_min) * 2;
vid_range_2 = (vid_turbo - vid_min) * 2;
/* Generate the remaining entries */
for (ratio = ratio_min + ((num_entries - 1) * ratio_step);
ratio >= ratio_min; ratio -= ratio_step) {
/* Calculate VID for this ratio */
vid = ((ratio - ratio_min) * vid_range_2) /
ratio_range_2 + vid_min;
/* Round up if remainder */
if (((ratio - ratio_min) * vid_range_2) % ratio_range_2)
vid++;
/* Calculate power at this ratio */
power = calculate_power(power_max, ratio_max, ratio);
clock = (ratio * pattrs->bclk_khz) / 1000;
control_status = (ratio << 8) | (vid & 0xff);
len_pss += acpigen_write_PSS_package(
clock, /*MHz*/
power, /*mW*/
10, /*lat1*/
10, /*lat2*/
control_status, /*control*/
control_status); /*status*/
}
/* Fix package length */
len_pss--;
acpigen_patch_len(len_pss);
return len + len_pss;
}
