示例#1
0
int kvmppc_core_emulate_op_44x(struct kvm_run *run, struct kvm_vcpu *vcpu,
			       unsigned int inst, int *advance)
{
	int emulated = EMULATE_DONE;
	int dcrn = get_dcrn(inst);
	int ra = get_ra(inst);
	int rb = get_rb(inst);
	int rc = get_rc(inst);
	int rs = get_rs(inst);
	int rt = get_rt(inst);
	int ws = get_ws(inst);

	switch (get_op(inst)) {
	case 31:
		switch (get_xop(inst)) {

		case XOP_MFDCR:
			emulated = emulate_mfdcr(vcpu, rt, dcrn);
			break;

		case XOP_MFDCRX:
			emulated = emulate_mfdcr(vcpu, rt,
					kvmppc_get_gpr(vcpu, ra));
			break;

		case XOP_MTDCR:
			emulated = emulate_mtdcr(vcpu, rs, dcrn);
			break;

		case XOP_MTDCRX:
			emulated = emulate_mtdcr(vcpu, rs,
					kvmppc_get_gpr(vcpu, ra));
			break;

		case XOP_TLBWE:
			emulated = kvmppc_44x_emul_tlbwe(vcpu, ra, rs, ws);
			break;

		case XOP_TLBSX:
			emulated = kvmppc_44x_emul_tlbsx(vcpu, rt, ra, rb, rc);
			break;

		case XOP_ICCCI:
			break;

		default:
			emulated = EMULATE_FAIL;
		}

		break;

	default:
		emulated = EMULATE_FAIL;
	}

	if (emulated == EMULATE_FAIL)
		emulated = kvmppc_booke_emulate_op(run, vcpu, inst, advance);

	return emulated;
}
示例#2
0
bool TxnManager::calvin_exec_phase_done() {
  bool ready =  (phase == CALVIN_DONE) && (get_rc() != WAIT);
  if(ready) {
    DEBUG("(%ld,%ld) calvin exec phase done!\n",txn->txn_id,txn->batch_id);
  }
  return ready;
}
示例#3
0
int main(void)
{
  uint16_t i;
  FRESULT rc;

  map_io();
  init_port();

  InitRTCC();

  uart2_init();
  xdev_out(uart2_put);
  xdev_in(uart2_get);

  dbg_printf("$" PROJECT_NAME "\n");
  dbg_printf("$" __DATE__ " " __TIME__ "\n");

  rc = f_mount(&fatfs, "", 1);
  dbg_printf("$FF,f_mount,%s\n", get_rc(rc));

  OpenTimer1(T1_PS_1_256 & T1_GATE_OFF & T1_SOURCE_INT & T1_IDLE_CON &
    T1_ON & T1_SYNC_EXT_OFF, 0xFFFF);
  ConfigIntTimer1(T1_INT_ON & T1_INT_PRIOR_1);
  OpenCapture1(IC_IDLE_STOP & IC_TIMER1_SRC & IC_INT_1CAPTURE & IC_EVERY_RISE_EDGE,
    IC_CASCADE_DISABLE & IC_TRIGGER_ENABLE & IC_UNTRIGGER_TIMER & IC_SYNC_TRIG_IN_DISABLE);
  ConfigIntCapture1(IC_INT_ON & IC_INT_PRIOR_5);
  _IC1IF = 0;
  

  while (1) {
    while (_RTCSYNC == 0);
    while (_RTCSYNC == 1);
    if (gps_pr > 0) {
      _T1IE = 0;
      float f = (float) TMR1 / gps_pr;
      _T1IE = 1;
      xprintf("%u\n", (uint16_t) (f * 1000));
    }
    if (ngpslines > 0) {
      ngpslines--;
      if (xgets(gps_line, 128)) {
        xprintf("$GPS%s\n", gps_line);
      }
    }
  }

  while (0) {
    while (_RTCSYNC == 0);
    while (_RTCSYNC == 1);
    if (gps_pr > 0) {
      _T1IE = 0;
      float f = (float) TMR1 / gps_pr;
      _T1IE = 1;
      xprintf("%u\n", (uint16_t) (f * 1000));
    }
  }

  return (EXIT_SUCCESS);
}
示例#4
0
double print_rc(Slice *psl,int state_index) {

    double op;
 
    printf("op = %g\n",(op=get_rc(psl)));

    return op;
}
示例#5
0
void
toytronics_set_sp_absolute_forward_from_rc()
{
  double dt = 1.0/RC_UPDATE_FREQ;
  const rc_t * const rc = get_rc();
  const quat_t * const q_n2b = get_q_n2b();
  const euler_t * const e_n2b = get_e_n2b();

  // local copies to allow implementing a deadband
  double rcp = rc->pitch;
  double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND);
  double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);

  euler_t e_n2sp;
  e_n2sp.pitch = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + absolute_forward_pitch_trim_deg)*M_PI/180.0;
  e_n2sp.roll  = rcr * SETPOINT_MAX_STICK_ANGLE_DEG * M_PI/180.0;

  // integrate stick to get setpoint heading
  setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcy;
  setpoint.setpoint_heading += dt*e_n2sp.roll*roll_to_yaw_rate_ff_factor;
  
  // accel turn coordination
  setpoint.setpoint_heading -= dt*tc_fading(q_n2b)*forward_accel_tc_gain*get_y_accel();

  wrap_to_pi( &setpoint.setpoint_heading );

  // bound setpoint heading
  setpoint.estimated_heading = get_fudged_yaw( q_n2b, e_n2b );
  double error_heading = setpoint.setpoint_heading - setpoint.estimated_heading;
  wrap_to_pi(&error_heading);
  BOUND(error_heading, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
  setpoint.setpoint_heading = setpoint.estimated_heading + error_heading;  

  // generate setpoint
  e_n2sp.yaw = setpoint.setpoint_heading;
  quat_of_euler321( &setpoint.q_n2sp, &e_n2sp );

  // smooth transition
  smooth_transition_angle -= dt*M_PI/180.0*SETPOINT_MODE_1_2_TRANSITION_DEGREES_PER_SEC;
  if (smooth_transition_angle < 0) smooth_transition_angle = 0.0;

  quat_t q_st;
  q_st.q0 = cos(0.5*smooth_transition_angle);
  q_st.q1 = 0.0;
  q_st.q2 = sin(0.5*smooth_transition_angle);
  q_st.q3 = 0.0;
  setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, q_st);

  // calculate body to setpoint quat
  quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));

  // set stabilization setpoint
  set_stabilization_setpoint(&setpoint.q_n2sp);
}
示例#6
0
int
main()
{
  init_joystick();

  while(1){
    read_joystick();
    const rc_t * rc = get_rc();
    printf("throttle: % .3f, yaw: % .3f, pitch: % .3f, roll: % .3f, "
           "enable: %d, mode: %d, gear: %d, aux2: %d\n",
           rc->throttle,rc->yaw,rc->pitch,rc->roll,rc->enable,rc->mode,rc->gear,rc->aux2);
    usleep(100000);
  }

}
示例#7
0
/**
 * Load configuration file.
 * @return L
 */
