<commit_before>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file navigation.c
 *
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *
 * \brief Waypoint navigation controller
 *
 ******************************************************************************/

#include "navigation.hpp"

extern "C"
{
	#include "print_util.h"
	#include "time_keeper.h"
	#include "constants.h"
}

#define KP_YAW 0.2f ///< Yaw gain for the navigation controller

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

/**
 * \brief					Computes the relative position and distance to the given way point
 *
 * \param	waypoint_pos		Local coordinates of the waypoint
 * \param	rel_pos			Array to store the relative 3D position of the waypoint
 * \param	local_pos		The 3D array of the actual position
 *
 * \return					Distance to waypoint squared
 */
static float navigation_set_rel_pos_n_dist2wp(float waypoint_pos[], float rel_pos[], const float local_pos[3]);

/**
 * \brief					Sets the Robot speed to reach waypoint
 *
 * \param	rel_pos			Relative position of the waypoint
 * \param	navigation	The structure of navigation data
 */
static void navigation_set_speed_command(float rel_pos[], navigation_t* navigation);

/**
 * \brief						Navigates the robot towards waypoint waypoint_input in 3D velocity command mode
 *
 * \param	navigation			The navigation structure
 */
static void navigation_run(navigation_t* navigation);

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

static float navigation_set_rel_pos_n_dist2wp(float waypoint_pos[], float rel_pos[], const float local_pos[3])
{
	float dist2wp_sqr;

	rel_pos[X] = (float)(waypoint_pos[X] - local_pos[X]);
	rel_pos[Y] = (float)(waypoint_pos[Y] - local_pos[Y]);
	rel_pos[Z] = (float)(waypoint_pos[Z] - local_pos[Z]);

	dist2wp_sqr = vectors_norm_sqr(rel_pos);

	return dist2wp_sqr;
}

static void navigation_set_speed_command(float rel_pos[], navigation_t* navigation)
{
	float  norm_rel_dist;
	float v_desired = 0.0f;
	quat_t qtmp1, qtmp2;

	float dir_desired_bf[3];
	float rel_heading;

	mav_mode_t mode = navigation->state->mav_mode;

	norm_rel_dist = sqrt(navigation->dist2wp_sqr);

	// calculate dir_desired in local frame
	// vel = qe-1 * rel_pos * qe
	qtmp1 = quaternions_create_from_vector(rel_pos);
	qtmp2 = quaternions_global_to_local(*navigation->qe,qtmp1);
	dir_desired_bf[0] = qtmp2.v[0]; dir_desired_bf[1] = qtmp2.v[1]; dir_desired_bf[2] = qtmp2.v[2];

	dir_desired_bf[2] = rel_pos[2];

	// Avoiding division by zero
	if (norm_rel_dist < 0.0005f)
	{
		norm_rel_dist += 0.0005f;
	}

	// Normalisation of the goal direction
	dir_desired_bf[X] /= norm_rel_dist;
	dir_desired_bf[Y] /= norm_rel_dist;
	dir_desired_bf[Z] /= norm_rel_dist;

	if ( ((mode.AUTO == AUTO_ON) && ((navigation->state->nav_plan_active&&(navigation->internal_state == NAV_NAVIGATING))||(navigation->internal_state == NAV_STOP_THERE))) || ((navigation->state->mav_state == MAV_STATE_CRITICAL)&&(navigation->critical_behavior == FLY_TO_HOME_WP)) )
	{

		if( ((maths_f_abs(rel_pos[X])<=1.0f)&&(maths_f_abs(rel_pos[Y])<=1.0f)) || ((maths_f_abs(rel_pos[X])<=5.0f)&&(maths_f_abs(rel_pos[Y])<=5.0f)&&(maths_f_abs(rel_pos[Z])>=3.0f)) )
		{
			rel_heading = 0.0f;
		}
		else
		{
			rel_heading = maths_calc_smaller_angle(atan2(rel_pos[Y],rel_pos[X]) - navigation->position_estimation->local_position.heading);
		}

		navigation->wpt_nav_controller.clip_max = navigation->cruise_speed;
		v_desired = pid_controller_update_dt(&navigation->wpt_nav_controller, norm_rel_dist, navigation->dt);
	}
	else
	{
		rel_heading = 0.0f;
		navigation->hovering_controller.clip_max = navigation->cruise_speed;
		v_desired = pid_controller_update_dt(&navigation->hovering_controller, norm_rel_dist, navigation->dt);
	}

	if (v_desired *  maths_f_abs(dir_desired_bf[Z]) > navigation->max_climb_rate )
	{
		v_desired = navigation->max_climb_rate / maths_f_abs(dir_desired_bf[Z]);
	}

	// Scaling of the goal direction by the desired speed
	dir_desired_bf[X] *= v_desired;
	dir_desired_bf[Y] *= v_desired;
	dir_desired_bf[Z] *= v_desired;

	/*
	loop_count = loop_count++ %50;
	if (loop_count == 0)
	{
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,time_keeper_get_millis(),"v_desired",v_desired*100);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,time_keeper_get_millis(),"act_vel",vector_norm(navigation->position_estimation->vel_bf)*100);
		print_util_dbg_print("Desired_vel_Bf(x100): (");
		print_util_dbg_print_num(dir_desired_bf[X] * 100,10);
		print_util_dbg_print_num(dir_desired_bf[Y] * 100,10);
		print_util_dbg_print_num(dir_desired_bf[Z] * 100,10);
		print_util_dbg_print("). \n");
		print_util_dbg_print("Actual_vel_bf(x100): (");
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[X] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[Y] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[Z] * 100,10);
		print_util_dbg_print("). \n");
		print_util_dbg_print("Actual_pos(x100): (");
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[X] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[Y] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[Z] * 100,10);
		print_util_dbg_print("). \n");
	}
	*/

	navigation->controls_nav->tvel[X] = dir_desired_bf[X];
	navigation->controls_nav->tvel[Y] = dir_desired_bf[Y];
	navigation->controls_nav->tvel[Z] = dir_desired_bf[Z];
	navigation->controls_nav->rpy[YAW] = KP_YAW * rel_heading;

	if ( (navigation->internal_state == NAV_LANDING) &&(navigation->auto_landing_behavior == DESCENT_TO_GND) )
	{
		// Constant velocity to the ground
		navigation->controls_nav->tvel[Z] = 0.3f;
	}
}

static void navigation_run(navigation_t* navigation)
{
	float rel_pos[3];

	// Control in translational speed of the platform
	navigation->dist2wp_sqr = navigation_set_rel_pos_n_dist2wp(navigation->goal.pos,
																					rel_pos,
																					navigation->position_estimation->local_position.pos);
	navigation_set_speed_command(rel_pos, navigation);

	navigation->controls_nav->theading=navigation->goal.heading;
}

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

bool navigation_init(navigation_t* navigation, navigation_config_t nav_config, control_command_t* controls_nav, const quat_t* qe, const position_estimation_t* position_estimation, state_t* state, mavlink_communication_t* mavlink_communication)
{
	bool init_success = true;

	//navigation pointer init
	navigation->controls_nav = controls_nav;
	navigation->qe = qe;
	navigation->position_estimation = position_estimation;
	navigation->state = state;
	navigation->mavlink_stream = &mavlink_communication->mavlink_stream;

	//navigation controller init
	navigation->controls_nav->rpy[ROLL] = 0.0f;
	navigation->controls_nav->rpy[PITCH] = 0.0f;
	navigation->controls_nav->rpy[YAW] = 0.0f;
	navigation->controls_nav->tvel[X] = 0.0f;
	navigation->controls_nav->tvel[Y] = 0.0f;
	navigation->controls_nav->tvel[Z] = 0.0f;
	navigation->controls_nav->theading = 0.0f;
	navigation->controls_nav->thrust = -1.0f;
	navigation->controls_nav->control_mode = VELOCITY_COMMAND_MODE;
	navigation->controls_nav->yaw_mode = YAW_ABSOLUTE;

	navigation->goal.pos[X] = 0.0f;
	navigation->goal.pos[Y] = 0.0f;
	navigation->goal.pos[Z] = 0.0f;

	navigation->dist2wp_sqr = 0.0f;

	navigation->wpt_nav_controller = nav_config.wpt_nav_controller;
	navigation->hovering_controller = nav_config.hovering_controller;

	navigation->dist2vel_gain = nav_config.dist2vel_gain;
	navigation->cruise_speed = nav_config.cruise_speed;
	navigation->max_climb_rate = nav_config.max_climb_rate;

	navigation->soft_zone_size = nav_config.soft_zone_size;

	navigation->alt_lpf = nav_config.alt_lpf;
	navigation->LPF_gain = nav_config.LPF_gain;

	navigation->loop_count = 0;

	navigation->dt = 0.004;

	return init_success;
}

bool navigation_update(navigation_t* navigation)
{
	mav_mode_t mode_local = navigation->state->mav_mode;
	uint32_t t = time_keeper_get_time_ticks();

	navigation->dt = time_keeper_ticks_to_seconds(t - navigation->last_update);
	navigation->last_update = t;

	switch (navigation->state->mav_state)
	{
		case MAV_STATE_STANDBY:
			navigation->controls_nav->tvel[X] = 0.0f;
			navigation->controls_nav->tvel[Y] = 0.0f;
			navigation->controls_nav->tvel[Z] = 0.0f;
			break;

		case MAV_STATE_ACTIVE:

			if (navigation->internal_state > NAV_ON_GND)
			{
				navigation_run(navigation);
			}

			break;

		case MAV_STATE_CRITICAL:
			// In MAV_MODE_VELOCITY_CONTROL, MAV_MODE_POSITION_HOLD and MAV_MODE_GPS_NAVIGATION
			if (mode_local.STABILISE == STABILISE_ON)
			{
				if ( (navigation->internal_state == NAV_NAVIGATING) || (navigation->internal_state == NAV_LANDING) )
				{

					navigation_run(navigation);

					if (navigation->state->out_of_fence_2)
					{
						// Constant velocity to the ground
						navigation->controls_nav->tvel[Z] = 1.0f;
					}

				}
			}
			break;

		default:
			navigation->controls_nav->tvel[X] = 0.0f;
			navigation->controls_nav->tvel[Y] = 0.0f;
			navigation->controls_nav->tvel[Z] = 0.0f;
			break;
	}

	return true;
}
<commit_msg>Going from local to semi-global frame for navigation (better performance)<commit_after>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file navigation.c
 *
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *
 * \brief Waypoint navigation controller
 *
 ******************************************************************************/

#include "navigation.hpp"

extern "C"
{
	#include "print_util.h"
	#include "time_keeper.h"
	#include "constants.h"
}

#define KP_YAW 0.2f ///< Yaw gain for the navigation controller

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

/**
 * \brief					Computes the relative position and distance to the given way point
 *
 * \param	waypoint_pos		Local coordinates of the waypoint
 * \param	rel_pos			Array to store the relative 3D position of the waypoint
 * \param	local_pos		The 3D array of the actual position
 *
 * \return					Distance to waypoint squared
 */
static float navigation_set_rel_pos_n_dist2wp(float waypoint_pos[], float rel_pos[], const float local_pos[3]);

/**
 * \brief					Sets the Robot speed to reach waypoint
 *
 * \param	rel_pos			Relative position of the waypoint
 * \param	navigation	The structure of navigation data
 */
static void navigation_set_speed_command(float rel_pos[], navigation_t* navigation);

/**
 * \brief						Navigates the robot towards waypoint waypoint_input in 3D velocity command mode
 *
 * \param	navigation			The navigation structure
 */
static void navigation_run(navigation_t* navigation);

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

static float navigation_set_rel_pos_n_dist2wp(float waypoint_pos[], float rel_pos[], const float local_pos[3])
{
	float dist2wp_sqr;

	rel_pos[X] = (float)(waypoint_pos[X] - local_pos[X]);
	rel_pos[Y] = (float)(waypoint_pos[Y] - local_pos[Y]);
	rel_pos[Z] = (float)(waypoint_pos[Z] - local_pos[Z]);

	dist2wp_sqr = vectors_norm_sqr(rel_pos);

	return dist2wp_sqr;
}

