<commit_before>// Copyright (c) 2011 Zeex
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

#include "amxcallstack.h"
#include "amxdebuginfo.h"

#include "amx/amx.h"
#include "amx/amxdbg.h"

static bool IsFunctionArgument(const AMXDebugInfo::Symbol &symbol, ucell functionAddress) {
	return symbol.IsLocal() && symbol.GetCodeStartAddress() == functionAddress;
}

class IsArgumentOf : public std::unary_function<AMXDebugInfo::Symbol, bool> {
public:
	IsArgumentOf(ucell function) : function_(function) {}

	bool operator()(AMXDebugInfo::Symbol symbol) const {
		return symbol.IsLocal() && symbol.GetCodeStartAddress() == function_;
	}

private:
	ucell function_;
};

static inline bool CompareArguments(const AMXDebugInfo::Symbol &left, const AMXDebugInfo::Symbol &right) {
	return left.GetAddress() < right.GetAddress();
}

class IsFunctionAt : public std::unary_function<const AMXDebugInfo::Symbol&, bool> {
public:
	IsFunctionAt(ucell address) : address_(address) {}

	bool operator()(const AMXDebugInfo::Symbol &symbol) const {
		return symbol.IsFunction() && symbol.GetAddress() == address_;
	}

private:
	ucell address_;
};

static inline const char *GetPublicFunctionName(AMX *amx, ucell address) {
	AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

	AMX_FUNCSTUBNT *publics = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->publics);
	AMX_FUNCSTUBNT *natives = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->natives);

	for (AMX_FUNCSTUBNT *p = publics; p < natives; ++p) {
		if (p->address == address) {
			return reinterpret_cast<const char*>(p->nameofs + amx->base);
		}
	}

	return 0;
}

static inline bool IsPublicFunction(AMX *amx, ucell address) {
	return GetPublicFunctionName(amx, address) != 0;
}

AMXStackFrame::AMXStackFrame(AMX *amx, ucell frameAddress, const AMXDebugInfo &debugInfo)
	: isPublic_(false)
	, frameAddress_(frameAddress)
	, callAddress_(0)
	, functionAddress_(0)
{
	if (IsOnStack(frameAddress_, amx)) {
		AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

		ucell data = reinterpret_cast<ucell>(amx->base + hdr->dat);
		ucell code = reinterpret_cast<ucell>(amx->base) + hdr->cod;

		callAddress_ = *(reinterpret_cast<ucell*>(data + frameAddress_) + 1) - 2*sizeof(cell);
		if (callAddress_ >= 0 && callAddress_ < static_cast<ucell>(hdr->dat - hdr->cod)) {
			functionAddress_ = *reinterpret_cast<ucell*>(callAddress_ + sizeof(cell) + code) - code;
			Init(amx, debugInfo);
		} else {
			callAddress_ = 0;
		}
	}
}

AMXStackFrame::AMXStackFrame(AMX *amx, ucell frameAddress, ucell callAddress,
		ucell functionAddress, const AMXDebugInfo &debugInfo)
	: isPublic_(false)
	, frameAddress_(frameAddress)
	, callAddress_(callAddress)
	, functionAddress_(functionAddress)
{
	Init(amx, debugInfo);
}

void AMXStackFrame::Init(AMX *amx, const AMXDebugInfo &debugInfo) {
	isPublic_ = IsPublicFunction(amx, functionAddress_);

	if (debugInfo.IsLoaded()) {
		if (callAddress_ != 0) {
			fileName_ = debugInfo.GetFileName(callAddress_);
			lineNumber_ = debugInfo.GetLineNumber(callAddress_);
		} else {
			// Entry point
			fileName_ = debugInfo.GetFileName(functionAddress_);
			lineNumber_ = debugInfo.GetLineNumber(functionAddress_);
		}

		// Get function return value tag
		AMXDebugInfo::SymbolTable::const_iterator it = std::find_if(
			debugInfo.GetSymbols().begin(),
			debugInfo.GetSymbols().end(),
			IsFunctionAt(functionAddress_)
		);
		if (it != debugInfo.GetSymbols().end()) {
			functionResultTag_ = debugInfo.GetTagName((*it).GetTag()) + ":";
			if (functionResultTag_ == "_:") {
				functionResultTag_.clear();
			}
		}

		// Get function name
		functionName_ = debugInfo.GetFunctionName(functionAddress_);

		// Get parameters
		std::remove_copy_if(
			debugInfo.GetSymbols().begin(),
			debugInfo.GetSymbols().end(),
			std::back_inserter(arguments_),
			std::not1(IsArgumentOf(functionAddress_))
		);

		// Order them by address
		std::sort(arguments_.begin(), arguments_.end(), CompareArguments);

		// Build a comma-separated list of arguments and their values
		std::stringstream argStream;
		for (std::vector<AMXDebugInfo::Symbol>::const_iterator arg = arguments_.begin();
				arg != arguments_.end(); ++arg) {
			if (arg != arguments_.begin()) {
				argStream << ", ";
			}

			// For reference parameters, print the '&' sign in front of their tag
			if (arg->IsReference()) {
				argStream << "&";
			}

			// Get parameter's tag
			std::string tag = debugInfo.GetTag(arg->GetTag()).GetName() + ":";
			if (tag != "_:") {
				argStream << tag;
			}

			// Get parameter name
			argStream << arg->GetName();

			cell value = arg->GetValue(amx);
			if (arg->IsVariable()) {
				if (tag == "bool:") {
					argStream << "=" << value ? "true" : "false";
				} else if (tag == "Float:") {
					argStream << "=" << std::fixed << std::setprecision(5) << amx_ctof(value);
				} else {
					argStream << "=" << value;
				}
			} else {
				if (arg->IsArray() || arg->IsArrayRef()) {
					// Get array dimensions
					std::vector<AMXDebugInfo::SymbolDim> dims = arg->GetDims();
					for (std::size_t i = 0; i < dims.size(); ++i) {
						if (dims[i].GetSize() == 0) {
							argStream << "[]";
						} else {
							std::string tag = debugInfo.GetTagName(dims[i].GetTag()) + ":";
							if (tag == "_:") tag.clear();
							argStream << "[" << tag << dims[i].GetSize() << "]";
						}
					}
				}
				argStream << "=@0x" << std::hex << value << std::dec;
			}
		}

		// The function prototype is of the following form:
		// [public ][tag:]name([arg1=value1[, arg2=value2, [...]]])
		if (IsPublic()) {
			functionPrototype_.append("public ");
		}
		functionPrototype_
			.append(functionResultTag_)
			.append(functionName_)
			.append("(")
			.append(argStream.str())
			.append(")");
	} else {
		// No debug info available...
		const char *name = GetPublicFunctionName(amx, functionAddress_);
		if (name != 0) {
			functionName_.assign(name);
		}
	}
}

AMXCallStack::AMXCallStack(AMX *amx, const AMXDebugInfo &debugInfo, ucell topFrame) {
	AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);
	ucell data = reinterpret_cast<ucell>(amx->base + hdr->dat);

	ucell frm = topFrame;
	if (frm == 0) {
		frm = amx->frm;
	}

	while (frm > static_cast<ucell>(amx->stk)) {
		AMXStackFrame frame(amx, frm, debugInfo);
		frames_.push_back(frame);
		if (frame.GetFunctionAddress() == 0) {
			// Invalid frame address
			break;
		}
		frm = *reinterpret_cast<ucell*>(data + frm);
	}

	// Correct entry point address (it's not on the stack)
	if (debugInfo.IsLoaded()) {
		if (frames_.size() > 1) {
			ucell epAddr = debugInfo.GetFunctionStartAddress(frames_[frames_.size() - 2].GetCallAddress());
			frames_.back() = AMXStackFrame(amx, frames_.back().GetFrameAddress(), 0, epAddr, debugInfo);
		} else if (frames_.size() != 0) {
			ucell epAddr = debugInfo.GetFunctionStartAddress(amx->cip);
			frames_.back() = AMXStackFrame(amx, frames_.back().GetFrameAddress(), 0, epAddr, debugInfo);
		}
	} else {
		// Can't fetch the address
		frames_.back() = AMXStackFrame(amx, 0, 0, 0);
	}
}
<commit_msg>AMXCallStack: Make array address fixed width (8 characters)<commit_after>// Copyright (c) 2011 Zeex
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

#include "amxcallstack.h"
#include "amxdebuginfo.h"

#include "amx/amx.h"
#include "amx/amxdbg.h"

static bool IsFunctionArgument(const AMXDebugInfo::Symbol &symbol, ucell functionAddress) {
	return symbol.IsLocal() && symbol.GetCodeStartAddress() == functionAddress;
}

class IsArgumentOf : public std::unary_function<AMXDebugInfo::Symbol, bool> {
public:
	IsArgumentOf(ucell function) : function_(function) {}

	bool operator()(AMXDebugInfo::Symbol symbol) const {
		return symbol.IsLocal() && symbol.GetCodeStartAddress() == function_;
	}

private:
	ucell function_;
};

static inline bool CompareArguments(const AMXDebugInfo::Symbol &left, const AMXDebugInfo::Symbol &right) {
	return left.GetAddress() < right.GetAddress();
}

class IsFunctionAt : public std::unary_function<const AMXDebugInfo::Symbol&, bool> {
public:
	IsFunctionAt(ucell address) : address_(address) {}

	bool operator()(const AMXDebugInfo::Symbol &symbol) const {
		return symbol.IsFunction() && symbol.GetAddress() == address_;
	}

private:
	ucell address_;
};

static inline const char *GetPublicFunctionName(AMX *amx, ucell address) {
	AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

	AMX_FUNCSTUBNT *publics = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->publics);
	AMX_FUNCSTUBNT *natives = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->natives);

	for (AMX_FUNCSTUBNT *p = publics; p < natives; ++p) {
		if (p->address == address) {
			return reinterpret_cast<const char*>(p->nameofs + amx->base);
		}
	}

	return 0;
}

static inline bool IsPublicFunction(AMX *amx, ucell address) {
	return GetPublicFunctionName(amx, address) != 0;
}

