Beispiel #1
0
bool
TaskAutoPilot::update_autopilot(TaskAccessor& task,
                                const AircraftState& state,
                                const AircraftState& state_last)
{
    update_mode(task, state);
    return update_computer(task, state);
}
Beispiel #2
0
bool
TaskAutoPilot::update_autopilot(TaskAccessor& task,
                                const AIRCRAFT_STATE& state,
                                const AIRCRAFT_STATE& state_last)
{
  update_mode(task, state);
  return update_computer(task, state);
}
Beispiel #3
0
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);
    }
}
Beispiel #5
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;
}
Beispiel #6
0
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;
}
Beispiel #8
0
Datei: achown.c Projekt: jskDr/mc
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);
    }
}
Beispiel #9
0
		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();
        }
    }
}
Beispiel #11
0
void Viewer::set_mode( Mode mode )
{
	m_mode = mode;
	update_mode( mode );
}
Beispiel #12
0
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 );
}
Beispiel #13
0
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;
}
Beispiel #14
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;
  }
Beispiel #15
0
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);
    }
}