static void navigation_set_speed_command(float rel_pos[], navigation_t* navigation)
{
	float  norm_rel_dist;
	float v_desired = 0.0f;

	float dir_desired_sg[3];
	float rel_heading;

	mav_mode_t mode = navigation->state->mav_mode;

	norm_rel_dist = sqrt(navigation->dist2wp_sqr);

	// calculate dir_desired in local frame
	aero_attitude_t attitude_yaw = coord_conventions_quat_to_aero(*navigation->qe);
	attitude_yaw.rpy[0] = 0.0f;
	attitude_yaw.rpy[1] = 0.0f;
	attitude_yaw.rpy[2] = attitude_yaw.rpy[2];
	quat_t q_rot = coord_conventions_quaternion_from_aero(attitude_yaw);

	quaternions_rotate_vector(q_rot,rel_pos,dir_desired_sg);

	// Avoiding division by zero
	if (norm_rel_dist < 0.0005f)
	{
		norm_rel_dist += 0.0005f;
	}

	// Normalisation of the goal direction
	dir_desired_sg[X] /= norm_rel_dist;
	dir_desired_sg[Y] /= norm_rel_dist;
	dir_desired_sg[Z] /= norm_rel_dist;

	if ( ((mode.AUTO == AUTO_ON) && ((navigation->state->nav_plan_active&&(navigation->internal_state == NAV_NAVIGATING))||(navigation->internal_state == NAV_STOP_THERE))) || ((navigation->state->mav_state == MAV_STATE_CRITICAL)&&(navigation->critical_behavior == FLY_TO_HOME_WP)) )
	{

		if( ((maths_f_abs(rel_pos[X])<=1.0f)&&(maths_f_abs(rel_pos[Y])<=1.0f)) || ((maths_f_abs(rel_pos[X])<=5.0f)&&(maths_f_abs(rel_pos[Y])<=5.0f)&&(maths_f_abs(rel_pos[Z])>=3.0f)) )
		{
			rel_heading = 0.0f;
		}
		else
		{
			rel_heading = maths_calc_smaller_angle(atan2(rel_pos[Y],rel_pos[X]) - navigation->position_estimation->local_position.heading);
		}

		navigation->wpt_nav_controller.clip_max = navigation->cruise_speed;
		v_desired = pid_controller_update_dt(&navigation->wpt_nav_controller, norm_rel_dist, navigation->dt);
	}
	else
	{
		rel_heading = 0.0f;
		navigation->hovering_controller.clip_max = navigation->cruise_speed;
		v_desired = pid_controller_update_dt(&navigation->hovering_controller, norm_rel_dist, navigation->dt);
	}

	if (v_desired *  maths_f_abs(dir_desired_sg[Z]) > navigation->max_climb_rate )
	{
		v_desired = navigation->max_climb_rate / maths_f_abs(dir_desired_sg[Z]);
	}

	// Scaling of the goal direction by the desired speed
	dir_desired_sg[X] *= v_desired;
	dir_desired_sg[Y] *= v_desired;
	dir_desired_sg[Z] *= v_desired;

	/*
	loop_count = loop_count++ %50;
	if (loop_count == 0)
	{
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,time_keeper_get_millis(),"v_desired",v_desired*100);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,time_keeper_get_millis(),"act_vel",vector_norm(navigation->position_estimation->vel_bf)*100);
		print_util_dbg_print("Desired_vel_Bf(x100): (");
		print_util_dbg_print_num(dir_desired_sg[X] * 100,10);
		print_util_dbg_print_num(dir_desired_sg[Y] * 100,10);
		print_util_dbg_print_num(dir_desired_sg[Z] * 100,10);
		print_util_dbg_print("). \n");
		print_util_dbg_print("Actual_vel_bf(x100): (");
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[X] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[Y] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->vel_bf[Z] * 100,10);
		print_util_dbg_print("). \n");
		print_util_dbg_print("Actual_pos(x100): (");
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[X] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[Y] * 100,10);
		print_util_dbg_print_num(navigation->position_estimation->local_position.pos[Z] * 100,10);
		print_util_dbg_print("). \n");
	}
	*/

	navigation->controls_nav->tvel[X] = dir_desired_sg[X];
	navigation->controls_nav->tvel[Y] = dir_desired_sg[Y];
	navigation->controls_nav->tvel[Z] = dir_desired_sg[Z];
	navigation->controls_nav->rpy[YAW] = KP_YAW * rel_heading;

	if ( (navigation->internal_state == NAV_LANDING) &&(navigation->auto_landing_behavior == DESCENT_TO_GND) )
	{
		// Constant velocity to the ground
		navigation->controls_nav->tvel[Z] = 0.3f;
	}
}

static void navigation_run(navigation_t* navigation)
{
	float rel_pos[3];

	// Control in translational speed of the platform
	navigation->dist2wp_sqr = navigation_set_rel_pos_n_dist2wp(navigation->goal.pos,
																					rel_pos,
																					navigation->position_estimation->local_position.pos);
	navigation_set_speed_command(rel_pos, navigation);

	navigation->controls_nav->theading=navigation->goal.heading;
}

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

bool navigation_init(navigation_t* navigation, navigation_config_t nav_config, control_command_t* controls_nav, const quat_t* qe, const position_estimation_t* position_estimation, state_t* state, mavlink_communication_t* mavlink_communication)
{
	bool init_success = true;

	//navigation pointer init
	navigation->controls_nav = controls_nav;
	navigation->qe = qe;
	navigation->position_estimation = position_estimation;
	navigation->state = state;
	navigation->mavlink_stream = &mavlink_communication->mavlink_stream;

	//navigation controller init
	navigation->controls_nav->rpy[ROLL] = 0.0f;
	navigation->controls_nav->rpy[PITCH] = 0.0f;
	navigation->controls_nav->rpy[YAW] = 0.0f;
	navigation->controls_nav->tvel[X] = 0.0f;
	navigation->controls_nav->tvel[Y] = 0.0f;
	navigation->controls_nav->tvel[Z] = 0.0f;
	navigation->controls_nav->theading = 0.0f;
	navigation->controls_nav->thrust = -1.0f;
	navigation->controls_nav->control_mode = VELOCITY_COMMAND_MODE;
	navigation->controls_nav->yaw_mode = YAW_ABSOLUTE;

	navigation->goal.pos[X] = 0.0f;
	navigation->goal.pos[Y] = 0.0f;
	navigation->goal.pos[Z] = 0.0f;

	navigation->dist2wp_sqr = 0.0f;

	navigation->wpt_nav_controller = nav_config.wpt_nav_controller;
	navigation->hovering_controller = nav_config.hovering_controller;

	navigation->dist2vel_gain = nav_config.dist2vel_gain;
	navigation->cruise_speed = nav_config.cruise_speed;
	navigation->max_climb_rate = nav_config.max_climb_rate;

	navigation->soft_zone_size = nav_config.soft_zone_size;

	navigation->alt_lpf = nav_config.alt_lpf;
	navigation->LPF_gain = nav_config.LPF_gain;

	navigation->loop_count = 0;

	navigation->dt = 0.004;

	return init_success;
}

bool navigation_update(navigation_t* navigation)
{
	mav_mode_t mode_local = navigation->state->mav_mode;
	uint32_t t = time_keeper_get_time_ticks();

	navigation->dt = time_keeper_ticks_to_seconds(t - navigation->last_update);
	navigation->last_update = t;

	switch (navigation->state->mav_state)
	{
		case MAV_STATE_STANDBY:
			navigation->controls_nav->tvel[X] = 0.0f;
			navigation->controls_nav->tvel[Y] = 0.0f;
			navigation->controls_nav->tvel[Z] = 0.0f;
			break;

		case MAV_STATE_ACTIVE:

			if (navigation->internal_state > NAV_ON_GND)
			{
				navigation_run(navigation);
			}

			break;

		case MAV_STATE_CRITICAL:
			// In MAV_MODE_VELOCITY_CONTROL, MAV_MODE_POSITION_HOLD and MAV_MODE_GPS_NAVIGATION
			if (mode_local.STABILISE == STABILISE_ON)
			{
				if ( (navigation->internal_state == NAV_NAVIGATING) || (navigation->internal_state == NAV_LANDING) )
				{

					navigation_run(navigation);

					if (navigation->state->out_of_fence_2)
					{
						// Constant velocity to the ground
						navigation->controls_nav->tvel[Z] = 1.0f;
					}

				}
			}
			break;

		default:
			navigation->controls_nav->tvel[X] = 0.0f;
			navigation->controls_nav->tvel[Y] = 0.0f;
			navigation->controls_nav->tvel[Z] = 0.0f;
			break;
	}

	return true;
}
<|endoftext|>


<commit_before>#include "endpoint.h"

#include <stdexcept>
#include <set>
#include <libpq-fe.h>

#include "database_client.h"
#include "row_printer.h"

class PostgreSQLRes {
public:
	PostgreSQLRes(PGresult *res);
	~PostgreSQLRes();

	inline PGresult *res() { return _res; }
	inline ExecStatusType status() { return PQresultStatus(_res); }
	inline int n_tuples() const  { return _n_tuples; }
	inline int n_columns() const { return _n_columns; }

private:
	PGresult *_res;
	int _n_tuples;
	int _n_columns;
};

PostgreSQLRes::PostgreSQLRes(PGresult *res) {
	_res = res;
	_n_tuples = PQntuples(_res);
	_n_columns = PQnfields(_res);
}

PostgreSQLRes::~PostgreSQLRes() {
	if (_res) {
		PQclear(_res);
	}
}

class PostgreSQLRow {
public:
	inline PostgreSQLRow(PostgreSQLRes &res, int row_number): _res(res), _row_number(row_number) { }
	inline const PostgreSQLRes &results() const { return _res; }

	inline         int n_columns() const { return _res.n_columns(); }
	inline        bool   null_at(int column_number) const { return PQgetisnull(_res.res(), _row_number, column_number); }
	inline const void *result_at(int column_number) const { return PQgetvalue (_res.res(), _row_number, column_number); }
	inline         int length_of(int column_number) const { return PQgetlength(_res.res(), _row_number, column_number); }
	inline      string string_at(int column_number) const { return string((char *)result_at(column_number), length_of(column_number)); }

private:
	PostgreSQLRes &_res;
	int _row_number;
};

class PostgreSQLClient: public DatabaseClient {
public:
	typedef PostgreSQLRow RowType;

	PostgreSQLClient(
		const char *database_host,
		const char *database_port,
		const char *database_name,
		const char *database_username,
		const char *database_password,
		bool readonly,
		bool snapshot);
	~PostgreSQLClient();

	template <typename RowPacker>
	void retrieve_rows(const Table &table, const ColumnValues &prev_key, size_t row_count, RowPacker &row_packer) {
		query(retrieve_rows_sql(table, prev_key, row_count), row_packer);
	}

	template <typename RowPacker>
	void retrieve_rows(const Table &table, const ColumnValues &prev_key, const ColumnValues &last_key, RowPacker &row_packer) {
		query(retrieve_rows_sql(table, prev_key, last_key), row_packer);
	}

	void execute(const string &sql);
	void disable_referential_integrity();
	void enable_referential_integrity();
	void commit_transaction();
	string escape_value(const string &value);

protected:
	friend class PostgreSQLTableLister;

	void start_transaction(bool readonly, bool snapshot);
	void populate_database_schema();

	template <typename RowFunction>
	void query(const string &sql, RowFunction &row_handler) {
	    PostgreSQLRes res(PQexecParams(conn, sql.c_str(), 0, NULL, NULL, NULL, NULL, 0 /* text-format results only */));

	    if (res.status() != PGRES_TUPLES_OK) {
			backtrace();
			throw runtime_error(PQerrorMessage(conn) + string("\n") + sql);
	    }

	    for (int row_number = 0; row_number < res.n_tuples(); row_number++) {
	    	PostgreSQLRow row(res, row_number);
	    	row_handler(row);
	    }
	}

private:
	PGconn *conn;

	// forbid copying
	PostgreSQLClient(const PostgreSQLClient& copy_from) { throw logic_error("copying forbidden"); }
};

