bool
TaskAutoPilot::update_autopilot(TaskAccessor& task,
const AircraftState& state,
const AircraftState& state_last)
{
update_mode(task, state);
return update_computer(task, state);
}
bool
TaskAutoPilot::update_autopilot(TaskAccessor& task,
const AIRCRAFT_STATE& state,
const AIRCRAFT_STATE& state_last)
{
update_mode(task, state);
return update_computer(task, state);
}
void Display::update_all()
{
update_text(0);
update_mode(1);
update_battery(2);
update_gps(3);
//update_gps_sats(4);
update_prearm(4);
update_ekf(5);
}
void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &report_msg);
_gimbalParams.set_channel(chan);
if(report_msg.target_system != 1) {
// gimbal must have been power cycled or reconnected
_gimbalParams.reset();
_gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
return;
}
_gimbalParams.update();
if(!_gimbalParams.initialized()){
return;
}
_last_report_msg_ms = hal.scheduler->millis();
extract_feedback(report_msg);
update_mode();
update_state();
switch(_mode) {
case GIMBAL_MODE_IDLE:
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
break;
case GIMBAL_MODE_POS_HOLD:
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
break;
case GIMBAL_MODE_POS_HOLD_FF:
case GIMBAL_MODE_STABILIZE:
send_control(chan);
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
default:
break;
}
float max_torque;
_gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
if (max_torque != _max_torque && max_torque != 0) {
_max_torque = max_torque;
}
if (!hal.util->get_soft_armed() || joints_near_limits()) {
_gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
} else {
_gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
}
}
bool AircraftSim::Update() {
update_state();
integrate();
update_mode();
task_manager.update(state, state_last);
task_manager.update_idle(state);
task_manager.update_auto_mc(state, fixed_zero);
state_last = state;
state.flying_state_moving(state.Time);
if (!far()) {
wait_prompt(time());
awp++;
if (awp>= w.size()) {
return false;
}
task_manager.setActiveTaskPoint(awp);
}
if (goto_target) {
if (task_manager.getActiveTaskPointIndex() < awp) {
// manual advance
task_manager.setActiveTaskPoint(awp);
if (verbose>1) {
printf("# manual advance to %d\n",awp);
}
}
}
if (task_manager.getActiveTaskPointIndex() > awp) {
awp = task_manager.getActiveTaskPointIndex();
}
if (awp>= w.size()) {
return false;
}
if (short_flight && awp>=1) {
return false;
}
if (task_manager.get_common_stats().task_finished) {
return false;
}
return true;
}
static int db_chmod(backend_store_interface* i, const char* rel_path, mode_t mode) {
logger_logf(LOG(i), "rel_path = %s, mode = %ld", rel_path, mode);
ERR_IF_DOESNT_EXIST(i, rel_path);
long owner = get_uid(rel_path);
long you = fuse_get_context()->uid;
if (owner != you) {
logger_logf(LOG(i), "can't chmod since you're not the owner. You are %ld, owner is %ld", you, owner);
return -EACCES;
}
update_mode(rel_path, mode);
return 0;
}
bool QMCDriverFactory::setQMCDriver(int curSeries, xmlNodePtr cur)
{
string curName((const char*)cur->name);
string update_mode("pbyp");
string qmc_mode("invalid");
string multi_tag("no");
string warp_tag("no");
string append_tag("no");
#if defined(QMC_CUDA)
string gpu_tag("yes");
#else
string gpu_tag("no");
#endif
OhmmsAttributeSet aAttrib;
aAttrib.add(qmc_mode,"method");
aAttrib.add(update_mode,"move");
aAttrib.add(multi_tag,"multiple");
aAttrib.add(warp_tag,"warp");
aAttrib.add(append_tag,"append");
aAttrib.add(gpu_tag,"gpu");
aAttrib.put(cur);
bitset<QMC_MODE_MAX> WhatToDo;
bool append_run =(append_tag == "yes");
WhatToDo[SPACEWARP_MODE]= (warp_tag == "yes");
WhatToDo[MULTIPLE_MODE]= (multi_tag == "yes");
WhatToDo[UPDATE_MODE]= (update_mode == "pbyp");
#if defined(QMC_CUDA)
WhatToDo[GPU_MODE ] = (gpu_tag == "yes");
#endif
OhmmsInfo::flush();
QMCRunType newRunType = DUMMY_RUN;
if(curName != "qmc")
qmc_mode=curName;
int nchars=qmc_mode.size();
if(qmc_mode.find("linear") < nchars)
{
if (qmc_mode.find("cslinear") < nchars)
newRunType=CS_LINEAR_OPTIMIZE_RUN;
else
newRunType=LINEAR_OPTIMIZE_RUN;
}
else if(qmc_mode.find("opt") < nchars)
{
newRunType=OPTIMIZE_RUN;
}
else
{
if(qmc_mode.find("ptcl")<nchars)
WhatToDo[UPDATE_MODE]=1;
if(qmc_mode.find("mul")<nchars)
WhatToDo[MULTIPLE_MODE]=1;
if(qmc_mode.find("warp")<nchars)
WhatToDo[SPACEWARP_MODE]=1;
// if (qmc_mode.find("rmcPbyP")<nchars)
// {
// newRunType=RMC_PBYP_RUN;
// }
// else
else if(qmc_mode.find("vmc")<nchars)
{
newRunType=VMC_RUN;
}
else if(qmc_mode.find("dmc")<nchars)
{
newRunType=DMC_RUN;
}
}
unsigned long newQmcMode=WhatToDo.to_ulong();
//initialize to 0
QMCDriver::BranchEngineType* branchEngine=0;
if(qmcDriver)
{
if( newRunType != curRunType || newQmcMode != curQmcMode)
{
if(curRunType == DUMMY_RUN)
{
APP_ABORT("QMCDriverFactory::setQMCDriver\n Other qmc sections cannot come after <qmc method=\"test\">.\n");
}
//pass to the new driver
branchEngine=qmcDriver->getBranchEngine();
delete qmcDriver;
//set to 0 so that a new driver is created
qmcDriver = 0;
//if the current qmc method is different from the previous one, append_run is set to false
append_run = false;
}
else
{
app_log() << " Reusing " << qmcDriver->getEngineName() << endl;
// if(curRunType == DMC_RUN)
qmcDriver->resetComponents(cur);
}
}
if(curSeries == 0)
append_run = false;
//continue with the existing qmcDriver
if(qmcDriver)
return append_run;
//need to create a qmcDriver
curRunType = newRunType;
curQmcMode = newQmcMode;
curQmcModeBits = WhatToDo;
//create a driver
createQMCDriver(cur);
//initialize QMCDriver::myComm
qmcDriver->initCommunicator(myComm);
//branchEngine has to be transferred to a new QMCDriver
if(branchEngine)
qmcDriver->setBranchEngine(branchEngine);
OhmmsInfo::flush();
return append_run;
}
static cb_ret_t
advanced_chown_callback (Widget * w, Widget * sender, widget_msg_t msg, int parm, void *data)
{
WDialog *h = DIALOG (w);
int i;
int f_pos;
unsigned long id;
id = dlg_get_current_widget_id (h);
for (i = 0; i < BUTTONS_PERM; i++)
if (chown_advanced_but[i].id == id)
break;
f_pos = i;
i = 0;
switch (msg)
{
case MSG_DRAW:
chown_refresh ();
chown_info_update ();
return MSG_HANDLED;
case MSG_POST_KEY:
if (f_pos < 3)
b_setpos (f_pos);
return MSG_HANDLED;
case MSG_FOCUS:
if (f_pos < 3)
{
if ((flag_pos / 3) != f_pos)
flag_pos = f_pos * 3;
b_setpos (f_pos);
}
else if (f_pos < BUTTONS_PERM)
flag_pos = f_pos + 6;
return MSG_HANDLED;
case MSG_KEY:
switch (parm)
{
case XCTRL ('b'):
case KEY_LEFT:
if (f_pos < BUTTONS_PERM)
return (dec_flag_pos (f_pos));
break;
case XCTRL ('f'):
case KEY_RIGHT:
if (f_pos < BUTTONS_PERM)
return (inc_flag_pos (f_pos));
break;
case ' ':
if (f_pos < 3)
return MSG_HANDLED;
break;
case '\n':
case KEY_ENTER:
if (f_pos <= 2 || f_pos >= BUTTONS_PERM)
break;
do_enter_key (h, f_pos);
return MSG_HANDLED;
case ALT ('x'):
i++;
case ALT ('w'):
i++;
case ALT ('r'):
parm = i + 3;
for (i = 0; i < 3; i++)
ch_flags[i * 3 + parm - 3] = (x_toggle & (1 << parm)) ? '-' : '+';
x_toggle ^= (1 << parm);
update_mode (h);
dlg_broadcast_msg (h, MSG_DRAW);
send_message (h->current->data, NULL, MSG_FOCUS, 0, NULL);
break;
case XCTRL ('x'):
i++;
case XCTRL ('w'):
i++;
case XCTRL ('r'):
parm = i;
for (i = 0; i < 3; i++)
ch_flags[i * 3 + parm] = (x_toggle & (1 << parm)) ? '-' : '+';
x_toggle ^= (1 << parm);
update_mode (h);
dlg_broadcast_msg (h, MSG_DRAW);
send_message (h->current->data, NULL, MSG_FOCUS, 0, NULL);
break;
case 'x':
i++;
case 'w':
i++;
case 'r':
if (f_pos > 2)
break;
flag_pos = f_pos * 3 + i; /* (strchr(ch_perm,parm)-ch_perm); */
if (BUTTON (h->current->data)->text.start[(flag_pos % 3)] == '-')
ch_flags[flag_pos] = '+';
else
ch_flags[flag_pos] = '-';
update_mode (h);
break;
case '4':
i++;
case '2':
i++;
case '1':
if (f_pos <= 2)
{
flag_pos = i + f_pos * 3;
ch_flags[flag_pos] = '=';
update_mode (h);
}
break;
case '-':
if (f_pos > 2)
break;
case '*':
if (parm == '*')
parm = '=';
case '=':
case '+':
if (f_pos <= 4)
{
ch_flags[flag_pos] = parm;
update_mode (h);
send_message (h, sender, MSG_KEY, KEY_RIGHT, NULL);
if (flag_pos > 8 || (flag_pos % 3) == 0)
dlg_one_down (h);
}
break;
default:
break;
}
return MSG_NOT_HANDLED;
default:
return dlg_default_callback (w, sender, msg, parm, data);
}
}
int dgea_alg::run()
{
timer elapsed_t;
// retrieve algorithm parameters
size_t pop_size=m_ppara->get_pop_size();
size_t num_dims=m_ppara->get_dim();
double vtr=m_ppara->get_vtr();
double d_h=m_ppara->get_dh();
double d_l=m_ppara->get_dh();
int m_cur_run;
int max_run=m_ppara->get_max_run();
// run/trial number
//shared_ptr<progress_display> pprog_dis;
//// algorithm progress indicator from boost
//alloc_prog_indicator(pprog_dis);
// allocate pop
population pop(pop_size);
allocate_pop(pop,num_dims);
population child_pop(pop_size);
allocate_pop(child_pop,num_dims);
// generate algorithm statistics output file name
ofstream stat_file(m_com_out_path.stat_path.c_str());
// alloc stop condition
alloc_stop_cond();
for(m_cur_run=0; m_cur_run<max_run; ++m_cur_run)
{
reset_run_stat();
set_orig_pop(pop);
update_diversity(pop);
print_run_times(stat_file,m_cur_run+1);
print_run_title(stat_file);
// output original population statistics
print_gen_stat(stat_file,1,m_alg_stat);
record_gen_vals(m_alg_stat,m_cur_run);
m_cur_gen=1;
int mode=exploit;
while ( false==(*m_pstop_cond) ) // for every iteration
{
update_mode(mode,d_l,d_h);
gen_child(mode,pop,child_pop);
eval_pop(child_pop, *m_pfunc, m_alg_stat);
select(pop,child_pop);
stat_pop(pop, m_alg_stat);
update_search_radius();
update_diversity(pop);
print_gen_stat(stat_file,m_cur_gen+1,m_alg_stat);
record_gen_vals(m_alg_stat,m_cur_run);
update_conv_stat(vtr);
m_cur_gen++;
}// while single run termination criterion is not met
// single run end
stat_run(pop,m_cur_run);// stat single run for algorithm analysis
if ( is_final_run(m_cur_run,max_run) )
print_run_stat(stat_file,m_alg_stat,max_run);
/*if ( !run_once )
++(*pprog_dis); */
}
print_avg_gen(stat_file,m_alg_stat.run_avg_gen);
// stat and output average time per run by second
m_alg_stat.run_avg_time=elapsed_t.elapsed();
m_alg_stat.run_avg_time /= (max_run*1.0);
print_avg_time(stat_file,m_alg_stat.run_avg_time);
print_best_x(stat_file,m_alg_stat.bst_ind);
write_stat_vals();
cout<<endl;// flush cout output
return 0;
}// end function Run
void Mips32Iss::op_cop0()
{
if (!isCopAccessible(0)) {
m_exception = X_CPU;
return;
}
enum {
MF = 0,
MT = 4,
MFMC0 = 0xb,
CO1 = 0x10,
};
enum {
ERET = 0x18,
WAIT = 0x20,
};
if ( m_ins.coproc.action & CO1 ) {
uint32_t co = m_ins.ins & 0x3f;
switch (co) {
case ERET:
#ifdef SOCLIB_MODULE_DEBUG
std::cout << name() << " ERET ";
#endif
if ( r_status.erl ) {
m_next_pc = r_error_epc;
r_status.erl = 0;
#ifdef SOCLIB_MODULE_DEBUG
std::cout << "erl";
#endif
} else if ( r_status.exl ) {
m_next_pc = r_epc;
r_status.exl = 0;
#ifdef SOCLIB_MODULE_DEBUG
std::cout << " exl";
#endif
} else {
std::cout << m_name << " calling ERET without exl nor erl, ignored" << std::endl;
}
#ifdef SOCLIB_MODULE_DEBUG
std::cout << " next_pc: " << m_next_pc << std::endl;
#endif
m_skip_next_instruction = true;
update_mode();
break;
case WAIT:
m_sleeping = true;
break;
default: // Not handled, so raise an exception
op_ill();
}
} else {
switch (m_ins.coproc.action) {
case MT:
cp0Set( m_ins.coproc.rd, m_ins.coproc.sel, r_gp[m_ins.i.rt] );
break;
case MF:
r_gp[m_ins.coproc.rt] = cp0Get( m_ins.coproc.rd, m_ins.coproc.sel );
break;
case MFMC0:
r_gp[m_ins.coproc.rt] = r_status.whole;
r_status.ie = m_ins.coproc.sc;
break;
default: // Not handled, so raise an exception
op_ill();
}
}
}
void Viewer::set_mode( Mode mode )
{
m_mode = mode;
update_mode( mode );
}
void Viewer::reset()
{
if ( !m_initflag )
{
update_mode( MODELROTATE );
}
// Default mouse information
m_button1 = false;
m_button2 = false;
m_button3 = false;
m_ixpos = 0.0;
m_xpos = 0.0;
m_iypos = 0.0;
m_ypos = 0.0;
m_txpos = 0.0;
// Initialize viewport
for ( int i = 0; i < 4; i += 1 )
{
m_viewport[i] = ( Point2D() );
}
// Default FOV of 30
m_fov = 30.0;
// Set the default far and near plane values
m_near = 2.0;
m_far = 20.0;
// Initialize all the transformation matrices
m_projection = Matrix4x4();
m_modelling = Matrix4x4();
m_viewing = Matrix4x4();
m_scaling = Matrix4x4();
// Start off by pushing the cube back into the screen
Vector4D row1, row2, row3, row4;
row1 = Vector4D( 1, 0, 0, 0 );
row2 = Vector4D( 0, 1, 0, 0 );
row3 = Vector4D( 0, 0, 1, 8 );
row4 = Vector4D( 0, 0, 0, 1 );
m_viewing = Matrix4x4( row1, row2, row3, row4 );
// Initialize the unit cubes
m_unitCube[0] = ( Point3D( 1.0, -1.0, -1.0) );
m_unitCube[1] = ( Point3D(-1.0, -1.0, -1.0) );
m_unitCube[2] = ( Point3D(-1.0, 1.0, -1.0) );
m_unitCube[3] = ( Point3D( 1.0, 1.0, -1.0) );
m_unitCube[4] = ( Point3D(-1.0, -1.0, 1.0) );
m_unitCube[5] = ( Point3D( 1.0, -1.0, 1.0) );
m_unitCube[6] = ( Point3D( 1.0, 1.0, 1.0) );
m_unitCube[7] = ( Point3D(-1.0, 1.0, 1.0) );
for ( int i = 0; i < 8; i += 1 )
{
m_unitCubeTrans[i] = ( Point3D() );
}
// Initialize the gnomons
m_gnomon[0] = ( Point3D(0.0, 0.0, 0.0) );
m_gnomon[1] = ( Point3D(0.5, 0.0, 0.0) );
m_gnomon[2] = ( Point3D(0.0, 0.5, 0.0) );
m_gnomon[3] = ( Point3D(0.0, 0.0, 0.5) );
for ( int i = 0; i < 4; i += 1 )
{
m_gnomonTrans[i] = ( Point3D() );
}
m_viewflag = false;
m_initflag = false;
// Initialize the perspective
set_perspective( m_fov, 1, m_near, m_far );
}
int EKFINS::update(const float gyro[3], const float acc_body[3], const float mag[3], devices::gps_data gps, sensors::px4flow_frame frame, float baro, float dt, bool armed, bool airborne)
{
if (!inited)
init_attitude(acc_body, gyro, mag);
if (update_mode(gyro, acc_body, mag, gps, frame, baro, dt, armed, airborne) < 0)
{
reset();
return -1;
}
bool use_gps = (gps.fix == 3 && gps.position_accuracy_horizontal < 3.5) || (gps_healthy && gps.position_accuracy_horizontal < 7);
bool use_flow = (frame.ground_distance > 0) && (frame.qual > 133);
// prepare matrices
float q0q0 = x[0] * x[0];
float q0q1 = x[0] * x[1];
float q0q2 = x[0] * x[2];
float q0q3 = x[0] * x[3];
float q1q1 = x[1] * x[1];
float q1q2 = x[1] * x[2];
float q1q3 = x[1] * x[3];
float q2q2 = x[2] * x[2];
float q2q3 = x[2] * x[3];
float q3q3 = x[3] * x[3];
float r[9] =
{
q0q0 + q1q1 - q2q2 - q3q3,// 11
2.f * (q1q2 - q0q3), // 21
2.f * (q1q3 + q0q2), // 31
2.f * (q1q2 + q0q3), // 12
q0q0 - q1q1 + q2q2 - q3q3,// 22
2.f * (q2q3 - q0q1), // 32
2.f * (q1q3 - q0q2), // 13
2.f * (q2q3 + q0q1), // 23
q0q0 - q1q1 - q2q2 + q3q3,// 33
};
float dtsq_2 = dt*dt/2;
float dt2 = dt/2;
const float *g = gyro;
acc_ned[0] = r[0]* acc_body[0] + r[1] *acc_body[1] + r[2] * acc_body[2];
acc_ned[1] = r[3]* acc_body[0] + r[4] *acc_body[1] + r[5] * acc_body[2];
acc_ned[2] = -(r[6]* acc_body[0] + r[7] *acc_body[1] + r[8] * acc_body[2] + G_in_ms2);
acc_ned[0] += r[0]* x[6] + r[1] *x[7] + r[2] * x[8];
acc_ned[1] += r[3]* x[6] + r[4] *x[7] + r[5] * x[8];
acc_ned[2] += -(r[6]* x[6] + r[7] *x[7] + r[8] * x[8]);
float Fvq0 = 2*(+x[0]*acc_body[0] - x[3]*acc_body[1] + x[2]*acc_body[2]) * dt;
float Fvq1 = 2*(+x[1]*acc_body[0] + x[2]*acc_body[1] + x[3]*acc_body[2]) * dt;
float Fvq2 = 2*(-x[2]*acc_body[0] + x[1]*acc_body[1] + x[0]*acc_body[2]) * dt;
float Fvq3 = 2*(-x[3]*acc_body[0] - x[0]*acc_body[1] + x[1]*acc_body[2]) * dt;
// process function
matrix F = matrix(19,19,
1.0, dt2*-g[0], dt2*-g[1], dt2*-g[2], dt2*-x[1], dt2*-x[2], dt2*-x[3], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
dt2*g[0], 1.0, dt2*g[2], dt2*-g[1], dt2*x[0], dt2*-x[3], dt2*x[2], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
dt2*g[1], dt2*-g[2], 1.0, dt2*g[0], dt2*x[3], dt2*x[0], dt2*-x[1], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
dt2*g[2], dt2*g[1], dt2*-g[0], 1.0, dt2*-x[2], dt2*x[1], dt2*x[0], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, dt, 0.0, 0.0, dtsq_2*r[0], dtsq_2*r[1], dtsq_2*r[2], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, 0.0, dt, 0.0, dtsq_2*r[3], dtsq_2*r[4], dtsq_2*r[5], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, 0.0, 0.0, dt, -dtsq_2*r[6], -dtsq_2*r[7], -dtsq_2*r[8], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, dt*r[0], dt*r[1], dt*r[2], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, dt*r[3], dt*r[4], dt*r[5], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, -dt*r[6], -dt*r[7], -dt*r[8], 0.0, 0.0, 0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0,
0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0
);
matrix Bu = matrix(19,1,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
dtsq_2 * (r[0]* acc_body[0] + r[1] *acc_body[1] + r[2] * acc_body[2]),
dtsq_2 * (r[3]* acc_body[0] + r[4] *acc_body[1] + r[5] * acc_body[2]),
-dtsq_2 * (r[6]* acc_body[0] + r[7] *acc_body[1] + r[8] * acc_body[2] + G_in_ms2),
dt * (r[0]* acc_body[0] + r[1] *acc_body[1] + r[2] * acc_body[2]),
dt * (r[3]* acc_body[0] + r[4] *acc_body[1] + r[5] * acc_body[2]),
-dt * (r[6]* acc_body[0] + r[7] *acc_body[1] + r[8] * acc_body[2] + G_in_ms2),
0.0,0.0,0.0,
0.0,0.0,0.0
);
// reset observation helper variables
predicted_observation.n = 1;
predicted_observation.m = 0;
zk.n = 1;
zk.m = 0;
H.n = F.n;
H.m = 0;
R_count = 0;
// prediction
float f1 = dt / 0.005f; // 0.005: tuned dt.
float f2 = dt*dt / (0.005f*0.005f);
matrix Q = matrix::diag(19,
1e-7, 1e-7, 1e-7, 1e-7, 1e-9, 1e-9, 1e-9,
4e-3, 4e-3, 4e-6,
1e-4, 1e-4, 1e-6,
1e-9, 1e-9, 1e-9,
1e-7, 1e-7, 5e-7);
Q *= f1;
matrix x1 = F * x + Bu;
F(10,0) = Fvq0; F(10,1) = Fvq1; F(10,2) = Fvq2; F(10,3) = Fvq3;
F(11,0) =-Fvq3; F(11,1) =-Fvq2; F(11,2) = Fvq1; F(11,3) = Fvq0;
F(12,0) = Fvq2; F(12,1) =-Fvq3; F(12,2) =-Fvq0; F(12,3) = Fvq1;
matrix P1 = F * P * F.transpos() + Q;
matrix Q0 = matrix::diag(12,
1e-4,1e-4,1e-4,
3e-5,3e-5,3e-5,
5e-4,5e-4,5e-4,
1e-2,1e-2,1e-2
); // [0-2][3-5][6-8][9-11] = [gyro_error][gyro_bias_random_walk][acc_error][acc_random_walk]
matrix G;
G.m = x.m;
G.n = Q0.m;
memset(G.data, 0, G.m*G.n*sizeof(float));
// dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
// dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
// dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
// dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
G(0,0) = dt2*-x[1]; G(0,1) = dt2*-x[2], G(0,2) = dt2*-x[3];
G(1,0) = dt2*+x[0]; G(1,1) = dt2*+x[2], G(1,2) = dt2*-x[3];
G(2,0) = dt2*+x[0]; G(2,1) = dt2*-x[1], G(2,2) = dt2*+x[3];
G(3,0) = dt2*+x[0]; G(3,1) = dt2*+x[1], G(3,2) = dt2*-x[2];
G(4,3) = 1; G(5,4) = 1; G(6,5) = 1;
G(13,9) = 1; G(14,10) = 1; G(15,11) = 1;
//G(16,12) = 1; G(17,13) = 1; G(18,14) = 1;
G(10, 6) = dt*r[0] ;G(10, 7) = dt*r[1] ;G(10, 8) = dt*r[2];
G(11, 6) = dt*r[3] ;G(11, 7) = dt*r[4] ;G(11, 8) = dt*r[5];
G(12, 6) = dt*r[6] ;G(12, 7) = dt*r[7] ;G(12, 8) = dt*r[8];
// Q = G * Q0 * Q0 * G.transpos();
// observation function and observation
float latitude_to_meter = 40007000.0f/360;
float longtitude_to_meter = 40007000.0f/360*cos(gps.latitude * PI / 180);
float pos_north = (gps.latitude - home_lat) * latitude_to_meter;
float pos_east = (gps.longitude - home_lon) * longtitude_to_meter;
float yaw_gps = gps.direction * 2 * PI / 360.0f;
float vel_north = cos(yaw_gps) * gps.speed;
float vel_east = sin(yaw_gps) * gps.speed;
if (!isnan(home_lat))
{
gps_north = pos_north;
gps_east = pos_east;
}
// baro
add_observation(160.0, baro, x1[9], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
float mag_0z[3] = {mag[0], mag[1], mag[2]};
remove_mag_ned_z(mag_0z, &x[0]);
float m_len = 1.0/sqrt(mag_0z[0] * mag_0z[0] + mag_0z[1] * mag_0z[1] + mag_0z[2] * mag_0z[2]);
float a_len = 1.0/sqrt(acc_body[0]*acc_body[0] + acc_body[1]*acc_body[1] + acc_body[2]*acc_body[2]);
// GNSS: position and velocity, plus vertical velocity
if (use_gps)
{
add_observation(15.0, pos_north, x1[7], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(15.0, pos_east, x1[8], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(5.0, vel_north, x1[10], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(5.0, vel_east, x1[11], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
// add_observation(125.0, gps.climb_rate, x1[12], 0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
}
// still motion, acc and gyro bias with very low noise.
if (still)
{
add_observation(1e-2, acc_body[0]*a_len, -r[6] + x1[13]/G_in_ms2, 2*x[2], 2*-x[3], 2*x[0], 2*-x[1], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0/G_in_ms2,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-2, acc_body[1]*a_len, -r[7] + x1[14]/G_in_ms2, -2*x[1], 2*-x[0], 2*-x[3], 2*-x[2], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0/G_in_ms2,0.0, 0.0,0.0,0.0);
add_observation(1e-2, acc_body[2]*a_len, -r[8] + x1[15]/G_in_ms2, -2*x[0], 2*x[1], 2*x[2], -2*x[3], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0/G_in_ms2, 0.0,0.0,0.0);
add_observation(1e-4, -gyro[0], x1[4], 0.0,0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-4, -gyro[1], x1[5], 0.0,0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-4, -gyro[2], x1[6], 0.0,0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-1, 0, x1[10], 0.0,0.0,0.0,0,0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-1, 0, x1[11], 0.0,0.0,0.0,0,0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-1, 0, x1[12], 0.0,0.0,0.0,0,0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
}
// add a attitude observation if no assisting available(flow or GNSS)
if (!still_inited || (!use_gps && !use_flow && !still))
{
add_observation(1, acc_body[0]*a_len, -r[6] + x1[13]/G_in_ms2, 2*x[2], 2*-x[3], 2*x[0], 2*-x[1], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 1.0/G_in_ms2,0.0,0.0, 0.0,0.0,0.0);
add_observation(1, acc_body[1]*a_len, -r[7] + x1[14]/G_in_ms2, -2*x[1], 2*-x[0], 2*-x[3], 2*-x[2], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,1.0/G_in_ms2,0.0, 0.0,0.0,0.0);
add_observation(1, acc_body[2]*a_len, -r[8] + x1[15]/G_in_ms2, -2*x[0], 2*x[1], 2*x[2], -2*x[3], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,1.0/G_in_ms2, 0.0,0.0,0.0);
}
// mag
if (mag_healthy)
{
add_observation(1e-1, mag_0z[0]*m_len, r[0], 2*x[0], 2*x[1], -2*x[2], -2*x[3], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-1, mag_0z[1]*m_len, r[1], -2*x[3], 2*x[2], 2*x[1], -2*x[0], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(1e-1, mag_0z[2]*m_len, r[2], 2*x[2], 2*x[3], 2*x[0], 2*x[1], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
}
else
{
add_observation(60.0, mag_0z[0]*m_len, r[0], 2*x[0], 2*x[1], -2*x[2], -2*x[3], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(60.0, mag_0z[1]*m_len, r[1], -2*x[3], 2*x[2], 2*x[1], -2*x[0], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
add_observation(60.0, mag_0z[2]*m_len, r[2], 2*x[2], 2*x[3], 2*x[0], 2*x[1], 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0);
}
// correction
matrix R = matrix::diag(R_count, R_diag);
matrix Hx = H * x;
matrix Sk = H * P1 * H.transpos() + R;
matrix K = P1 * H.transpos() * Sk.inversef();
matrix residual = (zk - predicted_observation);
matrix Kres = K*residual;
x = x1 + K*(zk - predicted_observation);
P = (matrix(P1.m) - K*H) * P1;
// renorm
float sqq = 1.0f/sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]);
x[0] *= sqq;
x[1] *= sqq;
x[2] *= sqq;
x[3] *= sqq;
if (x[0] < 0)
{
x[0] = -x[0];
x[1] = -x[1];
x[2] = -x[2];
x[3] = -x[3];
}
return 0;
}
bool QMCDriverFactory::setQMCDriver(int curSeries, xmlNodePtr cur)
{
string curName((const char*)cur->name);
string update_mode("walker");
string qmc_mode("invalid");
string multi_tag("no");
string warp_tag("no");
string append_tag("no");
OhmmsAttributeSet aAttrib;
aAttrib.add(qmc_mode,"method");
aAttrib.add(update_mode,"move");
aAttrib.add(multi_tag,"multiple");
aAttrib.add(warp_tag,"warp");
aAttrib.add(append_tag,"append");
aAttrib.put(cur);
bool append_run =(append_tag == "yes");
bitset<3> WhatToDo;
WhatToDo[SPACEWARP_MODE]= (warp_tag == "yes");
WhatToDo[MULTIPLE_MODE]= (multi_tag == "yes");
WhatToDo[UPDATE_MODE]= (update_mode == "pbyp");
QMCRunType newRunType = DUMMY_RUN;
if(curName != "qmc") qmc_mode=curName;
int nchars=qmc_mode.size();
if(qmc_mode.find("opt") < nchars)
{
newRunType=OPTIMIZE_RUN;
}
else
{
if(qmc_mode.find("vmc")<nchars)
{
newRunType=VMC_RUN;
}
else if(qmc_mode.find("dmc")<nchars)
{
newRunType=DMC_RUN;
}
else if(qmc_mode.find("rmc")<nchars)
{
newRunType=RMC_RUN;
}
if(qmc_mode.find("ptcl")<nchars) WhatToDo[UPDATE_MODE]=1;
if(qmc_mode.find("mul")<nchars) WhatToDo[MULTIPLE_MODE]=1;
if(qmc_mode.find("warp")<nchars) WhatToDo[SPACEWARP_MODE]=1;
}
unsigned long newQmcMode=WhatToDo.to_ulong();
//initialize to 0
QMCDriver::BranchEngineType* branchEngine=0;
if(qmcDriver) {
if(newRunType != curRunType || newQmcMode != curQmcMode) {
//copy the pointer of the BranchEngine
branchEngine=qmcDriver->getBranchEngine();
//remove the qmcDriver
delete qmcDriver;
//set to 0 so that a new driver is created
qmcDriver = 0;
//if the current qmc method is different from the previous one, append_run is set to false
append_run = false;
} else {
app_log() << " Reusing " << qmcDriver->getEngineName() << endl;
}
}
if(curSeries == 0) append_run = false;
//carryon with the existing qmcDriver
if(qmcDriver) return append_run;
//need to create a qmcDriver
curRunType = newRunType;
curQmcMode = newQmcMode;
curQmcModeBits = WhatToDo;
createQMCDriver(cur);
if(qmcComm)
qmcDriver->setCommunicator(qmcComm);
else
qmcDriver->setCommunicator(OHMMS::Controller);
//branchEngine has to be transferred to a new QMCDriver
if(branchEngine) qmcDriver->setBranchEngine(branchEngine);
return append_run;
}
static cb_ret_t
advanced_chown_callback (Dlg_head *h, dlg_msg_t msg, int parm)
{
int i = 0, f_pos = BUTTONS - h->current->dlg_id - single_set - 1;
switch (msg) {
case DLG_DRAW:
chown_refresh ();
chown_info_update ();
return MSG_HANDLED;
case DLG_POST_KEY:
if (f_pos < 3)
b_setpos (f_pos);
return MSG_HANDLED;
case DLG_FOCUS:
if (f_pos < 3) {
if ((flag_pos / 3) != f_pos)
flag_pos = f_pos * 3;
b_setpos (f_pos);
} else if (f_pos < 5)
flag_pos = f_pos + 6;
return MSG_HANDLED;
case DLG_KEY:
switch (parm) {
case XCTRL ('b'):
case KEY_LEFT:
if (f_pos < 5)
return (dec_flag_pos (f_pos));
break;
case XCTRL ('f'):
case KEY_RIGHT:
if (f_pos < 5)
return (inc_flag_pos (f_pos));
break;
case ' ':
if (f_pos < 3)
return MSG_HANDLED;
break;
case '\n':
case KEY_ENTER:
if (f_pos <= 2 || f_pos >= 5)
break;
do_enter_key (h, f_pos);
return MSG_HANDLED;
case ALT ('x'):
i++;
case ALT ('w'):
i++;
case ALT ('r'):
parm = i + 3;
for (i = 0; i < 3; i++)
ch_flags[i * 3 + parm - 3] =
(x_toggle & (1 << parm)) ? '-' : '+';
x_toggle ^= (1 << parm);
update_mode (h);
dlg_broadcast_msg (h, WIDGET_DRAW, 0);
send_message (h->current, WIDGET_FOCUS, 0);
break;
case XCTRL ('x'):
i++;
case XCTRL ('w'):
i++;
case XCTRL ('r'):
parm = i;
for (i = 0; i < 3; i++)
ch_flags[i * 3 + parm] =
(x_toggle & (1 << parm)) ? '-' : '+';
x_toggle ^= (1 << parm);
update_mode (h);
dlg_broadcast_msg (h, WIDGET_DRAW, 0);
send_message (h->current, WIDGET_FOCUS, 0);
break;
case 'x':
i++;
case 'w':
i++;
case 'r':
if (f_pos > 2)
break;
flag_pos = f_pos * 3 + i; /* (strchr(ch_perm,parm)-ch_perm); */
if (((WButton *) h->current)->text.start[(flag_pos % 3)] ==
'-')
ch_flags[flag_pos] = '+';
else
ch_flags[flag_pos] = '-';
update_mode (h);
break;
case '4':
i++;
case '2':
i++;
case '1':
if (f_pos > 2)
break;
flag_pos = i + f_pos * 3;
ch_flags[flag_pos] = '=';
update_mode (h);
break;
case '-':
if (f_pos > 2)
break;
case '*':
if (parm == '*')
parm = '=';
case '=':
case '+':
if (f_pos > 4)
break;
ch_flags[flag_pos] = parm;
update_mode (h);
advanced_chown_callback (h, DLG_KEY, KEY_RIGHT);
if (flag_pos > 8 || !(flag_pos % 3))
dlg_one_down (h);
break;
}
return MSG_NOT_HANDLED;
default:
return default_dlg_callback (h, msg, parm);
}
}