AMXStackFrame::AMXStackFrame(AMX *amx, ucell frameAddress, const AMXDebugInfo &debugInfo)
	: isPublic_(false)
	, frameAddress_(frameAddress)
	, callAddress_(0)
	, functionAddress_(0)
{
	if (IsOnStack(frameAddress_, amx)) {
		AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

		ucell data = reinterpret_cast<ucell>(amx->base + hdr->dat);
		ucell code = reinterpret_cast<ucell>(amx->base) + hdr->cod;

		callAddress_ = *(reinterpret_cast<ucell*>(data + frameAddress_) + 1) - 2*sizeof(cell);
		if (callAddress_ >= 0 && callAddress_ < static_cast<ucell>(hdr->dat - hdr->cod)) {
			functionAddress_ = *reinterpret_cast<ucell*>(callAddress_ + sizeof(cell) + code) - code;
			Init(amx, debugInfo);
		} else {
			callAddress_ = 0;
		}
	}
}

AMXStackFrame::AMXStackFrame(AMX *amx, ucell frameAddress, ucell callAddress,
		ucell functionAddress, const AMXDebugInfo &debugInfo)
	: isPublic_(false)
	, frameAddress_(frameAddress)
	, callAddress_(callAddress)
	, functionAddress_(functionAddress)
{
	Init(amx, debugInfo);
}

void AMXStackFrame::Init(AMX *amx, const AMXDebugInfo &debugInfo) {
	isPublic_ = IsPublicFunction(amx, functionAddress_);

	if (debugInfo.IsLoaded()) {
		if (callAddress_ != 0) {
			fileName_ = debugInfo.GetFileName(callAddress_);
			lineNumber_ = debugInfo.GetLineNumber(callAddress_);
		} else {
			// Entry point
			fileName_ = debugInfo.GetFileName(functionAddress_);
			lineNumber_ = debugInfo.GetLineNumber(functionAddress_);
		}

		// Get function return value tag
		AMXDebugInfo::SymbolTable::const_iterator it = std::find_if(
			debugInfo.GetSymbols().begin(),
			debugInfo.GetSymbols().end(),
			IsFunctionAt(functionAddress_)
		);
		if (it != debugInfo.GetSymbols().end()) {
			functionResultTag_ = debugInfo.GetTagName((*it).GetTag()) + ":";
			if (functionResultTag_ == "_:") {
				functionResultTag_.clear();
			}
		}

		// Get function name
		functionName_ = debugInfo.GetFunctionName(functionAddress_);

		// Get parameters
		std::remove_copy_if(
			debugInfo.GetSymbols().begin(),
			debugInfo.GetSymbols().end(),
			std::back_inserter(arguments_),
			std::not1(IsArgumentOf(functionAddress_))
		);

		// Order them by address
		std::sort(arguments_.begin(), arguments_.end(), CompareArguments);

		// Build a comma-separated list of arguments and their values
		std::stringstream argStream;
		for (std::vector<AMXDebugInfo::Symbol>::const_iterator arg = arguments_.begin();
				arg != arguments_.end(); ++arg) {
			if (arg != arguments_.begin()) {
				argStream << ", ";
			}

			// For reference parameters, print the '&' sign in front of their tag
			if (arg->IsReference()) {
				argStream << "&";
			}

			// Get parameter's tag
			std::string tag = debugInfo.GetTag(arg->GetTag()).GetName() + ":";
			if (tag != "_:") {
				argStream << tag;
			}

			// Get parameter name
			argStream << arg->GetName();

			cell value = arg->GetValue(amx);
			if (arg->IsVariable()) {
				if (tag == "bool:") {
					argStream << "=" << value ? "true" : "false";
				} else if (tag == "Float:") {
					argStream << "=" << std::fixed << std::setprecision(5) << amx_ctof(value);
				} else {
					argStream << "=" << value;
				}
			} else {
				if (arg->IsArray() || arg->IsArrayRef()) {
					// Get array dimensions
					std::vector<AMXDebugInfo::SymbolDim> dims = arg->GetDims();
					for (std::size_t i = 0; i < dims.size(); ++i) {
						if (dims[i].GetSize() == 0) {
							argStream << "[]";
						} else {
							std::string tag = debugInfo.GetTagName(dims[i].GetTag()) + ":";
							if (tag == "_:") tag.clear();
							argStream << "[" << tag << dims[i].GetSize() << "]";
						}
					}
				}
				argStream << "=@0x" << std::hex << std::setw(8) << std::setfill('0') << value << std::dec;
			}
		}

		// The function prototype is of the following form:
		// [public ][tag:]name([arg1=value1[, arg2=value2, [...]]])
		if (IsPublic()) {
			functionPrototype_.append("public ");
		}
		functionPrototype_
			.append(functionResultTag_)
			.append(functionName_)
			.append("(")
			.append(argStream.str())
			.append(")");
	} else {
		// No debug info available...
		const char *name = GetPublicFunctionName(amx, functionAddress_);
		if (name != 0) {
			functionName_.assign(name);
		}
	}
}

AMXCallStack::AMXCallStack(AMX *amx, const AMXDebugInfo &debugInfo, ucell topFrame) {
	AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);
	ucell data = reinterpret_cast<ucell>(amx->base + hdr->dat);

	ucell frm = topFrame;
	if (frm == 0) {
		frm = amx->frm;
	}

	while (frm > static_cast<ucell>(amx->stk)) {
		AMXStackFrame frame(amx, frm, debugInfo);
		frames_.push_back(frame);
		if (frame.GetFunctionAddress() == 0) {
			// Invalid frame address
			break;
		}
		frm = *reinterpret_cast<ucell*>(data + frm);
	}

	// Correct entry point address (it's not on the stack)
	if (debugInfo.IsLoaded()) {
		if (frames_.size() > 1) {
			ucell epAddr = debugInfo.GetFunctionStartAddress(frames_[frames_.size() - 2].GetCallAddress());
			frames_.back() = AMXStackFrame(amx, frames_.back().GetFrameAddress(), 0, epAddr, debugInfo);
		} else if (frames_.size() != 0) {
			ucell epAddr = debugInfo.GetFunctionStartAddress(amx->cip);
			frames_.back() = AMXStackFrame(amx, frames_.back().GetFrameAddress(), 0, epAddr, debugInfo);
		}
	} else {
		// Can't fetch the address
		frames_.back() = AMXStackFrame(amx, 0, 0, 0);
	}
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 * Copyright (c) 2016 PX4 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 PX4 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 OWNER 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 df_imu.cpp
 * Lightweight driver to access IMU driver of DriverFramework.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include <px4_config.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_getopt.h>
#include <errno.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_accel.h>

#include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>
//
#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>
#include <VirtDevObj.hpp>

#define ACCELSIM_DEVICE_PATH_ACCEL	"/dev/df_accel"

extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); }

using namespace DriverFramework;

class DfImu : public MPU9250
{
public:
	DfImu(/*enum Rotation rotation*/);
	~DfImu();

	//enum Rotation		_rotation;

	/**
	 * Start automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		start();

	/**
	 * Stop automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		stop();

private:
	virtual int _publish_callback(struct imu_sensor_data &data);

	orb_advert_t		_accel_topic;
	int			_accel_orb_class_instance;

	perf_counter_t		_accel_sample_perf;

};

DfImu::DfImu(/*enum Rotation rotation*/) :
	MPU9250(IMU_DEVICE_PATH),
	_accel_topic(nullptr),
	_accel_orb_class_instance(-1),
	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read"))
	/*_rotation(rotation)*/
{
	m_id.dev_id_s.bus = 1;
	m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}

DfImu::~DfImu()
{
}

int DfImu::start()
{
	struct accel_report arp = {};

	arp.timestamp = 1234;

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);

	if (_accel_topic == nullptr) {
		PX4_WARN("ADVERT ERR");
		return -1;
	}

	/* init device and start sensor */
	int ret = init();

	if (ret != 0) {
		PX4_ERR("MPU9250 init fail: %d", ret);
		return ret;
	}

	ret = MPU9250::start();

	if (ret != 0) {
		PX4_ERR("MPU9250 start fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfImu::stop()
{
	/* stop sensor */
	return 0;
}

int DfImu::_publish_callback(struct imu_sensor_data &data)
{
	accel_report accel_report = {};

	accel_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	accel_report.x_raw = NAN;
	accel_report.y_raw = NAN;
	accel_report.z_raw = NAN;

	accel_report.x = data.accel_m_s2_x;
	accel_report.y = data.accel_m_s2_y;
	accel_report.z = data.accel_m_s2_z;

	// TODO: get these right
	accel_report.scaling = -1.0f;
	accel_report.range_m_s2 = -1.0f;

	if (!(m_pub_blocked)) {
		/* publish it */

		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}
	}

	/* notify anyone waiting for data */
	DevMgr::updateNotify(*this);

	return 0;
};

namespace df_imu
{

DfImu *g_dev = nullptr;

int start(/* enum Rotation rotation */);
int stop();
int info();
void usage();

int start(/*enum Rotation rotation*/)
{
	g_dev = new DfImu(/*rotation*/);

	if (g_dev == nullptr) {
		PX4_ERR("failed instantiating DfImu object");
		return -1;
	}

	int ret = g_dev->start();
	if (ret != 0) {
		PX4_ERR("DfImu start failed");
		return ret;
	}

	// Open the IMU sensor
	DevHandle h;
	DevMgr::getHandle(IMU_DEVICE_PATH, h);
	if (!h.isValid())
	{
		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
			IMU_DEVICE_PATH, h.getError());
		return -1;
	}

	DevMgr::releaseHandle(h);

	return 0;
}

int stop()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	int ret = g_dev->stop();

	if (ret != 0) {
		PX4_ERR("driver could not be stopped");
		return ret;
	}

	delete g_dev;
	g_dev = nullptr;
	return 0;
}

/**
 * Print a little info about the driver.
 */
int
info()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	PX4_DEBUG("state @ %p", g_dev);

	return 0;
}