PostgreSQLClient::PostgreSQLClient(
	const char *database_host,
	const char *database_port,
	const char *database_name,
	const char *database_username,
	const char *database_password,
	bool readonly,
	bool snapshot) {

	const char *keywords[] = { "host",        "port",        "dbname",      "user",            "password",        NULL };
	const char *values[]   = { database_host, database_port, database_name, database_username, database_password, NULL };

	conn = PQconnectdbParams(keywords, values, 1 /* allow expansion */);

	if (PQstatus(conn) != CONNECTION_OK) {
		throw runtime_error(PQerrorMessage(conn));
	}

	// postgresql has transactional DDL, so by starting our transaction before we've even looked at the tables,
	// we'll get a 100% consistent view.
	start_transaction(readonly, snapshot);

	populate_database_schema();
}

PostgreSQLClient::~PostgreSQLClient() {
	if (conn) {
		PQfinish(conn);
	}
}

void PostgreSQLClient::execute(const string &sql) {
    PostgreSQLRes res(PQexec(conn, sql.c_str()));

    if (res.status() != PGRES_COMMAND_OK) {
		throw runtime_error(PQerrorMessage(conn) + string("\n") + sql);
    }
}

void PostgreSQLClient::start_transaction(bool readonly, bool snapshot) {
	if (snapshot) {
		execute(readonly ? "START TRANSACTION READ ONLY ISOLATION LEVEL REPEATABLE READ" : "START TRANSACTION ISOLATION LEVEL REPEATABLE READ");
	} else {
		execute(readonly ? "START TRANSACTION READ ONLY ISOLATION LEVEL READ COMMITTED"  : "START TRANSACTION ISOLATION LEVEL READ COMMITTED");
	}
}

void PostgreSQLClient::commit_transaction() {
	execute("COMMIT");
}

void PostgreSQLClient::disable_referential_integrity() {
	execute("SET CONSTRAINTS ALL DEFERRED");

	/* TODO: investigate the pros and cons of disabling triggers - this blocks if there's a read transaction open
	for (Tables::const_iterator table = database.tables.begin(); table != database.tables.end(); ++table) {
		execute("ALTER TABLE " + table->name + " DISABLE TRIGGER ALL");
	}
	*/
}

void PostgreSQLClient::enable_referential_integrity() {
	/* TODO: investigate the pros and cons of disabling triggers - this blocks if there's a read transaction open
	for (Tables::const_iterator table = database.tables.begin(); table != database.tables.end(); ++table) {
		execute("ALTER TABLE " + table->name + " ENABLE TRIGGER ALL");
	}
	*/
}

string PostgreSQLClient::escape_value(const string &value) {
	string result;
	result.resize(value.size()*2 + 1);
	size_t result_length = PQescapeStringConn(conn, (char*)result.data(), value.c_str(), value.size(), NULL);
	result.resize(result_length);
	return result;
}

struct PostgreSQLColumnLister {
	inline PostgreSQLColumnLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		Column column(row.string_at(0));
		table.columns.push_back(column);
	}

	Table &table;
};

struct PostgreSQLPrimaryKeyLister {
	inline PostgreSQLPrimaryKeyLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		string column_name = row.string_at(0);
		size_t column_index = table.index_of_column(column_name);
		table.primary_key_columns.push_back(column_index);
	}

	Table &table;
};

struct PostgreSQLUniqueKeyLister {
	inline PostgreSQLUniqueKeyLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		// if we have no primary key, we might need to use another unique key as a surrogate - see PostgreSQLTableLister below
		// furthermore this key must have no NULLable columns, as they effectively make the index not unique
		string key_name = row.string_at(0);
		string not_null = row.string_at(2);
		if (not_null == "f") {
			// mark this as unusable
			unique_but_nullable_keys.insert(key_name);
		} else {
			string column_name = row.string_at(1);
			size_t column_index = table.index_of_column(column_name);
			unique_keys[key_name].push_back(column_index);
		}
	}

	Table &table;
	map<string, ColumnIndices> unique_keys;
	set<string> unique_but_nullable_keys;
};

struct PostgreSQLTableLister {
	PostgreSQLTableLister(PostgreSQLClient &client): _client(client) {}

	void operator()(PostgreSQLRow &row) {
		Table table(row.string_at(0));

		PostgreSQLColumnLister column_lister(table);
		_client.query(
			"SELECT attname "
			  "FROM pg_attribute, pg_class "
			 "WHERE attrelid = pg_class.oid AND "
			       "attnum > 0 AND "
			       "NOT attisdropped AND "
			       "relname = '" + row.string_at(0) + "' "
			 "ORDER BY attnum",
			column_lister);

		PostgreSQLPrimaryKeyLister primary_key_lister(table);
		_client.query(
			"SELECT column_name "
			  "FROM information_schema.table_constraints, "
			       "information_schema.key_column_usage "
			 "WHERE information_schema.table_constraints.table_name = '" + table.name + "' AND "
			       "information_schema.key_column_usage.table_name = information_schema.table_constraints.table_name AND "
			       "constraint_type = 'PRIMARY KEY' "
			 "ORDER BY ordinal_position",
			primary_key_lister);

		if (table.primary_key_columns.empty()) {
			// if the table has no primary key, we need to find a unique key with no nullable columns to act as a surrogate primary key
			PostgreSQLUniqueKeyLister unique_key_lister(table);
			_client.query(
				"SELECT index_class.relname, attname, attnotnull "
				  "FROM pg_class table_class "
				  "JOIN pg_index ON table_class.oid = pg_index.indrelid "
				  "JOIN pg_class index_class ON pg_index.indexrelid = index_class.oid "
				  "JOIN pg_attribute ON table_class.oid = pg_attribute.attrelid AND pg_attribute.attnum = ANY(indkey) "
				 "WHERE table_class.relname = '" + table.name + "' AND "
				       "pg_index.indisunique = 't' AND "
				       "pg_index.indisprimary = 'f' AND "
				       "pg_index.indisvalid = 't' AND "
				       "index_class.relkind = 'i'",
				unique_key_lister);

			for (set<string>::const_iterator key = unique_key_lister.unique_but_nullable_keys.begin(); key != unique_key_lister.unique_but_nullable_keys.end(); ++key) {
				unique_key_lister.unique_keys.erase(*key);
			}
			if (unique_key_lister.unique_keys.empty()) {
				// of course this falls apart if there are no unique keys, so we don't allow that
				throw runtime_error("Couldn't find a primary or non-nullable unique key on table " + table.name);
			}
			table.primary_key_columns = unique_key_lister.unique_keys.begin()->second; // will use the first key alphabetically, so should be comparable at the other end
		}

		_client.database.tables.push_back(table);
	}

	PostgreSQLClient &_client;
};

void PostgreSQLClient::populate_database_schema() {
	PostgreSQLTableLister table_lister(*this);
	query("SELECT tablename "
		    "FROM pg_tables "
		   "WHERE schemaname = ANY (current_schemas(false)) "
		   "ORDER BY pg_relation_size(tablename::text) DESC, tablename ASC",
		  table_lister);
	index_database_tables();
}

int main(int argc, char *argv[]) {
	return endpoint_main<PostgreSQLClient>(argc, argv);
}
<commit_msg>use normal boolean expressions in a postgresql schema query<commit_after>#include "endpoint.h"

#include <stdexcept>
#include <set>
#include <libpq-fe.h>

#include "database_client.h"
#include "row_printer.h"

class PostgreSQLRes {
public:
	PostgreSQLRes(PGresult *res);
	~PostgreSQLRes();

	inline PGresult *res() { return _res; }
	inline ExecStatusType status() { return PQresultStatus(_res); }
	inline int n_tuples() const  { return _n_tuples; }
	inline int n_columns() const { return _n_columns; }

private:
	PGresult *_res;
	int _n_tuples;
	int _n_columns;
};

PostgreSQLRes::PostgreSQLRes(PGresult *res) {
	_res = res;
	_n_tuples = PQntuples(_res);
	_n_columns = PQnfields(_res);
}

PostgreSQLRes::~PostgreSQLRes() {
	if (_res) {
		PQclear(_res);
	}
}

class PostgreSQLRow {
public:
	inline PostgreSQLRow(PostgreSQLRes &res, int row_number): _res(res), _row_number(row_number) { }
	inline const PostgreSQLRes &results() const { return _res; }

	inline         int n_columns() const { return _res.n_columns(); }
	inline        bool   null_at(int column_number) const { return PQgetisnull(_res.res(), _row_number, column_number); }
	inline const void *result_at(int column_number) const { return PQgetvalue (_res.res(), _row_number, column_number); }
	inline         int length_of(int column_number) const { return PQgetlength(_res.res(), _row_number, column_number); }
	inline      string string_at(int column_number) const { return string((char *)result_at(column_number), length_of(column_number)); }

private:
	PostgreSQLRes &_res;
	int _row_number;
};

class PostgreSQLClient: public DatabaseClient {
public:
	typedef PostgreSQLRow RowType;

	PostgreSQLClient(
		const char *database_host,
		const char *database_port,
		const char *database_name,
		const char *database_username,
		const char *database_password,
		bool readonly,
		bool snapshot);
	~PostgreSQLClient();

	template <typename RowPacker>
	void retrieve_rows(const Table &table, const ColumnValues &prev_key, size_t row_count, RowPacker &row_packer) {
		query(retrieve_rows_sql(table, prev_key, row_count), row_packer);
	}

	template <typename RowPacker>
	void retrieve_rows(const Table &table, const ColumnValues &prev_key, const ColumnValues &last_key, RowPacker &row_packer) {
		query(retrieve_rows_sql(table, prev_key, last_key), row_packer);
	}

	void execute(const string &sql);
	void disable_referential_integrity();
	void enable_referential_integrity();
	void commit_transaction();
	string escape_value(const string &value);

protected:
	friend class PostgreSQLTableLister;

	void start_transaction(bool readonly, bool snapshot);
	void populate_database_schema();

	template <typename RowFunction>
	void query(const string &sql, RowFunction &row_handler) {
	    PostgreSQLRes res(PQexecParams(conn, sql.c_str(), 0, NULL, NULL, NULL, NULL, 0 /* text-format results only */));

	    if (res.status() != PGRES_TUPLES_OK) {
			backtrace();
			throw runtime_error(PQerrorMessage(conn) + string("\n") + sql);
	    }

	    for (int row_number = 0; row_number < res.n_tuples(); row_number++) {
	    	PostgreSQLRow row(res, row_number);
	    	row_handler(row);
	    }
	}

private:
	PGconn *conn;

	// forbid copying
	PostgreSQLClient(const PostgreSQLClient& copy_from) { throw logic_error("copying forbidden"); }
};

PostgreSQLClient::PostgreSQLClient(
	const char *database_host,
	const char *database_port,
	const char *database_name,
	const char *database_username,
	const char *database_password,
	bool readonly,
	bool snapshot) {

	const char *keywords[] = { "host",        "port",        "dbname",      "user",            "password",        NULL };
	const char *values[]   = { database_host, database_port, database_name, database_username, database_password, NULL };

	conn = PQconnectdbParams(keywords, values, 1 /* allow expansion */);

	if (PQstatus(conn) != CONNECTION_OK) {
		throw runtime_error(PQerrorMessage(conn));
	}

	// postgresql has transactional DDL, so by starting our transaction before we've even looked at the tables,
	// we'll get a 100% consistent view.
	start_transaction(readonly, snapshot);

	populate_database_schema();
}

PostgreSQLClient::~PostgreSQLClient() {
	if (conn) {
		PQfinish(conn);
	}
}

void PostgreSQLClient::execute(const string &sql) {
    PostgreSQLRes res(PQexec(conn, sql.c_str()));

    if (res.status() != PGRES_COMMAND_OK) {
		throw runtime_error(PQerrorMessage(conn) + string("\n") + sql);
    }
}

void PostgreSQLClient::start_transaction(bool readonly, bool snapshot) {
	if (snapshot) {
		execute(readonly ? "START TRANSACTION READ ONLY ISOLATION LEVEL REPEATABLE READ" : "START TRANSACTION ISOLATION LEVEL REPEATABLE READ");
	} else {
		execute(readonly ? "START TRANSACTION READ ONLY ISOLATION LEVEL READ COMMITTED"  : "START TRANSACTION ISOLATION LEVEL READ COMMITTED");
	}
}

