コード例 #1
0
ファイル: main.c プロジェクト: kjopek/freebsd
/*
 * 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
}
コード例 #2
0
ファイル: ArduPlane.cpp プロジェクト: Bjarne-Madsen/ardupilot
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;
    }
}
コード例 #3
0
ファイル: csngen.c プロジェクト: Firstyear/ds
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;
    }
}
コード例 #4
0
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>&nbsp;GMT/Local Time </TD><TD>&nbsp;");
            printTime( stream, gmtime(&secs) );
            HTML("&nbsp;&nbsp;<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>&nbsp;Local Time </TD><TD>&nbsp;");
            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>&nbsp;TimeZone </TD><TD>&nbsp;");
            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>&nbsp;UpTime </TD><TD>&nbsp;");
            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>&nbsp;2401 Id </TD><TD>&nbsp;");
            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;
}
コード例 #5
0
ファイル: planner.cpp プロジェクト: rafffel/EOSP
  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()
コード例 #6
0
ファイル: Motion.cpp プロジェクト: ScribbleJ/sjfw
// 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
}
コード例 #7
0
ファイル: ArduPlane.cpp プロジェクト: uziak/ardupilot
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;
    }
}
コード例 #8
0
ファイル: DDA.cpp プロジェクト: JostSchenck/RepRapFirmware
// 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;
}
コード例 #9
0
ファイル: rrd_fetch.c プロジェクト: BOBYou/ntopng
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);
}
コード例 #10
0
ファイル: imageio.c プロジェクト: imarsv/darktable
// 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;
}
コード例 #11
0
ファイル: dfs.c プロジェクト: CoryXie/nix-os
/*
 * 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;
}
コード例 #12
0
ファイル: servos.cpp プロジェクト: mblsktxy/ardupilot
/* 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;
}
コード例 #13
0
/*------------------------------------------------------*/
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;
				}
			}
コード例 #14
0
ファイル: AC_AttitudeControl.cpp プロジェクト: APM602/APM602
// 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;
}
コード例 #15
0
ファイル: receive-pack.c プロジェクト: 80520997/git
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;
}
コード例 #16
0
ファイル: planner.c プロジェクト: MezzoMill/MMFirmware
// 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();
}
コード例 #17
0
ファイル: temp.c プロジェクト: nrpatel/FiveD_on_Arduino
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);
			}
		}
	}
コード例 #18
0
//检查球是否被机器人捕捉
//通过半径判断
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);
}
コード例 #19
0
ファイル: planner.c プロジェクト: csdexter/grbl
// 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(); 
}
コード例 #20
0
ファイル: vdi_bezier.c プロジェクト: kelihlodversson/emutos
        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) {
コード例 #21
0
ファイル: mode_rtl.cpp プロジェクト: ArduPilot/ardupilot
// 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;
}
コード例 #22
0
ファイル: physics.cpp プロジェクト: Foran/dxx-rebirth
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;
	}

}
コード例 #23
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;
}
コード例 #24
0
ファイル: regress_thread.c プロジェクト: 17716580/Libevent
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);
}
コード例 #25
0
ファイル: GeneralizedTime.c プロジェクト: asiyagul/biosig
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;
}
コード例 #26
0
ファイル: gse_driver.c プロジェクト: obspy/branches
/******************************************************************
 *
 *	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 */
}
コード例 #27
0
ファイル: BX_VBN.cpp プロジェクト: RomanKoshelev/school
/*______________________________________________________________________________
[]                                                                            []
[]                                                                            []
[]` 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;
}
コード例 #28
0
ファイル: print.c プロジェクト: ArchRobison/julia
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--;
}
コード例 #29
0
ファイル: Builtins.c プロジェクト: GaZ3ll3/cython
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);
}
コード例 #30
0
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");
}