void
usage()
{
	PX4_WARN("Usage: df_imu 'start', 'info', 'stop'");
	PX4_WARN("options:");
	//PX4_WARN("    -R rotation");
}

} // namespace

int
df_imu_main(int argc, char *argv[])
{
	int ch;
	// enum Rotation rotation = ROTATION_NONE;
	int ret = 0;
	int myoptind = 1;
	const char *myoptarg = NULL;

	/* jump over start/off/etc and look at options first */
	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		//case 'R':
		//	rotation = (enum Rotation)atoi(myoptarg);
		//	break;

		default:
			df_imu::usage();
			return 0;
		}
	}

	if (argc <= 1) {
		df_imu::usage();
		return 1;
	}

	const char *verb = argv[myoptind];

	if (!strcmp(verb, "start")) {
		ret = df_imu::start(/*rotation*/);
	}

	else if (!strcmp(verb, "stop")) {
		ret = df_imu::stop();
	}

	else if (!strcmp(verb, "info")) {
		ret = df_imu::info();
	}

	else {
		df_imu::usage();
		return 1;
	}

	return ret;
}
<commit_msg>df_imu: remove debug relict<commit_after>/****************************************************************************
 *
 * Copyright (c) 2016 PX4 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 PX4 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 OWNER 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 df_imu.cpp
 * Lightweight driver to access IMU driver of DriverFramework.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include <px4_config.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_getopt.h>
#include <errno.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_accel.h>

#include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>
//
#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>
#include <VirtDevObj.hpp>

#define ACCELSIM_DEVICE_PATH_ACCEL	"/dev/df_accel"

extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); }

using namespace DriverFramework;

class DfImu : public MPU9250
{
public:
	DfImu(/*enum Rotation rotation*/);
	~DfImu();

	//enum Rotation		_rotation;

	/**
	 * Start automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		start();

	/**
	 * Stop automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		stop();

private:
	virtual int _publish_callback(struct imu_sensor_data &data);

	orb_advert_t		_accel_topic;
	int			_accel_orb_class_instance;

	perf_counter_t		_accel_sample_perf;

};

DfImu::DfImu(/*enum Rotation rotation*/) :
	MPU9250(IMU_DEVICE_PATH),
	_accel_topic(nullptr),
	_accel_orb_class_instance(-1),
	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read"))
	/*_rotation(rotation)*/
{
	m_id.dev_id_s.bus = 1;
	m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}

DfImu::~DfImu()
{
}

int DfImu::start()
{
	struct accel_report arp = {};

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);

	if (_accel_topic == nullptr) {
		PX4_WARN("ADVERT ERR");
		return -1;
	}

	/* init device and start sensor */
	int ret = init();

	if (ret != 0) {
		PX4_ERR("MPU9250 init fail: %d", ret);
		return ret;
	}

	ret = MPU9250::start();

	if (ret != 0) {
		PX4_ERR("MPU9250 start fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfImu::stop()
{
	/* stop sensor */
	return 0;
}

int DfImu::_publish_callback(struct imu_sensor_data &data)
{
	accel_report accel_report = {};

	accel_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	accel_report.x_raw = NAN;
	accel_report.y_raw = NAN;
	accel_report.z_raw = NAN;

	accel_report.x = data.accel_m_s2_x;
	accel_report.y = data.accel_m_s2_y;
	accel_report.z = data.accel_m_s2_z;

	// TODO: get these right
	accel_report.scaling = -1.0f;
	accel_report.range_m_s2 = -1.0f;

	if (!(m_pub_blocked)) {
		/* publish it */

		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}
	}

	/* notify anyone waiting for data */
	DevMgr::updateNotify(*this);

	return 0;
};

namespace df_imu
{

DfImu *g_dev = nullptr;

int start(/* enum Rotation rotation */);
int stop();
int info();
void usage();

int start(/*enum Rotation rotation*/)
{
	g_dev = new DfImu(/*rotation*/);

	if (g_dev == nullptr) {
		PX4_ERR("failed instantiating DfImu object");
		return -1;
	}

	int ret = g_dev->start();
	if (ret != 0) {
		PX4_ERR("DfImu start failed");
		return ret;
	}

	// Open the IMU sensor
	DevHandle h;
	DevMgr::getHandle(IMU_DEVICE_PATH, h);
	if (!h.isValid())
	{
		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
			IMU_DEVICE_PATH, h.getError());
		return -1;
	}

	DevMgr::releaseHandle(h);

	return 0;
}

int stop()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	int ret = g_dev->stop();

	if (ret != 0) {
		PX4_ERR("driver could not be stopped");
		return ret;
	}

	delete g_dev;
	g_dev = nullptr;
	return 0;
}

/**
 * Print a little info about the driver.
 */
int
info()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	PX4_DEBUG("state @ %p", g_dev);

	return 0;
}

void
usage()
{
	PX4_WARN("Usage: df_imu 'start', 'info', 'stop'");
	PX4_WARN("options:");
	//PX4_WARN("    -R rotation");
}

} // namespace

int
df_imu_main(int argc, char *argv[])
{
	int ch;
	// enum Rotation rotation = ROTATION_NONE;
	int ret = 0;
	int myoptind = 1;
	const char *myoptarg = NULL;

	/* jump over start/off/etc and look at options first */
	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		//case 'R':
		//	rotation = (enum Rotation)atoi(myoptarg);
		//	break;

		default:
			df_imu::usage();
			return 0;
		}
	}

	if (argc <= 1) {
		df_imu::usage();
		return 1;
	}

	const char *verb = argv[myoptind];

	if (!strcmp(verb, "start")) {
		ret = df_imu::start(/*rotation*/);
	}

	else if (!strcmp(verb, "stop")) {
		ret = df_imu::stop();
	}

	else if (!strcmp(verb, "info")) {
		ret = df_imu::info();
	}

	else {
		df_imu::usage();
		return 1;
	}

	return ret;
}
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qgeoroutesegment.h"
#include "qgeoroutesegment_p.h"

#include "qgeocoordinate.h"
#include <QDateTime>

QTM_BEGIN_NAMESPACE

/*!
    \class QGeoRouteSegment
    \brief The QGeoRouteSegment class represents a segment of a route.

    \inmodule QtLocation

    \ingroup maps-routing

    A QGeoRouteSegment instance has information about the physcial layout
    of the route segment, the length of the route and estimated time required
    to traverse the route segment and an optional QGeoManeuver associated with
    the end of the route segment.

    QGeoRouteSegment instances can be thought of as edges on a routing
    graph, with QGeoManeuver instances as optional labels attached to the
    vertices of the graph.
*/

/*!
    Constructs an invalid route segment object.

    The route segment will remain invalid until one of setNextRouteSegment(),
    setTravelTime(), setDistance(), setPath() or setManeuver() is called.
*/
QGeoRouteSegment::QGeoRouteSegment()
    : d_ptr(new QGeoRouteSegmentPrivate()) {}

/*!
    Constructs a route segment object from the contents of \a other.
*/
QGeoRouteSegment::QGeoRouteSegment(const QGeoRouteSegment &other)
    : d_ptr(other.d_ptr) {}

/*!
    \internal
*/
QGeoRouteSegment::QGeoRouteSegment(QExplicitlySharedDataPointer<QGeoRouteSegmentPrivate> &d_ptr)
    : d_ptr(d_ptr) {}

/*!
    Destroys this route segment object.
*/
QGeoRouteSegment::~QGeoRouteSegment() {}

/*!
    Assigns \a other to this route segment object and then returns a
    reference to this route segment object.
*/
QGeoRouteSegment& QGeoRouteSegment::operator= (const QGeoRouteSegment & other)
{
    d_ptr = other.d_ptr;
    return *this;
}

/*!
    Returns whether this route segment and \a other are equal.

    The value of nextRouteSegment() is not considered in the comparison.
*/
bool QGeoRouteSegment::operator ==(const QGeoRouteSegment &other) const
{
    return (d_ptr.constData() == other.d_ptr.constData());
}

/*!
    Returns whether this route segment and \a other are not equal.

    The value of nextRouteSegment() is not considered in the comparison.
*/
bool QGeoRouteSegment::operator !=(const QGeoRouteSegment &other) const
{
    return !(operator==(other));
}

/*!
    Returns whether this route segment is valid or not.

    If nextRouteSegment() is called on the last route segment of a route, the
    returned value will be an invalid route segment.
*/
bool QGeoRouteSegment::isValid() const
{
    return d_ptr->valid;
}

/*!
    Sets the next route segment in the route to \a routeSegment.
*/
void QGeoRouteSegment::setNextRouteSegment(const QGeoRouteSegment &routeSegment)
{
    d_ptr->valid = true;
    d_ptr->nextSegment = routeSegment.d_ptr;
}

/*!
    Returns the next route segment in the route.

    Will return an invalid route segment if this is the last route
    segment in the route.
*/
QGeoRouteSegment QGeoRouteSegment::nextRouteSegment() const
{
    if (d_ptr->valid && d_ptr->nextSegment)
        return QGeoRouteSegment(d_ptr->nextSegment);

    QGeoRouteSegment segment;
    segment.d_ptr->valid = false;
    return segment;
}

/*!
    Sets the estimated amount of time it will take to traverse this segment of
    the route, in seconds, to \a secs.
*/
void QGeoRouteSegment::setTravelTime(int secs)
{
    d_ptr->valid = true;
    d_ptr->travelTime = secs;
}

/*!
    Returns the estimated amount of time it will take to traverse this segment
    of the route, in seconds.
*/
int QGeoRouteSegment::travelTime() const
{
    return d_ptr->travelTime;
}

/*!
    Sets the distance covered by this segment of the route, in metres, to \a distance.
*/
void QGeoRouteSegment::setDistance(qreal distance)
{
    d_ptr->valid = true;
    d_ptr->distance = distance;
}

/*!
    Returns the distance covered by this segment of the route, in metres.
*/
qreal QGeoRouteSegment::distance() const
{
    return d_ptr->distance;
}