int		load_config(lua_State *L)
{
  char		*rc_file;
  int		success;

  rc_file = get_rc();
  if (rc_file != NULL)
    {
      success = ! luaL_loadfile(L, rc_file) && ! lua_pcall(L, 0, 0, 0);
      free(rc_file);
      if (success)
	return 0;
      error("%s.\n", lua_tostring(L, -1)); /* error raised on stack */
    }
  lua_close(L);
  return -1;
}
示例#8
0
int kvmppc_core_emulate_op(struct kvm_run *run, struct kvm_vcpu *vcpu,
                           unsigned int inst, int *advance)
{
	int emulated = EMULATE_DONE;
	int dcrn;
	int ra;
	int rb;
	int rc;
	int rs;
	int rt;
	int ws;

	switch (get_op(inst)) {
	case 31:
		switch (get_xop(inst)) {

		case XOP_MFDCR:
			dcrn = get_dcrn(inst);
			rt = get_rt(inst);

			/* The guest may access CPR0 registers to determine the timebase
			 * frequency, and it must know the real host frequency because it
			 * can directly access the timebase registers.
			 *
			 * It would be possible to emulate those accesses in userspace,
			 * but userspace can really only figure out the end frequency.
			 * We could decompose that into the factors that compute it, but
			 * that's tricky math, and it's easier to just report the real
			 * CPR0 values.
			 */
			switch (dcrn) {
			case DCRN_CPR0_CONFIG_ADDR:
				kvmppc_set_gpr(vcpu, rt, vcpu->arch.cpr0_cfgaddr);
				break;
			case DCRN_CPR0_CONFIG_DATA:
				local_irq_disable();
				mtdcr(DCRN_CPR0_CONFIG_ADDR,
					  vcpu->arch.cpr0_cfgaddr);
				kvmppc_set_gpr(vcpu, rt,
					       mfdcr(DCRN_CPR0_CONFIG_DATA));
				local_irq_enable();
				break;
			default:
				run->dcr.dcrn = dcrn;
				run->dcr.data =  0;
				run->dcr.is_write = 0;
				vcpu->arch.io_gpr = rt;
				vcpu->arch.dcr_needed = 1;
				kvmppc_account_exit(vcpu, DCR_EXITS);
				emulated = EMULATE_DO_DCR;
			}

			break;

		case XOP_MTDCR:
			dcrn = get_dcrn(inst);
			rs = get_rs(inst);

			/* emulate some access in kernel */
			switch (dcrn) {
			case DCRN_CPR0_CONFIG_ADDR:
				vcpu->arch.cpr0_cfgaddr = kvmppc_get_gpr(vcpu, rs);
				break;
			default:
				run->dcr.dcrn = dcrn;
				run->dcr.data = kvmppc_get_gpr(vcpu, rs);
				run->dcr.is_write = 1;
				vcpu->arch.dcr_needed = 1;
				kvmppc_account_exit(vcpu, DCR_EXITS);
				emulated = EMULATE_DO_DCR;
			}

			break;

		case XOP_TLBWE:
			ra = get_ra(inst);
			rs = get_rs(inst);
			ws = get_ws(inst);
			emulated = kvmppc_44x_emul_tlbwe(vcpu, ra, rs, ws);
			break;

		case XOP_TLBSX:
			rt = get_rt(inst);
			ra = get_ra(inst);
			rb = get_rb(inst);
			rc = get_rc(inst);
			emulated = kvmppc_44x_emul_tlbsx(vcpu, rt, ra, rb, rc);
			break;

		case XOP_ICCCI:
			break;

		default:
			emulated = EMULATE_FAIL;
		}

		break;

	default:
		emulated = EMULATE_FAIL;
	}

	if (emulated == EMULATE_FAIL)
		emulated = kvmppc_booke_emulate_op(run, vcpu, inst, advance);

	return emulated;
}
示例#9
0
void
toytronics_set_sp_incremental_from_rc()
{
  double dt = 1.0/RC_UPDATE_FREQ;
  const rc_t * const rc = get_rc();
  const quat_t * const q_n2b = get_q_n2b();

  // local copies to allow implementing a deadband
  double rcp = apply_deadband(rc->pitch, SETPOINT_DEADBAND);
  double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND);
  double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
  
  // rotation vector in body frame
  xyz_t w_dt_body = {rcr * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
                     rcp * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
                     rcy * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt};

  // try accelerometer turn coordination
  w_dt_body.z -= dt*tc_fading(q_n2b)*aerobatic_accel_tc_gain*get_y_accel();
 
  // old body to setpoint quat q_b2sp
  quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
  
  // rotation vector in setpoint frame
  xyz_t w_dt_sp;
  rot_vec_by_quat_a2b( &w_dt_sp, &(setpoint.q_b2sp), &w_dt_body);

  // form diff quat
  double total_angle = sqrt( w_dt_sp.x*w_dt_sp.x 
                             + w_dt_sp.y*w_dt_sp.y 
                             + w_dt_sp.z*w_dt_sp.z 
                             + 1e-9);
  quat_t diff_quat = {cos(total_angle/2.0),
                      sin(total_angle/2.0)*w_dt_sp.x/total_angle,
                      sin(total_angle/2.0)*w_dt_sp.y/total_angle,
                      sin(total_angle/2.0)*w_dt_sp.z/total_angle};

  // use diff quat to update setpoint quat
  setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, diff_quat);

  // calculate body to setpoint quat
  quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
  
  // now bound setpoint quat to not get too far away from estimated quat
  BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0);
  BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0);
  BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0);

  // let setpoint decay back to body
  discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt );
  discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt );
  discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt );

  // normalize
  setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3));
  
  // update n2sp quat
  quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp));

  // set stabilization setpoint
  set_stabilization_setpoint(&setpoint.q_n2sp);
}
示例#10
0
void
toytronics_set_sp_hover_forward_from_rc()
{
  double dt = 1.0/RC_UPDATE_FREQ;
  const rc_t * const rc = get_rc();
  const quat_t * const q_n2b = get_q_n2b();
  
  // estimated heading for telemetry and bounding
  setpoint.estimated_heading = hover_forward_yaw_of_quat( q_n2b );

  // local copies to allow implementing a deadband
  double rcp = rc->pitch;
  double rcr = rc->roll;
  double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);

  // set pitch/yaw from stick
  double pitch_body = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + hover_pitch_trim_deg)*M_PI/180.0;
  double roll_body   = rcr * SETPOINT_MAX_STICK_ANGLE_DEG*M_PI/180.0;

  // integrate stick to get setpoint heading
  setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcy;

  // body roll feedforward turn coordination
  #define START_FADING_DEG -50
  #define FINISH_FADING_DEG -70
  double ff_fading_slider;
  if (pitch_body > START_FADING_DEG*M_PI/180.0)
    ff_fading_slider = 0;
  else if (pitch_body < FINISH_FADING_DEG*M_PI/180.0)
    ff_fading_slider = 1;
  else
    ff_fading_slider = (pitch_body*180.0/M_PI - START_FADING_DEG)/(FINISH_FADING_DEG - START_FADING_DEG);

  setpoint.setpoint_heading += dt*roll_body*roll_to_yaw_rate_ff_factor*ff_fading_slider;

  // accel y turn coordination
  setpoint.setpoint_heading -= dt*tc_fading(q_n2b)*hover_forward_accel_tc_gain*get_y_accel();

  // bound heading error
  double heading_error = setpoint.setpoint_heading - setpoint.estimated_heading;
  wrap_to_pi(&heading_error);
  BOUND(heading_error, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
  setpoint.setpoint_heading = setpoint.estimated_heading + heading_error;
  wrap_to_pi(&setpoint.setpoint_heading);

  // start straight up
  quat_t q_y90 = {sqrt(2)/2.0, 0, sqrt(2)/2.0, 0};
  quat_memcpy( &setpoint.q_n2sp, &q_y90 );

  // yaw
  quat_t q_yaw = {1,0,0,0};
  q_yaw.q0 =  cos(0.5*setpoint.setpoint_heading);
  q_yaw.q1 = -sin(0.5*setpoint.setpoint_heading);
  quat_t q_temp;
  quat_memcpy( &q_temp, &setpoint.q_n2sp );
  quat_mult( &setpoint.q_n2sp, &q_temp, &q_yaw );

  // roll
  quat_t q_roll = {1,0,0,0};
  q_roll.q0 = cos(0.5*roll_body);
  q_roll.q3 = sin(0.5*roll_body);
  quat_memcpy( &q_temp, &setpoint.q_n2sp );
  quat_mult( &setpoint.q_n2sp, &q_temp, &q_roll );

  // pitch
  quat_t q_pitch = {1,0,0,0};
  q_pitch.q0 = cos(0.5*pitch_body);
  q_pitch.q2 = sin(0.5*pitch_body);
  quat_memcpy( &q_temp, &setpoint.q_n2sp );
  quat_mult( &setpoint.q_n2sp, &q_temp, &q_pitch );

  /* // calculate body to setpoint quat */
  /* quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp)); */
  
  /* // now bound setpoint quat to not get too far away from estimated quat */
  /* BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0); */
  /* BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0); */
  /* BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0); */

  /* // let setpoint decay back to body */
  /* discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt ); */
  /* discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt ); */
  /* discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt ); */

  /* // normalize */
  /* setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3)); */
  
  /* // update n2sp quat */
  /* quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp)); */

  // update setpoint.heading
  setpoint.setpoint_heading = hover_forward_yaw_of_quat( &setpoint.q_n2sp );

  // set stabilization setpoint
  set_stabilization_setpoint(&setpoint.q_n2sp);
}
示例#11
0
/****************** core functions ******************/
void
toytronics_set_sp_absolute_hover_from_rc()
{
  double dt = 1.0/RC_UPDATE_FREQ;
  const rc_t * const rc = get_rc();
  const quat_t * const q_n2b = get_q_n2b();
  
#ifdef SWAP_STICKS_FOR_SCOTT
  // local copies to allow implementing a deadband
  double rcp = rc->pitch;
  double rcy = rc->yaw;
  double rcr = -apply_deadband(rc->roll, SETPOINT_DEADBAND);
#else
  // local copies to allow implementing a deadband
  double rcp = rc->pitch;
  double rcy = rc->roll;
  double rcr = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
#endif  

  // integrate stick to get setpoint heading
  setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcr;
  wrap_to_pi(&(setpoint.setpoint_heading));

  // set pitch/yaw from stick based on current body roll
  double pitch_body = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + hover_pitch_trim_deg)*M_PI/180.0;
  double yaw_body   = rcy * SETPOINT_MAX_STICK_ANGLE_DEG*M_PI/180.0;

  // rotate pitch/yaw commands into setpoint frame
  setpoint.estimated_heading = get_heading_from_q_n2b(q_n2b);
  double error_heading = setpoint.setpoint_heading - setpoint.estimated_heading;
  wrap_to_pi(&error_heading);

  // don't let setpoint drift too far
  BOUND(error_heading, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
  setpoint.setpoint_heading = setpoint.estimated_heading + error_heading;  
  
  double pitch_setpoint =  pitch_body*cos(error_heading) - yaw_body*sin(error_heading);
  double yaw_setpoint   =  pitch_body*sin(error_heading) + yaw_body*cos(error_heading);

  // generate the setpoint quaternion
  // start straight up

  setpoint.q_n2sp.q0 = sqrt(2.0)/2.0;
  setpoint.q_n2sp.q1 = 0.0;
  setpoint.q_n2sp.q2 = sqrt(2.0)/2.0;
  setpoint.q_n2sp.q3 = 0.0;
  
  // rotate by heading
  quat_t q_heading = {cos(0.5*setpoint.setpoint_heading), -sin(0.5*setpoint.setpoint_heading), 0.0,0.0};
  setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, q_heading);
 
  // rotate by stick commands
  double total_angle = sqrt(pitch_setpoint*pitch_setpoint + yaw_setpoint*yaw_setpoint+1e-9);
  setpoint.q_pitch_yaw_setpoint.q0 = cos(0.5*total_angle);
  setpoint.q_pitch_yaw_setpoint.q1 = 0.0;
  setpoint.q_pitch_yaw_setpoint.q2 = sin(0.5*total_angle)*pitch_setpoint/total_angle;
  setpoint.q_pitch_yaw_setpoint.q3 = sin(0.5*total_angle)*yaw_setpoint/total_angle;
  setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, setpoint.q_pitch_yaw_setpoint);

  // calculate body to setpoint quat
  quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));

  // output "pitch"/"yaw" estimated quat
  quat_mult_inv(&(setpoint.q_pitch_yaw_estimated), &(setpoint.q_pitch_yaw_setpoint), 
                &(setpoint.q_b2sp));

  // set stabilization setpoint
  set_stabilization_setpoint(&setpoint.q_n2sp);
}