/*
called when we change state to see if we should change gains
*/
void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
{
switch (state) {
case DEMAND_UNSATURATED:
break;
case DEMAND_UNDER_POS:
case DEMAND_UNDER_NEG:
// we increase P if we have not saturated the surfaces during
// this state, and we have
if (state_time_ms >= AUTOTUNE_UNDERSHOOT_TIME && !saturated_surfaces) {
current.P.set(current.P * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
if (current.P > AUTOTUNE_MAX_P) {
current.P = AUTOTUNE_MAX_P;
}
Debug("UNDER P -> %.3f\n", current.P.get());
}
current.D.set( pgm_read_float(&tuning_table[aparm.autotune_level-1].Dratio) * current.P);
break;
case DEMAND_OVER_POS:
case DEMAND_OVER_NEG:
if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
current.P.set(current.P * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
if (current.P < AUTOTUNE_MIN_P) {
current.P = AUTOTUNE_MIN_P;
}
Debug("OVER P -> %.3f\n", current.P.get());
}
current.D.set( pgm_read_float(&tuning_table[aparm.autotune_level-1].Dratio) * current.P);
break;
}
}
/*
* get temperature based on read voltage
*/
int16_t amt1001_gettemperature(uint16_t adcvalue) {
float t = 0.0;
float mint = 0;
float maxt = 0;
//return error for invalid adcvalues
if(adcvalue<amt1001_lookupadcfirst || adcvalue>amt1001_lookupadcfirst+amt1001_lookupadcstep*(amt1001_lookuptablesize-1)) {
return -1;
}
uint8_t i = 0;
uint16_t a = amt1001_lookupadcfirst;
for(i=0; i<amt1001_lookuptablesize; i++) {
if(adcvalue < a)
break;
a += amt1001_lookupadcstep;
}
maxt = pgm_read_float(&amt1001_lookuptable[i]); //highest interval value
if(i==0)
mint = maxt;
else
mint = pgm_read_float(&amt1001_lookuptable[i-1]); //smallest interval value
//do interpolation
a = a-amt1001_lookupadcstep;
t = mint + ((maxt-mint)/amt1001_lookupadcstep) * (adcvalue-a);
return t;
}
/*
start an autotune session
*/
void AP_AutoTune::start(void)
{
running = true;
state = DEMAND_UNSATURATED;
uint32_t now = AP_HAL::millis();
state_enter_ms = now;
last_save_ms = now;
last_save = current;
restore = current;
uint8_t level = aparm.autotune_level;
if (level > ARRAY_SIZE(tuning_table)) {
level = ARRAY_SIZE(tuning_table);
}
if (level < 1) {
level = 1;
}
current.rmax.set(pgm_read_float(&tuning_table[level-1].rmax));
// D gain is scaled to a fixed ratio of P gain
current.D.set( pgm_read_float(&tuning_table[level-1].Dratio) * current.P);
current.tau.set( pgm_read_float(&tuning_table[level-1].tau));
current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);
// force a fixed ratio of I to D gain on the rate feedback path
current.I = 0.5f * current.D / current.tau;
next_save = current;
Debug("START P -> %.3f\n", current.P.get());
}
int main(void)
{
float32_t* test;
float32_t test2;
test = (float32_t*)(&SENSOR_GAIN[0] );
test2 = (float32_t)pgm_read_float( (uint16_t*)(&SENSOR_GAIN[0]) );
return 0;
}
mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) {
float closest = 99999.99;
mesh_index_pair return_val;
return_val.x_index = return_val.y_index = -1;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (!is_bit_set(circle_flags, i, j)) {
const float mx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // We found a circle that needs to be printed
my = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
// Get the distance to this intersection
float f = HYPOT(X - mx, Y - my);
// It is possible that we are being called with the values
// to let us find the closest circle to the start position.
// But if this is not the case, add a small weighting to the
// distance calculation to help it choose a better place to continue.
f += HYPOT(x_pos - mx, y_pos - my) / 15.0;
// Add in the specified amount of Random Noise to our search
if (random_deviation > 1.0)
f += random(0.0, random_deviation);
if (f < closest) {
closest = f; // We found a closer location that is still
return_val.x_index = i; // un-printed --- save the data for it
return_val.y_index = j;
return_val.distance = closest;
}
}
}
}
bit_set(circle_flags, return_val.x_index, return_val.y_index); // Mark this location as done.
return return_val;
}
void
histogram_do(histogram_t *t,
float *samples,
size_t n_samples)
{
int32_t max = 0;
uint32_t offset = 0;
uint32_t range = 0;
uint16_t i;
for (i = 0; i < n_samples; i++) {
max = HISTOGRAM_MAX(max, pgm_read_float(&samples[i]) * t->precision);
t->min = HISTOGRAM_MIN(t->min, pgm_read_float(&samples[i]) * t->precision);
}
range = max - t->min;
t->baseline = range / t->n_baselines;
for (i = 0; i < n_samples; i++) {
offset = ((pgm_read_float(&samples[i]) * t->precision) - t->min) / t->baseline;
if (offset < t->n_baselines)
t->histogram[offset]++;
}
}
float ConstantTable<float>::operator[](int index) const
{
return pgm_read_float(arr + index);
}
unsigned char menu_input(unsigned char cmd)
{
static unsigned char state = 'q';
static unsigned char substate = 0;
static unsigned char input_idx=0;
static unsigned char cyl_counter =0;
unsigned int tmpi = 0;
float tmpf = 0;
unsigned char i = 0;
static unsigned char subst = 0;
//just dump the menu if we're at the top level
//and user hit enter
if(state == 'q' || state == 'm' )
{
switch( cmd )
{
//set rpm points for knock table
case 'r': state = 'r'; cyl_counter = 0; substate = 0; break;
//set voltage points for knock table
//will be dumped to eeprom when done
case 'v': state = 'v'; cyl_counter = 0; substate = 0; break;
//get the current engine settings
case 'g': state = 'g'; break;
//configure the engine settings
//will be dumped to eeprom when done
case 'c': state = 'c'; break;
//dump the status of internal variables
case 'd': state = 'd'; break;
//if we get a 'q' shut down the menu
case 'q': return 0;
//set the state to main menu
default: state = 'm'; print_main_menu(); break;
}
return 1;
}
else
{
switch(state)
{
case 'g':
dump_config();
newline();
print_main_menu();
state = 'm';
break;
case 'd':
dump_variables();
newline();
print_main_menu();
state = 'm';
break;
case 'r':
switch(substate)
{
case 0:
newline();
snprintf_P(output, OUTPUT_LEN, cylinder_prompt, cfg.firing_order[cyl_counter]);
print_str(output);
substate++;
input_idx = 0;
break;
case 1:
//set value from substate 0 query
//if \r\n our work here is done
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
subst = 0;
input_idx = 0;
for(i = 0; i < OUTPUT_LEN && output[i] != '\0'; i++)
{
if(output[i] == ',')
{
output[i] = '\0';
//this is actually stored in timer ticks to avoid division in code
//introduces +/- 8 rpm error around 10k rpm
cfg.rpm_points[cyl_counter][input_idx++] = (TICKS_PER_MIN / atoi( (output+subst) ) );
subst = i+1;
}
}
cfg.rpm_points[cyl_counter][input_idx] = (TICKS_PER_MIN / atoi( (output+subst) ) );
}
if(++cyl_counter == cfg.num_cyls )
{
print_str_P(table_layout);
for(i = 0; i < cfg.num_cyls; i++)
{
calculate_fslopes(i);
newline();
newline();
snprintf_P(output, OUTPUT_LEN, table_header, cfg.firing_order[i]);
print_str(output);
newline();
dump_rpm_table(i);
newline();
dump_voltage_table(i);
newline();
dump_fslopes(i);
}
print_str_P(prompt);
substate++;
input_idx = 0;
}
else
{
newline();
input_idx = 0;
snprintf_P(output, OUTPUT_LEN, cylinder_prompt, cfg.firing_order[cyl_counter]);
print_str(output);
}
}
else if( input_idx < OUTPUT_LEN -1 && (isdigit(cmd) || cmd == ',' ) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 2:
if( cmd == 'y' || cmd == 'Y')
{
write_cfg_to_eep();
setup();
}
else if( cmd == 'n' || cmd == 'N' )
{
substate = 0;
input_idx = 0;
cyl_counter = 0;
break;
}
else if( cmd == 'q' || cmd == 'Q')
{
read_cfg_from_eep();
}
else
{
print_str_P(prompt);
break;
}
state = 'm';
default: substate =0; input_idx = 0; cyl_counter = 0; break;
}
break;
case 'v':
switch(substate)
{
case 0:
newline();
snprintf_P(output, OUTPUT_LEN, voltage_prompt , cfg.firing_order[cyl_counter]);
print_str(output);
substate++;
break;
case 1:
//set value from substate 0 query
//if \n our work here is done
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
subst = 0;
input_idx = 0;
for(i = 0; i < OUTPUT_LEN && output[i] != '\0'; i++)
{
if(output[i] == ',')
{
output[i] = '\0';
cfg.knock_voltage[cyl_counter][input_idx++] = (unsigned int ) (atof((output+subst)) /TEN_BIT_LSB) ;
subst = i+1;
}
}
cfg.knock_voltage[cyl_counter][input_idx] = (unsigned int ) (atof((output+subst)) /TEN_BIT_LSB) ;
}
if(++cyl_counter == cfg.num_cyls)
{
print_str_P(table_layout);
for(i = 0; i < cfg.num_cyls; i++)
{
calculate_fslopes(i);
newline();
newline();
snprintf_P(output, OUTPUT_LEN, table_header, cfg.firing_order[i]);
print_str(output);
newline();
dump_rpm_table(i);
newline();
dump_voltage_table(i);
newline();
dump_fslopes(i);
}
print_str_P(prompt);
substate++;
input_idx = 0;
}
else
{
newline();
snprintf_P(output, OUTPUT_LEN, voltage_prompt, cfg.firing_order[cyl_counter]);
print_str(output);
input_idx = 0;
}
}
else if( input_idx < OUTPUT_LEN -1 && (isdigit(cmd) || cmd == ','|| cmd == '.' ) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 2:
if( cmd == 'y' || cmd == 'Y')
{
write_cfg_to_eep();
setup();
}
else if( cmd == 'n' || cmd == 'N' )
{
substate = 0;
input_idx = 0;
cyl_counter = 0;
break;
}
else if( cmd == 'q' || cmd == 'Q')
{
read_cfg_from_eep();
}
else
{
print_str_P(prompt);
break;
}
state = 'm';
default: substate = 0; input_idx = 0; cyl_counter = 0; break;
}
break;
case 'c':
switch(substate)
{
case 0:
newline();
print_str_P(dump_cfg[substate++] );
break;
case 1:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.num_cyls = (unsigned char)atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 2:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.window_length_deg = (unsigned char)atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 3:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.window_open_deg_atdc = (unsigned char)atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 4:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.mech_advance_deg_btdc = (unsigned char)atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 5:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.window_integ_div = (unsigned char)atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 6:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
tmpi = (unsigned int)atoi(output);
//search for the next largest bandpass frequenc and
//set the index of the current center frequency in the cfg section
for(i = 0; i < NUM_BANDPASS_FREQS && pgm_read_word(&bandpass_freq_value[i]) < tmpi ; i++);
cfg.tpic_freq = i;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 7:
if(cmd == '\n' || cmd == '\r' )
{
if(input_idx > 0)
{
output[input_idx] = '\0';
tmpf = (float)atof(output);
//search for the next smallest gain
//set the index of the gain in the cfg section
for(i = 0; i < NUM_GAIN && pgm_read_float(&gain_value[i]) > tmpf ; i++);
cfg.tpic_gain = i;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && (isdigit(cmd) || cmd == '.' ) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 8:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
subst = 0;
input_idx = 0;
for(i = 0; i < OUTPUT_LEN && output[i] != '\0'; i++)
{
if(output[i] == ',')
{
output[i] = '\0';
cfg.firing_order[input_idx++] = atoi( (output+subst) );
subst = i+1;
}
}
cfg.firing_order[input_idx] = atoi( (output+subst) );
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && (isdigit(cmd) || cmd == ',' ) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 9:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.datalog_frequency = MAX_DATALOGS_PER_SEC/(unsigned char)atoi(output);
if(cfg.datalog_frequency < 2)
cfg.datalog_frequency = 1;
else
cfg.datalog_frequency--;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 10:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.datalog_header_count = (unsigned char)atoi(output);
if(cfg.datalog_header_count == 0)
cfg.datalog_header_count = 1;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 11:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.wheelmode = (unsigned char)atoi(output);
if(cfg.wheelmode != HEP && cfg.wheelmode != FOURTWENTYA )
cfg.wheelmode = 0;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 12:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.tpic_chan = ( (unsigned char)atoi(output) -1 );
if(cfg.tpic_chan != 1 )
cfg.tpic_chan = 0;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 13:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.datalogmode = ( (unsigned char)atoi(output) );
if( cfg.datalogmode )
cfg.datalogmode = 1;
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 14:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
strncpy(cfg.seperator, output, MAX_SEP_LEN-1);
cfg.seperator[MAX_SEP_LEN - 1] = '\0';
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 15:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.pulse_events = (unsigned char) atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 16:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.pulse_tenms = (unsigned char) atoi(output);
input_idx=0;
}
newline();
print_str_P(dump_cfg[substate++]);
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 17:
if(cmd == '\n' || cmd == '\r')
{
if(input_idx > 0)
{
output[input_idx] = '\0';
cfg.pulse_timer_select = (unsigned char) atoi(output);
if(cfg.pulse_timer_select)
cfg.pulse_timer_select = 1;
input_idx=0;
}
print_str_P(config_done);
dump_config();
print_str_P(prompt);
newline();
substate++;
}
else if( input_idx < OUTPUT_LEN && isdigit(cmd) )
output[input_idx++]=cmd;
else if ( input_idx > 0 && ( cmd == '\b' || cmd == 0x7F || cmd == 0x08 ) )
input_idx--;
break;
case 18:
if( cmd == 'y' || cmd == 'Y')
{
write_cfg_to_eep();
setup();
}
else if( cmd == 'n' || cmd == 'N' )
{
substate = 0;
input_idx = 0;
cyl_counter = 0;
break;
}
else if( cmd == 'q' || cmd == 'Q')
{
read_cfg_from_eep();
}
else
{
print_str_P(prompt);
break;
}
state = 'm';
default: substate = 0; input_idx = 0; cyl_counter = 0; break;
}//switch(substate)
}//switch(state)
}//else
return 1;
}//void menu_input()
void dump_config( void )
{
unsigned char i;
char flt[8];
newline();
print_str_P(dump_cfg[0]);
snprintf(output, OUTPUT_LEN, "%u", cfg.num_cyls);
print_str(output);
newline();
print_str_P(dump_cfg[1]);
snprintf(output, OUTPUT_LEN, "%u", cfg.window_length_deg);
print_str(output);
newline();
print_str_P(dump_cfg[2]);
snprintf(output, OUTPUT_LEN, "%u", cfg.window_open_deg_atdc);
print_str(output);
newline();
print_str_P(dump_cfg[3]);
snprintf(output, OUTPUT_LEN, "%u", cfg.mech_advance_deg_btdc);
print_str(output);
newline();
print_str_P(dump_cfg[4]);
snprintf(output, OUTPUT_LEN, "%u", cfg.window_integ_div);
print_str(output);
newline();
// include additional string for index at end of line
print_str_P(dump_cfg[5]);
snprintf(output, OUTPUT_LEN, "%u index: %u", pgm_read_word(&bandpass_freq_value[cfg.tpic_freq]), cfg.tpic_freq );
print_str(output);
newline();
dtostrf( pgm_read_float(&gain_value[cfg.tpic_gain] ) , 5, 3, flt);
print_str_P(dump_cfg[6]);
snprintf(output,OUTPUT_LEN, "%s index: %u", flt, cfg.tpic_gain);
print_str(output);
newline();
print_str_P(dump_cfg[7]);
for(i = 0; i < ( cfg.num_cyls -1); i++)
{
snprintf(output, OUTPUT_LEN, "%u,", cfg.firing_order[i]);
print_str(output);
}
snprintf(output, OUTPUT_LEN, "%u", cfg.firing_order[i]);
print_str(output);
newline();
print_str_P(dump_cfg[8]);
snprintf(output,OUTPUT_LEN, "%u", MAX_DATALOGS_PER_SEC/(cfg.datalog_frequency) );
print_str(output);
newline();
print_str_P(dump_cfg[9]);
snprintf(output,OUTPUT_LEN, "%u", cfg.datalog_header_count );
print_str(output);
newline();
print_str_P(dump_cfg[10]);
snprintf(output,OUTPUT_LEN, "%u", cfg.wheelmode );
print_str(output);
newline();
print_str_P(dump_cfg[11]);
snprintf(output,OUTPUT_LEN, "%u", cfg.tpic_chan + 1 );
print_str(output);
newline();
print_str_P(dump_cfg[12]);
snprintf(output,OUTPUT_LEN, "%u", cfg.datalogmode );
print_str(output);
newline();
print_str_P(dump_cfg[13]);
snprintf(output,OUTPUT_LEN, "\"%s\"", cfg.seperator);
print_str(output);
newline();
print_str_P(dump_cfg[14]);
snprintf(output,OUTPUT_LEN, "%u", cfg.pulse_events);
print_str(output);
newline();
print_str_P(dump_cfg[15]);
snprintf(output,OUTPUT_LEN, "%u", cfg.pulse_tenms);
print_str(output);
newline();
print_str_P(dump_cfg[16]);
snprintf(output,OUTPUT_LEN, "%u", cfg.pulse_timer_select);
print_str(output);
newline();
for(i = 0; i < cfg.num_cyls; i++)
{
newline();
snprintf_P(output, OUTPUT_LEN, table_header, cfg.firing_order[i]);
print_str(output);
newline();
dump_rpm_table( i);
newline();
dump_voltage_table( i );
newline();
dump_fslopes( i );
newline();
}
newline();
return;
}
void look_for_lines_to_connect() {
float sx, sy, ex, ey;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (i < GRID_MAX_POINTS_X) { // We can't connect to anything to the right than GRID_MAX_POINTS_X.
// This is already a half circle because we are at the edge of the bed.
if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i + 1, j)) { // check if we can do a line to the left
if (!is_bit_set(horizontal_mesh_line_flags, i, j)) {
//
// We found two circles that need a horizontal line to connect them
// Print it!
//
sx = pgm_read_float(&ubl.mesh_index_to_xpos[ i ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);
sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy);
SERIAL_ECHOPAIR(") -> (ex=", ex);
SERIAL_ECHOPAIR(", ey=", ey);
SERIAL_CHAR(')');
SERIAL_EOL;
//debug_current_and_destination(PSTR("Connecting horizontal line."));
}
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again
}
}
if (j < GRID_MAX_POINTS_Y) { // We can't connect to anything further back than GRID_MAX_POINTS_Y.
// This is already a half circle because we are at the edge of the bed.
if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i, j + 1)) { // check if we can do a line straight down
if (!is_bit_set( vertical_mesh_line_flags, i, j)) {
//
// We found two circles that need a vertical line to connect them
// Print it!
//
sy = pgm_read_float(&ubl.mesh_index_to_ypos[ j ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1);
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy);
SERIAL_ECHOPAIR(") -> (ex=", ex);
SERIAL_ECHOPAIR(", ey=", ey);
SERIAL_CHAR(')');
SERIAL_EOL;
debug_current_and_destination(PSTR("Connecting vertical line."));
}
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again
}
}
}
}
}
}
}
/**
* G26: Mesh Validation Pattern generation.
*
* Used to interactively edit UBL's Mesh by placing the
* nozzle in a problem area and doing a G29 P4 R command.
*/
void gcode_G26() {
SERIAL_ECHOLNPGM("G26 command started. Waiting for heater(s).");
float tmp, start_angle, end_angle;
int i, xi, yi;
mesh_index_pair location;
// Don't allow Mesh Validation without homing first,
// or if the parameter parsing did not go OK, abort
if (axis_unhomed_error(true, true, true) || parse_G26_parameters()) return;
if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
stepper.synchronize();
set_current_to_destination();
}
if (turn_on_heaters()) goto LEAVE;
current_position[E_AXIS] = 0.0;
sync_plan_position_e();
if (prime_flag && prime_nozzle()) goto LEAVE;
/**
* Bed is preheated
*
* Nozzle is at temperature
*
* Filament is primed!
*
* It's "Show Time" !!!
*/
ZERO(circle_flags);
ZERO(horizontal_mesh_line_flags);
ZERO(vertical_mesh_line_flags);
// Move nozzle to the specified height for the first layer
set_destination_to_current();
destination[Z_AXIS] = layer_height;
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0);
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], ooze_amount);
ubl.has_control_of_lcd_panel = true;
//debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern."));
/**
* Declare and generate a sin() & cos() table to be used during the circle drawing. This will lighten
* the CPU load and make the arc drawing faster and more smooth
*/
float sin_table[360 / 30 + 1], cos_table[360 / 30 + 1];
for (i = 0; i <= 360 / 30; i++) {
cos_table[i] = SIZE_OF_INTERSECTION_CIRCLES * cos(RADIANS(valid_trig_angle(i * 30.0)));
sin_table[i] = SIZE_OF_INTERSECTION_CIRCLES * sin(RADIANS(valid_trig_angle(i * 30.0)));
}
do {
if (ubl_lcd_clicked()) { // Check if the user wants to stop the Mesh Validation
#if ENABLED(ULTRA_LCD)
lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), 99);
lcd_quick_feedback();
#endif
while (!ubl_lcd_clicked()) { // Wait until the user is done pressing the
idle(); // Encoder Wheel if that is why we are leaving
lcd_reset_alert_level();
lcd_setstatuspgm(PSTR(""));
}
while (ubl_lcd_clicked()) { // Wait until the user is done pressing the
idle(); // Encoder Wheel if that is why we are leaving
lcd_setstatuspgm(PSTR("Unpress Wheel"), 99);
}
goto LEAVE;
}
location = continue_with_closest
? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS])
: find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now.
if (location.x_index >= 0 && location.y_index >= 0) {
const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem
#ifdef DELTA
if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS.");
goto LEAVE;
}
#endif
// TODO: Change this to use `position_is_reachable`
if (!WITHIN(circle_x, X_MIN_POS, X_MAX_POS) || !WITHIN(circle_y, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print off the bed.");
goto LEAVE;
}
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index;
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Doing circle at: (xi=", xi);
SERIAL_ECHOPAIR(", yi=", yi);
SERIAL_CHAR(')');
SERIAL_EOL;
}
start_angle = 0.0; // assume it is going to be a full circle
end_angle = 360.0;
if (xi == 0) { // Check for bottom edge
start_angle = -90.0;
end_angle = 90.0;
if (yi == 0) // it is an edge, check for the two left corners
start_angle = 0.0;
else if (yi == GRID_MAX_POINTS_Y - 1)
end_angle = 0.0;
}
else if (xi == GRID_MAX_POINTS_X - 1) { // Check for top edge
start_angle = 90.0;
end_angle = 270.0;
if (yi == 0) // it is an edge, check for the two right corners
end_angle = 180.0;
else if (yi == GRID_MAX_POINTS_Y - 1)
start_angle = 180.0;
}
else if (yi == 0) {
start_angle = 0.0; // only do the top side of the cirlce
end_angle = 180.0;
}
else if (yi == GRID_MAX_POINTS_Y - 1) {
start_angle = 180.0; // only do the bottom side of the cirlce
end_angle = 360.0;
}
for (tmp = start_angle; tmp < end_angle - 0.1; tmp += 30.0) {
int tmp_div_30 = tmp / 30.0;
if (tmp_div_30 < 0) tmp_div_30 += 360 / 30;
if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30;
float x = circle_x + cos_table[tmp_div_30], // for speed, these are now a lookup table entry
y = circle_y + sin_table[tmp_div_30],
xe = circle_x + cos_table[tmp_div_30 + 1],
ye = circle_y + sin_table[tmp_div_30 + 1];
#ifdef DELTA
if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of
continue; // the 'circle' is on the bed. If
#else // not, we need to skip
x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1);
xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1);
ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
#endif
//if (ubl.g26_debug_flag) {
// char ccc, *cptr, seg_msg[50], seg_num[10];
// strcpy(seg_msg, " segment: ");
// strcpy(seg_num, " \n");
// cptr = (char*) "01234567890ABCDEF????????";
// ccc = cptr[tmp_div_30];
// seg_num[1] = ccc;
// strcat(seg_msg, seg_num);
// debug_current_and_destination(seg_msg);
//}
print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), layer_height);
}
//debug_current_and_destination(PSTR("Looking for lines to connect."));
look_for_lines_to_connect();
//debug_current_and_destination(PSTR("Done with line connect."));
}
//debug_current_and_destination(PSTR("Done with current circle."));
} while (location.x_index >= 0 && location.y_index >= 0);
LEAVE:
lcd_reset_alert_level();
lcd_setstatuspgm(PSTR("Leaving G26"));
retract_filament(destination);
destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
//debug_current_and_destination(PSTR("ready to do Z-Raise."));
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle
//debug_current_and_destination(PSTR("done doing Z-Raise."));
destination[X_AXIS] = x_pos; // Move back to the starting position
destination[Y_AXIS] = y_pos;
//destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position
//debug_current_and_destination(PSTR("done doing X/Y move."));
ubl.has_control_of_lcd_panel = false; // Give back control of the LCD Panel!
if (!keep_heaters_on) {
#if HAS_TEMP_BED
thermalManager.setTargetBed(0);
#endif
thermalManager.setTargetHotend(0, 0);
}
}