/*!
    Sets the geometric shape of this segment of the route to \a path.

    The coordinates in \a path should be listed in the order in which they
    would be traversed by someone traveling along this segment of the route.
*/
void QGeoRouteSegment::setPath(const QList<QGeoCoordinate> &path)
{
    d_ptr->valid = true;
    d_ptr->path = path;
}

/*!
    Returns the geometric shape of this route segment of the route.

    The coordinates should be listed in the order in which they
    would be traversed by someone traveling along this segment of the route.
*/

QList<QGeoCoordinate> QGeoRouteSegment::path() const
{
    return d_ptr->path;
}

/*!
    Sets the maneuver for this route segement to \a maneuver.
*/
void QGeoRouteSegment::setManeuver(const QGeoManeuver &maneuver)
{
    d_ptr->valid = true;
    d_ptr->maneuver = maneuver;
}

/*!
    Returns the manevuer for this route segment.

    Will return an invalid QGeoManeuver if no information has been attached
    to the endpoint of this route segment.
*/
QGeoManeuver QGeoRouteSegment::maneuver() const
{
    return d_ptr->maneuver;
}

/*******************************************************************************
*******************************************************************************/

QGeoRouteSegmentPrivate::QGeoRouteSegmentPrivate()
    : valid(true),
      travelTime(0),
      distance(0.0) {}

QGeoRouteSegmentPrivate::QGeoRouteSegmentPrivate(const QGeoRouteSegmentPrivate &other)
    : QSharedData(other),
      valid(other.valid),
      travelTime(other.travelTime),
      distance(other.distance),
      path(other.path),
      maneuver(other.maneuver),
      nextSegment(other.nextSegment) {}

QGeoRouteSegmentPrivate::~QGeoRouteSegmentPrivate()
{
    nextSegment.reset();
}

bool QGeoRouteSegmentPrivate::operator ==(const QGeoRouteSegmentPrivate &other) const
{
    return ((valid == other.valid)
            && (travelTime == other.travelTime)
            && (distance == other.distance)
            && (path == other.path)
            && (maneuver == other.maneuver));
}

/*******************************************************************************
*******************************************************************************/

QTM_END_NAMESPACE

<commit_msg>Fixed faulty default value of QGeoRouteSegmentPrivate::valid<commit_after>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qgeoroutesegment.h"
#include "qgeoroutesegment_p.h"

#include "qgeocoordinate.h"
#include <QDateTime>

QTM_BEGIN_NAMESPACE

/*!
    \class QGeoRouteSegment
    \brief The QGeoRouteSegment class represents a segment of a route.

    \inmodule QtLocation

    \ingroup maps-routing

    A QGeoRouteSegment instance has information about the physcial layout
    of the route segment, the length of the route and estimated time required
    to traverse the route segment and an optional QGeoManeuver associated with
    the end of the route segment.

    QGeoRouteSegment instances can be thought of as edges on a routing
    graph, with QGeoManeuver instances as optional labels attached to the
    vertices of the graph.
*/

/*!
    Constructs an invalid route segment object.

    The route segment will remain invalid until one of setNextRouteSegment(),
    setTravelTime(), setDistance(), setPath() or setManeuver() is called.
*/
QGeoRouteSegment::QGeoRouteSegment()
    : d_ptr(new QGeoRouteSegmentPrivate()) {}

/*!
    Constructs a route segment object from the contents of \a other.
*/
QGeoRouteSegment::QGeoRouteSegment(const QGeoRouteSegment &other)
    : d_ptr(other.d_ptr) {}

/*!
    \internal
*/
QGeoRouteSegment::QGeoRouteSegment(QExplicitlySharedDataPointer<QGeoRouteSegmentPrivate> &d_ptr)
    : d_ptr(d_ptr) {}

/*!
    Destroys this route segment object.
*/
QGeoRouteSegment::~QGeoRouteSegment() {}

/*!
    Assigns \a other to this route segment object and then returns a
    reference to this route segment object.
*/
QGeoRouteSegment& QGeoRouteSegment::operator= (const QGeoRouteSegment & other)
{
    d_ptr = other.d_ptr;
    return *this;
}

/*!
    Returns whether this route segment and \a other are equal.

    The value of nextRouteSegment() is not considered in the comparison.
*/
bool QGeoRouteSegment::operator ==(const QGeoRouteSegment &other) const
{
    return (d_ptr.constData() == other.d_ptr.constData());
}

/*!
    Returns whether this route segment and \a other are not equal.

    The value of nextRouteSegment() is not considered in the comparison.
*/
bool QGeoRouteSegment::operator !=(const QGeoRouteSegment &other) const
{
    return !(operator==(other));
}

/*!
    Returns whether this route segment is valid or not.

    If nextRouteSegment() is called on the last route segment of a route, the
    returned value will be an invalid route segment.
*/
bool QGeoRouteSegment::isValid() const
{
    return d_ptr->valid;
}

/*!
    Sets the next route segment in the route to \a routeSegment.
*/
void QGeoRouteSegment::setNextRouteSegment(const QGeoRouteSegment &routeSegment)
{
    d_ptr->valid = true;
    d_ptr->nextSegment = routeSegment.d_ptr;
}

/*!
    Returns the next route segment in the route.

    Will return an invalid route segment if this is the last route
    segment in the route.
*/
QGeoRouteSegment QGeoRouteSegment::nextRouteSegment() const
{
    if (d_ptr->valid && d_ptr->nextSegment)
        return QGeoRouteSegment(d_ptr->nextSegment);

    QGeoRouteSegment segment;
    segment.d_ptr->valid = false;
    return segment;
}

/*!
    Sets the estimated amount of time it will take to traverse this segment of
    the route, in seconds, to \a secs.
*/
void QGeoRouteSegment::setTravelTime(int secs)
{
    d_ptr->valid = true;
    d_ptr->travelTime = secs;
}

/*!
    Returns the estimated amount of time it will take to traverse this segment
    of the route, in seconds.
*/
int QGeoRouteSegment::travelTime() const
{
    return d_ptr->travelTime;
}

/*!
    Sets the distance covered by this segment of the route, in metres, to \a distance.
*/
void QGeoRouteSegment::setDistance(qreal distance)
{
    d_ptr->valid = true;
    d_ptr->distance = distance;
}

/*!
    Returns the distance covered by this segment of the route, in metres.
*/
qreal QGeoRouteSegment::distance() const
{
    return d_ptr->distance;
}

/*!
    Sets the geometric shape of this segment of the route to \a path.

    The coordinates in \a path should be listed in the order in which they
    would be traversed by someone traveling along this segment of the route.
*/
void QGeoRouteSegment::setPath(const QList<QGeoCoordinate> &path)
{
    d_ptr->valid = true;
    d_ptr->path = path;
}

/*!
    Returns the geometric shape of this route segment of the route.

    The coordinates should be listed in the order in which they
    would be traversed by someone traveling along this segment of the route.
*/

QList<QGeoCoordinate> QGeoRouteSegment::path() const
{
    return d_ptr->path;
}

/*!
    Sets the maneuver for this route segement to \a maneuver.
*/
void QGeoRouteSegment::setManeuver(const QGeoManeuver &maneuver)
{
    d_ptr->valid = true;
    d_ptr->maneuver = maneuver;
}

/*!
    Returns the manevuer for this route segment.

    Will return an invalid QGeoManeuver if no information has been attached
    to the endpoint of this route segment.
*/
QGeoManeuver QGeoRouteSegment::maneuver() const
{
    return d_ptr->maneuver;
}

/*******************************************************************************
*******************************************************************************/

QGeoRouteSegmentPrivate::QGeoRouteSegmentPrivate()
    : valid(false),
      travelTime(0),
      distance(0.0) {}

QGeoRouteSegmentPrivate::QGeoRouteSegmentPrivate(const QGeoRouteSegmentPrivate &other)
    : QSharedData(other),
      valid(other.valid),
      travelTime(other.travelTime),
      distance(other.distance),
      path(other.path),
      maneuver(other.maneuver),
      nextSegment(other.nextSegment) {}

QGeoRouteSegmentPrivate::~QGeoRouteSegmentPrivate()
{
    nextSegment.reset();
}

bool QGeoRouteSegmentPrivate::operator ==(const QGeoRouteSegmentPrivate &other) const
{
    return ((valid == other.valid)
            && (travelTime == other.travelTime)
            && (distance == other.distance)
            && (path == other.path)
            && (maneuver == other.maneuver));
}

/*******************************************************************************
*******************************************************************************/

QTM_END_NAMESPACE

<|endoftext|>


<commit_before>#include <stdio.h>
#include <sys/sysinfo.h>
#include <sys/statfs.h>
#include <sys/vfs.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <sys/ioctl.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <net/if.h>
#include "event/event.h"
#include "event/bufferevent.h"
#include "event/buffer.h"
#include "event/thread.h"
#include "Config.h"
#include "json/json.h"
#include "Protocol.h"

