/* * gettimeofday_cached() * * Clones the event_base_gettimeofday_cached() interface but ensures the * times are always on the gettimeofday() 1970 scale. Older libevent 2 * sometimes used gettimeofday(), sometimes the since-system-start * clock_gettime(CLOCK_MONOTONIC), depending on the platform. * * It is not cleanly possible to tell which timescale older libevent is * using. * * The strategy involves 1 hour thresholds chosen to be far longer than * the duration of a round of libevent callbacks, which share a cached * start-of-round time. First compare the last cached time with the * current gettimeofday() time. If they are within one hour, libevent * is using the proper timescale so leave the offset 0. Otherwise, * compare libevent's cached time and the current time on the monotonic * scale. If they are within an hour, libevent is using the monotonic * scale so calculate the offset to add to such times to bring them to * gettimeofday()'s scale. */ int gettimeofday_cached( struct event_base * b, struct timeval * caller_tv ) { #if defined(_EVENT_HAVE_CLOCK_GETTIME) && defined(CLOCK_MONOTONIC) static struct event_base * cached_b; static struct timeval cached; static struct timeval adj_cached; static struct timeval offset; static int offset_ready; struct timeval latest; struct timeval systemt; struct timespec ts; struct timeval mono; struct timeval diff; int cgt_rc; int gtod_rc; event_base_gettimeofday_cached(b, &latest); if (b == cached_b && !memcmp(&latest, &cached, sizeof(latest))) { *caller_tv = adj_cached; return 0; } cached = latest; cached_b = b; if (!offset_ready) { cgt_rc = clock_gettime(CLOCK_MONOTONIC, &ts); gtod_rc = gettimeofday(&systemt, NULL); if (0 != gtod_rc) { msyslog(LOG_ERR, "%s: gettimeofday() error %m", progname); exit(1); } diff = sub_tval(systemt, latest); if (debug > 1) printf("system minus cached %+ld.%06ld\n", (long)diff.tv_sec, (long)diff.tv_usec); if (0 != cgt_rc || labs((long)diff.tv_sec) < 3600) { /* * Either use_monotonic == 0, or this libevent * has been repaired. Leave offset at zero. */ } else { mono.tv_sec = ts.tv_sec; mono.tv_usec = ts.tv_nsec / 1000; diff = sub_tval(latest, mono); if (debug > 1) printf("cached minus monotonic %+ld.%06ld\n", (long)diff.tv_sec, (long)diff.tv_usec); if (labs((long)diff.tv_sec) < 3600) { /* older libevent2 using monotonic */ offset = sub_tval(systemt, mono); TRACE(1, ("%s: Offsetting libevent CLOCK_MONOTONIC times by %+ld.%06ld\n", "gettimeofday_cached", (long)offset.tv_sec, (long)offset.tv_usec)); } } offset_ready = TRUE; } adj_cached = add_tval(cached, offset); *caller_tv = adj_cached; return 0; #else return event_base_gettimeofday_cached(b, caller_tv); #endif }
void Plane::update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ // distance and bearing calcs only switch(control_mode) { case AUTO: update_commands(); break; case RTL: if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && nav_controller->reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence jump_to_landing_sequence(); // prevent running the expensive jump_to_landing_sequence // on every loop auto_state.checked_for_autoland = true; } else if (g.rtl_autoland == 2 && !auto_state.checked_for_autoland) { // Go directly to the landing sequence jump_to_landing_sequence(); // prevent running the expensive jump_to_landing_sequence // on every loop auto_state.checked_for_autoland = true; } // fall through to LOITER case LOITER: case GUIDED: // allow loiter direction to be changed in flight if (g.loiter_radius < 0) { loiter.direction = -1; } else { loiter.direction = 1; } update_loiter(); break; case CRUISE: update_cruise(); break; case MANUAL: case STABILIZE: case TRAINING: case INITIALISING: case ACRO: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: // nothing to do break; } }
static int _csngen_adjust_local_time (CSNGen *gen, time_t cur_time) { extern int config_get_ignore_time_skew(void); int ignore_time_skew = config_get_ignore_time_skew(); time_t time_diff = cur_time - gen->state.sampled_time; if (time_diff == 0) { /* This is a no op - _csngen_adjust_local_time should never be called in this case, because there is nothing to adjust - but just return here to protect ourselves */ return CSN_SUCCESS; } else if (time_diff > 0) { time_t ts_before = CSN_CALC_TSTAMP(gen); time_t ts_after = 0; if (slapi_is_loglevel_set(SLAPI_LOG_REPL)) { time_t new_time = CSN_CALC_TSTAMP(gen); slapi_log_err(SLAPI_LOG_REPL, "_csngen_adjust_local_time", "gen state before %08lx%04x:%ld:%ld:%ld\n", new_time, gen->state.seq_num, gen->state.sampled_time, gen->state.local_offset, gen->state.remote_offset); } gen->state.sampled_time = cur_time; if (time_diff > gen->state.local_offset) gen->state.local_offset = 0; else gen->state.local_offset = gen->state.local_offset - time_diff; /* only reset the seq_num if the new timestamp part of the CSN is going to be greater than the old one - if they are the same after the above adjustment (which can happen if csngen_adjust_time has to store the offset in the local_offset field) we must not allow the CSN to regress or generate duplicate numbers */ ts_after = CSN_CALC_TSTAMP(gen); if (ts_after > ts_before) { gen->state.seq_num = 0; /* only reset if new time > old time */ } if (slapi_is_loglevel_set(SLAPI_LOG_REPL)) { time_t new_time = CSN_CALC_TSTAMP(gen); slapi_log_err(SLAPI_LOG_REPL, "_csngen_adjust_local_time", "gen state after %08lx%04x:%ld:%ld:%ld\n", new_time, gen->state.seq_num, gen->state.sampled_time, gen->state.local_offset, gen->state.remote_offset); } return CSN_SUCCESS; } else /* time was turned back */ { if (slapi_is_loglevel_set(SLAPI_LOG_REPL)) { time_t new_time = CSN_CALC_TSTAMP(gen); slapi_log_err(SLAPI_LOG_REPL, "_csngen_adjust_local_time", "gen state back before %08lx%04x:%ld:%ld:%ld\n", new_time, gen->state.seq_num, gen->state.sampled_time, gen->state.local_offset, gen->state.remote_offset); } if (!ignore_time_skew && (labs (time_diff) > CSN_MAX_TIME_ADJUST)) { slapi_log_err(SLAPI_LOG_ERR, "_csngen_adjust_local_time", "Adjustment limit exceeded; value - %ld, limit - %d\n", labs (time_diff), CSN_MAX_TIME_ADJUST); return CSN_LIMIT_EXCEEDED; } gen->state.sampled_time = cur_time; gen->state.local_offset = MAX_VAL (gen->state.local_offset, labs (time_diff)); gen->state.seq_num = 0; if (slapi_is_loglevel_set(SLAPI_LOG_REPL)) { time_t new_time = CSN_CALC_TSTAMP(gen); slapi_log_err(SLAPI_LOG_REPL, "_csngen_adjust_local_time", "gen state back after %08lx%04x:%ld:%ld:%ld\n", new_time, gen->state.seq_num, gen->state.sampled_time, gen->state.local_offset, gen->state.remote_offset); } return CSN_SUCCESS; } }
static int CgiStatusRow( FILE * stream, int row_no ) { time_t secs = time(NULL); switch( row_no ) { case -1: if( sntp_available ) subhdr( stream, "Time (SNTP)" ); else subhdr( stream, "Time" ); break; case 0: { HTML("<TR><TD> GMT/Local Time </TD><TD> "); printTime( stream, gmtime(&secs) ); HTML(" <b>"); printTime( stream, localtime(&secs) ); HTML("</b> </TD></TR>\r\n"); //ShowTableRow3( stream, char *c1, char *c2, char *c3 ); } break; case 1: /* { HTML("<TR><TD> Local Time </TD><TD> "); printTime( stream, localtime(&secs) ); HTML(" </TD></TR>\r\n"); } */ break; case 2: { char minus = _timezone < 0; int32_t seconds = labs(_timezone); int32_t minutes = seconds / 60UL; int32_t hours = minutes / 60UL; minutes %= 60UL; seconds %= 60UL; HTML("<TR><TD> TimeZone </TD><TD> "); fprintf(stream, "%s%ld:%ld:%ld (%ld)", (minus ? "-" : ""), hours, minutes, seconds, _timezone ); HTML(" </TD></TR>\r\n"); } break; case 3: { uint32_t seconds = NutGetSeconds(); uint32_t minutes = seconds / 60UL; uint32_t hours = minutes / 60UL; uint32_t days = hours / 24UL; minutes %= 60UL; seconds %= 60UL; hours %= 24UL; HTML("<TR><TD> UpTime </TD><TD> "); fprintf(stream, "%lu days %lu:%lu:%lu", days, hours, minutes, seconds); HTML(" </TD></TR>\r\n"); } break; //case 4: ShowTableRow2b( stream, "DST", _daylight ); break; //case 4: ShowTableRow2b( stream, "Used SNTP", sntp_available ); break; case 4: break; //case 6: HTML("<th colspan=\"2\">FirmWare</th>"); break; case 5: subhdr( stream, "FirmWare" ); break; case 6: ShowTableRow2( stream, "Build", makeDate ); break; case 7: ShowTableRow2( stream, "Name", DEVICE_NAME ); break; case 8: ShowTableRow2( stream, "ModBus Id", modbus_device_id ); break; //case 9: HTML("<th colspan=\"2\">ModBus</th>"); break; case 9: subhdr( stream, "ModBus" ); break; case 10: ShowTableRow2i( stream, "IO count", modbus_event_cnt ); break; case 11: ShowTableRow2i( stream, "CRC count", modbus_crc_cnt ); break; case 12: ShowTableRow2i( stream, "exceptions count", modbus_exceptions_cnt );break; case 13: ShowTableRow2i( stream, "err flags", modbus_error_flags ); break; case 14: /*subhdr( stream, "1-Wire" );*/ break; case 15: /*ShowTableRow2b( stream, "Devices detected", onewire_available ); */ break; case 16: /*ShowTableRow2i( stream, "Temp sensors count", nTempSensors ); */ break; case 17: #if SERVANT_1WMAC /*{ HTML("<TR><TD> 2401 Id </TD><TD> "); fprintf(stream, "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X", serialNumber [7], serialNumber [6], serialNumber [5], serialNumber [4], serialNumber [3], serialNumber [2], serialNumber [1], serialNumber [0] ); HTML(" </TD></TR>\r\n"); }*/ #endif break; case 18: subhdr( stream, "OS" ); break; case 19: ShowTableRow2i( stream, "Free mem, K", NutHeapAvailable()/1024 ); break; case 20: ShowTableRow2i( stream, "Size of EEPROM cfg", sizeof(struct eeprom_cfg) ); break; case 21: { uint32_t tx_total, rx_total; uint8_t active; #if SERVANT_TUN0 || SERVANT_TUN1 subhdr( stream, "Tunnels" ); #if SERVANT_TUN0 get_tunnel_stats( 0, &tx_total, &rx_total, &active ); ShowTableRow2b( stream, "Tun0 active", active ); ShowTableRow2i( stream, "Tun0 RX, K", rx_total/1024 ); ShowTableRow2i( stream, "Tun0 TX, K", tx_total/1024 ); #endif #if SERVANT_TUN1 get_tunnel_stats( 1, &tx_total, &rx_total, &active ); ShowTableRow2b( stream, "Tun1 active", active ); ShowTableRow2i( stream, "Tun1 RX, K", rx_total/1024 ); ShowTableRow2i( stream, "Tun1 TX, K", tx_total/1024 ); #endif #endif break; } case 22: subhdr( stream, "DevMap" ); break; case 23: ShowTableRow2i( stream, "Major dev count", n_major_total ); break; case 24: ShowTableRow2i( stream, "Minor dev count", n_minor_total ); break; default: return 0; } return 1; }
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder) #endif // AUTO_BED_LEVELING_FEATURE { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while (block_buffer_tail == next_buffer_head) idle(); #if ENABLED(MESH_BED_LEVELING) if (mbl.active) z += mbl.get_z(x, y); #elif ENABLED(AUTO_BED_LEVELING_FEATURE) apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // The target position of the tool in absolute steps // Calculate target position in absolute steps //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow long target[NUM_AXIS]; target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]); target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]); long dx = target[X_AXIS] - position[X_AXIS], dy = target[Y_AXIS] - position[Y_AXIS], dz = target[Z_AXIS] - position[Z_AXIS]; // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied if (DEBUGGING(DRYRUN)) position[E_AXIS] = target[E_AXIS]; long de = target[E_AXIS] - position[E_AXIS]; #if ENABLED(PREVENT_DANGEROUS_EXTRUDE) if (de) { if (degHotend(extruder) < extrude_min_temp) { position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part de = 0; // no difference SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); } #if ENABLED(PREVENT_LENGTHY_EXTRUDE) if (labs(de) > axis_steps_per_unit[E_AXIS] * (EXTRUDE_MAXLENGTH)) { position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part de = 0; // no difference SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); } #endif } #endif // Prepare to set up new block block_t* block = &block_buffer[block_buffer_head]; // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; // Number of steps for each axis #if ENABLED(COREXY) // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html block->steps[A_AXIS] = labs(dx + dy); block->steps[B_AXIS] = labs(dx - dy); block->steps[Z_AXIS] = labs(dz); #elif ENABLED(COREXZ) // corexz planning block->steps[A_AXIS] = labs(dx + dz); block->steps[Y_AXIS] = labs(dy); block->steps[C_AXIS] = labs(dx - dz); #else // default non-h-bot planning block->steps[X_AXIS] = labs(dx); block->steps[Y_AXIS] = labs(dy); block->steps[Z_AXIS] = labs(dz); #endif block->steps[E_AXIS] = labs(de); block->steps[E_AXIS] *= volumetric_multiplier[extruder]; block->steps[E_AXIS] *= extruder_multiplier[extruder]; block->steps[E_AXIS] /= 100; block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS]))); // Bail if this is a zero-length block if (block->step_event_count <= dropsegments) return; #if FAN_COUNT > 0 for (uint8_t i = 0; i < FAN_COUNT; i++) block->fan_speed[i] = fanSpeeds[i]; #endif #if ENABLED(BARICUDA) block->valve_pressure = ValvePressure; block->e_to_p_pressure = EtoPPressure; #endif // Compute direction bits for this block uint8_t db = 0; #if ENABLED(COREXY) if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (dy < 0) SBI(db, Y_HEAD); // ...and Y if (dz < 0) SBI(db, Z_AXIS); if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction #elif ENABLED(COREXZ) if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (dy < 0) SBI(db, Y_AXIS); if (dz < 0) SBI(db, Z_HEAD); // ...and Z if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction if (dx - dz < 0) SBI(db, C_AXIS); // Motor B direction #else if (dx < 0) SBI(db, X_AXIS); if (dy < 0) SBI(db, Y_AXIS); if (dz < 0) SBI(db, Z_AXIS); #endif if (de < 0) SBI(db, E_AXIS); block->direction_bits = db; block->active_extruder = extruder; //enable active axes #if ENABLED(COREXY) if (block->steps[A_AXIS] || block->steps[B_AXIS]) { enable_x(); enable_y(); } #if DISABLED(Z_LATE_ENABLE) if (block->steps[Z_AXIS]) enable_z(); #endif #elif ENABLED(COREXZ) if (block->steps[A_AXIS] || block->steps[C_AXIS]) { enable_x(); enable_z(); } if (block->steps[Y_AXIS]) enable_y(); #else if (block->steps[X_AXIS]) enable_x(); if (block->steps[Y_AXIS]) enable_y(); #if DISABLED(Z_LATE_ENABLE) if (block->steps[Z_AXIS]) enable_z(); #endif #endif // Enable extruder(s) if (block->steps[E_AXIS]) { if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder for (int i = 0; i < EXTRUDERS; i++) if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--; switch(extruder) { case 0: enable_e0(); #if ENABLED(DUAL_X_CARRIAGE) if (extruder_duplication_enabled) { enable_e1(); g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2; } #endif g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2; #if EXTRUDERS > 1 if (g_uc_extruder_last_move[1] == 0) disable_e1(); #if EXTRUDERS > 2 if (g_uc_extruder_last_move[2] == 0) disable_e2(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif #endif #endif break; #if EXTRUDERS > 1 case 1: enable_e1(); g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); #if EXTRUDERS > 2 if (g_uc_extruder_last_move[2] == 0) disable_e2(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif #endif break; #if EXTRUDERS > 2 case 2: enable_e2(); g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[1] == 0) disable_e1(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif break; #if EXTRUDERS > 3 case 3: enable_e3(); g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[1] == 0) disable_e1(); if (g_uc_extruder_last_move[2] == 0) disable_e2(); break; #endif // EXTRUDERS > 3 #endif // EXTRUDERS > 2 #endif // EXTRUDERS > 1 } } else { // enable all enable_e0(); enable_e1(); enable_e2(); enable_e3(); } } if (block->steps[E_AXIS]) NOLESS(feed_rate, minimumfeedrate); else NOLESS(feed_rate, mintravelfeedrate); /** * This part of the code calculates the total length of the movement. * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS. * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y. * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed. */ #if ENABLED(COREXY) float delta_mm[6]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS]; delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS]; #elif ENABLED(COREXZ) float delta_mm[6]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS]; delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS]; delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS]; delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS]; #else float delta_mm[4]; delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; #endif delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0; if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) { block->millimeters = fabs(delta_mm[E_AXIS]); } else { block->millimeters = sqrt( #if ENABLED(COREXY) square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]) #elif ENABLED(COREXZ) square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD]) #else square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]) #endif ); } float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides // Calculate moves/second for this move. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; int moves_queued = movesplanned(); // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill #if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN) bool mq = moves_queued > 1 && moves_queued < (BLOCK_BUFFER_SIZE) / 2; #if ENABLED(OLD_SLOWDOWN) if (mq) feed_rate *= 2.0 * moves_queued / (BLOCK_BUFFER_SIZE); #endif #if ENABLED(SLOWDOWN) // segment time im micro seconds unsigned long segment_time = lround(1000000.0/inverse_second); if (mq) { if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued)); #ifdef XY_FREQUENCY_LIMIT segment_time = lround(1000000.0 / inverse_second); #endif } } #endif #endif block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 #if ENABLED(FILAMENT_WIDTH_SENSOR) //FMM update ring buffer used for delay with filament measurements if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10; delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer while (delay_dist < 0) delay_dist += MMD10; delay_index1 = delay_dist / 10.0; // calculate index delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above) if (delay_index1 != delay_index2) { // moved index int8_t meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char while (delay_index1 != delay_index2) { // Increment and loop around buffer if (++delay_index2 >= MMD) delay_index2 -= MMD; delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY); measurement_delay[delay_index2] = meas_sample; } } } #endif // Calculate and limit speed in mm/sec for each axis float current_speed[NUM_AXIS]; float speed_factor = 1.0; //factor <=1 do decrease speed for (int i = 0; i < NUM_AXIS; i++) { current_speed[i] = delta_mm[i] * inverse_second; float cs = fabs(current_speed[i]), mf = max_feedrate[i]; if (cs > mf) speed_factor = min(speed_factor, mf / cs); } // Max segement time in us. #ifdef XY_FREQUENCY_LIMIT // Check and limit the xy direction change frequency unsigned char direction_change = block->direction_bits ^ old_direction_bits; old_direction_bits = block->direction_bits; segment_time = lround((float)segment_time / speed_factor); long xs0 = axis_segment_time[X_AXIS][0], xs1 = axis_segment_time[X_AXIS][1], xs2 = axis_segment_time[X_AXIS][2], ys0 = axis_segment_time[Y_AXIS][0], ys1 = axis_segment_time[Y_AXIS][1], ys2 = axis_segment_time[Y_AXIS][2]; if (TEST(direction_change, X_AXIS)) { xs2 = axis_segment_time[X_AXIS][2] = xs1; xs1 = axis_segment_time[X_AXIS][1] = xs0; xs0 = 0; } xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time; if (TEST(direction_change, Y_AXIS)) { ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1]; ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0]; ys0 = 0; } ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time; long max_x_segment_time = max(xs0, max(xs1, xs2)), max_y_segment_time = max(ys0, max(ys1, ys2)), min_xy_segment_time = min(max_x_segment_time, max_y_segment_time); if (min_xy_segment_time < MAX_FREQ_TIME) { float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME); speed_factor = min(speed_factor, low_sf); } #endif // XY_FREQUENCY_LIMIT // Correct the speed if (speed_factor < 1.0) { for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor; block->nominal_speed *= speed_factor; block->nominal_rate *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. float steps_per_mm = block->step_event_count / block->millimeters; unsigned long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS]; if (bsx == 0 && bsy == 0 && bsz == 0) { block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else if (bse == 0) { block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else { block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } // Limit acceleration per axis unsigned long acc_st = block->acceleration_st, xsteps = axis_steps_per_sqr_second[X_AXIS], ysteps = axis_steps_per_sqr_second[Y_AXIS], zsteps = axis_steps_per_sqr_second[Z_AXIS], esteps = axis_steps_per_sqr_second[E_AXIS], allsteps = block->step_event_count; if (xsteps < (acc_st * bsx) / allsteps) acc_st = (xsteps * allsteps) / bsx; if (ysteps < (acc_st * bsy) / allsteps) acc_st = (ysteps * allsteps) / bsy; if (zsteps < (acc_st * bsz) / allsteps) acc_st = (zsteps * allsteps) / bsz; if (esteps < (acc_st * bse) / allsteps) acc_st = (esteps * allsteps) / bse; block->acceleration_st = acc_st; block->acceleration = acc_st / steps_per_mm; block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0)); #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // collinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed, block->nominal_speed); // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2))); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk / 2; float vmax_junction_factor = 1.0; float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2; float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS]; if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2); if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2); vmax_junction = min(vmax_junction, block->nominal_speed); float safe_speed = vmax_junction; if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { float dsx = current_speed[X_AXIS] - previous_speed[X_AXIS], dsy = current_speed[Y_AXIS] - previous_speed[Y_AXIS], dsz = fabs(csz - previous_speed[Z_AXIS]), dse = fabs(cse - previous_speed[E_AXIS]), jerk = sqrt(dsx * dsx + dsy * dsy); // if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { vmax_junction = block->nominal_speed; // } if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk; if (dsz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dsz); if (dse > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / dse); vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. block->nominal_length_flag = (block->nominal_speed <= v_allowable); block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i]; previous_nominal_speed = block->nominal_speed; #if ENABLED(ADVANCE) // Calculate advance rate if (!bse || (!bsx && !bsy && !bsz)) { block->advance_rate = 0; block->advance = 0; } else { long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * (cse * cse * (EXTRUSION_AREA) * (EXTRUSION_AREA)) * 256; block->advance = advance; block->advance_rate = acc_dist ? advance / (float)acc_dist : 0; } /** SERIAL_ECHO_START; SERIAL_ECHOPGM("advance :"); SERIAL_ECHO(block->advance/256.0); SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOLN(block->advance_rate/256.0); */ #endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed); // Move buffer head block_buffer_head = next_buffer_head; // Update position for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i]; planner_recalculate(); st_wake_up(); } // plan_buffer_line()
// This is the lookahead algorithm. From a high level, we simply // limit our end speed and the next start speed to appropriate limits, // then further limit them to a speed where reaching a stop is possible. void Motion::gcode_optimize(GCode& gcode, GCode& nextg) { #ifdef LOOKAHEAD // THIS NEEDS SERIOUS REFACTORIN if(gcode.optimized) return; gcode.optimized = true; // If this isn't a move, we have nothing to do. if(gcode[G].isUnused() || gcode[G].getInt() != 1 || gcode.movesteps == 0) return; // If the NEXT gcode isn't a move, then we will just recomute our accels // (assuming the previous move changed them during it's optimization) if(nextg[G].isUnused() || nextg[G].getInt() != 1 || nextg.movesteps == 0) { computeAccel(gcode); return; } // Calculate the requested speed of each axis at its peak during this move, // including a direction sign. TODO: We calculated this once already, maybe we can store it. int32_t axisspeeds[NUM_AXES]; int32_t endspeeds[NUM_AXES]; int32_t nextspeeds[NUM_AXES]; int32_t startspeeds[NUM_AXES]; float mult1 = (float)gcode.maxfeed / gcode.actualmm; float mult2 = (float)nextg.maxfeed / nextg.actualmm; for(int ax=0;ax<NUM_AXES;ax++) { axisspeeds[ax] = (float)((float)gcode.axismovesteps[ax] / AXES[ax].getStepsPerMM()) * mult1; axisspeeds[ax] *= gcode.axisdirs[ax] ? 1 : -1; endspeeds[ax] = axisspeeds[ax]; nextspeeds[ax] = (float)((float)nextg.axismovesteps[ax] / AXES[ax].getStepsPerMM()) * mult2; nextspeeds[ax] *= nextg.axisdirs[ax] ? 1 : -1; startspeeds[ax] = nextspeeds[ax]; } int32_t *ends, *starts, *temp; ends = endspeeds; starts = startspeeds; // TODO this is stupid shit. int MAX_OPTS = (float)AXES[0].getMaxFeed() / (float)AXES[0].getStartFeed(); if(MAX_OPTS < 4) MAX_OPTS = 4; int c; // this algorithm is crap. for(c=0;c<MAX_OPTS;c++) { // Calculate the end speeds (of this move) we can use to meet the start speeds (of the next move). if(!join_moves(ends,starts) && c >= 1) break; temp = ends; ends = starts; starts = temp; } for(int ax=0;ax<NUM_AXES;ax++) { #ifdef DEBUG_OPT HOST.labelnum("OPTS:",c); HOST.labelnum("ASP[", ax, false); HOST.labelnum("]:", axisspeeds[ax],false); HOST.labelnum("->", endspeeds[ax],false); HOST.labelnum(", ", startspeeds[ax],false); HOST.labelnum("->", nextspeeds[ax], false); HOST.labelnum(" @jump:", AXES[ax].getStartFeed()); #endif if(labs(endspeeds[ax] - startspeeds[ax]) > AXES[ax].getStartFeed()) { if(labs(endspeeds[ax] - startspeeds[ax]) - AXES[ax].getStartFeed() > (float)AXES[ax].getStartFeed()) { #ifdef DEBUG_JUMP HOST.labelnum("JUMP EXCEED LINE ", gcode.linenum); #endif endspeeds[gcode.leading_axis] = AXES[gcode.leading_axis].getStartFeed() / 2; startspeeds[nextg.leading_axis] = AXES[nextg.leading_axis].getStartFeed() / 2; } } } // Speed at which the next move's leading axis can reach 0 during the next move. float speedto0 = AXES[nextg.leading_axis]. getSpeedAtEnd(AXES[nextg.leading_axis].getStartFeed()/2, nextg.accel, nextg.movesteps); // NOT TODO: startspeeds/endspeeds is signed previous to this operation but unsigned // after. Not important, though. // If the next move cannot reach 0, limit it so it can. if(speedto0 < labs(startspeeds[nextg.leading_axis])) { #ifdef DEBUG_LAME HOST.labelnum("st0beg:",speedto0); HOST.labelnum("beg:",startspeeds[nextg.leading_axis]); #endif startspeeds[nextg.leading_axis] = speedto0; } // now speedto0 is the maximum speed the primary in the next move can be going. We need the equivalent speed of the primary in this move. speedto0 = (speedto0 * gcode.axisratio[nextg.leading_axis]); // If this move isn't moving the primary of the next move, then speedto0 will come out to 0. // This is obviously incorrect... if(speedto0 < labs(endspeeds[gcode.leading_axis])) { #ifdef DEBUG_LAME HOST.labelnum("st0end:",speedto0); HOST.labelnum("end:",endspeeds[gcode.leading_axis]); #endif endspeeds[gcode.leading_axis] = speedto0; } #ifdef DEBUG_LAME HOST.labelnum("ef: ", gcode.endfeed, false); HOST.labelnum("sf: ", gcode.startfeed, false); #endif gcode.endfeed = max(gcode.endfeed,labs(endspeeds[gcode.leading_axis])); nextg.startfeed = max(nextg.startfeed,labs(startspeeds[nextg.leading_axis])); // because we only lookahead one move, our maximum exit speed has to be either the desired // exit speed or the speed that the next move can reach to 0 (0+jerk, actually) during. computeAccel(gcode, &nextg); #endif // LOOKAHEAD }
void Plane::update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ uint16_t radius = 0; switch(control_mode) { case AUTO: if (home_is_set != HOME_UNSET) { mission.update(); } break; case RTL: if (quadplane.available() && quadplane.rtl_mode == 1 && nav_controller->reached_loiter_target()) { set_mode(QRTL, MODE_REASON_UNKNOWN); break; } else if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO set_mode(AUTO, MODE_REASON_UNKNOWN); } // prevent running the expensive jump_to_landing_sequence // on every loop auto_state.checked_for_autoland = true; } else if (g.rtl_autoland == 2 && !auto_state.checked_for_autoland) { // Go directly to the landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO set_mode(AUTO, MODE_REASON_UNKNOWN); } // prevent running the expensive jump_to_landing_sequence // on every loop auto_state.checked_for_autoland = true; } radius = abs(g.rtl_radius); if (radius > 0) { loiter.direction = (g.rtl_radius < 0) ? -1 : 1; } // no break, fall through to LOITER case LOITER: case AVOID_ADSB: case GUIDED: update_loiter(radius); break; case CRUISE: update_cruise(); break; case MANUAL: case STABILIZE: case TRAINING: case INITIALISING: case ACRO: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: // nothing to do break; } }
// Set up a real move. Return true if it represents real movement, else false. bool DDA::Init(const GCodes::RawMove *nextMove, bool doMotorMapping) { // 1. Compute the new endpoints and the movement vector const int32_t *positionNow = prev->DriveCoordinates(); const Move *move = reprap.GetMove(); if (doMotorMapping) { move->MotorTransform(nextMove->coords, endPoint); // transform the axis coordinates if on a delta or CoreXY printer isDeltaMovement = move->IsDeltaMode() && (endPoint[X_AXIS] != positionNow[X_AXIS] || endPoint[Y_AXIS] != positionNow[Y_AXIS] || endPoint[Z_AXIS] != positionNow[Z_AXIS]); } else { isDeltaMovement = false; } isPrintingMove = false; bool realMove = false, xyMoving = false; const bool isSpecialDeltaMove = (move->IsDeltaMode() && !doMotorMapping); float accelerations[DRIVES]; const float *normalAccelerations = reprap.GetPlatform()->Accelerations(); for (size_t drive = 0; drive < DRIVES; drive++) { accelerations[drive] = normalAccelerations[drive]; if (drive >= AXES || !doMotorMapping) { endPoint[drive] = Move::MotorEndPointToMachine(drive, nextMove->coords[drive]); } int32_t delta; if (drive < AXES) { endCoordinates[drive] = nextMove->coords[drive]; delta = endPoint[drive] - positionNow[drive]; } else { delta = endPoint[drive]; } DriveMovement& dm = ddm[drive]; if (drive < AXES && !isSpecialDeltaMove) { directionVector[drive] = nextMove->coords[drive] - prev->GetEndCoordinate(drive, false); dm.state = (isDeltaMovement || delta != 0) ? DMState::moving // on a delta printer, if one tower moves then we assume they all do : DMState::idle; } else { directionVector[drive] = (float)delta/reprap.GetPlatform()->DriveStepsPerUnit(drive); dm.state = (delta != 0) ? DMState::moving : DMState::idle; } if (dm.state == DMState::moving) { dm.totalSteps = labs(delta); // for now this is the number of net steps, but gets adjusted later if there is a reverse in direction dm.direction = (delta >= 0); // for now this is the direction of net movement, but gets adjusted later if it is a delta movement realMove = true; if (drive < Z_AXIS) { xyMoving = true; } if (drive >= AXES && xyMoving) { if (delta > 0) { isPrintingMove = true; // we have both movement and extrusion } float compensationTime = reprap.GetPlatform()->GetElasticComp(drive); if (compensationTime > 0.0) { // Compensation causes instant velocity changes equal to acceleration * k, so we may need to limit the acceleration accelerations[drive] = min<float>(accelerations[drive], reprap.GetPlatform()->ConfiguredInstantDv(drive)/compensationTime); } } } } // 2. Throw it away if there's no real movement. if (!realMove) { return false; } // 3. Store some values endStopsToCheck = nextMove->endStopsToCheck; filePos = nextMove->filePos; usePressureAdvance = nextMove->usePressureAdvance; // The end coordinates will be valid at the end of this move if it does not involve endstop checks and is not a special move on a delta printer endCoordinatesValid = (endStopsToCheck == 0) && (doMotorMapping || !move->IsDeltaMode()); // 4. Normalise the direction vector and compute the amount of motion. // If there is any XYZ movement, then we normalise it so that the total XYZ movement has unit length. // This means that the user gets the feed rate that he asked for. It also makes the delta calculations simpler. if (xyMoving || ddm[Z_AXIS].state == DMState::moving) { totalDistance = Normalise(directionVector, DRIVES, AXES); if (isDeltaMovement) { // The following are only needed when doing delta movements. We could defer computing them until Prepare(), which would make simulation faster. a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc); const DeltaParameters& dparams = move->GetDeltaParams(); const float initialX = prev->GetEndCoordinate(X_AXIS, false); const float initialY = prev->GetEndCoordinate(Y_AXIS, false); const float diagonalSquared = fsquare(dparams.GetDiagonal()); const float a2b2D2 = a2plusb2 * diagonalSquared; for (size_t drive = 0; drive < AXES; ++drive) { const float A = initialX - dparams.GetTowerX(drive); const float B = initialY - dparams.GetTowerY(drive); const float stepsPerMm = reprap.GetPlatform()->DriveStepsPerUnit(drive); DriveMovement& dm = ddm[drive]; const float aAplusbB = A * directionVector[X_AXIS] + B * directionVector[Y_AXIS]; const float dSquaredMinusAsquaredMinusBsquared = diagonalSquared - fsquare(A) - fsquare(B); float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); dm.mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2); dm.mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2); dm.mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = (int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); // Calculate the distance at which we need to reverse direction. if (a2plusb2 <= 0.0) { // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. dm.direction = (directionVector[Z_AXIS] >= 0.0); dm.mp.delta.reverseStartStep = dm.totalSteps + 1; } else { // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being above the bed, // the other root corresponds to the carriages being above the bed. const float drev = ((directionVector[Z_AXIS] * sqrt(a2b2D2 - fsquare(A * directionVector[Y_AXIS] - B * directionVector[X_AXIS]))) - aAplusbB)/a2plusb2; if (drev > 0.0 && drev < totalDistance) // if the reversal point is within range { // Calculate how many steps we need to move up before reversing float hrev = directionVector[Z_AXIS] * drev + sqrt(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev)); int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); // We may be almost at the peak height already, in which case we don't really have a reversal. // We must not set reverseStartStep to 1, because then we would set the direction when Prepare() calls CalcStepTime(), before the previous move finishes. if (numStepsUp < 1 || (dm.direction && (uint32_t)numStepsUp <= dm.totalSteps)) { dm.mp.delta.reverseStartStep = dm.totalSteps + 1; } else { dm.mp.delta.reverseStartStep = (uint32_t)numStepsUp + 1; // Correct the initial direction and the total number of steps if (dm.direction) { // Net movement is up, so we will go up a bit and then down by a lesser amount dm.totalSteps = (2 * numStepsUp) - dm.totalSteps; } else { // Net movement is down, so we will go up first and then down by a greater amount dm.direction = true; dm.totalSteps = (2 * numStepsUp) + dm.totalSteps; } } } else { dm.mp.delta.reverseStartStep = dm.totalSteps + 1; } } } } } else { totalDistance = Normalise(directionVector, DRIVES, DRIVES); } // 5. Compute the maximum acceleration available and maximum top speed float normalisedDirectionVector[DRIVES]; // Used to hold a unit-length vector in the direction of motion memcpy(normalisedDirectionVector, directionVector, sizeof(normalisedDirectionVector)); Absolute(normalisedDirectionVector, DRIVES); acceleration = VectorBoxIntersection(normalisedDirectionVector, accelerations, DRIVES); // Set the speed to the smaller of the requested and maximum speed. // Also enforce a minimum speed of 0.5mm/sec. We need a minimum speed to avoid overflow in the movement calculations. float reqSpeed = nextMove->feedRate; if (isSpecialDeltaMove) { // Special case of a raw or homing move on a delta printer // We use the Cartesian motion system to implement these moves, so the feed rate will be interpreted in Cartesian coordinates. // This is wrong, we want the feed rate to apply to the drive that is moving the farthest. float maxDistance = 0.0; for (size_t axis = 0; axis < AXES; ++axis) { if (normalisedDirectionVector[axis] > maxDistance) { maxDistance = normalisedDirectionVector[axis]; } } if (maxDistance != 0.0) // should always be true { reqSpeed /= maxDistance; // because normalisedDirectionVector is unit-normalised } } requestedSpeed = max<float>(0.5, min<float>(reqSpeed, VectorBoxIntersection(normalisedDirectionVector, reprap.GetPlatform()->MaxFeedrates(), DRIVES))); // On a Cartesian or CoreXY printer, it is OK to limit the X and Y speeds and accelerations independently, and in consequence to allow greater values // for diagonal moves. On a delta, this is not OK and any movement in the XY plane should be limited to the X/Y axis values, which we assume to be equal. if (isDeltaMovement) { const float xyFactor = sqrt(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[X_AXIS])); const float maxSpeed = reprap.GetPlatform()->MaxFeedrates()[X_AXIS]; if (requestedSpeed * xyFactor > maxSpeed) { requestedSpeed = maxSpeed/xyFactor; } const float maxAcceleration = normalAccelerations[X_AXIS]; if (acceleration * xyFactor > maxAcceleration) { acceleration = maxAcceleration/xyFactor; } } // 6. Calculate the provisional accelerate and decelerate distances and the top speed endSpeed = 0.0; // until the next move asks us to adjust it if (prev->state != provisional) { // There is no previous move that we can adjust, so this move must start at zero speed. startSpeed = 0.0; } else { // Try to meld this move to the previous move to avoid stop/start // Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as float maxStartSpeed = sqrtf(acceleration * totalDistance * 2.0); prev->targetNextSpeed = min<float>(maxStartSpeed, requestedSpeed); DoLookahead(prev); startSpeed = prev->targetNextSpeed; } RecalculateMove(); state = provisional; return true; }
int rrd_fetch_fn( const char *filename, /* name of the rrd */ enum cf_en cf_idx, /* which consolidation function ? */ time_t *start, time_t *end, /* which time frame do you want ? * will be changed to represent reality */ unsigned long *step, /* which stepsize do you want? * will be changed to represent reality */ unsigned long *ds_cnt, /* number of data sources in file */ char ***ds_namv, /* names of data_sources */ rrd_value_t **data) { /* two dimensional array containing the data */ long i, ii; time_t cal_start, cal_end, rra_start_time, rra_end_time; long best_full_rra = 0, best_part_rra = 0, chosen_rra = 0, rra_pointer = 0; long best_full_step_diff = 0, best_part_step_diff = 0, tmp_step_diff = 0, tmp_match = 0, best_match = 0; long full_match, rra_base; off_t start_offset, end_offset; int first_full = 1; int first_part = 1; rrd_t rrd; rrd_file_t *rrd_file; rrd_value_t *data_ptr; unsigned long rows; #ifdef DEBUG fprintf(stderr, "Entered rrd_fetch_fn() searching for the best match\n"); fprintf(stderr, "Looking for: start %10lu end %10lu step %5lu\n", *start, *end, *step); #endif #ifdef HAVE_LIBDBI /* handle libdbi datasources */ if (strncmp("sql//",filename,5)==0) { return rrd_fetch_fn_libdbi(filename,cf_idx,start,end,step,ds_cnt,ds_namv,data); } #endif rrd_init(&rrd); rrd_file = rrd_open(filename, &rrd, RRD_READONLY); if (rrd_file == NULL) goto err_free; /* when was the really last update of this file ? */ if (((*ds_namv) = (char **) malloc(rrd.stat_head->ds_cnt * sizeof(char *))) == NULL) { rrd_set_error("malloc fetch ds_namv array"); goto err_close; } for (i = 0; (unsigned long) i < rrd.stat_head->ds_cnt; i++) { if ((((*ds_namv)[i]) = (char*)malloc(sizeof(char) * DS_NAM_SIZE)) == NULL) { rrd_set_error("malloc fetch ds_namv entry"); goto err_free_ds_namv; } strncpy((*ds_namv)[i], rrd.ds_def[i].ds_nam, DS_NAM_SIZE - 1); (*ds_namv)[i][DS_NAM_SIZE - 1] = '\0'; } /* find the rra which best matches the requirements */ for (i = 0; (unsigned) i < rrd.stat_head->rra_cnt; i++) { if (cf_conv(rrd.rra_def[i].cf_nam) == cf_idx) { cal_end = (rrd.live_head->last_up - (rrd.live_head->last_up % (rrd.rra_def[i].pdp_cnt * rrd.stat_head-> pdp_step))); cal_start = (cal_end - (rrd.rra_def[i].pdp_cnt * rrd.rra_def[i].row_cnt * rrd.stat_head->pdp_step)); full_match = *end - *start; #ifdef DEBUG fprintf(stderr, "Considering: start %10lu end %10lu step %5lu ", cal_start, cal_end, rrd.stat_head->pdp_step * rrd.rra_def[i].pdp_cnt); #endif /* we need step difference in either full or partial case */ tmp_step_diff = labs(*step - (rrd.stat_head->pdp_step * rrd.rra_def[i].pdp_cnt)); /* best full match */ if (cal_start <= *start) { if (first_full || (tmp_step_diff < best_full_step_diff)) { first_full = 0; best_full_step_diff = tmp_step_diff; best_full_rra = i; #ifdef DEBUG fprintf(stderr, "best full match so far\n"); } else { fprintf(stderr, "full match, not best\n"); #endif } } else { /* best partial match */ tmp_match = full_match; if (cal_start > *start) tmp_match -= (cal_start - *start); if (first_part || (best_match < tmp_match) || (best_match == tmp_match && tmp_step_diff < best_part_step_diff)) { #ifdef DEBUG fprintf(stderr, "best partial so far\n"); #endif first_part = 0; best_match = tmp_match; best_part_step_diff = tmp_step_diff; best_part_rra = i; } else { #ifdef DEBUG fprintf(stderr, "partial match, not best\n"); #endif } } } } /* lets see how the matching went. */ if (first_full == 0) chosen_rra = best_full_rra; else if (first_part == 0) chosen_rra = best_part_rra; else { rrd_set_error ("the RRD does not contain an RRA matching the chosen CF"); goto err_free_all_ds_namv; } /* set the wish parameters to their real values */ *step = rrd.stat_head->pdp_step * rrd.rra_def[chosen_rra].pdp_cnt; *start -= (*start % *step); *end += (*step - *end % *step); rows = (*end - *start) / *step + 1; #ifdef DEBUG fprintf(stderr, "We found: start %10lu end %10lu step %5lu rows %lu\n", *start, *end, *step, rows); #endif /* Start and end are now multiples of the step size. The amount of ** steps we want is (end-start)/step and *not* an extra one. ** Reasoning: if step is s and we want to graph from t to t+s, ** we need exactly ((t+s)-t)/s rows. The row to collect from the ** database is the one with time stamp (t+s) which means t to t+s. */ *ds_cnt = rrd.stat_head->ds_cnt; if (((*data) = (rrd_value_t*)malloc(*ds_cnt * rows * sizeof(rrd_value_t))) == NULL) { rrd_set_error("malloc fetch data area"); goto err_free_all_ds_namv; } data_ptr = (*data); /* find base address of rra */ rra_base = rrd_file->header_len; for (i = 0; i < chosen_rra; i++) rra_base += (*ds_cnt * rrd.rra_def[i].row_cnt * sizeof(rrd_value_t)); /* find start and end offset */ rra_end_time = (rrd.live_head->last_up - (rrd.live_head->last_up % *step)); rra_start_time = (rra_end_time - (*step * (rrd.rra_def[chosen_rra].row_cnt - 1))); /* here's an error by one if we don't be careful */ start_offset = ((long long) *start + (long long)*step - (long long)rra_start_time) / (long long) *step; end_offset = ((long long) rra_end_time - (long long)*end) / (long long) *step; #ifdef DEBUG fprintf(stderr, "rra_start %lu, rra_end %lu, start_off %li, end_off %li\n", rra_start_time, rra_end_time, start_offset, end_offset); #endif /* only seek if the start time is before the end time */ if (*start <= rra_end_time && *end >= rra_start_time - (off_t)*step ){ if (start_offset <= 0) rra_pointer = rrd.rra_ptr[chosen_rra].cur_row + 1; else rra_pointer = rrd.rra_ptr[chosen_rra].cur_row + 1 + start_offset; rra_pointer = rra_pointer % (signed) rrd.rra_def[chosen_rra].row_cnt; if (rrd_seek(rrd_file, (rra_base + (rra_pointer * (*ds_cnt) * sizeof(rrd_value_t))), SEEK_SET) != 0) { rrd_set_error("seek error in RRA"); goto err_free_data; } #ifdef DEBUG fprintf(stderr, "First Seek: rra_base %lu rra_pointer %lu\n", rra_base, rra_pointer); #endif } /* step trough the array */ for (i = start_offset; i < (signed) rrd.rra_def[chosen_rra].row_cnt - end_offset; i++) { /* no valid data yet */ if (i < 0) { #ifdef DEBUG fprintf(stderr, "pre fetch %li -- ", i); #endif for (ii = 0; (unsigned) ii < *ds_cnt; ii++) { *(data_ptr++) = DNAN; #ifdef DEBUG fprintf(stderr, "%10.2f ", *(data_ptr - 1)); #endif } } /* past the valid data area */ else if (i >= (signed) rrd.rra_def[chosen_rra].row_cnt) { #ifdef DEBUG fprintf(stderr, "past fetch %li -- ", i); #endif for (ii = 0; (unsigned) ii < *ds_cnt; ii++) { *(data_ptr++) = DNAN; #ifdef DEBUG fprintf(stderr, "%10.2f ", *(data_ptr - 1)); #endif } } else { /* OK we are inside the valid area but the pointer has to * be wrapped*/ if (rra_pointer >= (signed) rrd.rra_def[chosen_rra].row_cnt) { rra_pointer -= rrd.rra_def[chosen_rra].row_cnt; if (rrd_seek(rrd_file, (rra_base + rra_pointer * (*ds_cnt) * sizeof(rrd_value_t)), SEEK_SET) != 0) { rrd_set_error("wrap seek in RRA did fail"); goto err_free_data; } #ifdef DEBUG fprintf(stderr, "wrap seek ...\n"); #endif } if (rrd_read(rrd_file, data_ptr, sizeof(rrd_value_t) * (*ds_cnt)) != (ssize_t) (sizeof(rrd_value_t) * (*ds_cnt))) { rrd_set_error("fetching cdp from rra"); goto err_free_data; } #ifdef DEBUG fprintf(stderr, "post fetch %li -- ", i); for (ii = 0; ii < *ds_cnt; ii++) fprintf(stderr, "%10.2f ", *(data_ptr + ii)); #endif data_ptr += *ds_cnt; rra_pointer++; } #ifdef DEBUG fprintf(stderr, "\n"); #endif } rrd_close(rrd_file); rrd_free(&rrd); return (0); err_free_data: free(*data); *data = NULL; err_free_all_ds_namv: for (i = 0; (unsigned long) i < rrd.stat_head->ds_cnt; ++i) free((*ds_namv)[i]); err_free_ds_namv: free(*ds_namv); err_close: rrd_close(rrd_file); err_free: rrd_free(&rrd); return (-1); }
// internal function: to avoid exif blob reading + 8-bit byteorder flag + high-quality override int dt_imageio_export_with_flags(const uint32_t imgid, const char *filename, dt_imageio_module_format_t *format, dt_imageio_module_data_t *format_params, const int32_t ignore_exif, const int32_t display_byteorder, const gboolean high_quality, const gboolean upscale, const int32_t thumbnail_export, const char *filter, const gboolean copy_metadata, dt_imageio_module_storage_t *storage, dt_imageio_module_data_t *storage_params, int num, int total) { dt_develop_t dev; dt_dev_init(&dev, 0); dt_dev_load_image(&dev, imgid); const int buf_is_downscaled = (thumbnail_export && dt_conf_get_bool("plugins/lighttable/low_quality_thumbnails")); dt_mipmap_buffer_t buf; if(buf_is_downscaled) dt_mipmap_cache_get(darktable.mipmap_cache, &buf, imgid, DT_MIPMAP_F, DT_MIPMAP_BLOCKING, 'r'); else dt_mipmap_cache_get(darktable.mipmap_cache, &buf, imgid, DT_MIPMAP_FULL, DT_MIPMAP_BLOCKING, 'r'); const dt_image_t *img = &dev.image_storage; if(!buf.buf || !buf.width || !buf.height) { fprintf(stderr, "allocation failed???\n"); dt_control_log(_("image `%s' is not available!"), img->filename); goto error_early; } const int wd = img->width; const int ht = img->height; const float max_scale = upscale ? 100.0 : 1.0; int res = 0; dt_times_t start; dt_get_times(&start); dt_dev_pixelpipe_t pipe; res = thumbnail_export ? dt_dev_pixelpipe_init_thumbnail(&pipe, wd, ht) : dt_dev_pixelpipe_init_export(&pipe, wd, ht, format->levels(format_params)); if(!res) { dt_control_log( _("failed to allocate memory for %s, please lower the threads used for export or buy more memory."), thumbnail_export ? C_("noun", "thumbnail export") : C_("noun", "export")); goto error; } // If a style is to be applied during export, add the iop params into the history if(!thumbnail_export && format_params->style[0] != '\0') { GList *stls; dt_iop_module_t *m = NULL; if((stls = dt_styles_get_item_list(format_params->style, TRUE, -1)) == 0) { dt_control_log(_("cannot find the style '%s' to apply during export."), format_params->style); goto error; } // remove everything above history_end GList *history = g_list_nth(dev.history, dev.history_end); while(history) { GList *next = g_list_next(history); dt_dev_history_item_t *hist = (dt_dev_history_item_t *)(history->data); free(hist->params); free(hist->blend_params); free(history->data); dev.history = g_list_delete_link(dev.history, history); history = next; } // Add each params while(stls) { dt_style_item_t *s = (dt_style_item_t *)stls->data; gboolean module_found = FALSE; GList *modules = dev.iop; while(modules) { m = (dt_iop_module_t *)modules->data; // since the name in the style is returned with a possible multi-name, just check the start of the // name if(strncmp(m->op, s->name, strlen(m->op)) == 0) { dt_dev_history_item_t *h = malloc(sizeof(dt_dev_history_item_t)); dt_iop_module_t *sty_module = m; if(format_params->style_append && !(m->flags() & IOP_FLAGS_ONE_INSTANCE)) { sty_module = dt_dev_module_duplicate(m->dev, m, 0); if(!sty_module) { free(h); goto error; } } h->params = s->params; h->blend_params = s->blendop_params; h->enabled = s->enabled; h->module = sty_module; h->multi_priority = 1; g_strlcpy(h->multi_name, "<style>", sizeof(h->multi_name)); if(m->legacy_params && (s->module_version != m->version())) { void *new_params = malloc(m->params_size); m->legacy_params(m, h->params, s->module_version, new_params, labs(m->version())); free(h->params); h->params = new_params; } dev.history_end++; dev.history = g_list_append(dev.history, h); module_found = TRUE; g_free(s->name); break; } modules = g_list_next(modules); } if(!module_found) dt_style_item_free(s); stls = g_list_next(stls); } g_list_free(stls); } dt_dev_pixelpipe_set_input(&pipe, &dev, (float *)buf.buf, buf.width, buf.height, buf.iscale); dt_dev_pixelpipe_create_nodes(&pipe, &dev); dt_dev_pixelpipe_synch_all(&pipe, &dev); if(filter) { if(!strncmp(filter, "pre:", 4)) dt_dev_pixelpipe_disable_after(&pipe, filter + 4); if(!strncmp(filter, "post:", 5)) dt_dev_pixelpipe_disable_before(&pipe, filter + 5); } dt_dev_pixelpipe_get_dimensions(&pipe, &dev, pipe.iwidth, pipe.iheight, &pipe.processed_width, &pipe.processed_height); dt_show_times(&start, "[export] creating pixelpipe", NULL); // find output color profile for this image: int sRGB = 1; int icctype = dt_conf_get_int("plugins/lighttable/export/icctype"); if(icctype == DT_COLORSPACE_SRGB) { sRGB = 1; } else if(icctype == DT_COLORSPACE_NONE) { GList *modules = dev.iop; dt_iop_module_t *colorout = NULL; while(modules) { colorout = (dt_iop_module_t *)modules->data; if(colorout->get_p && strcmp(colorout->op, "colorout") == 0) { const dt_colorspaces_color_profile_type_t *type = colorout->get_p(colorout->params, "type"); sRGB = (!type || *type == DT_COLORSPACE_SRGB); break; // colorout can't have > 1 instance } modules = g_list_next(modules); } } else { sRGB = 0; } // get only once at the beginning, in case the user changes it on the way: const gboolean high_quality_processing = ((format_params->max_width == 0 || format_params->max_width >= pipe.processed_width) && (format_params->max_height == 0 || format_params->max_height >= pipe.processed_height)) ? FALSE : high_quality; const int width = format_params->max_width; const int height = format_params->max_height; const double scalex = width > 0 ? fminf(width / (double)pipe.processed_width, max_scale) : 1.0; const double scaley = height > 0 ? fminf(height / (double)pipe.processed_height, max_scale) : 1.0; const double scale = fminf(scalex, scaley); const int processed_width = scale * pipe.processed_width + .5f; const int processed_height = scale * pipe.processed_height + .5f; const int bpp = format->bpp(format_params); dt_get_times(&start); if(high_quality_processing) { /* * if high quality processing was requested, downsampling will be done * at the very end of the pipe (just before border and watermark) */ dt_dev_pixelpipe_process_no_gamma(&pipe, &dev, 0, 0, processed_width, processed_height, scale); } else { // else, downsampling will be right after demosaic // so we need to turn temporarily disable in-pipe late downsampling iop. // find the finalscale module dt_dev_pixelpipe_iop_t *finalscale = NULL; { GList *nodes = g_list_last(pipe.nodes); while(nodes) { dt_dev_pixelpipe_iop_t *node = (dt_dev_pixelpipe_iop_t *)(nodes->data); if(!strcmp(node->module->op, "finalscale")) { finalscale = node; break; } nodes = g_list_previous(nodes); } } if(finalscale) finalscale->enabled = 0; // do the processing (8-bit with special treatment, to make sure we can use openmp further down): if(bpp == 8) dt_dev_pixelpipe_process(&pipe, &dev, 0, 0, processed_width, processed_height, scale); else dt_dev_pixelpipe_process_no_gamma(&pipe, &dev, 0, 0, processed_width, processed_height, scale); if(finalscale) finalscale->enabled = 1; } dt_show_times(&start, thumbnail_export ? "[dev_process_thumbnail] pixel pipeline processing" : "[dev_process_export] pixel pipeline processing", NULL); uint8_t *outbuf = pipe.backbuf; // downconversion to low-precision formats: if(bpp == 8) { if(display_byteorder) { if(high_quality_processing) { const float *const inbuf = (float *)outbuf; for(size_t k = 0; k < (size_t)processed_width * processed_height; k++) { // convert in place, this is unfortunately very serial.. const uint8_t r = CLAMP(inbuf[4 * k + 2] * 0xff, 0, 0xff); const uint8_t g = CLAMP(inbuf[4 * k + 1] * 0xff, 0, 0xff); const uint8_t b = CLAMP(inbuf[4 * k + 0] * 0xff, 0, 0xff); outbuf[4 * k + 0] = r; outbuf[4 * k + 1] = g; outbuf[4 * k + 2] = b; } } // else processing output was 8-bit already, and no need to swap order } else // need to flip { // ldr output: char if(high_quality_processing) { const float *const inbuf = (float *)outbuf; for(size_t k = 0; k < (size_t)processed_width * processed_height; k++) { // convert in place, this is unfortunately very serial.. const uint8_t r = CLAMP(inbuf[4 * k + 0] * 0xff, 0, 0xff); const uint8_t g = CLAMP(inbuf[4 * k + 1] * 0xff, 0, 0xff); const uint8_t b = CLAMP(inbuf[4 * k + 2] * 0xff, 0, 0xff); outbuf[4 * k + 0] = r; outbuf[4 * k + 1] = g; outbuf[4 * k + 2] = b; } } else { // !display_byteorder, need to swap: uint8_t *const buf8 = pipe.backbuf; #ifdef _OPENMP #pragma omp parallel for default(none) schedule(static) #endif // just flip byte order for(size_t k = 0; k < (size_t)processed_width * processed_height; k++) { uint8_t tmp = buf8[4 * k + 0]; buf8[4 * k + 0] = buf8[4 * k + 2]; buf8[4 * k + 2] = tmp; } } } } else if(bpp == 16) { // uint16_t per color channel float *buff = (float *)outbuf; uint16_t *buf16 = (uint16_t *)outbuf; for(int y = 0; y < processed_height; y++) for(int x = 0; x < processed_width; x++) { // convert in place const size_t k = (size_t)processed_width * y + x; for(int i = 0; i < 3; i++) buf16[4 * k + i] = CLAMP(buff[4 * k + i] * 0x10000, 0, 0xffff); } } // else output float, no further harm done to the pixels :) format_params->width = processed_width; format_params->height = processed_height; if(!ignore_exif) { int length; uint8_t *exif_profile = NULL; // Exif data should be 65536 bytes max, but if original size is close to that, // adding new tags could make it go over that... so let it be and see what // happens when we write the image char pathname[PATH_MAX] = { 0 }; gboolean from_cache = TRUE; dt_image_full_path(imgid, pathname, sizeof(pathname), &from_cache); // last param is dng mode, it's false here length = dt_exif_read_blob(&exif_profile, pathname, imgid, sRGB, processed_width, processed_height, 0); res = format->write_image(format_params, filename, outbuf, exif_profile, length, imgid, num, total); free(exif_profile); } else { res = format->write_image(format_params, filename, outbuf, NULL, 0, imgid, num, total); } dt_dev_pixelpipe_cleanup(&pipe); dt_dev_cleanup(&dev); dt_mipmap_cache_release(darktable.mipmap_cache, &buf); /* now write xmp into that container, if possible */ if(copy_metadata && (format->flags(format_params) & FORMAT_FLAGS_SUPPORT_XMP)) { dt_exif_xmp_attach(imgid, filename); // no need to cancel the export if this fail } if(!thumbnail_export && strcmp(format->mime(format_params), "memory") && !(format->flags(format_params) & FORMAT_FLAGS_NO_TMPFILE)) { dt_control_signal_raise(darktable.signals, DT_SIGNAL_IMAGE_EXPORT_TMPFILE, imgid, filename, format, format_params, storage, storage_params); } return res; error: dt_dev_pixelpipe_cleanup(&pipe); error_early: dt_dev_cleanup(&dev); dt_mipmap_cache_release(darktable.mipmap_cache, &buf); return 1; }
/* * Rtt_tol is the fractional tollerance for RTT comparisons. * If a later (further down the list) host's RTT is less than * 1/Rtt_tol better than my current best then I don't bother * with it. This biases me towards entries at the top of the list * which Active Directory has already chosen for me and prevents * noise in RTTs from pushing me to more distant machines. */ static int remap(Dfscache *cp, Refer *re) { int n; long rtt; char *p, *a[4]; enum { Hostname = 1, Sharename = 2, Pathname = 3, Rtt_tol = 10 }; if(Debug && strstr(Debug, "dfs") != nil) print(" remap %s\n", re->addr); for(p = re->addr; *p; p++) if(*p == '\\') *p = '/'; if(cp->prox < re->prox){ if(Debug && strstr(Debug, "dfs") != nil) print(" remap %d < %d\n", cp->prox, re->prox); return -1; } if((n = getfields(re->addr, a, sizeof(a), 0, "/")) < 3){ if(Debug && strstr(Debug, "dfs") != nil) print(" remap nfields=%d\n", n); return -1; } if((rtt = ping(a[Hostname], Dfstout)) == -1){ if(Debug && strstr(Debug, "dfs") != nil) print(" remap ping failed\n"); return -1; } if(cp->rtt < rtt && (rtt/labs(rtt-cp->rtt)) < Rtt_tol){ if(Debug && strstr(Debug, "dfs") != nil) print(" remap bad ping %ld < %ld && %ld < %d\n", cp->rtt, rtt, (rtt/labs(rtt-cp->rtt)), Rtt_tol); return -1; } if(n < 4) a[Pathname] = ""; if(re->ttl == 0) re->ttl = 60*5; free(cp->host); free(cp->share); free(cp->path); cp->rtt = rtt; cp->prox = re->prox; cp->expiry = time(nil)+re->ttl; cp->host = estrdup9p(a[Hostname]); cp->share = estrdup9p(trimshare(a[Sharename])); cp->path = estrdup9p(a[Pathname]); if(Debug && strstr(Debug, "dfs") != nil) print(" remap ping OK prox=%d host=%s share=%s path=%s\n", cp->prox, cp->host, cp->share, cp->path); return 0; }
/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. Disable throttle if following conditions are met: * 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher * AND * 2 - Our reported altitude is within 10 meters of the home altitude. * 3 - Our reported speed is under 5 meters per second. * 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached * OR * 5 - Home location is not set * OR * 6- Landing does not want to allow throttle */ bool Plane::suppress_throttle(void) { #if PARACHUTE == ENABLED if (auto_throttle_mode && parachute.release_initiated()) { // throttle always suppressed in auto-throttle modes after parachute release initiated throttle_suppressed = true; return true; } #endif if (landing.is_throttle_suppressed()) { return true; } if (!throttle_suppressed) { // we've previously met a condition for unsupressing the throttle return false; } if (!auto_throttle_mode) { // the user controls the throttle throttle_suppressed = false; return false; } if (control_mode == &mode_auto && g.auto_fbw_steer == 42) { // user has throttle control return false; } bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5); if (control_mode == &mode_auto && auto_state.takeoff_complete == false) { uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000; if (is_flying() && millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch gps_movement) { // definite gps movement // we're already flying, do not suppress the throttle. We can get // stuck in this condition if we reset a mission and cmd 1 is takeoff // but we're currently flying around below the takeoff altitude throttle_suppressed = false; return false; } if (auto_takeoff_check()) { // we're in auto takeoff throttle_suppressed = false; auto_state.baro_takeoff_alt = barometer.get_altitude(); return false; } // keep throttle suppressed return true; } if (fabsf(relative_altitude) >= 10.0f) { // we're more than 10m from the home altitude throttle_suppressed = false; return false; } if (gps_movement) { // if we have an airspeed sensor, then check it too, and // require 5m/s. This prevents throttle up due to spiky GPS // groundspeed with bad GPS reception if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { // we're moving at more than 5 m/s throttle_suppressed = false; return false; } } if (quadplane.is_flying()) { throttle_suppressed = false; return false; } // throttle remains suppressed return true; }
/*------------------------------------------------------*/ void ListToPc (char *s) { int i; lib *l; command *c; #ifndef WIN32 void heap_start(void); static long *p; char cc[32]; #endif uchar r[LINK_LEN],rr[16]; switch(*s) { case 'l': l=Ln; do { l->check=null; l=l->next; } while(l != Ln); c=CP; if(c) do { c->chk=null; c=c->next; } while(c != CP); SetTask(CheckScreen,0,RTC_task); SetTask(CheckLib,0,RTC_task); break; case 's': CheckScr=null; if(!CheckTask(CheckScreen,RTC_task)) CheckScreen(); else SetTask(CheckScreen,0,RTC_task); break; case 'o': sprintf(r,"=o %s,%d\r\n>",LinkAddrText(LinkAddr(OBP),rr),targets_xn(OBP)); puts_pc(r); break; case 'p': puts_pc("=p "); putsLLA(OBP); puts_pc("\r\n"); break; case 'm': for(i=0; _MenuCode[i]; ++i) { sprintf(r,"=m[%d] ",i); for(s=_MenuCode[i]; *s; ++s) if(*s == ' ') sprintf(strchr(r,0)," "); else { /* if(*s < ENTER) sprintf(strchr(r,0),"%c",*s); else */ sprintf(strchr(r,0),"%02X",*s); } sprintf(strchr(r,0),"\r\n"); puts_pc(r); } break; case 't': for(i=0; _TextCode[i]; ++i) { sprintf(r,"=t[%d] ",i); for(s=_TextCode[i]; *s; ++s) sprintf(strchr(r,0),"%02X",*s); sprintf(strchr(r,0),"\r\n"); puts_pc(r); } break; case 'h': for(i=0; _MenuHead[i]; ++i) { sprintf(r,"=h[%d] ",i); for(s=_MenuHead[i]; *s; ++s) sprintf(strchr(r,0),"%02X",*s); sprintf(strchr(r,0),"\r\n"); puts_pc(r); } break; #ifndef WIN32 case 'H': for(p=(long *)heap_start; *p; p=(long *)labs(*p)) { if(*p>0) sprintf(cc,"=H %08lX,%08lX,!\r\n",p,*p); else sprintf(cc,"=H %08lX,%08lX,-\r\n",p,-(*p)); puts_pc(cc); } break; #endif case 'f': ListPar(); break; case 'z': ListZone(NULL); break; case 'v': puts_pc(">=v"); SysMessage(CHPC); break; case 'G': WriteGunData(Guns,NULL); break; case 'D': ListDatum(); break; case 'e': shaft_enable(); break; default: ReadLib(s); break; } }
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame) void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf) { Vector3f angle_ef_error; float rate_change, rate_change_limit; // update the rate feed forward with angular acceleration limits if (_accel_roll_max > 0.0f) { rate_change_limit = _accel_roll_max * _dt; rate_change = roll_rate_bf - _rate_bf_desired.x; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.x += rate_change; } else { _rate_bf_desired.x = roll_rate_bf; } // update the rate feed forward with angular acceleration limits if (_accel_pitch_max > 0.0f) { rate_change_limit = _accel_pitch_max * _dt; rate_change = pitch_rate_bf - _rate_bf_desired.y; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.y += rate_change; } else { _rate_bf_desired.y = pitch_rate_bf; } if (_accel_yaw_max > 0.0f) { rate_change_limit = _accel_yaw_max * _dt; rate_change = yaw_rate_bf - _rate_bf_desired.z; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.z += rate_change; } else { _rate_bf_desired.z = yaw_rate_bf; } // Update angle error if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) { _acro_angle_switch = 6000; // convert body-frame rates to earth-frame rates frame_conversion_bf_to_ef(_rate_bf_desired, _rate_ef_desired); // update earth frame angle targets and errors update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); } else { _acro_angle_switch = 4500; integrate_bf_rate_error_to_angle_errors(); if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) { _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); } if (_angle_ef_target.y > 9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } if (_angle_ef_target.y < -9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } } // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // body-frame rate commands added _rate_bf_target += _rate_bf_desired; }
static const char *check_nonce(const char *buf, size_t len) { char *nonce = find_header(buf, len, "nonce"); unsigned long stamp, ostamp; char *bohmac, *expect = NULL; const char *retval = NONCE_BAD; if (!nonce) { retval = NONCE_MISSING; goto leave; } else if (!push_cert_nonce) { retval = NONCE_UNSOLICITED; goto leave; } else if (!strcmp(push_cert_nonce, nonce)) { retval = NONCE_OK; goto leave; } if (!stateless_rpc) { /* returned nonce MUST match what we gave out earlier */ retval = NONCE_BAD; goto leave; } /* * In stateless mode, we may be receiving a nonce issued by * another instance of the server that serving the same * repository, and the timestamps may not match, but the * nonce-seed and dir should match, so we can recompute and * report the time slop. * * In addition, when a nonce issued by another instance has * timestamp within receive.certnonceslop seconds, we pretend * as if we issued that nonce when reporting to the hook. */ /* nonce is concat(<seconds-since-epoch>, "-", <hmac>) */ if (*nonce <= '0' || '9' < *nonce) { retval = NONCE_BAD; goto leave; } stamp = strtoul(nonce, &bohmac, 10); if (bohmac == nonce || bohmac[0] != '-') { retval = NONCE_BAD; goto leave; } expect = prepare_push_cert_nonce(service_dir, stamp); if (strcmp(expect, nonce)) { /* Not what we would have signed earlier */ retval = NONCE_BAD; goto leave; } /* * By how many seconds is this nonce stale? Negative value * would mean it was issued by another server with its clock * skewed in the future. */ ostamp = strtoul(push_cert_nonce, NULL, 10); nonce_stamp_slop = (long)ostamp - (long)stamp; if (nonce_stamp_slop_limit && labs(nonce_stamp_slop) <= nonce_stamp_slop_limit) { /* * Pretend as if the received nonce (which passes the * HMAC check, so it is not a forged by third-party) * is what we issued. */ free((void *)push_cert_nonce); push_cert_nonce = xstrdup(nonce); retval = NONCE_OK; } else { retval = NONCE_SLOP; } leave: free(nonce); free(expect); return retval; }
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration // calculation the caller must also provide the physical length of the line in millimeters. void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) { // The target position of the tool in absolute steps // Calculate target position in absolute steps int32_t target[3]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); // Calculate the buffer head after we push this byte uint8_t next_buffer_head = (block_buffer_head + 1); next_buffer_head = next_buffer_head % BLOCK_BUFFER_SIZE; // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. uint8_t bufferWasFull = block_buffer_tail == next_buffer_head; if( bufferWasFull) { //printPgmString(PSTR("::buf_full::\r\n")); } while(block_buffer_tail == next_buffer_head) { st_pause_wait_resume(); sleep_mode(); } if(bufferWasFull) { //printPgmString(PSTR("::not_full::\r\n")); } // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm)); uint32_t microseconds; if (!invert_feed_rate) { microseconds = lround((block->millimeters/feed_rate)*1000000); } else { microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate); } // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/microseconds; block->speed_x = delta_x_mm * multiplier; block->speed_y = delta_y_mm * multiplier; block->speed_z = delta_z_mm * multiplier; block->nominal_speed = block->millimeters * multiplier; block->nominal_rate = ceil(block->step_event_count * multiplier); block->entry_factor = 0.0; // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: double travel_per_step = block->millimeters/block->step_event_count; block->rate_delta = ceil( ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick travel_per_step); // convert to: acceleration steps/min/acceleration_tick if (acceleration_manager_enabled) { // compute a preliminary conservative acceleration trapezoid double safe_speed_factor = factor_for_safe_speed(block); calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); } else { block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; block->decelerate_after = block->step_event_count; block->rate_delta = 0; } // Compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); } if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); } // Move buffer head block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); }
void temp_sensor_tick() { temp_sensor_t i = 0; for (; i < NUM_TEMP_SENSORS; i++) { if (temp_sensors_runtime[i].next_read_time) { temp_sensors_runtime[i].next_read_time--; } else { uint16_t temp = 0; //time to deal with this temp sensor switch(temp_sensors[i].temp_type) { #ifdef TEMP_MAX6675 case TT_MAX6675: #ifdef PRR PRR &= ~MASK(PRSPI); #elif defined PRR0 PRR0 &= ~MASK(PRSPI); #endif SPCR = MASK(MSTR) | MASK(SPE) | MASK(SPR0); // enable TT_MAX6675 WRITE(SS, 0); // ensure 100ns delay - a bit extra is fine delay(1); // read MSB SPDR = 0; for (;(SPSR & MASK(SPIF)) == 0;); temp = SPDR; temp <<= 8; // read LSB SPDR = 0; for (;(SPSR & MASK(SPIF)) == 0;); temp |= SPDR; // disable TT_MAX6675 WRITE(SS, 1); temp_sensors_runtime[i].temp_flags = 0; if ((temp & 0x8002) == 0) { // got "device id" temp_sensors_runtime[i].temp_flags |= PRESENT; if (temp & 4) { // thermocouple open temp_sensors_runtime[i].temp_flags |= TCOPEN; } else { temp = temp >> 3; } } // this number depends on how frequently temp_sensor_tick is called. the MAX6675 can give a reading every 0.22s, so set this to about 250ms temp_sensors_runtime[i].next_read_time = 25; break; #endif /* TEMP_MAX6675 */ #ifdef TEMP_THERMISTOR case TT_THERMISTOR: do { uint8_t j; //Read current temperature temp = analog_read(temp_sensors[i].temp_pin); //Calculate real temperature based on lookup table for (j = 1; j < NUMTEMPS; j++) { if (pgm_read_word(&(temptable[j][0])) > temp) { // multiply by 4 because internal temp is stored as 14.2 fixed point temp = pgm_read_word(&(temptable[j][1])) * 4 + (temp - pgm_read_word(&(temptable[j-1][0]))) * 4 * (pgm_read_word(&(temptable[j][1])) - pgm_read_word(&(temptable[j-1][1]))) / (pgm_read_word(&(temptable[j][0])) - pgm_read_word(&(temptable[j-1][0]))); break; } } //Clamp for overflows if (j == NUMTEMPS) temp = temptable[NUMTEMPS-1][1] * 4; temp_sensors_runtime[i].next_read_time = 0; } while (0); break; #endif /* TEMP_THERMISTOR */ #ifdef TEMP_AD595 case TT_AD595: temp = analog_read(temp_pin); // convert // >>8 instead of >>10 because internal temp is stored as 14.2 fixed point temp = (temp * 500L) >> 8; temp_sensors_runtime[i].next_read_time = 0; break; #endif /* TEMP_AD595 */ #ifdef TEMP_PT100 case TT_PT100: #warning TODO: PT100 code break #endif /* TEMP_PT100 */ #ifdef TEMP_INTERCOM case TT_INTERCOM: temp = get_read_cmd() << 2; start_send(); temp_sensors_runtime[i].next_read_time = 0; break; #endif /* TEMP_INTERCOM */ #ifdef TEMP_DUMMY case TT_DUMMY: temp = temp_sensors_runtime[i].last_read_temp; if (temp_sensors_runtime[i].target_temp > temp) temp++; else if (temp_sensors_runtime[i].target_temp < temp) temp--; temp_sensors_runtime[i].next_read_time = 0; break; #endif /* TEMP_DUMMY */ } temp_sensors_runtime[i].last_read_temp = temp; if (labs(temp - temp_sensors_runtime[i].target_temp) < TEMP_HYSTERESIS) { if (temp_sensors_runtime[i].temp_residency < TEMP_RESIDENCY_TIME) temp_sensors_runtime[i].temp_residency++; } else { temp_sensors_runtime[i].temp_residency = 0; } if (temp_sensors[i].heater_index < NUM_HEATERS) { heater_tick(temp_sensors[i].heater_index, i, temp_sensors_runtime[i].last_read_temp, temp_sensors_runtime[i].target_temp); } } }
//检查球是否被机器人捕捉 //通过半径判断 bool Simulator::RobotBallCollisionRadius(SimRobot &r, SimBall &b, double dt) { static int iDribbleBy=-1; //球相对机器人坐标 MyVector2d ball_rel = b.pos - r.pos.p; //球相对机器人矢量旋转到X轴方向 MyVector2d robot_ball = ball_rel.rotate(-r.pos.dir); //球在机器人内部 //bool ball_in_front = (robot_ball.x > -20) && // (fabs(robot_ball.y) < 60); //球在机器人前方 //bool ball_on_front = ball_in_front && (robot_ball.x < 120); double dist = ball_rel.length(); //计算球相对机器人方向与机器人正方向的角度差 double da=anglemod(ball_rel.angle()-r.pos.dir); if(da>M_PI) { da-=M_2PI; } if(da<-M_PI) { da+=M_2PI; } //检查球与机器人是否碰撞 if (dist > robotHalfSize + BALL_RADIUS) { return (false); } bool bBounce=false; //if(ball_on_front) if(fabs(da)<M_PI/4) { if (r.vcmd.kick_power) { long kick_power=labs(r.vcmd.kick_power); if(kick_power>giShootPowerMax) { kick_power=giShootPowerMax; } b.vel.set(DVAR(OMNIBOT_KICKER_SPEED)*kick_power*1.0/giShootPowerMax,0); b.vel=b.vel.rotate(r.pos.dir); printf("Kicked=%d\r\n",r.vcmd.kick_power); b.pos.set(OMNIBOT_RADIUS+BALL_RADIUS+10,0); b.pos=b.pos.rotate(r.pos.dir); b.pos+= r.pos.p; return true; } else { //计算球相对机器人速度 MyVector2d brel = b.vel - r.vel.v; //计算相对速度在机器人正方向上投影 MyVector2d n(1,0); n = n.rotate(r.pos.dir); MyVector2d p = n.perp(); MyVector2d bdrib(dot(brel, n), dot(brel, p)); MyVector2d bpos=b.pos-r.pos.p; double bposx=dot(bpos, n); double bposy=dot(bpos, p); if(bposx<OMNIBOT_RADIUS-5) { b.vel.set(0,0); if(r.vcmd.dribble_power>0) { b.pos.set(OMNIBOT_RADIUS-18,0); } else { b.pos.set(OMNIBOT_RADIUS-18,bposy); } b.pos=b.pos.rotate(r.pos.dir); b.pos+= r.pos.p; //iDribbleBy=r.id; } //else if(bposx<OMNIBOT_RADIUS-5) //{ // b.pos.set(OMNIBOT_RADIUS-18,0); // b.pos=b.pos.rotate(r.pos.dir); // b.pos+= r.pos.p; //} //if(r.vcmd.dribble_power>0 && //if (bposx<OMNIBOT_RADIUS-5 && (bdrib.x < 0.001 || (r.vcmd.dribble_power>0) || iDribbleBy>=0) ) //{ // //printf("Dribble!!!!... %4.3f,%4.3f\n",bdrib.x,bdrib.y); // double dlimit; // dlimit = DVAR(OMNIBOT_DRIB_CATCH_SPEED)*3; // if (fabs(bdrib.x) < dlimit) // { // b.vel.x = 0; // } // else // { // // b.vel.x *= DVAR(OMNIBOT_DRIB_BOUNCE_DECAY); // } // //b.vel = r.vel.v.rotate(r.pos.dir); // //b.vel += r.vel.v; // b.vel.set(0,0); // b.pos.set(OMNIBOT_RADIUS-18,0); // b.pos=b.pos.rotate(r.pos.dir); // b.pos+= r.pos.p; // iDribbleBy=r.id; //} //else //{ // iDribbleBy=-1; //} return true; } } else { iDribbleBy=-1; bBounce=true; } if(!bBounce) { return true; } /* last final catch all check to prevent screw ups */ //如果机器人与球相撞,球被弹开到机器人半径加球半径的位置 b.pos = r.pos.p + ball_rel.norm() * (robotHalfSize + BALL_RADIUS); b.vel=r.vel.v.rotate(r.pos.dir)-b.vel; b.pos = b.pos + b.vel*dt; /* find the closest point on teh line segment to our robot * center and check if it is close enough. If so we hit the ball */ // MyVector2d closest; // dist = ball_seg.Distance(r.pos, closest); // if (dist > robotHalfSize + BALL_RADIUS) // return (false); /* TO DO: need to work this part out still */ return (true); }
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in // millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) { // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Calculate target position in absolute steps int32_t target[3]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); // Compute direction bits for this block block->dir_bits.value = 0x00; if(target[X_AXIS] < pl.position[X_AXIS]) block->dir_bits.flags.dir_x = true; if(target[Y_AXIS] < pl.position[Y_AXIS]) block->dir_bits.flags.dir_y = true; if(target[Z_AXIS] < pl.position[Z_AXIS]) block->dir_bits.flags.dir_z = true; // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions float delta_mm[3]; delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c float inverse_minute; if (!invert_feed_rate) { inverse_minute = feed_rate * inverse_millimeters; } else { inverse_minute = 1.0 / feed_rate; } block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with constant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter block->rate_delta = ceil( block->step_event_count*inverse_millimeters * settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) // Compute path unit vector float unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. // NOTE: This is basically an exact path mode (G61), but it doesn't come to a complete stop unless // the junction deviation value is high. In the future, if continuous mode (G64) is desired, the // math here is exactly the same. Instead of motioning all the way to junction point, the machine // will just need to follow the arc circle defined above and check if the arc radii are no longer // than half of either line segment to ensure no overlapping. Right now, the Arduino likely doesn't // have the horsepower to do these calculations at high feed rates. float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed); // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. float v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed = block->nominal_speed; // Update buffer head and next buffer head indices block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); // Update planner position memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] planner_recalculate(); }
q--; } d1x = ((3L * d1x) << (2 * bez_qual)) + ((3L * d2x) << bez_qual) + d3x; /* assert( d1x <= 0x5f408000 ); */ /* assert( d1x >= -0x5f408000 ); */ d3x = 6L*d3x; /* assert( d3x <= 0xbffe8 ); */ /* assert( d3x >= -0xbffe8 ); */ d2x = ((6L*d2x)<<bez_qual) + d3x; /* assert( d2x <= 0x2f6fa12 ); */ /* assert( d2x >= -0x2f6fa12 ); */ x0 = labs(array[0]); while( x0 >= (0x7fffffffL>>q) ) q--, qd++; x0 = (((LONG)array[0]) << q) + (1L << (q - 1)); for(i = 1 << bez_qual; i > 0; i--) { x = (WORD)(x0 >> q); *px = x; if ( x < *pmin ) *pmin = x; if ( x > *pmax ) *pmax = x; px+=2; if (labs( (x0 >> 1) + (d1x >> (qd + 1)) ) >= 0x3ffffffeL) {
// rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more void Copter::ModeRTL::descent_run() { float target_roll = 0.0f; float target_pitch = 0.0f; float target_yaw_rate = 0.0f; // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } // process pilot's input if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { if (!ap.land_repo_active) { copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE); } ap.land_repo_active = true; } } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // process roll, pitch inputs loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // run loiter controller loiter_nav->update(); // call z-axis position controller pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; }
static void do_physics_align_object( dxxobject * obj ) { vms_vector desired_upvec; fixang delta_ang,roll_ang; //vms_vector forvec = {0,0,f1_0}; vms_matrix temp_matrix; fix d,largest_d=-f1_0; int i,best_side; best_side=0; // bank player according to segment orientation //find side of segment that player is most alligned with for (i=0;i<6;i++) { #ifdef COMPACT_SEGS vms_vector _tv1; get_side_normal( &Segments[obj->segnum], i, 0, &_tv1 ); d = vm_vec_dot(&_tv1,&obj->orient.uvec); #else d = vm_vec_dot(&Segments[obj->segnum].sides[i].normals[0],&obj->orient.uvec); #endif if (d > largest_d) {largest_d = d; best_side=i;} } if (floor_levelling) { // old way: used floor's normal as upvec #ifdef COMPACT_SEGS get_side_normal(&Segments[obj->segnum], 3, 0, &desired_upvec ); #else desired_upvec = Segments[obj->segnum].sides[3].normals[0]; #endif } else // new player leveling code: use normal of side closest to our up vec if (get_num_faces(&Segments[obj->segnum].sides[best_side])==2) { #ifdef COMPACT_SEGS vms_vector normals[2]; get_side_normals(&Segments[obj->segnum], best_side, &normals[0], &normals[1] ); desired_upvec.x = (normals[0].x + normals[1].x) / 2; desired_upvec.y = (normals[0].y + normals[1].y) / 2; desired_upvec.z = (normals[0].z + normals[1].z) / 2; vm_vec_normalize(&desired_upvec); #else side_t *s = &Segments[obj->segnum].sides[best_side]; desired_upvec.x = (s->normals[0].x + s->normals[1].x) / 2; desired_upvec.y = (s->normals[0].y + s->normals[1].y) / 2; desired_upvec.z = (s->normals[0].z + s->normals[1].z) / 2; vm_vec_normalize(&desired_upvec); #endif } else #ifdef COMPACT_SEGS get_side_normal(&Segments[obj->segnum], best_side, 0, &desired_upvec ); #else desired_upvec = Segments[obj->segnum].sides[best_side].normals[0]; #endif if (labs(vm_vec_dot(&desired_upvec,&obj->orient.fvec)) < f1_0/2) { vms_angvec tangles; vm_vector_2_matrix(&temp_matrix,&obj->orient.fvec,&desired_upvec,NULL); delta_ang = vm_vec_delta_ang(&obj->orient.uvec,&temp_matrix.uvec,&obj->orient.fvec); delta_ang += obj->mtype.phys_info.turnroll; if (abs(delta_ang) > DAMP_ANG) { vms_matrix rotmat, new_pm; roll_ang = fixmul(FrameTime,ROLL_RATE); if (abs(delta_ang) < roll_ang) roll_ang = delta_ang; else if (delta_ang<0) roll_ang = -roll_ang; tangles.p = tangles.h = 0; tangles.b = roll_ang; vm_angles_2_matrix(&rotmat,&tangles); vm_matrix_x_matrix(&new_pm,&obj->orient,&rotmat); obj->orient = new_pm; } else floor_levelling=0; } }
static time_t check_file(LinphoneCoreManager* mgr) { time_t cur_time = get_current_time(); char* filepath = linphone_core_compress_log_collection(mgr->lc); time_t log_time = -1; uint32_t timediff = 0; FILE *file = NULL; BC_ASSERT_PTR_NOT_NULL(filepath); if (filepath != NULL) { int line_count = 0; char *line = NULL; size_t line_size = 256; #ifndef WIN32 struct tm tm_curr = {0}; time_t time_prev = 0; #endif #if HAVE_ZLIB // 0) if zlib is enabled, we must decompress the file first file = gzuncompress(filepath); #else file = fopen(filepath, "rb"); #endif BC_ASSERT_PTR_NOT_NULL(file); if (!file) return 0; // 1) expect to find folder name in filename path BC_ASSERT_PTR_NOT_NULL(strstr(filepath, bc_tester_writable_dir_prefix)); // 2) check file contents while (getline(&line, &line_size, file) != -1) { // a) there should be at least 25 lines ++line_count; #ifndef WIN32 // b) logs should be ordered by date (format: 2014-11-04 15:22:12:606) if (strlen(line) > 24) { char date[24] = {'\0'}; memcpy(date, line, 23); /*reset tm_curr to reset milliseconds and below fields*/ memset(&tm_curr, 0, sizeof(struct tm)); if (strptime(date, "%Y-%m-%d %H:%M:%S", &tm_curr) != NULL) { tm_curr.tm_isdst = -1; // LOL log_time = mktime(&tm_curr); BC_ASSERT_TRUE(log_time >= time_prev); time_prev = log_time; } } #endif } BC_ASSERT_TRUE(line_count > 25); free(line); fclose(file); ms_free(filepath); timediff = labs((long int)log_time - (long int)cur_time); (void)timediff; #ifndef WIN32 BC_ASSERT_TRUE( timediff <= 1 ); if( !(timediff <= 1) ){ char buffers[2][128] = {{0}}; strftime(buffers[0], sizeof(buffers[0]), "%Y-%m-%d %H:%M:%S", localtime(&log_time)); strftime(buffers[1], sizeof(buffers[1]), "%Y-%m-%d %H:%M:%S", localtime(&cur_time)); ms_error("log_time: %ld (%s), cur_time: %ld (%s) timediff: %u" , (long int)log_time, buffers[0] , (long int)cur_time, buffers[1] , timediff ); } #else ms_warning("strptime() not available for this platform, test is incomplete."); #endif } // return latest time in file return log_time; }
static void thread_conditions_simple(void *arg) { struct timeval tv_signal, tv_timeout, tv_broadcast; struct alerted_record alerted[NUM_THREADS]; THREAD_T threads[NUM_THREADS]; struct cond_wait cond; int i; struct timeval launched_at; struct event wake_one; struct event wake_all; struct basic_test_data *data = arg; struct event_base *base = data->base; int n_timed_out=0, n_signal=0, n_broadcast=0; tv_signal.tv_sec = tv_timeout.tv_sec = tv_broadcast.tv_sec = 0; tv_signal.tv_usec = 30*1000; tv_timeout.tv_usec = 150*1000; tv_broadcast.tv_usec = 500*1000; EVTHREAD_ALLOC_LOCK(cond.lock, EVTHREAD_LOCKTYPE_RECURSIVE); EVTHREAD_ALLOC_COND(cond.cond); tt_assert(cond.lock); tt_assert(cond.cond); for (i = 0; i < NUM_THREADS; ++i) { memset(&alerted[i], 0, sizeof(struct alerted_record)); alerted[i].cond = &cond; } /* Threads 5 and 6 will be allowed to time out */ memcpy(&alerted[5].delay, &tv_timeout, sizeof(tv_timeout)); memcpy(&alerted[6].delay, &tv_timeout, sizeof(tv_timeout)); evtimer_assign(&wake_one, base, wake_one_timeout, &cond); evtimer_assign(&wake_all, base, wake_all_timeout, &cond); evutil_gettimeofday(&launched_at, NULL); /* Launch the threads... */ for (i = 0; i < NUM_THREADS; ++i) { THREAD_START(threads[i], wait_for_condition, &alerted[i]); } /* Start the timers... */ tt_int_op(event_add(&wake_one, &tv_signal), ==, 0); tt_int_op(event_add(&wake_all, &tv_broadcast), ==, 0); /* And run for a bit... */ event_base_dispatch(base); /* And wait till the threads are done. */ for (i = 0; i < NUM_THREADS; ++i) THREAD_JOIN(threads[i]); /* Now, let's see what happened. At least one of 5 or 6 should * have timed out. */ n_timed_out = alerted[5].timed_out + alerted[6].timed_out; tt_int_op(n_timed_out, >=, 1); tt_int_op(n_timed_out, <=, 2); for (i = 0; i < NUM_THREADS; ++i) { const struct timeval *target_delay; struct timeval target_time, actual_delay; if (alerted[i].timed_out) { TT_BLATHER(("%d looks like a timeout\n", i)); target_delay = &tv_timeout; tt_assert(i == 5 || i == 6); } else if (evutil_timerisset(&alerted[i].alerted_at)) { long diff1,diff2; evutil_timersub(&alerted[i].alerted_at, &launched_at, &actual_delay); diff1 = timeval_msec_diff(&actual_delay, &tv_signal); diff2 = timeval_msec_diff(&actual_delay, &tv_broadcast); if (labs(diff1) < labs(diff2)) { TT_BLATHER(("%d looks like a signal\n", i)); target_delay = &tv_signal; ++n_signal; } else { TT_BLATHER(("%d looks like a broadcast\n", i)); target_delay = &tv_broadcast; ++n_broadcast; } } else { TT_FAIL(("Thread %d never got woken", i)); continue; } evutil_timeradd(target_delay, &launched_at, &target_time); test_timeval_diff_leq(&target_time, &alerted[i].alerted_at, 0, 50); } tt_int_op(n_broadcast + n_signal + n_timed_out, ==, NUM_THREADS); tt_int_op(n_signal, ==, 1); end: EVTHREAD_FREE_LOCK(cond.lock, EVTHREAD_LOCKTYPE_RECURSIVE); EVTHREAD_FREE_COND(cond.cond); }
GeneralizedTime_t * asn_time2GT_frac(GeneralizedTime_t *opt_gt, const struct tm *tm, int frac_value, int frac_digits, int force_gmt) { struct tm tm_s; long gmtoff; const unsigned int buf_size = 4 + 2 + 2 /* yyyymmdd */ + 2 + 2 + 2 /* hhmmss */ + 1 + 6 /* .ffffff */ + 1 + 4 /* +hhmm */ + 1 /* '\0' */ ; char *buf; char *p; int size; /* Check arguments */ if(!tm) { errno = EINVAL; return 0; } /* Pre-allocate a buffer of sufficient yet small length */ buf = (char *)MALLOC(buf_size); if(!buf) return 0; gmtoff = GMTOFF(*tm); if(force_gmt && gmtoff) { tm_s = *tm; tm_s.tm_sec -= gmtoff; timegm(&tm_s); /* Fix the time */ tm = &tm_s; #ifdef HAVE_TM_GMTOFF assert(!GMTOFF(tm_s)); /* Will fix itself */ #else /* !HAVE_TM_GMTOFF */ gmtoff = 0; #endif } size = snprintf(buf, buf_size, "%04d%02d%02d%02d%02d%02d", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec ); if(size != 14) { /* Could be assert(size == 14); */ FREEMEM(buf); errno = EINVAL; return 0; } p = buf + size; /* * Deal with fractions. */ if(frac_value > 0 && frac_digits > 0) { char *end = p + 1 + 6; /* '.' + maximum 6 digits */ char *z = p; long fbase; *z++ = '.'; /* Place bounds on precision */ while(frac_digits-- > 6) frac_value /= 10; /* emulate fbase = pow(10, frac_digits) */ for(fbase = 1; frac_digits--;) fbase *= 10; do { int digit = frac_value / fbase; if(digit > 9) { z = 0; break; } *z++ = digit + 0x30; frac_value %= fbase; fbase /= 10; } while(fbase > 0 && frac_value > 0 && z < end); if(z) { for(--z; *z == 0x30; --z); /* Strip zeroes */ p = z + (*z != '.'); size = p - buf; } } if(force_gmt) { *p++ = 0x5a; /* "Z" */ *p++ = 0; size++; } else { int ret; gmtoff %= 86400; ret = snprintf(p, buf_size - size, "%+03ld%02ld", gmtoff / 3600, labs(gmtoff % 3600)); if(ret != 5) { FREEMEM(buf); errno = EINVAL; return 0; } size += ret; } if(opt_gt) { if(opt_gt->buf) FREEMEM(opt_gt->buf); } else { opt_gt = (GeneralizedTime_t *)CALLOC(1, sizeof *opt_gt); if(!opt_gt) { FREEMEM(buf); return 0; } } opt_gt->buf = (unsigned char *)buf; opt_gt->size = size; return opt_gt; }
/****************************************************************** * * GSE2.0 example driver (sts 14.4.2001) * ******************************************************************/ main() { int i=0,ierr,ii=-1,i3=1; long data[20],*data2; int n,n2=0; long chksum=0,chksum2=0; /* initialize checksum to zero! */ FILE *fp, *fp2; struct header head, head2; char hline[121],tline[82]=""; buf_init (); /* initialize the character buffer */ fp = fopen ("test.gse","w"); /* open the output file */ head.d_year=1998; /* now, collect example */ head.d_mon=4; /* header information */ head.d_day=27; /* into the header structure */ head.t_hour=9; /* */ head.t_min=29; /* */ head.t_sec=3.123; /* */ strcpy (head.station,"FELD "); /* */ strcpy (head.channel,"SHZ"); /* */ strcpy (head.auxid,"VEL "); /* we have velocity data */ strcpy (head.datatype,"CM6"); /* */ head.n_samps=n=13; /* */ head.samp_rate=62.5; /* */ head.calib=1.; /* */ head.calper=6.283; /* that's ~ 2pi for our convenience */ strcpy (head.instype,"LE-3D ");/* */ head.hang=-1.0; /* */ head.vang=0.; /* header completed */ printf(">>>> %4d/%02d/%02d %02d:%02d:%06.3f %-5s %-3s %-4s %-3s %8d %11.6f %10.4e %7.3f %-6s %5.1f %4.1f\n", head.d_year, head.d_mon, head.d_day, head.t_hour, head.t_min, head.t_sec, head.station, head.channel, head.auxid, head.datatype, head.n_samps, head.samp_rate, head.calib, head.calper, head.instype, head.hang, head.vang); data[0]=13; printf("%d %ld\n",i,data[0]); for (i=1;i<n;i++) // let's create some data (13 points) { i3=i3*ii; data[i]=i3*data[i-1]*4+5; printf("%d %ld\n",i,data[i]); } /* 1st, compute the checksum */ chksum=labs(check_sum (data,n,chksum)); diff_2nd (data,n,0); /* 2nd, compute the 2nd differences */ ierr=compress_6b (data,n); /* 3rd, character-encode the data */ printf("error status after compression: %d\n",ierr); write_header(fp,&head); /* 4th, write the header to the output file */ buf_dump (fp); /* 5th, write the data to the output file */ fprintf (fp,"CHK2 %8ld\n\n",chksum); /* 6th, write checksum and closing line */ fclose(fp); /* close the output file */ buf_free (); /* clean up! */ /* That's all! Now, we can try to read the GSE2.0 file */ fp2 = fopen ("test.gse","r"); read_header(fp2,&head2); /* find and read the header line */ printf("<<<< %4d/%02d/%02d %02d:%02d:%06.3f %-5s %-3s %-4s %-3s %8d %11.6f %10.4e %7.3f %-6s %5.1f %4.1f\n", head2.d_year, head2.d_mon, head2.d_day, head2.t_hour, head2.t_min, head2.t_sec, head2.station, head2.channel, head2.auxid, head2.datatype, head2.n_samps, head2.samp_rate, head2.calib, head2.calper, head2.instype, head2.hang, head2.vang); data2 = (long *) calloc (head2.n_samps,sizeof(long)); /* allocate data vector */ n2 = decomp_6b (fp2, head2.n_samps, data2); /* read and decode the data */ printf("actual number of data read: %d\n",n2); rem_2nd_diff (data2, n2); /* remove second differences */ chksum=0; if (fgets(tline,82,fp2) == NULL) /* read next line (there might be */ { printf ("GSE: No CHK2 found before EOF\n"); /* an additional */ return; } /* blank line) */ if (strncmp(tline,"CHK2",4)) /* and look for CHK2 */ { if (fgets(tline,82,fp2) == NULL) /* read another line */ { printf ("GSE: No CHK2 found before EOF\n"); return; } } if (strncmp(tline,"CHK2",4)) { printf ("GSE: No CHK2 found!\n"); return; } sscanf(tline,"%*s %ld",&chksum); /* extract checksum */ chksum2 = check_sum (data2, n2, chksum2); /* compute checksum from data */ printf("checksum read : %ld \nchecksum computed: %ld\n",chksum,chksum2); for (i=0;i<n2;i++) /* print out data. We got velocity data */ { /* hence, we multiply by 2pi*calib/calper */ data2[i] = (long) data2[i] * 6.283 * head2.calib / head2.calper; printf("%d %ld \n",i,data2[i]); } close(fp2); free (data2); /* clean up */ }
/*______________________________________________________________________________ [] [] [] [] []` BX_VBN_New [] [] [] []____________________________________________________________________________[] */ BX_RESULT BX_VBN_New(BX_VBN_Args& arg) { if (!CheckAdminPassword(arg.AdminPassword)) return BXE_BadAdminPassword; // Пустые пароли назначать нельзя: if (arg.Password.Size() < 1) return BXE_VBN_New_PasswordVoid; if (oc_reg.m_Language.Size() == 0) return BXE_NoLanguages; //----------------------------------------------------------------------------[] OC_VBN oc_vbn; // Найти последний порядковый объекта: int LastNo = -1; oc_vbn.SetIndex_No(); if (oc_vbn.GoLast()) LastNo = oc_vbn.m_No; // Создать объект oc_vbn.New(); SERVER_DEBUG_ASSERT_RETURN( oc_vbn.IsValid(), "BX_VBN_New(): Error creating object.", BX_SYSTEM_ERROR); // Присвоить порядковый номер oc_vbn.m_No = byte(LastNo + 1); // Установить пароль: oc_vbn.m_Password = arg.Password; // Зарегистрировать в реестре: oc_reg.m_VBN.Add(oc_vbn.GetObjectID()); arg.objID = oc_vbn.GetObjectID(); //----------------------------------------------------------------------------[] // Установить параметры сети: // Name if (arg.Name.Size() > 0) oc_vbn.m_Name = arg.Name; else if ( (string(oc_vbn.m_Name)).Size() == 0) { arg.Name.Format("VBN #%d", (int)oc_vbn.m_No); oc_vbn.m_Name = arg.Name; } // Description oc_vbn.m_Description = arg.Description; // IsModerated oc_vbn.m_IsModerated = arg.IsModerated; // DefaultLanguage if (OC_Exist(OBJ_CLASS_ID_Language, arg.DefaultLanguage)) oc_vbn.m_DefaultLanguage = arg.DefaultLanguage; else oc_vbn.m_DefaultLanguage = oc_reg.m_Language[0]; // DefaultTZOffset if (labs(arg.DefaultTZOffset) <= SECS_IN_DAY/2) { oc_vbn.m_DefaultTZOffset = arg.DefaultTZOffset; } // ConceptMask VALIDATE_ID_ARRAY(OBJ_CLASS_ID_Concept, arg.ConceptMask); ExpandWithParentConcepts(arg.ConceptMask); _IDsToBits_Set( arg.ConceptMask, oc_vbn.m_ConceptMask); //- - - - - - - - - - - - - - - - - - - - - - - - - -[]` return BX_OK; }
void fl_print_child(fl_context_t *fl_ctx, ios_t *f, value_t v) { char *name, *str; char buf[64]; if (fl_ctx->print_level >= 0 && fl_ctx->P_LEVEL >= fl_ctx->print_level && (iscons(v) || isvector(v) || isclosure(v))) { outc(fl_ctx, '#', f); return; } fl_ctx->P_LEVEL++; switch (tag(v)) { case TAG_NUM : case TAG_NUM1: //fl_ctx->HPOS+=ios_printf(f, "%ld", numval(v)); break; str = uint2str(&buf[1], sizeof(buf)-1, labs(numval(v)), 10); if (numval(v)<0) *(--str) = '-'; outs(fl_ctx, str, f); break; case TAG_SYM: name = symbol_name(fl_ctx, v); if (fl_ctx->print_princ) outs(fl_ctx, name, f); else if (ismanaged(fl_ctx, v)) { outsn(fl_ctx, "#:", f, 2); outs(fl_ctx, name, f); } else print_symbol_name(fl_ctx, f, name); break; case TAG_FUNCTION: if (v == fl_ctx->T) { outsn(fl_ctx, "#t", f, 2); } else if (v == fl_ctx->F) { outsn(fl_ctx, "#f", f, 2); } else if (v == fl_ctx->NIL) { outsn(fl_ctx, "()", f, 2); } else if (v == fl_ctx->FL_EOF) { outsn(fl_ctx, "#<eof>", f, 6); } else if (isbuiltin(v)) { if (!fl_ctx->print_princ) outsn(fl_ctx, "#.", f, 2); outs(fl_ctx, builtin_names[uintval(v)], f); } else { assert(isclosure(v)); if (!fl_ctx->print_princ) { if (print_circle_prefix(fl_ctx, f, v)) break; function_t *fn = (function_t*)ptr(v); outs(fl_ctx, "#fn(", f); char *data = (char*)cvalue_data(fn->bcode); size_t i, sz = cvalue_len(fn->bcode); for(i=0; i < sz; i++) data[i] += 48; fl_print_child(fl_ctx, f, fn->bcode); for(i=0; i < sz; i++) data[i] -= 48; outc(fl_ctx, ' ', f); fl_print_child(fl_ctx, f, fn->vals); if (fn->env != fl_ctx->NIL) { outc(fl_ctx, ' ', f); fl_print_child(fl_ctx, f, fn->env); } if (fn->name != fl_ctx->LAMBDA) { outc(fl_ctx, ' ', f); fl_print_child(fl_ctx, f, fn->name); } outc(fl_ctx, ')', f); } else { outs(fl_ctx, "#<function>", f); } } break; case TAG_CVALUE: case TAG_CPRIM: if (v == UNBOUND) { outs(fl_ctx, "#<undefined>", f); break; } case TAG_VECTOR: case TAG_CONS: if (print_circle_prefix(fl_ctx, f, v)) break; if (isvector(v)) { outc(fl_ctx, '[', f); int newindent = fl_ctx->HPOS, est; int i, sz = vector_size(v); for(i=0; i < sz; i++) { if (fl_ctx->print_length >= 0 && i >= fl_ctx->print_length && i < sz-1) { outsn(fl_ctx, "...", f, 3); break; } fl_print_child(fl_ctx, f, vector_elt(v,i)); if (i < sz-1) { if (!fl_ctx->print_pretty) { outc(fl_ctx, ' ', f); } else { est = lengthestimate(fl_ctx, vector_elt(v,i+1)); if (fl_ctx->HPOS > fl_ctx->SCR_WIDTH-4 || (est!=-1 && (fl_ctx->HPOS+est > fl_ctx->SCR_WIDTH-2)) || (fl_ctx->HPOS > fl_ctx->SCR_WIDTH/2 && !smallp(fl_ctx, vector_elt(v,i+1)) && !tinyp(fl_ctx, vector_elt(v,i)))) newindent = outindent(fl_ctx, newindent, f); else outc(fl_ctx, ' ', f); } } } outc(fl_ctx, ']', f); break; } if (iscvalue(v) || iscprim(v)) cvalue_print(fl_ctx, f, v); else print_pair(fl_ctx, f, v); break; } fl_ctx->P_LEVEL--; }
static CYTHON_INLINE unsigned long __Pyx_abs_long(long x) { if (unlikely(x == -LONG_MAX-1)) return ((unsigned long)LONG_MAX) + 1U; return (unsigned long) labs(x); }
static void csv_event(auparse_state_t *au, auparse_cb_event_t cb_event_type, void *user_data) { if (cb_event_type != AUPARSE_CB_EVENT_READY) return; if (csv_header_done == 0) { csv_header_done = 1; printf( "NODE,EVENT,DATE,TIME,%sSERIAL_NUM,EVENT_KIND," "SESSION,SUBJ_PRIME,SUBJ_SEC,SUBJ_KIND,%sACTION," "RESULT,OBJ_PRIME,OBJ_SEC,%s%sOBJ_KIND,HOW%s\n", extra_time ? "YEAR,MONTH,DAY,WEEKDAY,HOUR,GMT_OFFSET," : "", extra_labels ? "SUBJ_LABEL," : "", extra_obj2 ? "OBJ2," : "", extra_labels ? "OBJ_LABEL," : "", extra_keys ? ",KEY" : ""); } char tmp[20]; const char *item, *type, *evkind, *subj_kind, *action, *str, *how; int rc; time_t t = auparse_get_time(au); struct tm *tv = localtime(&t); // NODE item = auparse_get_node(au); if (item) { printf("%s", auparse_interpret_field(au)); free((void *)item); } putchar(','); // Event type = auparse_get_type_name(au); if (type) printf("%s", type); putchar(','); // Normalize rc = auparse_normalize(au, extra_labels ? NORM_OPT_ALL : NORM_OPT_NO_ATTRS); //DATE if (tv) { strftime(tmp, sizeof(tmp), "%x", tv); printf("%s", tmp); } putchar(','); // TIME if (tv) { strftime(tmp, sizeof(tmp), "%T", tv); printf("%s", tmp); } putchar(','); if (extra_time) { // YEAR if (tv) { strftime(tmp, sizeof(tmp), "%Y", tv); printf("%s", tmp); } putchar(','); // MONTH if (tv) { strftime(tmp, sizeof(tmp), "%m", tv); printf("%s", tmp); } putchar(','); // DAY if (tv) { strftime(tmp, sizeof(tmp), "%d", tv); printf("%s", tmp); } putchar(','); // WEEKDAY if (tv) { strftime(tmp, sizeof(tmp), "%u", tv); printf("%s", tmp); } putchar(','); // HOUR if (tv) { strftime(tmp, sizeof(tmp), "%k", tv); printf("%s", tmp); } putchar(','); if (tv) { char sign = tv->tm_gmtoff >= 0 ? '+' : '-'; unsigned long total = labs(tv->tm_gmtoff); unsigned long hour = total/3600; unsigned long min = (total - (hour * 3600))%60; printf("%c%02lu:%02lu", sign, hour, min); } putchar(','); } // SERIAL_NUMBER printf("%lu,", auparse_get_serial(au)); if (rc) { fprintf(stderr, "error normalizing %s\n", type); // Just dump an empty frame printf(",,,,,,,,,%s%s\n", extra_labels ? ",," : "", extra_keys ? "," : ""); return; } // EVENT_KIND evkind = auparse_normalize_get_event_kind(au); printf("%s", evkind ? evkind : "unknown"); putchar(','); // SESSION rc = auparse_normalize_session(au); if (rc == 1) printf("%s", auparse_interpret_field(au)); putchar(','); // SUBJ_PRIME rc = auparse_normalize_subject_primary(au); if (rc == 1) { const char *subj = auparse_interpret_field(au); if (strcmp(subj, "unset") == 0) subj = "system"; printf("%s", subj); } putchar(','); // SUBJ_SEC rc = auparse_normalize_subject_secondary(au); if (rc == 1) printf("%s", auparse_interpret_field(au)); putchar(','); // SUBJ_KIND subj_kind = auparse_normalize_subject_kind(au); if (subj_kind) printf("%s", subj_kind); putchar(','); // SUBJ_LABEL if (extra_labels) { rc = auparse_normalize_subject_first_attribute(au); do { if (rc == 1) { const char *name = auparse_get_field_name(au); if (strcmp(name, "subj") == 0) { printf("%s", auparse_interpret_field(au)); break; } } } while (auparse_normalize_subject_next_attribute(au) == 1); putchar(','); } // ACTION action = auparse_normalize_get_action(au); printf("%s", action ? action : "did-unknown"); putchar(','); // RESULT rc = auparse_normalize_get_results(au); if (rc == 1) { int i = 0; const char *res[] = { "failed", "success" }; item = auparse_interpret_field(au); if (strcmp(item, "yes") == 0) i = 1; else if (strncmp(item, "suc", 3) == 0) i = 1; else if (auparse_get_field_type(au) == AUPARSE_TYPE_SECCOMP && strcmp(item, "allow") == 0) i = 1; printf("%s", res[i]); } putchar(','); // OBJ_PRIME rc = auparse_normalize_object_primary(au); if (rc == 1) { const char *val; if (auparse_get_field_type(au) == AUPARSE_TYPE_ESCAPED_FILE) val = auparse_interpret_realpath(au); else val = auparse_interpret_field(au); printf("%s", val); } putchar(','); // OBJ_SEC rc = auparse_normalize_object_secondary(au); if (rc == 1) printf("%s", auparse_interpret_field(au)); putchar(','); // OBJECT 2 if (extra_obj2) { rc = auparse_normalize_object_primary2(au); if (rc == 1) { const char *val; if (auparse_get_field_type(au) == AUPARSE_TYPE_ESCAPED_FILE) val = auparse_interpret_realpath(au); else val = auparse_interpret_field(au); printf("%s", val); } putchar(','); } // OBJ_LABEL if (extra_labels) { rc = auparse_normalize_object_first_attribute(au); do { if (rc == 1) { const char *name = auparse_get_field_name(au); if (strcmp(name, "obj") == 0) { printf("%s", auparse_interpret_field(au)); break; } } } while (auparse_normalize_object_next_attribute(au) == 1); putchar(','); } // OBJ_KIND str = auparse_normalize_object_kind(au); printf("%s,", str); // HOW how = auparse_normalize_how(au); if (how) printf("%s", how); // KEY if (extra_keys) { putchar(','); // This is to close out HOW rc = auparse_normalize_key(au); if (rc == 1) printf("%s", auparse_interpret_field(au)); } printf("\n"); }