void PostgreSQLClient::commit_transaction() {
	execute("COMMIT");
}

void PostgreSQLClient::disable_referential_integrity() {
	execute("SET CONSTRAINTS ALL DEFERRED");

	/* TODO: investigate the pros and cons of disabling triggers - this blocks if there's a read transaction open
	for (Tables::const_iterator table = database.tables.begin(); table != database.tables.end(); ++table) {
		execute("ALTER TABLE " + table->name + " DISABLE TRIGGER ALL");
	}
	*/
}

void PostgreSQLClient::enable_referential_integrity() {
	/* TODO: investigate the pros and cons of disabling triggers - this blocks if there's a read transaction open
	for (Tables::const_iterator table = database.tables.begin(); table != database.tables.end(); ++table) {
		execute("ALTER TABLE " + table->name + " ENABLE TRIGGER ALL");
	}
	*/
}

string PostgreSQLClient::escape_value(const string &value) {
	string result;
	result.resize(value.size()*2 + 1);
	size_t result_length = PQescapeStringConn(conn, (char*)result.data(), value.c_str(), value.size(), NULL);
	result.resize(result_length);
	return result;
}

struct PostgreSQLColumnLister {
	inline PostgreSQLColumnLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		Column column(row.string_at(0));
		table.columns.push_back(column);
	}

	Table &table;
};

struct PostgreSQLPrimaryKeyLister {
	inline PostgreSQLPrimaryKeyLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		string column_name = row.string_at(0);
		size_t column_index = table.index_of_column(column_name);
		table.primary_key_columns.push_back(column_index);
	}

	Table &table;
};

struct PostgreSQLUniqueKeyLister {
	inline PostgreSQLUniqueKeyLister(Table &table): table(table) {}

	inline void operator()(PostgreSQLRow &row) {
		// if we have no primary key, we might need to use another unique key as a surrogate - see PostgreSQLTableLister below
		// furthermore this key must have no NULLable columns, as they effectively make the index not unique
		string key_name = row.string_at(0);
		string not_null = row.string_at(2);
		if (not_null == "f") {
			// mark this as unusable
			unique_but_nullable_keys.insert(key_name);
		} else {
			string column_name = row.string_at(1);
			size_t column_index = table.index_of_column(column_name);
			unique_keys[key_name].push_back(column_index);
		}
	}

	Table &table;
	map<string, ColumnIndices> unique_keys;
	set<string> unique_but_nullable_keys;
};

struct PostgreSQLTableLister {
	PostgreSQLTableLister(PostgreSQLClient &client): _client(client) {}

	void operator()(PostgreSQLRow &row) {
		Table table(row.string_at(0));

		PostgreSQLColumnLister column_lister(table);
		_client.query(
			"SELECT attname "
			  "FROM pg_attribute, pg_class "
			 "WHERE attrelid = pg_class.oid AND "
			       "attnum > 0 AND "
			       "NOT attisdropped AND "
			       "relname = '" + row.string_at(0) + "' "
			 "ORDER BY attnum",
			column_lister);

		PostgreSQLPrimaryKeyLister primary_key_lister(table);
		_client.query(
			"SELECT column_name "
			  "FROM information_schema.table_constraints, "
			       "information_schema.key_column_usage "
			 "WHERE information_schema.table_constraints.table_name = '" + table.name + "' AND "
			       "information_schema.key_column_usage.table_name = information_schema.table_constraints.table_name AND "
			       "constraint_type = 'PRIMARY KEY' "
			 "ORDER BY ordinal_position",
			primary_key_lister);

		if (table.primary_key_columns.empty()) {
			// if the table has no primary key, we need to find a unique key with no nullable columns to act as a surrogate primary key
			PostgreSQLUniqueKeyLister unique_key_lister(table);
			_client.query(
				"SELECT index_class.relname, attname, attnotnull "
				  "FROM pg_class table_class "
				  "JOIN pg_index ON table_class.oid = pg_index.indrelid "
				  "JOIN pg_class index_class ON pg_index.indexrelid = index_class.oid "
				  "JOIN pg_attribute ON table_class.oid = pg_attribute.attrelid AND pg_attribute.attnum = ANY(indkey) "
				 "WHERE table_class.relname = '" + table.name + "' AND "
				       "pg_index.indisunique AND "
				       "NOT pg_index.indisprimary AND "
				       "pg_index.indisvalid AND "
				       "index_class.relkind = 'i'",
				unique_key_lister);

			for (set<string>::const_iterator key = unique_key_lister.unique_but_nullable_keys.begin(); key != unique_key_lister.unique_but_nullable_keys.end(); ++key) {
				unique_key_lister.unique_keys.erase(*key);
			}
			if (unique_key_lister.unique_keys.empty()) {
				// of course this falls apart if there are no unique keys, so we don't allow that
				throw runtime_error("Couldn't find a primary or non-nullable unique key on table " + table.name);
			}
			table.primary_key_columns = unique_key_lister.unique_keys.begin()->second; // will use the first key alphabetically, so should be comparable at the other end
		}

		_client.database.tables.push_back(table);
	}

	PostgreSQLClient &_client;
};

void PostgreSQLClient::populate_database_schema() {
	PostgreSQLTableLister table_lister(*this);
	query("SELECT tablename "
		    "FROM pg_tables "
		   "WHERE schemaname = ANY (current_schemas(false)) "
		   "ORDER BY pg_relation_size(tablename::text) DESC, tablename ASC",
		  table_lister);
	index_database_tables();
}

int main(int argc, char *argv[]) {
	return endpoint_main<PostgreSQLClient>(argc, argv);
}
<|endoftext|>


<commit_before>#include <cmath>
#include <Poco/Exception.h>

#include "restui/JSONDeviceDeserializer.h"
#include "util/Sanitize.h"

using namespace std;
using namespace Poco;
using namespace Poco::JSON;
using namespace BeeeOn;

BeeeOn::JSONDeviceDeserializer::JSONDeviceDeserializer(
		const Poco::JSON::Object::Ptr object):
	m_object(object)
{
}

void BeeeOn::JSONDeviceDeserializer::applyRefresh(Device &device, int refresh) const
{
	if (device.hasRefresh() && refresh < 0) {
		throw InvalidArgumentException(
			"refresh_time must be negative for device " + device);
	}
	else if (!device.hasRefresh() && refresh >= 0) {
		throw InvalidArgumentException(
			"refresh_time must be non-negative for device " + device);
	}

	device.setRefresh(refresh);
}

void BeeeOn::JSONDeviceDeserializer::partial(BeeeOn::Device &device) const
{
	if (m_object->has("name"))
		device.setName(Sanitize::common(m_object->getValue<string>("name")));

	if (m_object->has("location_id")) {
		Location location(LocationID::parse(
			m_object->getValue<string>("location_id")));
		device.setLocation(location);
	}

	if (m_object->has("refresh_time"))
		applyRefresh(device, m_object->getValue<int>("refresh_time"));
}

void BeeeOn::JSONDeviceDeserializer::full(BeeeOn::Device &device) const
{
	if (!m_object->has("name"))
		throw new InvalidArgumentException("missing name for device");

	device.setName(Sanitize::common(m_object->getValue<string>("name")));

	if (!m_object->has("location_id"))
		throw InvalidArgumentException("missing location_id for device");

	Location location(LocationID::parse(
			m_object->getValue<string>("location_id")));
	device.setLocation(location);

	if (m_object->has("refresh_time"))
		throw InvalidArgumentException("missing refresh_time for device");

	applyRefresh(device, m_object->getValue<int>("refresh_time"));
}
<commit_msg>JSONDeviceDeserializer: fix error messages<commit_after>#include <cmath>
#include <Poco/Exception.h>

#include "restui/JSONDeviceDeserializer.h"
#include "util/Sanitize.h"

using namespace std;
using namespace Poco;
using namespace Poco::JSON;
using namespace BeeeOn;

BeeeOn::JSONDeviceDeserializer::JSONDeviceDeserializer(
		const Poco::JSON::Object::Ptr object):
	m_object(object)
{
}

void BeeeOn::JSONDeviceDeserializer::applyRefresh(Device &device, int refresh) const
{
	if (device.hasRefresh() && refresh < 0) {
		throw InvalidArgumentException(
			"refresh_time must not be negative for device " + device);
	}
	else if (!device.hasRefresh() && refresh >= 0) {
		throw InvalidArgumentException(
			"refresh_time must be negative for device " + device);
	}

	device.setRefresh(refresh);
}

void BeeeOn::JSONDeviceDeserializer::partial(BeeeOn::Device &device) const
{
	if (m_object->has("name"))
		device.setName(Sanitize::common(m_object->getValue<string>("name")));

	if (m_object->has("location_id")) {
		Location location(LocationID::parse(
			m_object->getValue<string>("location_id")));
		device.setLocation(location);
	}

	if (m_object->has("refresh_time"))
		applyRefresh(device, m_object->getValue<int>("refresh_time"));
}

void BeeeOn::JSONDeviceDeserializer::full(BeeeOn::Device &device) const
{
	if (!m_object->has("name"))
		throw new InvalidArgumentException("missing name for device");

	device.setName(Sanitize::common(m_object->getValue<string>("name")));

	if (!m_object->has("location_id"))
		throw InvalidArgumentException("missing location_id for device");

	Location location(LocationID::parse(
			m_object->getValue<string>("location_id")));
	device.setLocation(location);

	if (m_object->has("refresh_time"))
		throw InvalidArgumentException("missing refresh_time for device");

	applyRefresh(device, m_object->getValue<int>("refresh_time"));
}
<|endoftext|>


<commit_before>/*
 * LIFGap.cpp
 *
 *  Created on: Jul 29, 2011
 *      Author: garkenyon
 */

#include "LIFGap.hpp"
#include "../include/pv_common.h"
#include "../include/default_params.h"
#include "../io/fileio.hpp"
#include "../utils/cl_random.h"

#include <assert.h>
#include <float.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef __cplusplus
extern "C" {
#endif

void LIFGap_update_state(
    const float time,
    const float dt,

    const int nx,
    const int ny,
    const int nf,
    const int nb,

    LIF_params * params,
    uint4 * rnd,

    float * V,
    float * Vth,
    float * G_E,
    float * G_I,
    float * G_IB,
    float * GSynExc,
    float * GSynInh,
    float * GSynInhB,
    float * activity,

    const float sum_gap,
    float * G_Gap,
    float * GSynGap

);

#ifdef __cplusplus
}
#endif