using namespace std;
#define CONFIG_PATH "/etc/smart_car_device.conf"
typedef void (*cfunc)(struct bufferevent *,Json::Value&);
string network_card_name;
string device_name;
string api_host;
int api_port;
struct event_base* baseEvent;
map<string,cfunc> client_api_list;
//获取基本信息
void handlerGetDeviceBaseInfo(struct bufferevent * bufEvent,Json::Value &data){
    //获取内存大小
    struct sysinfo memInfo;
    sysinfo(&memInfo);
    double totalMemSize = (double)memInfo.totalram/(1024.0*1024.0);
    double usedMemSize = (double)(memInfo.totalram-memInfo.freeram)/(1024.0*1024.0);
    //获取磁盘大小
    struct statfs diskInfo;
    statfs("/", &diskInfo);
    double totalDiskSize = (double)(diskInfo.f_bsize*diskInfo.f_blocks)/(1024.0*1024.0);
    double usedDiskSize = (double)(diskInfo.f_bsize*diskInfo.f_blocks-diskInfo.f_bsize*diskInfo.f_bfree)/(1024.0*1024.0);
    printf("mem total:%.2fMB,mem used:%.2fMB,disk total:%.2fMB,disk used:%.2fMB\n",totalMemSize,usedMemSize,totalDiskSize,usedDiskSize );
    Json::Value root;
    Json::Value re_data;
    root["is_app"] = false;
    root["protocol"] = API_DEVICE_BASE_INFO;
    root["data"] = re_data;
    bufferevent_write(bufEvent, root.toStyledString().c_str(), root.toStyledString().length());
}
//键盘按下
void handlerKeyDown(struct bufferevent * bufEvent,Json::Value &data){
    printf("key down:%s\n", data["data"].toStyledString().c_str());
}
//获取MAC地址
string getMacAddress(){
    FILE *fstream = NULL;
    char buff[32];
    memset (buff ,'\0', sizeof(buff));
    string cmd = "ip addr |grep -A 2 "+network_card_name+" | awk 'NR>1'|awk 'NR<2'|awk '{print $2}'"
    // 通过管道来回去系统命令返回的值
    if(NULL == (fstream = popen (cmd.c_str (), "r"))) {
        perror("popen");
        exit(0);
    }
    if(NULL == fgets (buff, sizeof(buff), fstream)){
        printf("not find mac address !!!\n");
        exit(0);
    }
    pclose(fstream);
    return string(buff);
}
//调用方法
void callFunc(struct bufferevent * bufEvent,Json::Value &request_data,const string func){
    if(func.length() == 0){
        return;
    }
    if (client_api_list.count(func)) {
        (*(client_api_list[func]))(bufEvent,request_data);
    }
}

void sendDeviceInfo(struct bufferevent * bufEvent){
    Json::Value root;
    Json::Value data;
    //获取MAC地址
    data["mac"] = getMacAddress();
    data["name"] = device_name;
    root["protocol"] = API_DEVICE_INFO;
    root["is_app"] = false;
    root["data"] = data;
    string json = root.toStyledString();
    bufferevent_write(bufEvent, json.c_str(), json.length());
}

//读操作
void ReadEventCb(struct bufferevent *bufEvent, void *args){
   Json::Reader reader;
    Json::Value data;
    //获取输入缓存
    struct evbuffer * pInput = bufferevent_get_input(bufEvent);
    //获取输入缓存数据的长度
    int len = evbuffer_get_length(pInput);
    //获取数据
    char* body = new char[len+1];
    memset(body,0,sizeof(char)*(len+1));
    evbuffer_remove(pInput, body, len);
    if(reader.parse(body, data)){
        string func = data["protocol"].asString();
        callFunc(bufEvent,data,func);
    }
    delete[] body;
    return ;
}
//写操作
void WriteEventCb(struct bufferevent *bufEvent, void *args){

}
//关闭
void SignalEventCb(struct bufferevent * bufEvent, short sEvent, void * args){
    //请求的连接过程已经完成
    if(BEV_EVENT_CONNECTED == sEvent){
        bufferevent_enable(bufEvent, EV_READ);
        //设置读超时时间 10s
        struct timeval tTimeout = {10, 0};
        bufferevent_set_timeouts( bufEvent, &tTimeout, NULL);
        string mac = getMacAddress();
        if(mac.length() == 0){
            printf("MAC地址获取错误请检查网卡配置\n");
            event_base_loopexit(baseEvent, NULL);
            exit(0);
        }
        //发送基本信息
        sendDeviceInfo(bufEvent);
    }
    //写操作发生事件
    if(BEV_EVENT_WRITING & sEvent){}
    //操作时发生错误
    if (sEvent & BEV_EVENT_ERROR){
        perror("event");
        event_base_loopexit(baseEvent, NULL);
    }
    //结束指示
    if (sEvent & BEV_EVENT_EOF){
        perror("event");
        event_base_loopexit(baseEvent, NULL);
    }
    //读取发生事件或者超时处理
    if(0 != (sEvent & (BEV_EVENT_TIMEOUT|BEV_EVENT_READING)) ){
        //发送心跳包
        //
        //重新注册可读事件
        bufferevent_enable(bufEvent, EV_READ);
    }
    return ;
}

void setConfig(const char* config_path){
    Config config;
    //检测配置文件是否存在
    if(!config.FileExist(config_path)){
      printf("config: not find config file\n");
      exit(0);
    }
    //读取配置
    config.ReadFile(config_path);
    api_host = config.Read("SERVER_HOST", api_host);
    api_port = config.Read("API_PORT", api_port);
    network_card_name = config.Read("NETWORK_CARD", network_card_name);
    device_name = config.Read("DEVICE_NAME", device_name);
}

