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;
}
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;
}
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);
}
double print_rc(Slice *psl,int state_index) {
double op;
printf("op = %g\n",(op=get_rc(psl)));
return op;
}
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);
}
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);
}
}
/**
* 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;
}
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;
}
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);
}
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);
}
/****************** 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);
}