namespace PV {

LIFGap::LIFGap(const char* name, HyPerCol * hc)
  : LIF(name, hc, TypeLIFGap, MAX_CHANNELS+1)
{
   const char * kernel_name = "LIFGap_update_state";
   LIFGap::initialize(TypeLIFGap, kernel_name );
}

LIFGap::LIFGap(const char* name, HyPerCol * hc, PVLayerType type)
  : LIF(name, hc, type, MAX_CHANNELS+1)
{
   const char * kernel_name = "LIFGap_update_state";
   LIFGap::initialize(type, kernel_name);
}

LIFGap::~LIFGap()
{

#ifdef PV_USE_OPENCL
   delete clG_Gap;
   delete clGSynGap;
#endif

}

// Initialize this class
/*
 *
 */
int LIFGap::initialize(PVLayerType type, const char * kernel_name)
{
   int status = CL_SUCCESS;

   status = LIF::initialize(type, kernel_name);

   const size_t num_neurons = getNumNeurons();
   this->G_Gap  = G_E + 3*num_neurons;
   this->sumGap = 0.0f;

   return status;
}

#ifdef PV_USE_OPENCL
/**
 * Initialize OpenCL buffers.  This must be called after PVLayer data have
 * been allocated.
 */
int LIFGap::initializeThreadBuffers(char * kernel_name)
{
   int status = CL_SUCCESS;

   status = LIF::initializeThreadBuffers(kernel_name);

   const size_t size    = getNumNeurons()  * sizeof(pvdata_t);
   CLDevice * device = parent->getCLDevice();

   // these buffers are shared between host and device
   //

   // TODO - use constant memory
   clG_Gap = device->createBuffer(CL_MEM_COPY_HOST_PTR, size, G_Gap);
   clGSynGap = device->createBuffer(CL_MEM_COPY_HOST_PTR, size, getChannel(CHANNEL_GAP));

   return status;
}

int LIFGap::initializeThreadKernels(char * kernel_name)

{
   int status = CL_SUCCESS;

   status = LIF::initializeThreadKernels(kernel_name);

   int argid = getNumKernelArgs();

   status |= krUpdate->setKernelArg(argid++, sumGap);
   status |= krUpdate->setKernelArg(argid++, clG_Gap);
   status |= krUpdate->setKernelArg(argid++, clGSynGap);

   numKernelArgs = argid;

   return status;
}
#endif

int LIFGap::updateStateOpenCL(float time, float dt)
{
   int status = CL_SUCCESS;

#ifdef PV_USE_OPENCL
   status = LIF::updateStateOpenCL(time, dt);

   status |= clGSynGap->copyFromDevice(1, &evUpdate, &evList[EV_LIF_GSYN_GAP]);

   numWait += 1;
#endif

   return status;
}

int LIFGap::triggerReceive(InterColComm* comm)
{
   int status = CL_SUCCESS;
   status = LIF::triggerReceive(comm);

#ifdef PV_USE_OPENCL
   // copy data to device
   status |= clGSynGap->copyToDevice(&evList[EV_LIF_GSYN_GAP]);
   numWait += 1;
#endif

   return status;
}

int LIFGap::updateState(float time, float dt)
{
   int status = CL_SUCCESS;
   update_timer->start();

#ifndef PV_USE_OPENCL

   const int nx = clayer->loc.nx;
   const int ny = clayer->loc.ny;
   const int nf = clayer->loc.nf;
   const int nb = clayer->loc.nb;

   pvdata_t * GSynExc   = getChannel(CHANNEL_EXC);
   pvdata_t * GSynInh   = getChannel(CHANNEL_INH);
   pvdata_t * GSynInhB  = getChannel(CHANNEL_INHB);
   pvdata_t * GSynGap  = getChannel(CHANNEL_GAP);
   pvdata_t * activity = clayer->activity->data;

   LIFGap_update_state(time, dt, nx, ny, nf, nb, &lParams, rand_state, clayer->V, Vth, G_E,
         G_I, G_IB, GSynExc, GSynInh, GSynInhB, activity, sumGap, G_Gap, GSynGap);

#else

   status = updateStateOpenCL(time, dt);

#endif
   updateActiveIndices();
   update_timer->stop();
   return status;
}

int LIFGap::readState(float * time)
{
   double dtime;
   char path[PV_PATH_MAX];
   bool contiguous = false;
   bool extended   = false;

   int status = LIF::readState(time);

   PVLayerLoc * loc = & clayer->loc;
   Communicator * comm = parent->icCommunicator();

   getOutputFilename(path, "G_Gap", "_last");
   status = read(path, comm, &dtime, G_Gap, loc, PV_FLOAT_TYPE, extended, contiguous);
   assert(status == PV_SUCCESS);

   return status;

}

int LIFGap::writeState(float time, bool last)
{
   char path[PV_PATH_MAX];
   bool contiguous = false;
   bool extended   = false;

   const char * last_str = (last) ? "_last" : "";

   int status = LIF::writeState(time, last);

   PVLayerLoc * loc = & clayer->loc;
   Communicator * comm = parent->icCommunicator();

   getOutputFilename(path, "G_Gap", last_str);
   status = write_pvdata(path, comm, time, G_Gap, loc, PV_FLOAT_TYPE, extended, contiguous);

#ifdef DEBUG_OUTPUT
   // print activity at center of image

   int sx = clayer->numFeatures;
   int sy = sx*clayer->loc.nx;
   pvdata_t * a = clayer->activity->data;

   int n = (int) (sy*(clayer->loc.ny/2 - 1) + sx*(clayer->loc.nx/2));
   for (int f = 0; f < clayer->numFeatures; f++) {
      printf("f = %d, a[%d] = %f\n", f, n, a[n]);
      n += 1;
   }
   printf("\n");

   n = (int) (sy*(clayer->loc.ny/2 - 1) + sx*(clayer->loc.nx/2));
   n -= 8;
   for (int f = 0; f < clayer->numFeatures; f++) {
      printf("f = %d, a[%d] = %f\n", f, n, a[n]);
      n += 1;
   }
#endif

   return 0;
}

} // namespace PV

///////////////////////////////////////////////////////
//
// implementation of LIF kernels
//

#ifdef __cplusplus
extern "C" {
#endif

#ifndef PV_USE_OPENCL
#  include "../kernels/LIFGap_update_state.cl"
#endif

#ifdef __cplusplus
}
#endif
<commit_msg>Removed CL buffers copies with #if PV_CL_COPY_BUFFERS<commit_after>/*
 * LIFGap.cpp
 *
 *  Created on: Jul 29, 2011
 *      Author: garkenyon
 */

#include "LIFGap.hpp"
#include "../include/pv_common.h"
#include "../include/default_params.h"
#include "../io/fileio.hpp"
#include "../utils/cl_random.h"

#include <assert.h>
#include <float.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef __cplusplus
extern "C" {
#endif

void LIFGap_update_state(
    const float time,
    const float dt,

    const int nx,
    const int ny,
    const int nf,
    const int nb,

    LIF_params * params,
    uint4 * rnd,

    float * V,
    float * Vth,
    float * G_E,
    float * G_I,
    float * G_IB,
    float * GSynExc,
    float * GSynInh,
    float * GSynInhB,
    float * activity,

    const float sum_gap,
    float * G_Gap,
    float * GSynGap

);

#ifdef __cplusplus
}
#endif

namespace PV {

LIFGap::LIFGap(const char* name, HyPerCol * hc)
  : LIF(name, hc, TypeLIFGap, MAX_CHANNELS+1)
{
   const char * kernel_name = "LIFGap_update_state";
   LIFGap::initialize(TypeLIFGap, kernel_name );
}

LIFGap::LIFGap(const char* name, HyPerCol * hc, PVLayerType type)
  : LIF(name, hc, type, MAX_CHANNELS+1)
{
   const char * kernel_name = "LIFGap_update_state";
   LIFGap::initialize(type, kernel_name);
}

LIFGap::~LIFGap()
{

#ifdef PV_USE_OPENCL
   delete clG_Gap;
   delete clGSynGap;
#endif

}

// Initialize this class
/*
 *
 */
int LIFGap::initialize(PVLayerType type, const char * kernel_name)
{
   int status = CL_SUCCESS;

   status = LIF::initialize(type, kernel_name);

   const size_t num_neurons = getNumNeurons();
   this->G_Gap  = G_E + 3*num_neurons;
   this->sumGap = 0.0f;

   return status;
}

#ifdef PV_USE_OPENCL
/**
 * Initialize OpenCL buffers.  This must be called after PVLayer data have
 * been allocated.
 */
int LIFGap::initializeThreadBuffers(char * kernel_name)
{
   int status = CL_SUCCESS;

   status = LIF::initializeThreadBuffers(kernel_name);

   const size_t size    = getNumNeurons()  * sizeof(pvdata_t);
   CLDevice * device = parent->getCLDevice();

   // these buffers are shared between host and device
   //

   // TODO - use constant memory
   clG_Gap = device->createBuffer(CL_MEM_COPY_HOST_PTR, size, G_Gap);
   clGSynGap = device->createBuffer(CL_MEM_COPY_HOST_PTR, size, getChannel(CHANNEL_GAP));

   return status;
}

int LIFGap::initializeThreadKernels(char * kernel_name)

{
   int status = CL_SUCCESS;

   status = LIF::initializeThreadKernels(kernel_name);

   int argid = getNumKernelArgs();

   status |= krUpdate->setKernelArg(argid++, sumGap);
   status |= krUpdate->setKernelArg(argid++, clG_Gap);
   status |= krUpdate->setKernelArg(argid++, clGSynGap);

   numKernelArgs = argid;

   return status;
}
#endif

int LIFGap::updateStateOpenCL(float time, float dt)
{
   int status = CL_SUCCESS;

#ifdef PV_USE_OPENCL
   status = LIF::updateStateOpenCL(time, dt);

#if PV_CL_COPY_BUFFERS
   status |= clGSynGap->copyFromDevice(1, &evUpdate, &evList[EV_LIF_GSYN_GAP]);
#endif

   numWait += 1;
#endif

   return status;
}

int LIFGap::triggerReceive(InterColComm* comm)
{
   int status = CL_SUCCESS;
   status = LIF::triggerReceive(comm);

#ifdef PV_USE_OPENCL
   // copy data to device
#if PV_CL_COPY_BUFFERS
   status |= clGSynGap->copyToDevice(&evList[EV_LIF_GSYN_GAP]);
   numWait += 1;
#endif
#endif

   return status;
}

int LIFGap::updateState(float time, float dt)
{
   int status = CL_SUCCESS;
   update_timer->start();

#ifndef PV_USE_OPENCL

   const int nx = clayer->loc.nx;
   const int ny = clayer->loc.ny;
   const int nf = clayer->loc.nf;
   const int nb = clayer->loc.nb;

   pvdata_t * GSynExc   = getChannel(CHANNEL_EXC);
   pvdata_t * GSynInh   = getChannel(CHANNEL_INH);
   pvdata_t * GSynInhB  = getChannel(CHANNEL_INHB);
   pvdata_t * GSynGap  = getChannel(CHANNEL_GAP);
   pvdata_t * activity = clayer->activity->data;

   LIFGap_update_state(time, dt, nx, ny, nf, nb, &lParams, rand_state, clayer->V, Vth, G_E,
         G_I, G_IB, GSynExc, GSynInh, GSynInhB, activity, sumGap, G_Gap, GSynGap);

#else

   status = updateStateOpenCL(time, dt);

#endif
   updateActiveIndices();
   update_timer->stop();
   return status;
}

int LIFGap::readState(float * time)
{
   double dtime;
   char path[PV_PATH_MAX];
   bool contiguous = false;
   bool extended   = false;

   int status = LIF::readState(time);

   PVLayerLoc * loc = & clayer->loc;
   Communicator * comm = parent->icCommunicator();

   getOutputFilename(path, "G_Gap", "_last");
   status = read(path, comm, &dtime, G_Gap, loc, PV_FLOAT_TYPE, extended, contiguous);
   assert(status == PV_SUCCESS);

   return status;

}

int LIFGap::writeState(float time, bool last)
{
   char path[PV_PATH_MAX];
   bool contiguous = false;
   bool extended   = false;

   const char * last_str = (last) ? "_last" : "";

   int status = LIF::writeState(time, last);

   PVLayerLoc * loc = & clayer->loc;
   Communicator * comm = parent->icCommunicator();

   getOutputFilename(path, "G_Gap", last_str);
   status = write_pvdata(path, comm, time, G_Gap, loc, PV_FLOAT_TYPE, extended, contiguous);

#ifdef DEBUG_OUTPUT
   // print activity at center of image

   int sx = clayer->numFeatures;
   int sy = sx*clayer->loc.nx;
   pvdata_t * a = clayer->activity->data;

   int n = (int) (sy*(clayer->loc.ny/2 - 1) + sx*(clayer->loc.nx/2));
   for (int f = 0; f < clayer->numFeatures; f++) {
      printf("f = %d, a[%d] = %f\n", f, n, a[n]);
      n += 1;
   }
   printf("\n");

   n = (int) (sy*(clayer->loc.ny/2 - 1) + sx*(clayer->loc.nx/2));
   n -= 8;
   for (int f = 0; f < clayer->numFeatures; f++) {
      printf("f = %d, a[%d] = %f\n", f, n, a[n]);
      n += 1;
   }
#endif

   return 0;
}

} // namespace PV

///////////////////////////////////////////////////////
//
// implementation of LIF kernels
//

#ifdef __cplusplus
extern "C" {
#endif

#ifndef PV_USE_OPENCL
#  include "../kernels/LIFGap_update_state.cl"
#endif

#ifdef __cplusplus
}
#endif
<|endoftext|>