void startRun(const char* ip,int port){
    //创建事件驱动句柄
    baseEvent = event_base_new();
    //创建socket类型的bufferevent
    struct bufferevent* bufferEvent = bufferevent_socket_new(baseEvent, -1, 0);
    //构造服务器地址
    struct sockaddr_in sin;
    memset(&sin, 0, sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr = inet_addr(api_host.c_str());
    sin.sin_port = htons(api_port);
    //连接服务器
    if( bufferevent_socket_connect(bufferEvent, (struct sockaddr*)&sin, sizeof(sin)) < 0){
        perror("socket");
        return;
    }
    //设置回调函数, 及回调函数的参数
    bufferevent_setcb(bufferEvent, ReadEventCb, WriteEventCb, SignalEventCb,NULL);
    //开始事件循环
    event_base_dispatch(baseEvent);
    //事件循环结束 资源清理
    bufferevent_free(bufferEvent);
    event_base_free(baseEvent);
}
//初始化API列表
void initApiList() {
  client_api_list[API_DEVICE_BASE_INFO] = &handlerGetDeviceBaseInfo;
  client_api_list[API_DEVICE_KEY_DOWN] = &handlerKeyDown;
}

int main(){
    //加载API列表
    initApiList();
    //加载配置文件
    setConfig(CONFIG_PATH);
    //启动sockt
    startRun(api_host.c_str(),api_port);
    return 0;
}
<commit_msg>printf wanl0<commit_after>#include <stdio.h>
#include <sys/sysinfo.h>
#include <sys/statfs.h>
#include <sys/vfs.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <sys/ioctl.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <net/if.h>
#include "event/event.h"
#include "event/bufferevent.h"
#include "event/buffer.h"
#include "event/thread.h"
#include "Config.h"
#include "json/json.h"
#include "Protocol.h"

using namespace std;
#define CONFIG_PATH "/etc/smart_car_device.conf"
typedef void (*cfunc)(struct bufferevent *,Json::Value&);
string network_card_name;
string device_name;
string api_host;
int api_port;
struct event_base* baseEvent;
map<string,cfunc> client_api_list;
//获取基本信息
void handlerGetDeviceBaseInfo(struct bufferevent * bufEvent,Json::Value &data){
    //获取内存大小
    struct sysinfo memInfo;
    sysinfo(&memInfo);
    double totalMemSize = (double)memInfo.totalram/(1024.0*1024.0);
    double usedMemSize = (double)(memInfo.totalram-memInfo.freeram)/(1024.0*1024.0);
    //获取磁盘大小
    struct statfs diskInfo;
    statfs("/", &diskInfo);
    double totalDiskSize = (double)(diskInfo.f_bsize*diskInfo.f_blocks)/(1024.0*1024.0);
    double usedDiskSize = (double)(diskInfo.f_bsize*diskInfo.f_blocks-diskInfo.f_bsize*diskInfo.f_bfree)/(1024.0*1024.0);
    printf("mem total:%.2fMB,mem used:%.2fMB,disk total:%.2fMB,disk used:%.2fMB\n",totalMemSize,usedMemSize,totalDiskSize,usedDiskSize );
    Json::Value root;
    Json::Value re_data;
    root["is_app"] = false;
    root["protocol"] = API_DEVICE_BASE_INFO;
    root["data"] = re_data;
    bufferevent_write(bufEvent, root.toStyledString().c_str(), root.toStyledString().length());
}
//键盘按下
void handlerKeyDown(struct bufferevent * bufEvent,Json::Value &data){
    printf("key down:%s\n", data["data"].toStyledString().c_str());
}
//获取MAC地址
string getMacAddress(){
    char buff[32];
    memset (buff ,'\0', sizeof(buff));
    string cmd = "ip addr |grep -A 2 "+network_card_name+" | awk 'NR>1'|awk 'NR<2'|awk '{print $2}'"
    // 通过管道来回去系统命令返回的值
    FILE *fstream = popen(cmd.c_str (), "r");
    if(fstream  == NULL) {
        perror("popen");
        exit(0);
    }
    if(NULL == fgets (buff, sizeof(buff), fstream)){
        printf("not find mac address !!!\n");
        exit(0);
    }
    pclose(fstream);
    return string(buff);
}
//调用方法
void callFunc(struct bufferevent * bufEvent,Json::Value &request_data,const string func){
    if(func.length() == 0){
        return;
    }
    if (client_api_list.count(func)) {
        (*(client_api_list[func]))(bufEvent,request_data);
    }
}

void sendDeviceInfo(struct bufferevent * bufEvent){
    Json::Value root;
    Json::Value data;
    //获取MAC地址
    data["mac"] = getMacAddress();
    data["name"] = device_name;
    root["protocol"] = API_DEVICE_INFO;
    root["is_app"] = false;
    root["data"] = data;
    string json = root.toStyledString();
    bufferevent_write(bufEvent, json.c_str(), json.length());
}

//读操作
void ReadEventCb(struct bufferevent *bufEvent, void *args){
   Json::Reader reader;
    Json::Value data;
    //获取输入缓存
    struct evbuffer * pInput = bufferevent_get_input(bufEvent);
    //获取输入缓存数据的长度
    int len = evbuffer_get_length(pInput);
    //获取数据
    char* body = new char[len+1];
    memset(body,0,sizeof(char)*(len+1));
    evbuffer_remove(pInput, body, len);
    if(reader.parse(body, data)){
        string func = data["protocol"].asString();
        callFunc(bufEvent,data,func);
    }
    delete[] body;
    return ;
}
//写操作
void WriteEventCb(struct bufferevent *bufEvent, void *args){

}
//关闭
void SignalEventCb(struct bufferevent * bufEvent, short sEvent, void * args){
    //请求的连接过程已经完成
    if(BEV_EVENT_CONNECTED == sEvent){
        bufferevent_enable(bufEvent, EV_READ);
        //设置读超时时间 10s
        struct timeval tTimeout = {10, 0};
        bufferevent_set_timeouts( bufEvent, &tTimeout, NULL);
        string mac = getMacAddress();
        if(mac.length() == 0){
            printf("MAC地址获取错误请检查网卡配置\n");
            event_base_loopexit(baseEvent, NULL);
            exit(0);
        }
        //发送基本信息
        sendDeviceInfo(bufEvent);
    }
    //写操作发生事件
    if(BEV_EVENT_WRITING & sEvent){}
    //操作时发生错误
    if (sEvent & BEV_EVENT_ERROR){
        perror("event");
        event_base_loopexit(baseEvent, NULL);
    }
    //结束指示
    if (sEvent & BEV_EVENT_EOF){
        perror("event");
        event_base_loopexit(baseEvent, NULL);
    }
    //读取发生事件或者超时处理
    if(0 != (sEvent & (BEV_EVENT_TIMEOUT|BEV_EVENT_READING)) ){
        //发送心跳包
        //
        //重新注册可读事件
        bufferevent_enable(bufEvent, EV_READ);
    }
    return ;
}

void setConfig(const char* config_path){
    Config config;
    //检测配置文件是否存在
    if(!config.FileExist(config_path)){
      printf("config: not find config file\n");
      exit(0);
    }
    //读取配置
    config.ReadFile(config_path);
    api_host = config.Read("SERVER_HOST", api_host);
    api_port = config.Read("API_PORT", api_port);
    network_card_name = config.Read("NETWORK_CARD", network_card_name);
    device_name = config.Read("DEVICE_NAME", device_name);
}

void startRun(const char* ip,int port){
    //创建事件驱动句柄
    baseEvent = event_base_new();
    //创建socket类型的bufferevent
    struct bufferevent* bufferEvent = bufferevent_socket_new(baseEvent, -1, 0);
    //构造服务器地址
    struct sockaddr_in sin;
    memset(&sin, 0, sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr = inet_addr(api_host.c_str());
    sin.sin_port = htons(api_port);
    //连接服务器
    if( bufferevent_socket_connect(bufferEvent, (struct sockaddr*)&sin, sizeof(sin)) < 0){
        perror("socket");
        return;
    }
    //设置回调函数, 及回调函数的参数
    bufferevent_setcb(bufferEvent, ReadEventCb, WriteEventCb, SignalEventCb,NULL);
    //开始事件循环
    event_base_dispatch(baseEvent);
    //事件循环结束 资源清理
    bufferevent_free(bufferEvent);
    event_base_free(baseEvent);
}
//初始化API列表
void initApiList() {
  client_api_list[API_DEVICE_BASE_INFO] = &handlerGetDeviceBaseInfo;
  client_api_list[API_DEVICE_KEY_DOWN] = &handlerKeyDown;
}

int main(){
    //加载API列表
    initApiList();
    //加载配置文件
    setConfig(CONFIG_PATH);
    //启动sockt
    startRun(api_host.c_str(),api_port);
    return 0;
}
<|endoftext|>


<commit_before>/*===========================================================================*\
 *                                                                           *
 *                               OpenMesh                                    *
 *      Copyright (C) 2001-2011 by Computer Graphics Group, RWTH Aachen      *
 *                           www.openmesh.org                                *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of OpenMesh.                                           *
 *                                                                           *
 *  OpenMesh is free software: you can redistribute it and/or modify         *
 *  it under the terms of the GNU Lesser General Public License as           *
 *  published by the Free Software Foundation, either version 3 of           *
 *  the License, or (at your option) any later version with the              *
 *  following exceptions:                                                    *
 *                                                                           *
 *  If other files instantiate templates or use macros                       *
 *  or inline functions from this file, or you compile this file and         *
 *  link it with other files to produce an executable, this file does        *
 *  not by itself cause the resulting executable to be covered by the        *
 *  GNU Lesser General Public License. This exception does not however       *
 *  invalidate any other reasons why the executable file might be            *
 *  covered by the GNU Lesser General Public License.                        *
 *                                                                           *
 *  OpenMesh is distributed in the hope that it will be useful,              *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
 *  GNU Lesser General Public License for more details.                      *
 *                                                                           *
 *  You should have received a copy of the GNU LesserGeneral Public          *
 *  License along with OpenMesh.  If not,                                    *
 *  see <http://www.gnu.org/licenses/>.                                      *
 *                                                                           *
\*===========================================================================*/

/*===========================================================================*\
 *                                                                           *
 *   $Revision$                                                         *
 *   $Date$                   *
 *                                                                           *
\*===========================================================================*/

//=============================================================================
//
//  CLASS PolyMeshT - IMPLEMENTATION
//
//=============================================================================

#define OPENMESH_POLYMESH_C

//== INCLUDES =================================================================

#include <OpenMesh/Core/Mesh/PolyMeshT.hh>
#include <OpenMesh/Core/Geometry/LoopSchemeMaskT.hh>
#include <OpenMesh/Core/Utils/vector_cast.hh>
#include <OpenMesh/Core/System/omstream.hh>
#include <vector>

//== NAMESPACES ===============================================================

namespace OpenMesh {

//== IMPLEMENTATION ==========================================================

template <class Kernel>
uint PolyMeshT<Kernel>::find_feature_edges(Scalar _angle_tresh)
{
  assert(Kernel::has_edge_status());//this function needs edge status property
  uint n_feature_edges = 0;
  for (EdgeIter e_it = Kernel::edges_begin(); e_it != Kernel::edges_end(); ++e_it)
  {
    if (fabs(calc_dihedral_angle(e_it)) > _angle_tresh)
    {//note: could be optimized by comparing cos(dih_angle) vs. cos(_angle_tresh)
      status(e_it).set_feature(true);
      n_feature_edges++;
    }
    else
    {
      status(e_it).set_feature(false);
    }
  }
  return n_feature_edges;
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_face_normal(FaceHandle _fh) const
{
  assert(this->halfedge_handle(_fh).is_valid());
  ConstFaceVertexIter fv_it(this->cfv_iter(_fh));

  Point p0 = this->point(fv_it);
  Point p0i = p0; //save point of vertex 0
  ++fv_it;
  Point p1 = this->point(fv_it);
  Point p1i = p1; //save point of vertex 1
  ++fv_it;
  Point p2;

  //calculate area-weighted average normal of polygon's ears
  Normal n(0,0,0);
  for(; fv_it; ++fv_it)
  {
    p2 = this->point(fv_it);
    n += vector_cast<Normal>(calc_face_normal(p0, p1, p2));
    p0 = p1;
    p1 = p2;
  }

  //two additional steps since we started at vertex 2, not 0
  n += vector_cast<Normal>(calc_face_normal(p0i, p0, p1));
  n += vector_cast<Normal>(calc_face_normal(p1i, p0i, p1));

  typename Normal::value_type norm = n.length();

  // The expression ((n *= (1.0/norm)),n) is used because the OpenSG
  // vector class does not return self after component-wise
  // self-multiplication with a scalar!!!
  return (norm != typename Normal::value_type(0)) ? ((n *= (typename Normal::value_type(1)/norm)),n) : Normal(0,0,0);
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_face_normal(const Point& _p0,
     const Point& _p1,
     const Point& _p2) const
{
#if 1
  // The OpenSG <Vector>::operator -= () does not support the type Point
  // as rhs. Therefore use vector_cast at this point!!!
  // Note! OpenSG distinguishes between Normal and Point!!!
  Normal p1p0(vector_cast<Normal>(_p0));  p1p0 -= vector_cast<Normal>(_p1);
  Normal p1p2(vector_cast<Normal>(_p2));  p1p2 -= vector_cast<Normal>(_p1);

  Normal n    = cross(p1p2, p1p0);
  typename Normal::value_type norm = n.length();

  // The expression ((n *= (1.0/norm)),n) is used because the OpenSG
  // vector class does not return self after component-wise
  // self-multiplication with a scalar!!!
  return (norm != typename Normal::value_type(0)) ? ((n *= (typename Normal::value_type(1)/norm)),n) : Normal(0,0,0);
#else
  Point p1p0 = _p0;  p1p0 -= _p1;
  Point p1p2 = _p2;  p1p2 -= _p1;

  Normal n = vector_cast<Normal>(cross(p1p2, p1p0));
  typename Normal::value_type norm = n.length();

  return (norm != 0.0) ? n *= (1.0/norm) : Normal(0,0,0);
#endif
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
calc_face_centroid(FaceHandle _fh, Point& _pt) const
{
  _pt.vectorize(0);
  Scalar valence = 0.0;
  for (ConstFaceVertexIter cfv_it = this->cfv_iter(_fh); cfv_it; ++cfv_it, valence += 1.0)
  {
    _pt += this->point(cfv_it);
  }
  _pt /= valence;
}
//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_normals()
{
  // Face normals are required to compute the vertex and the halfedge normals
  if (Kernel::has_face_normals() ) {
    update_face_normals();

    if (Kernel::has_vertex_normals() ) update_vertex_normals();
    if (Kernel::has_halfedge_normals()) update_halfedge_normals();
  }
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_face_normals()
{
  FaceIter f_it(Kernel::faces_begin()), f_end(Kernel::faces_end());

  for (; f_it != f_end; ++f_it)
    this->set_normal(f_it.handle(), calc_face_normal(f_it.handle()));
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_halfedge_normals(const double _feature_angle)
{
  HalfedgeIter h_it(Kernel::halfedges_begin()), h_end(Kernel::halfedges_end());

  for (; h_it != h_end; ++h_it)
    this->set_normal(h_it.handle(), calc_halfedge_normal(h_it.handle(), _feature_angle));
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_halfedge_normal(HalfedgeHandle _heh, const double _feature_angle) const
{
  if(Kernel::is_boundary(_heh))
    return Normal(0,0,0);
  else
  {
    std::vector<FaceHandle> fhs; fhs.reserve(10);

    HalfedgeHandle heh = _heh;

    // collect CW face-handles
    do
    {
      fhs.push_back(Kernel::face_handle(heh));

      heh = Kernel::next_halfedge_handle(heh);
      heh = Kernel::opposite_halfedge_handle(heh);
    }
    while(heh != _heh && !Kernel::is_boundary(heh) && !is_estimated_feature_edge(heh, _feature_angle));

    // collect CCW face-handles
    if(heh != _heh && !is_estimated_feature_edge(_heh, _feature_angle))
    {
      heh = Kernel::opposite_halfedge_handle(_heh);

      if ( !Kernel::is_boundary(heh) ) {
        do
        {

          fhs.push_back(Kernel::face_handle(heh));

          heh = Kernel::prev_halfedge_handle(heh);
          heh = Kernel::opposite_halfedge_handle(heh);
        }
        while(!Kernel::is_boundary(heh) && !is_estimated_feature_edge(heh, _feature_angle));
      }
    }

    Normal n(0,0,0);
    for(unsigned int i=0; i<fhs.size(); ++i)
      n += Kernel::normal(fhs[i]);

    return n.normalize();
  }
}

//-----------------------------------------------------------------------------

template <class Kernel>
bool
PolyMeshT<Kernel>::
is_estimated_feature_edge(HalfedgeHandle _heh, const double _feature_angle) const
{
  EdgeHandle eh = Kernel::edge_handle(_heh);

  if(Kernel::has_edge_status())
  {
    if(Kernel::status(eh).feature())
      return true;
  }

  if(Kernel::is_boundary(eh))
    return false;

  // compute angle between faces
  FaceHandle fh0 = Kernel::face_handle(_heh);
  FaceHandle fh1 = Kernel::face_handle(Kernel::opposite_halfedge_handle(_heh));

  Normal fn0 = Kernel::normal(fh0);
  Normal fn1 = Kernel::normal(fh1);

  // dihedral angle above angle threshold
  return ( dot(fn0,fn1) < cos(_feature_angle) );
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_vertex_normal(VertexHandle _vh) const
{
  Normal n;
  calc_vertex_normal_fast(_vh,n);

  Scalar norm = n.length();
  if (norm != 0.0) n *= (1.0/norm);

  return n;
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_fast(VertexHandle _vh, Normal& _n) const
{
  _n.vectorize(0.0);
  for (ConstVertexFaceIter vf_it=this->cvf_iter(_vh); vf_it; ++vf_it)
    _n += this->normal(vf_it.handle());
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_correct(VertexHandle _vh, Normal& _n) const
{
  _n.vectorize(0.0);
  ConstVertexIHalfedgeIter cvih_it = cvih_iter(_vh);
  if (!cvih_it)
  {//don't crash on isolated vertices
    return;
  }
  Normal in_he_vec;
  calc_edge_vector(cvih_it, in_he_vec);
  for ( ; cvih_it; ++cvih_it)
  {//calculates the sector normal defined by cvih_it and adds it to _n
    if (is_boundary(cvih_it))
    {
      continue;
    }
    HalfedgeHandle out_heh(next_halfedge_handle(cvih_it));
    Normal out_he_vec;
    calc_edge_vector(out_heh, out_he_vec);
    _n += cross(in_he_vec, out_he_vec);//sector area is taken into account
    in_he_vec = out_he_vec;
    in_he_vec *= -1;//change the orientation
  }
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_loop(VertexHandle _vh, Normal& _n) const
{
  static const LoopSchemeMaskDouble& loop_scheme_mask__ =
                  LoopSchemeMaskDoubleSingleton::Instance();

  Normal t_v(0.0,0.0,0.0), t_w(0.0,0.0,0.0);
  unsigned int vh_val = valence(_vh);
  unsigned int i = 0;
  for (ConstVertexOHalfedgeIter cvoh_it = cvoh_iter(_vh); cvoh_it; ++cvoh_it, ++i)
  {
    VertexHandle r1_v(to_vertex_handle(cvoh_it));
    t_v += (typename Point::value_type)(loop_scheme_mask__.tang0_weight(vh_val, i))*this->point(r1_v);
    t_w += (typename Point::value_type)(loop_scheme_mask__.tang1_weight(vh_val, i))*this->point(r1_v);
  }
  _n = cross(t_w, t_v);//hack: should be cross(t_v, t_w), but then the normals are reversed?
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_vertex_normals()
{
  VertexIter  v_it(Kernel::vertices_begin()), v_end(Kernel::vertices_end());

  for (; v_it!=v_end; ++v_it)
    this->set_normal(v_it.handle(), calc_vertex_normal(v_it.handle()));
}

//=============================================================================
} // namespace OpenMesh
//=============================================================================
<commit_msg>Fixed the usage of vector traits such that the traits are used and not the vector types value_type. (Thanks to Mario Deuss for the patch)<commit_after>/*===========================================================================*\
 *                                                                           *
 *                               OpenMesh                                    *
 *      Copyright (C) 2001-2011 by Computer Graphics Group, RWTH Aachen      *
 *                           www.openmesh.org                                *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of OpenMesh.                                           *
 *                                                                           *
 *  OpenMesh is free software: you can redistribute it and/or modify         *
 *  it under the terms of the GNU Lesser General Public License as           *
 *  published by the Free Software Foundation, either version 3 of           *
 *  the License, or (at your option) any later version with the              *
 *  following exceptions:                                                    *
 *                                                                           *
 *  If other files instantiate templates or use macros                       *
 *  or inline functions from this file, or you compile this file and         *
 *  link it with other files to produce an executable, this file does        *
 *  not by itself cause the resulting executable to be covered by the        *
 *  GNU Lesser General Public License. This exception does not however       *
 *  invalidate any other reasons why the executable file might be            *
 *  covered by the GNU Lesser General Public License.                        *
 *                                                                           *
 *  OpenMesh is distributed in the hope that it will be useful,              *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
 *  GNU Lesser General Public License for more details.                      *
 *                                                                           *
 *  You should have received a copy of the GNU LesserGeneral Public          *
 *  License along with OpenMesh.  If not,                                    *
 *  see <http://www.gnu.org/licenses/>.                                      *
 *                                                                           *
\*===========================================================================*/

/*===========================================================================*\
 *                                                                           *
 *   $Revision$                                                         *
 *   $Date$                   *
 *                                                                           *
\*===========================================================================*/

//=============================================================================
//
//  CLASS PolyMeshT - IMPLEMENTATION
//
//=============================================================================

#define OPENMESH_POLYMESH_C

//== INCLUDES =================================================================

#include <OpenMesh/Core/Mesh/PolyMeshT.hh>
#include <OpenMesh/Core/Geometry/LoopSchemeMaskT.hh>
#include <OpenMesh/Core/Utils/vector_cast.hh>
#include <OpenMesh/Core/System/omstream.hh>
#include <vector>

//== NAMESPACES ===============================================================

namespace OpenMesh {

//== IMPLEMENTATION ==========================================================

template <class Kernel>
uint PolyMeshT<Kernel>::find_feature_edges(Scalar _angle_tresh)
{
  assert(Kernel::has_edge_status());//this function needs edge status property
  uint n_feature_edges = 0;
  for (EdgeIter e_it = Kernel::edges_begin(); e_it != Kernel::edges_end(); ++e_it)
  {
    if (fabs(calc_dihedral_angle(e_it)) > _angle_tresh)
    {//note: could be optimized by comparing cos(dih_angle) vs. cos(_angle_tresh)
      status(e_it).set_feature(true);
      n_feature_edges++;
    }
    else
    {
      status(e_it).set_feature(false);
    }
  }
  return n_feature_edges;
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_face_normal(FaceHandle _fh) const
{
  assert(this->halfedge_handle(_fh).is_valid());
  ConstFaceVertexIter fv_it(this->cfv_iter(_fh));

  Point p0 = this->point(fv_it);
  Point p0i = p0; //save point of vertex 0
  ++fv_it;
  Point p1 = this->point(fv_it);
  Point p1i = p1; //save point of vertex 1
  ++fv_it;
  Point p2;

  //calculate area-weighted average normal of polygon's ears
  Normal n(0,0,0);
  for(; fv_it; ++fv_it)
  {
    p2 = this->point(fv_it);
    n += vector_cast<Normal>(calc_face_normal(p0, p1, p2));
    p0 = p1;
    p1 = p2;
  }

  //two additional steps since we started at vertex 2, not 0
  n += vector_cast<Normal>(calc_face_normal(p0i, p0, p1));
  n += vector_cast<Normal>(calc_face_normal(p1i, p0i, p1));

  typename vector_traits<Normal>::value_type norm = n.length();

  // The expression ((n *= (1.0/norm)),n) is used because the OpenSG
  // vector class does not return self after component-wise
  // self-multiplication with a scalar!!!
  return (norm != typename vector_traits<Normal>::value_type(0)) ? ((n *= (typename vector_traits<Normal>::value_type(1)/norm)),n) : Normal(0,0,0);
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_face_normal(const Point& _p0,
     const Point& _p1,
     const Point& _p2) const
{
#if 1
  // The OpenSG <Vector>::operator -= () does not support the type Point
  // as rhs. Therefore use vector_cast at this point!!!
  // Note! OpenSG distinguishes between Normal and Point!!!
  Normal p1p0(vector_cast<Normal>(_p0));  p1p0 -= vector_cast<Normal>(_p1);
  Normal p1p2(vector_cast<Normal>(_p2));  p1p2 -= vector_cast<Normal>(_p1);

  Normal n    = cross(p1p2, p1p0);
  typename vector_traits<Normal>::value_type norm = n.length();

  // The expression ((n *= (1.0/norm)),n) is used because the OpenSG
  // vector class does not return self after component-wise
  // self-multiplication with a scalar!!!
  return (norm != typename vector_traits<Normal>::value_type(0)) ? ((n *= (typename vector_traits<Normal>::value_type(1)/norm)),n) : Normal(0,0,0);
#else
  Point p1p0 = _p0;  p1p0 -= _p1;
  Point p1p2 = _p2;  p1p2 -= _p1;

  Normal n = vector_cast<Normal>(cross(p1p2, p1p0));
  typename vector_traits<Normal>::value_type norm = n.length();

  return (norm != 0.0) ? n *= (1.0/norm) : Normal(0,0,0);
#endif
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
calc_face_centroid(FaceHandle _fh, Point& _pt) const
{
  _pt.vectorize(0);
  Scalar valence = 0.0;
  for (ConstFaceVertexIter cfv_it = this->cfv_iter(_fh); cfv_it; ++cfv_it, valence += 1.0)
  {
    _pt += this->point(cfv_it);
  }
  _pt /= valence;
}
//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_normals()
{
  // Face normals are required to compute the vertex and the halfedge normals
  if (Kernel::has_face_normals() ) {
    update_face_normals();

    if (Kernel::has_vertex_normals() ) update_vertex_normals();
    if (Kernel::has_halfedge_normals()) update_halfedge_normals();
  }
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_face_normals()
{
  FaceIter f_it(Kernel::faces_begin()), f_end(Kernel::faces_end());

  for (; f_it != f_end; ++f_it)
    this->set_normal(f_it.handle(), calc_face_normal(f_it.handle()));
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_halfedge_normals(const double _feature_angle)
{
  HalfedgeIter h_it(Kernel::halfedges_begin()), h_end(Kernel::halfedges_end());

  for (; h_it != h_end; ++h_it)
    this->set_normal(h_it.handle(), calc_halfedge_normal(h_it.handle(), _feature_angle));
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_halfedge_normal(HalfedgeHandle _heh, const double _feature_angle) const
{
  if(Kernel::is_boundary(_heh))
    return Normal(0,0,0);
  else
  {
    std::vector<FaceHandle> fhs; fhs.reserve(10);

    HalfedgeHandle heh = _heh;

    // collect CW face-handles
    do
    {
      fhs.push_back(Kernel::face_handle(heh));

      heh = Kernel::next_halfedge_handle(heh);
      heh = Kernel::opposite_halfedge_handle(heh);
    }
    while(heh != _heh && !Kernel::is_boundary(heh) && !is_estimated_feature_edge(heh, _feature_angle));

    // collect CCW face-handles
    if(heh != _heh && !is_estimated_feature_edge(_heh, _feature_angle))
    {
      heh = Kernel::opposite_halfedge_handle(_heh);

      if ( !Kernel::is_boundary(heh) ) {
        do
        {

          fhs.push_back(Kernel::face_handle(heh));

          heh = Kernel::prev_halfedge_handle(heh);
          heh = Kernel::opposite_halfedge_handle(heh);
        }
        while(!Kernel::is_boundary(heh) && !is_estimated_feature_edge(heh, _feature_angle));
      }
    }

    Normal n(0,0,0);
    for(unsigned int i=0; i<fhs.size(); ++i)
      n += Kernel::normal(fhs[i]);

    return n.normalize();
  }
}

//-----------------------------------------------------------------------------

template <class Kernel>
bool
PolyMeshT<Kernel>::
is_estimated_feature_edge(HalfedgeHandle _heh, const double _feature_angle) const
{
  EdgeHandle eh = Kernel::edge_handle(_heh);

  if(Kernel::has_edge_status())
  {
    if(Kernel::status(eh).feature())
      return true;
  }

  if(Kernel::is_boundary(eh))
    return false;

  // compute angle between faces
  FaceHandle fh0 = Kernel::face_handle(_heh);
  FaceHandle fh1 = Kernel::face_handle(Kernel::opposite_halfedge_handle(_heh));

  Normal fn0 = Kernel::normal(fh0);
  Normal fn1 = Kernel::normal(fh1);

  // dihedral angle above angle threshold
  return ( dot(fn0,fn1) < cos(_feature_angle) );
}

//-----------------------------------------------------------------------------

template <class Kernel>
typename PolyMeshT<Kernel>::Normal
PolyMeshT<Kernel>::
calc_vertex_normal(VertexHandle _vh) const
{
  Normal n;
  calc_vertex_normal_fast(_vh,n);

  Scalar norm = n.length();
  if (norm != 0.0) n *= (1.0/norm);

  return n;
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_fast(VertexHandle _vh, Normal& _n) const
{
  _n.vectorize(0.0);
  for (ConstVertexFaceIter vf_it=this->cvf_iter(_vh); vf_it; ++vf_it)
    _n += this->normal(vf_it.handle());
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_correct(VertexHandle _vh, Normal& _n) const
{
  _n.vectorize(0.0);
  ConstVertexIHalfedgeIter cvih_it = cvih_iter(_vh);
  if (!cvih_it)
  {//don't crash on isolated vertices
    return;
  }
  Normal in_he_vec;
  calc_edge_vector(cvih_it, in_he_vec);
  for ( ; cvih_it; ++cvih_it)
  {//calculates the sector normal defined by cvih_it and adds it to _n
    if (is_boundary(cvih_it))
    {
      continue;
    }
    HalfedgeHandle out_heh(next_halfedge_handle(cvih_it));
    Normal out_he_vec;
    calc_edge_vector(out_heh, out_he_vec);
    _n += cross(in_he_vec, out_he_vec);//sector area is taken into account
    in_he_vec = out_he_vec;
    in_he_vec *= -1;//change the orientation
  }
}

//-----------------------------------------------------------------------------
template <class Kernel>
void PolyMeshT<Kernel>::
calc_vertex_normal_loop(VertexHandle _vh, Normal& _n) const
{
  static const LoopSchemeMaskDouble& loop_scheme_mask__ =
                  LoopSchemeMaskDoubleSingleton::Instance();

  Normal t_v(0.0,0.0,0.0), t_w(0.0,0.0,0.0);
  unsigned int vh_val = valence(_vh);
  unsigned int i = 0;
  for (ConstVertexOHalfedgeIter cvoh_it = cvoh_iter(_vh); cvoh_it; ++cvoh_it, ++i)
  {
    VertexHandle r1_v(to_vertex_handle(cvoh_it));
    t_v += (typename vector_traits<Point>::value_type)(loop_scheme_mask__.tang0_weight(vh_val, i))*this->point(r1_v);
    t_w += (typename vector_traits<Point>::value_type)(loop_scheme_mask__.tang1_weight(vh_val, i))*this->point(r1_v);
  }
  _n = cross(t_w, t_v);//hack: should be cross(t_v, t_w), but then the normals are reversed?
}

//-----------------------------------------------------------------------------

template <class Kernel>
void
PolyMeshT<Kernel>::
update_vertex_normals()
{
  VertexIter  v_it(Kernel::vertices_begin()), v_end(Kernel::vertices_end());

  for (; v_it!=v_end; ++v_it)
    this->set_normal(v_it.handle(), calc_vertex_normal(v_it.handle()));
}

//=============================================================================
} // namespace OpenMesh
//=============================================================================
<|endoftext|>


<commit_before>#pragma once

#include <agency/detail/config.hpp>
#include <thrust/system_error.h>
#include <thrust/system/cuda/error.h>
#include <cuda_runtime.h>

namespace agency
{
namespace cuda
{
namespace detail
{

template<class T>
class managed_allocator
{
  public:
    using value_type = T;

    value_type* allocate(size_t n)
    {
      value_type* result = nullptr;

      cudaError_t error = cudaMallocManaged(&result, n * sizeof(T), cudaMemAttachGlobal);

      if(error != cudaSuccess)
      {
        throw thrust::system_error(error, thrust::cuda_category(), "managed_allocator::allocate(): cudaMallocManaged");
      }

      return result;
    }

    void deallocate(value_type* ptr, size_t)
    {
      cudaError_t error = cudaFree(ptr);

      if(error != cudaSuccess)
      {
        throw thrust::system_error(error, thrust::cuda_category(), "managed_allocator::deallocate(): cudaFree");
      }
    }

    template<class U, class... Args>
    void construct(U* ptr, Args&&... args)
    {
      cudaError_t error = cudaDeviceSynchronize();

      if(error != cudaSuccess)
      {
        throw thrust::system_error(error, thrust::cuda_category(), "managed_allocator::allocate(): cudaDeviceSynchronize");
      }

      new(ptr) T(std::forward<Args>(args)...);
    }
};

} // end detail
} // end cuda
} // end agency

<commit_msg>Associate a device with managed_allocator Synchronize the entire system within managed_allocator::construct()<commit_after>#pragma once

#include <agency/detail/config.hpp>
#include <agency/cuda/device.hpp>
#include <thrust/system_error.h>
#include <thrust/system/cuda/error.h>
#include <cuda_runtime.h>

namespace agency
{
namespace cuda
{
namespace detail
{

template<class T>
class managed_allocator
{
  private:
    device_id device_;

    const device_id& device() const
    {
      return device_;
    }

  public:
    using value_type = T;

    managed_allocator()
      : device_()
    {
      auto devices = all_devices();

      if(!devices.empty())
      {
        device_ = devices[0];
      }
    }

    value_type* allocate(size_t n)
    {
      // switch to our device
      scoped_current_device set_current_device(device());

      value_type* result = nullptr;

      cudaError_t error = cudaMallocManaged(&result, n * sizeof(T), cudaMemAttachGlobal);

      if(error != cudaSuccess)
      {
        throw thrust::system_error(error, thrust::cuda_category(), "managed_allocator::allocate(): cudaMallocManaged");
      }

      return result;
    }

    void deallocate(value_type* ptr, size_t)
    {
      // switch to our device
      scoped_current_device set_current_device(device());

      cudaError_t error = cudaFree(ptr);

      if(error != cudaSuccess)
      {
        throw thrust::system_error(error, thrust::cuda_category(), "managed_allocator::deallocate(): cudaFree");
      }
    }

    template<class U, class... Args>
    void construct(U* ptr, Args&&... args)
    {
      // we need to synchronize with all devices before touching the ptr
      detail::wait(all_devices());

      new(ptr) T(std::forward<Args>(args)...);
    }
};

} // end detail
} // end cuda
} // end agency

<|endoftext|>


<commit_before><commit_msg>[PartDesign] Remove 'Set tip' from Body contextual menu ; fixes #4304<commit_after><|endoftext|>