<commit_before>// Copyright (C) 2012  JINMEI Tatuya
//
// Permission to use, copy, modify, and/or distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH
// REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
// AND FITNESS.  IN NO EVENT SHALL ISC BE LIABLE FOR ANY SPECIAL, DIRECT,
// INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
// LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
// OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
// PERFORMANCE OF THIS SOFTWARE.

#include <query_context.h>
#include <query_repository.h>
#include <dispatcher.h>
#include <message_manager.h>
#include <asio_message_manager.h>

#include <util/buffer.h>

#include <dns/message.h>

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>

#include <algorithm>
#include <cassert>
#include <list>

#include <netinet/in.h>

using namespace std;
using namespace isc::util;
using namespace isc::dns;
using namespace Queryperf;
using boost::scoped_ptr;
using boost::shared_ptr;
using namespace boost::posix_time;
using boost::posix_time::seconds;

namespace {
class QueryEvent {
    typedef boost::function<void(qid_t, const Message*)> RestartCallback;
public:
    QueryEvent(MessageManager& mgr, qid_t qid, QueryContext* ctx,
               RestartCallback restart_callback) :
        ctx_(ctx), qid_(qid), restart_callback_(restart_callback),
        timer_(mgr.createMessageTimer(
                   boost::bind(&QueryEvent::queryTimerCallback, this)))
    {}

    ~QueryEvent() {
        delete ctx_;
    }

    QueryContext::WireData start(qid_t qid, const time_duration& timeout) {
        assert(ctx_ != NULL);
        qid_ = qid;
        timer_->start(timeout);
        return (ctx_->start(qid_));
    }

    bool matchResponse(qid_t qid) const { return (qid_ == qid); }

private:
    void queryTimerCallback() {
        cout << "[Timeout] Query timed out: msg id: " << qid_ << endl;
        restart_callback_(qid_, NULL);
    }

    QueryContext* ctx_;
    qid_t qid_;
    RestartCallback restart_callback_;
    shared_ptr<MessageTimer> timer_;
};

typedef shared_ptr<QueryEvent> QueryEventPtr;
} // unnamed namespace

namespace Queryperf {
struct Dispatcher::DispatcherImpl {
    DispatcherImpl(MessageManager& msg_mgr,
                   QueryContextCreator& ctx_creator) :
        msg_mgr_(&msg_mgr), qryctx_creator_(&ctx_creator),
        response_(Message::PARSE)
    {
        initParams();
    }

    DispatcherImpl(const string& data_file) :
        qry_repo_local_(new QueryRepository(data_file)),
        msg_mgr_local_(new ASIOMessageManager),
        qryctx_creator_local_(new QueryContextCreator(*qry_repo_local_)),
        msg_mgr_(msg_mgr_local_.get()),
        qryctx_creator_(qryctx_creator_local_.get()),
        response_(Message::PARSE)
    {
        initParams();
    }

    void initParams() {
        keep_sending_ = true;
        window_ = DEFAULT_WINDOW;
        qid_ = 0;
        queries_sent_ = 0;
        queries_completed_ = 0;
        server_address_ = DEFAULT_SERVER;
        server_port_ = DEFAULT_PORT;
        test_duration_ = DEFAULT_DURATION;
        query_timeout_ = seconds(DEFAULT_QUERY_TIMEOUT);
    }

    void run();

    // Callback from the message manager called when a response to a query is
    // delivered.
    void responseCallback(const MessageSocket::Event& sockev);

    void restartQuery(qid_t qid, const Message* response);

    // Callback from the message manager on expiration of the session timer.
    // Stop sending more queries; only wait for outstanding ones.
    void sessionTimerCallback() {
        keep_sending_ = false;
    }

    // These are placeholders for the support class objects when they are
    // built within the context.
    scoped_ptr<QueryRepository> qry_repo_local_;
    scoped_ptr<ASIOMessageManager> msg_mgr_local_;
    scoped_ptr<QueryContextCreator> qryctx_creator_local_;

    // These are pointers to the objects actually used in the object
    MessageManager* msg_mgr_;
    QueryContextCreator* qryctx_creator_;

    // Note that these should be placed after msg_mgr_local_; in the destructor
    // these should be released first.
    scoped_ptr<MessageSocket> udp_socket_;
    scoped_ptr<MessageTimer> session_timer_;

    // Configurable parameters
    string server_address_;
    uint16_t server_port_;
    size_t test_duration_;
    time_duration query_timeout_;

    bool keep_sending_; // whether to send next query on getting a response
    size_t window_;
    qid_t qid_;
    Message response_;          // placeholder for response messages
    list<shared_ptr<QueryEvent> > outstanding_;
    //list<> available_;

    // statistics
    size_t queries_sent_;
    size_t queries_completed_;
    ptime start_time_;
    ptime end_time_;
};

void
Dispatcher::DispatcherImpl::run() {
    // Allocate resources used throughout the test session:
    // common UDP socket and the whole session timer.
    udp_socket_.reset(msg_mgr_->createMessageSocket(
                          IPPROTO_UDP, server_address_, server_port_,
                          boost::bind(&DispatcherImpl::responseCallback,
                                      this, _1)));
    session_timer_.reset(msg_mgr_->createMessageTimer(
                             boost::bind(&DispatcherImpl::sessionTimerCallback,
                                         this)));

    // Start the session timer.
    session_timer_->start(seconds(test_duration_));

    // Create a pool of query contexts.  Setting QID to 0 for now.
    for (size_t i = 0; i < window_; ++i) {
        QueryEventPtr qev(new QueryEvent(
                              *msg_mgr_, 0, qryctx_creator_->create(),
                              boost::bind(&DispatcherImpl::restartQuery,
                                          this, _1, _2)));
        outstanding_.push_back(qev);
    }

    // Record the start time and dispatch initial queries at once.
    start_time_ = microsec_clock::local_time();
    BOOST_FOREACH(shared_ptr<QueryEvent>& qev, outstanding_) {
        QueryContext::WireData qry_data = qev->start(qid_, query_timeout_);
        udp_socket_->send(qry_data.data, qry_data.len);
        ++queries_sent_;
        ++qid_;
    }

    // Enter the event loop.
    msg_mgr_->run();
}

void
Dispatcher::DispatcherImpl::responseCallback(
    const MessageSocket::Event& sockev)
{
    // Parse the header of the response
    InputBuffer buffer(sockev.data, sockev.datalen);
    response_.clear(Message::PARSE);
    response_.parseHeader(buffer);
    // TODO: catch exception due to bogus response

    restartQuery(response_.getQid(), &response_);
}

void
Dispatcher::DispatcherImpl::restartQuery(qid_t qid, const Message* response) {
    // Identify the matching query from the outstanding queue.
    const list<shared_ptr<QueryEvent> >::iterator qev_it =
        find_if(outstanding_.begin(), outstanding_.end(),
                boost::bind(&QueryEvent::matchResponse, _1, qid));
    if (qev_it != outstanding_.end()) {
        if (response != NULL) {
            // TODO: let the context check the response further
            ++queries_completed_;
        }

        // If necessary, create a new query and dispatch it.
        if (keep_sending_) {
            QueryContext::WireData qry_data = (*qev_it)->start(qid_,
                                                               query_timeout_);
            udp_socket_->send(qry_data.data, qry_data.len);
            ++queries_sent_;
            ++qid_;

            // Move this context to the end of the queue.
            outstanding_.splice(qev_it, outstanding_, outstanding_.end());
        } else {
            outstanding_.erase(qev_it);
            if (outstanding_.empty()) {
                msg_mgr_->stop();
            }
        }
    } else {
        // TODO: record the mismatched resonse
    }
}

Dispatcher::Dispatcher(MessageManager& msg_mgr,
                       QueryContextCreator& ctx_creator) :
    impl_(new DispatcherImpl(msg_mgr, ctx_creator))
{
}

const char* const Dispatcher::DEFAULT_SERVER = "::1";

Dispatcher::Dispatcher(const string& data_file) :
    impl_(new DispatcherImpl(data_file))
{
}

Dispatcher::~Dispatcher() {
    delete impl_;
}

void
Dispatcher::run() {
    assert(impl_->udp_socket_ == NULL);
    impl_->run();
    impl_->end_time_ = microsec_clock::local_time();
}

string
Dispatcher::getServerAddress() const {
    return (impl_->server_address_);
}

void
Dispatcher::setServerAddress(const string& address) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("server address cannot be reset after run()");
    }
    impl_->server_address_ = address;
}

uint16_t
Dispatcher::getServerPort() const {
    return (impl_->server_port_);
}

void
Dispatcher::setServerPort(uint16_t port) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("server port cannot be reset after run()");
    }
    impl_->server_port_ = port;
}

size_t
Dispatcher::getTestDuration() const {
    return (impl_->test_duration_);
}

void
Dispatcher::setTestDuration(size_t duration) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("test duration cannot be reset after run()");
    }
    impl_->test_duration_ = duration;
}

size_t
Dispatcher::getQueriesSent() const {
    return (impl_->queries_sent_);
}

size_t
Dispatcher::getQueriesCompleted() const {
    return (impl_->queries_completed_);
}

const ptime&
Dispatcher::getStartTime() const {
    return (impl_->start_time_);
}

const ptime&
Dispatcher::getEndTime() const {
    return (impl_->end_time_);
}

} // end of QueryPerf
<commit_msg>comment update<commit_after>// Copyright (C) 2012  JINMEI Tatuya
//
// Permission to use, copy, modify, and/or distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH
// REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
// AND FITNESS.  IN NO EVENT SHALL ISC BE LIABLE FOR ANY SPECIAL, DIRECT,
// INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
// LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
// OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
// PERFORMANCE OF THIS SOFTWARE.

#include <query_context.h>
#include <query_repository.h>
#include <dispatcher.h>
#include <message_manager.h>
#include <asio_message_manager.h>

#include <util/buffer.h>

#include <dns/message.h>

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>

#include <algorithm>
#include <cassert>
#include <list>

#include <netinet/in.h>

using namespace std;
using namespace isc::util;
using namespace isc::dns;
using namespace Queryperf;
using boost::scoped_ptr;
using boost::shared_ptr;
using namespace boost::posix_time;
using boost::posix_time::seconds;

namespace {
class QueryEvent {
    typedef boost::function<void(qid_t, const Message*)> RestartCallback;
public:
    QueryEvent(MessageManager& mgr, qid_t qid, QueryContext* ctx,
               RestartCallback restart_callback) :
        ctx_(ctx), qid_(qid), restart_callback_(restart_callback),
        timer_(mgr.createMessageTimer(
                   boost::bind(&QueryEvent::queryTimerCallback, this)))
    {}

    ~QueryEvent() {
        delete ctx_;
    }

    QueryContext::WireData start(qid_t qid, const time_duration& timeout) {
        assert(ctx_ != NULL);
        qid_ = qid;
        timer_->start(timeout);
        return (ctx_->start(qid_));
    }

    bool matchResponse(qid_t qid) const { return (qid_ == qid); }

private:
    void queryTimerCallback() {
        cout << "[Timeout] Query timed out: msg id: " << qid_ << endl;
        restart_callback_(qid_, NULL);
    }

    QueryContext* ctx_;
    qid_t qid_;
    RestartCallback restart_callback_;
    shared_ptr<MessageTimer> timer_;
};

typedef shared_ptr<QueryEvent> QueryEventPtr;
} // unnamed namespace

namespace Queryperf {
struct Dispatcher::DispatcherImpl {
    DispatcherImpl(MessageManager& msg_mgr,
                   QueryContextCreator& ctx_creator) :
        msg_mgr_(&msg_mgr), qryctx_creator_(&ctx_creator),
        response_(Message::PARSE)
    {
        initParams();
    }

    DispatcherImpl(const string& data_file) :
        qry_repo_local_(new QueryRepository(data_file)),
        msg_mgr_local_(new ASIOMessageManager),
        qryctx_creator_local_(new QueryContextCreator(*qry_repo_local_)),
        msg_mgr_(msg_mgr_local_.get()),
        qryctx_creator_(qryctx_creator_local_.get()),
        response_(Message::PARSE)
    {
        initParams();
    }

    void initParams() {
        keep_sending_ = true;
        window_ = DEFAULT_WINDOW;
        qid_ = 0;
        queries_sent_ = 0;
        queries_completed_ = 0;
        server_address_ = DEFAULT_SERVER;
        server_port_ = DEFAULT_PORT;
        test_duration_ = DEFAULT_DURATION;
        query_timeout_ = seconds(DEFAULT_QUERY_TIMEOUT);
    }

    void run();

    // Callback from the message manager called when a response to a query is
    // delivered.
    void responseCallback(const MessageSocket::Event& sockev);

    // Generate next query either due to completion or timeout.
    void restartQuery(qid_t qid, const Message* response);

    // Callback from the message manager on expiration of the session timer.
    // Stop sending more queries; only wait for outstanding ones.
    void sessionTimerCallback() {
        keep_sending_ = false;
    }

    // These are placeholders for the support class objects when they are
    // built within the context.
    scoped_ptr<QueryRepository> qry_repo_local_;
    scoped_ptr<ASIOMessageManager> msg_mgr_local_;
    scoped_ptr<QueryContextCreator> qryctx_creator_local_;

    // These are pointers to the objects actually used in the object
    MessageManager* msg_mgr_;
    QueryContextCreator* qryctx_creator_;

    // Note that these should be placed after msg_mgr_local_; in the destructor
    // these should be released first.
    scoped_ptr<MessageSocket> udp_socket_;
    scoped_ptr<MessageTimer> session_timer_;

    // Configurable parameters
    string server_address_;
    uint16_t server_port_;
    size_t test_duration_;
    time_duration query_timeout_;

    bool keep_sending_; // whether to send next query on getting a response
    size_t window_;
    qid_t qid_;
    Message response_;          // placeholder for response messages
    list<shared_ptr<QueryEvent> > outstanding_;
    //list<> available_;

    // statistics
    size_t queries_sent_;
    size_t queries_completed_;
    ptime start_time_;
    ptime end_time_;
};

void
Dispatcher::DispatcherImpl::run() {
    // Allocate resources used throughout the test session:
    // common UDP socket and the whole session timer.
    udp_socket_.reset(msg_mgr_->createMessageSocket(
                          IPPROTO_UDP, server_address_, server_port_,
                          boost::bind(&DispatcherImpl::responseCallback,
                                      this, _1)));
    session_timer_.reset(msg_mgr_->createMessageTimer(
                             boost::bind(&DispatcherImpl::sessionTimerCallback,
                                         this)));

    // Start the session timer.
    session_timer_->start(seconds(test_duration_));

    // Create a pool of query contexts.  Setting QID to 0 for now.
    for (size_t i = 0; i < window_; ++i) {
        QueryEventPtr qev(new QueryEvent(
                              *msg_mgr_, 0, qryctx_creator_->create(),
                              boost::bind(&DispatcherImpl::restartQuery,
                                          this, _1, _2)));
        outstanding_.push_back(qev);
    }

    // Record the start time and dispatch initial queries at once.
    start_time_ = microsec_clock::local_time();
    BOOST_FOREACH(shared_ptr<QueryEvent>& qev, outstanding_) {
        QueryContext::WireData qry_data = qev->start(qid_, query_timeout_);
        udp_socket_->send(qry_data.data, qry_data.len);
        ++queries_sent_;
        ++qid_;
    }

    // Enter the event loop.
    msg_mgr_->run();
}

void
Dispatcher::DispatcherImpl::responseCallback(
    const MessageSocket::Event& sockev)
{
    // Parse the header of the response
    InputBuffer buffer(sockev.data, sockev.datalen);
    response_.clear(Message::PARSE);
    response_.parseHeader(buffer);
    // TODO: catch exception due to bogus response

    restartQuery(response_.getQid(), &response_);
}

void
Dispatcher::DispatcherImpl::restartQuery(qid_t qid, const Message* response) {
    // Identify the matching query from the outstanding queue.
    const list<shared_ptr<QueryEvent> >::iterator qev_it =
        find_if(outstanding_.begin(), outstanding_.end(),
                boost::bind(&QueryEvent::matchResponse, _1, qid));
    if (qev_it != outstanding_.end()) {
        if (response != NULL) {
            // TODO: let the context check the response further
            ++queries_completed_;
        }

        // If necessary, create a new query and dispatch it.
        if (keep_sending_) {
            QueryContext::WireData qry_data = (*qev_it)->start(qid_,
                                                               query_timeout_);
            udp_socket_->send(qry_data.data, qry_data.len);
            ++queries_sent_;
            ++qid_;

            // Move this context to the end of the queue.
            outstanding_.splice(qev_it, outstanding_, outstanding_.end());
        } else {
            outstanding_.erase(qev_it);
            if (outstanding_.empty()) {
                msg_mgr_->stop();
            }
        }
    } else {
        // TODO: record the mismatched resonse
    }
}

Dispatcher::Dispatcher(MessageManager& msg_mgr,
                       QueryContextCreator& ctx_creator) :
    impl_(new DispatcherImpl(msg_mgr, ctx_creator))
{
}

const char* const Dispatcher::DEFAULT_SERVER = "::1";

Dispatcher::Dispatcher(const string& data_file) :
    impl_(new DispatcherImpl(data_file))
{
}

Dispatcher::~Dispatcher() {
    delete impl_;
}

void
Dispatcher::run() {
    assert(impl_->udp_socket_ == NULL);
    impl_->run();
    impl_->end_time_ = microsec_clock::local_time();
}

string
Dispatcher::getServerAddress() const {
    return (impl_->server_address_);
}

void
Dispatcher::setServerAddress(const string& address) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("server address cannot be reset after run()");
    }
    impl_->server_address_ = address;
}

uint16_t
Dispatcher::getServerPort() const {
    return (impl_->server_port_);
}

void
Dispatcher::setServerPort(uint16_t port) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("server port cannot be reset after run()");
    }
    impl_->server_port_ = port;
}

size_t
Dispatcher::getTestDuration() const {
    return (impl_->test_duration_);
}

void
Dispatcher::setTestDuration(size_t duration) {
    if (!impl_->start_time_.is_special()) {
        throw DispatcherError("test duration cannot be reset after run()");
    }
    impl_->test_duration_ = duration;
}

size_t
Dispatcher::getQueriesSent() const {
    return (impl_->queries_sent_);
}

size_t
Dispatcher::getQueriesCompleted() const {
    return (impl_->queries_completed_);
}

const ptime&
Dispatcher::getStartTime() const {
    return (impl_->start_time_);
}

const ptime&
Dispatcher::getEndTime() const {
    return (impl_->end_time_);
}

} // end of QueryPerf
<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <cassert>

#define OMPI_SKIP_MPICXX 1
#include <mpi.h>

#ifdef HAVE_PETSC
#include <petscsys.h>
#include <petscviewer.h>
#endif

#include <sys/types.h>
#include <sys/stat.h>

#if !(defined IOSTREAMH)
#include <iostream>
#include <sstream>
using namespace std;
#endif

#ifdef intel
#include <direct.h>
#define chdir _chdir
#else
#include <unistd.h>
#endif

#include "phasta_version.h"
#include "common_c.h"
#include "Input.h"
#include "phiostats.h"
#include "phstream.h"
#include "streamio.h"

#include <FCMangle.h>
#define input FortranCInterface_GLOBAL_(input,INPUT)
#define proces FortranCInterface_GLOBAL_(proces,PROCES)
#define timer FortranCInterface_GLOBAL_(timer,TIMER)

extern "C" char phasta_iotype[80];
char phasta_iotype[80];

extern int SONFATH;
extern "C" void proces();
extern "C" void input();
extern int input_fform(phSolver::Input&);
extern void setIOparam(); // For SyncIO
extern "C" void initPhastaCommonVars();

int myrank; /* made file global for ease in debugging */

void
catchDebugger() {
    while (1) {
      int debuggerPresent=0;
      int fakeSTOP = 1; // please stop HERE and assign as next line
      // assign or set debuggerPresent=1
      if(debuggerPresent) {
        break;
      }
    }
}

// some useful debugging functions

void
pdarray( void* darray , int start, int end ) {
    for( int i=start; i < end; i++ ){
        cout << ((double*)darray)[i] << endl;
    }
}

void
piarray( void* iarray , int start, int end ) {
    for( int i=start; i < end; i++ ){
        cout << ((int*)iarray)[i] << endl;
    }
}

namespace {
  int cdToParent() {
    if( chdir("..") ) {
      fprintf(stderr,"could not change to the parent directory\n");
      return 1;
    } else {
      return 0;
    }
  }
  int run(phSolver::Input& ctrl) {
    int size,ierr;
    char inpfilename[100];
    MPI_Comm_size (MPI_COMM_WORLD, &size);
    MPI_Comm_rank (MPI_COMM_WORLD, &myrank);

    if(!myrank)
      printf("PHASTA Git hash %s\n", phasta_version());

    workfc.numpe = size;
    workfc.myrank = myrank;

    initPhastaCommonVars();
    /* Input data  */
    ierr = input_fform(ctrl);
    if(!ierr){
      sprintf(inpfilename,"%d-procs_case/",size);
      if( chdir( inpfilename ) ) {
        cerr << "could not change to the problem directory "
          << inpfilename << endl;
        return -1;
      }
      MPI_Barrier(MPI_COMM_WORLD);
      phastaio_initStats();
      input();
      /* now we can start the solver */
      proces();
      phastaio_printStats();
    }
    else{
      printf("error during reading ascii input \n");
    }
    MPI_Barrier(MPI_COMM_WORLD);
    if ( myrank == 0 ) {
      printf("phasta.cc - last call before finalize!\n");
    }
    if( cdToParent() )
      return -1;
    return timdat.lstep;
  }
}

int phasta(phSolver::Input& ctrl) {
  outpar.input_mode = 0; //FIXME magic value for posix
  outpar.output_mode = 0; //FIXME magic value for posix
  return run(ctrl);
}

int phasta(phSolver::Input& ctrl, grstream grs) {
  assert(grs);
  outpar.input_mode = -1; //FIXME magic value for streams
  outpar.output_mode = 1; //FIXME magic value for syncio
  streamio_set_gr(grs);
  return run(ctrl);
}

int phasta(phSolver::Input& ctrl, RStream* rs) {
  fprintf(stderr, "HEY! if you see this email Cameron and tell him "
      "to implement %s(...) on line %d of %s "
      "... returning an error\n", __func__, __LINE__, __FILE__);
  return -1;
}

int phasta(phSolver::Input& ctrl, GRStream* grs, RStream* rs) {
  outpar.input_mode = -1; //FIXME magic value for streams
  outpar.output_mode = -1; //FIXME magic value for streams
  assert(grs);
  assert(rs);
  streamio_set_gr(grs);
  streamio_set_r(rs);
  return run(ctrl);
}

int phasta( int argc, char *argv[] ) {
    int size,ierr;
    char inpfilename[100];
    char* pauseDebugger = getenv("catchDebugger");
    MPI_Comm_size (MPI_COMM_WORLD, &size);
    MPI_Comm_rank (MPI_COMM_WORLD, &myrank);
    if(!myrank)
      printf("PHASTA Git hash %s\n", phasta_version());

#ifdef HAVE_PETSC
    PETSC_COMM_WORLD=MPI_COMM_WORLD;
    PetscInitialize(&argc,&argv,PETSC_NULL,PETSC_NULL);
    PetscInitializeFortran();
    PetscPopSignalHandler(); //Let us segfault in peace ;-)
// ok with Master    PetscOptionsView(NULL,PETSC_VIEWER_STDOUT_WORLD);
// ok with 3.6x    PetscOptionsView(PETSC_VIEWER_STDOUT_WORLD);
    PetscOptionsView(NULL,PETSC_VIEWER_STDOUT_WORLD);
    if(sizeof(PetscInt) != sizeof(long long int))
    {
      //PetscInt and gcorp_t (gen_ncorp.c)
      //must be the same size. hard-coded for now
      //FIXME
	    if(myrank == 0)
	    {
		    printf("WARNING: PETSc Index Size Mismatch\n");
		    printf("WARNING: Proceed at your own risk\n");
	    }
    }
    MPI_Barrier(MPI_COMM_WORLD);
    if(myrank == 0)
    {
	    printf("PETSc Initialized\n");
	    fflush(stdout);
    }
#endif
    workfc.numpe = size;
    workfc.myrank = myrank;

#if (defined WIN32)
    if(argc > 2 ){
      catchDebugger();
    }
#endif
#if (1) // ALWAYS ( defined LAUNCH_GDB ) && !( defined WIN32 )

    if ( pauseDebugger ) {

        int parent_pid = getpid();
        int gdb_child = fork();
        cout << "gdb_child" << gdb_child << endl;

        if( gdb_child == 0 ) {

            cout << "Debugger Process initiating" << endl;
            stringstream exec_string;

#if ( defined decalp )
            exec_string <<"xterm -e idb "
                        << " -pid "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined LINUX )
            exec_string <<"xterm -e gdb"
                        << " -pid "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined SUN4 )
            exec_string <<"xterm -e dbx "
                        << " - "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined IRIX )
            exec_string <<"xterm -e dbx "
                        << " -p "<< parent_pid <<" "<< argv[0] << endl;
#endif
            string s = exec_string.str();
            system( s.c_str() );
            exit(0);
        }
        catchDebugger();
    }

#endif

    /* Input data  */
    if(argc > 1 ){
        strcpy(inpfilename,argv[1]);
    } else {
        strcpy(inpfilename,"solver.inp");
    }
    string defaultConf = ".";
    const char* path_to_config = getenv("PHASTA_CONFIG");
    if(path_to_config)
      defaultConf = path_to_config;
    defaultConf.append("/input.config");
    string userConf(inpfilename);
    phSolver::Input ctrl(userConf, defaultConf);
    initPhastaCommonVars();
    ierr = input_fform(ctrl);
    if(!ierr){
      sprintf(inpfilename,"%d-procs_case/",size);
      if( chdir( inpfilename ) ) {
        cerr << "could not change to the problem directory "
          << inpfilename << endl;
        return -1;
      }
      MPI_Barrier(MPI_COMM_WORLD);
      setIOparam();
      outpar.input_mode = outpar.nsynciofiles; //FIXME this is awful
      outpar.output_mode = outpar.nsynciofiles; //FIXME this is awful
      phastaio_initStats();
      input();
      /* now we can start the solver */
      proces();
      phastaio_printStats();
    }
    else{
        printf("error during reading ascii input \n");
    }
#ifdef HAVE_PETSC
    PetscFinalize();
#endif
    MPI_Barrier(MPI_COMM_WORLD);
    if ( myrank == 0 ) {
      printf("phasta.cc - last call before finalize!\n");
    }
    if( cdToParent() )
      return -1;
    return timdat.lstep;
}
<commit_msg>set stack size to avoid large problem crashes on hardware with small stack size defaults.  Alternative ulimit -s unlimited before running.<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <cassert>

#define OMPI_SKIP_MPICXX 1
#include <mpi.h>

#ifdef HAVE_PETSC
#include <petscsys.h>
#include <petscviewer.h>
#endif

#include <sys/types.h>
#include <sys/stat.h>

#if !(defined IOSTREAMH)
#include <iostream>
#include <sstream>
using namespace std;
#endif

#ifdef intel
#include <direct.h>
#define chdir _chdir
#else
#include <unistd.h>
#endif
#if defined(__linux__) && !defined(__bgq__)
/* BM's proposed fix to the stacksize issue ...More below same comment*/
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include "phasta_version.h"
#include "common_c.h"
#include "Input.h"
#include "phiostats.h"
#include "phstream.h"
#include "streamio.h"

#include <FCMangle.h>
#define input FortranCInterface_GLOBAL_(input,INPUT)
#define proces FortranCInterface_GLOBAL_(proces,PROCES)
#define timer FortranCInterface_GLOBAL_(timer,TIMER)

extern "C" char phasta_iotype[80];
char phasta_iotype[80];

extern int SONFATH;
extern "C" void proces();
extern "C" void input();
extern int input_fform(phSolver::Input&);
extern void setIOparam(); // For SyncIO
extern "C" void initPhastaCommonVars();

int myrank; /* made file global for ease in debugging */

void
catchDebugger() {
    while (1) {
      int debuggerPresent=0;
      int fakeSTOP = 1; // please stop HERE and assign as next line
      // assign or set debuggerPresent=1
      if(debuggerPresent) {
        break;
      }
    }
}

// some useful debugging functions

void
pdarray( void* darray , int start, int end ) {
    for( int i=start; i < end; i++ ){
        cout << ((double*)darray)[i] << endl;
    }
}

void
piarray( void* iarray , int start, int end ) {
    for( int i=start; i < end; i++ ){
        cout << ((int*)iarray)[i] << endl;
    }
}

namespace {
  int cdToParent() {
    if( chdir("..") ) {
      fprintf(stderr,"could not change to the parent directory\n");
      return 1;
    } else {
      return 0;
    }
  }
  int run(phSolver::Input& ctrl) {
    int size,ierr;
    char inpfilename[100];
    MPI_Comm_size (MPI_COMM_WORLD, &size);
    MPI_Comm_rank (MPI_COMM_WORLD, &myrank);

    if(!myrank)
      printf("PHASTA Git hash %s\n", phasta_version());

    workfc.numpe = size;
    workfc.myrank = myrank;

    initPhastaCommonVars();
    /* Input data  */
    ierr = input_fform(ctrl);
    if(!ierr){
      sprintf(inpfilename,"%d-procs_case/",size);
      if( chdir( inpfilename ) ) {
        cerr << "could not change to the problem directory "
          << inpfilename << endl;
        return -1;
      }
      MPI_Barrier(MPI_COMM_WORLD);
      phastaio_initStats();
      input();
      /* now we can start the solver */
      proces();
      phastaio_printStats();
    }
    else{
      printf("error during reading ascii input \n");
    }
    MPI_Barrier(MPI_COMM_WORLD);
    if ( myrank == 0 ) {
      printf("phasta.cc - last call before finalize!\n");
    }
    if( cdToParent() )
      return -1;
    return timdat.lstep;
  }
}

int phasta(phSolver::Input& ctrl) {
  outpar.input_mode = 0; //FIXME magic value for posix
  outpar.output_mode = 0; //FIXME magic value for posix
  return run(ctrl);
}

int phasta(phSolver::Input& ctrl, grstream grs) {
  assert(grs);
  outpar.input_mode = -1; //FIXME magic value for streams
  outpar.output_mode = 1; //FIXME magic value for syncio
  streamio_set_gr(grs);
  return run(ctrl);
}

int phasta(phSolver::Input& ctrl, RStream* rs) {
  fprintf(stderr, "HEY! if you see this email Cameron and tell him "
      "to implement %s(...) on line %d of %s "
      "... returning an error\n", __func__, __LINE__, __FILE__);
  return -1;
}

int phasta(phSolver::Input& ctrl, GRStream* grs, RStream* rs) {
  outpar.input_mode = -1; //FIXME magic value for streams
  outpar.output_mode = -1; //FIXME magic value for streams
  assert(grs);
  assert(rs);
  streamio_set_gr(grs);
  streamio_set_r(rs);
  return run(ctrl);
}

int phasta( int argc, char *argv[] ) {
    int size,ierr;
    char inpfilename[100];
    char* pauseDebugger = getenv("catchDebugger");
    MPI_Comm_size (MPI_COMM_WORLD, &size);
    MPI_Comm_rank (MPI_COMM_WORLD, &myrank);
    if(!myrank)
      printf("PHASTA Git hash %s\n", phasta_version());

#if defined(__linux__) && !defined(__bgq__)
/* BM's proposed fix to the stacksize issue */
    struct rlimit stack_size_lim;
    int rlim_result;
    memset(&stack_size_lim, 0, sizeof(struct rlimit));
    stack_size_lim.rlim_cur = RLIM_INFINITY;
    stack_size_lim.rlim_max = RLIM_INFINITY;
    rlim_result = setrlimit(RLIMIT_STACK, &stack_size_lim);
    if(myrank == 0) fprintf(stderr, "Attempting to set ulimit from phasta exec ");
    if(rlim_result == -1) perror("setrlimit: ");
#endif

#ifdef HAVE_PETSC
    PETSC_COMM_WORLD=MPI_COMM_WORLD;
    PetscInitialize(&argc,&argv,PETSC_NULL,PETSC_NULL);
    PetscInitializeFortran();
    PetscPopSignalHandler(); //Let us segfault in peace ;-)
// ok with Master    PetscOptionsView(NULL,PETSC_VIEWER_STDOUT_WORLD);
// ok with 3.6x    PetscOptionsView(PETSC_VIEWER_STDOUT_WORLD);
    PetscOptionsView(NULL,PETSC_VIEWER_STDOUT_WORLD);
    if(sizeof(PetscInt) != sizeof(long long int))
    {
      //PetscInt and gcorp_t (gen_ncorp.c)
      //must be the same size. hard-coded for now
      //FIXME
	    if(myrank == 0)
	    {
		    printf("WARNING: PETSc Index Size Mismatch\n");
		    printf("WARNING: Proceed at your own risk\n");
	    }
    }
    MPI_Barrier(MPI_COMM_WORLD);
    if(myrank == 0)
    {
	    printf("PETSc Initialized\n");
	    fflush(stdout);
    }
#endif
    workfc.numpe = size;
    workfc.myrank = myrank;

#if (defined WIN32)
    if(argc > 2 ){
      catchDebugger();
    }
#endif
#if (1) // ALWAYS ( defined LAUNCH_GDB ) && !( defined WIN32 )

    if ( pauseDebugger ) {

        int parent_pid = getpid();
        int gdb_child = fork();
        cout << "gdb_child" << gdb_child << endl;

        if( gdb_child == 0 ) {

            cout << "Debugger Process initiating" << endl;
            stringstream exec_string;

#if ( defined decalp )
            exec_string <<"xterm -e idb "
                        << " -pid "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined LINUX )
            exec_string <<"xterm -e gdb"
                        << " -pid "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined SUN4 )
            exec_string <<"xterm -e dbx "
                        << " - "<< parent_pid <<" "<< argv[0] << endl;
#endif
#if ( defined IRIX )
            exec_string <<"xterm -e dbx "
                        << " -p "<< parent_pid <<" "<< argv[0] << endl;
#endif
            string s = exec_string.str();
            system( s.c_str() );
            exit(0);
        }
        catchDebugger();
    }

#endif

    /* Input data  */
    if(argc > 1 ){
        strcpy(inpfilename,argv[1]);
    } else {
        strcpy(inpfilename,"solver.inp");
    }
    string defaultConf = ".";
    const char* path_to_config = getenv("PHASTA_CONFIG");
    if(path_to_config)
      defaultConf = path_to_config;
    defaultConf.append("/input.config");
    string userConf(inpfilename);
    phSolver::Input ctrl(userConf, defaultConf);
    initPhastaCommonVars();
    ierr = input_fform(ctrl);
    if(!ierr){
      sprintf(inpfilename,"%d-procs_case/",size);
      if( chdir( inpfilename ) ) {
        cerr << "could not change to the problem directory "
          << inpfilename << endl;
        return -1;
      }
      MPI_Barrier(MPI_COMM_WORLD);
      setIOparam();
      outpar.input_mode = outpar.nsynciofiles; //FIXME this is awful
      outpar.output_mode = outpar.nsynciofiles; //FIXME this is awful
      phastaio_initStats();
      input();
      /* now we can start the solver */
      proces();
      phastaio_printStats();
    }
    else{
        printf("error during reading ascii input \n");
    }
#ifdef HAVE_PETSC
    PetscFinalize();
#endif
    MPI_Barrier(MPI_COMM_WORLD);
    if ( myrank == 0 ) {
      printf("phasta.cc - last call before finalize!\n");
    }
    if( cdToParent() )
      return -1;
    return timdat.lstep;
}
<|endoftext|>


<commit_before><commit_msg>修复编译警告<commit_after><|endoftext|>