<commit_before>/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** rtspvideocapturer.cpp
**
** -------------------------------------------------------------------------*/

#ifdef HAVE_LIVE555

#include <chrono>

#include "rtc_base/timeutils.h"
#include "rtc_base/logging.h"

#include "modules/video_coding/h264_sprop_parameter_sets.h"
#include "api/video/i420_buffer.h"

#include "libyuv/video_common.h"
#include "libyuv/convert.h"

#include "rtspvideocapturer.h"

uint8_t marker[] = { 0, 0, 0, 1};

int decodeTimeoutOption(const std::map<std::string,std::string> & opts) {
	int timeout = 10;
	if (opts.find("timeout") != opts.end())
	{
		std::string timeoutString = opts.at("timeout");
		timeout = std::stoi(timeoutString);
	}
	return timeout;
}

int decodeRTPTransport(const std::map<std::string,std::string> & opts)
{
	int rtptransport = RTSPConnection::RTPUDPUNICAST;
	if (opts.find("rtptransport") != opts.end())
	{
		std::string rtpTransportString = opts.at("rtptransport");
		if (rtpTransportString == "tcp") {
			rtptransport = RTSPConnection::RTPOVERTCP;
		} else if (rtpTransportString == "http") {
			rtptransport = RTSPConnection::RTPOVERHTTP;
		} else if (rtpTransportString == "multicast") {
			rtptransport = RTSPConnection::RTPUDPMULTICAST;
		}
	}
	return rtptransport;
}

RTSPVideoCapturer::RTSPVideoCapturer(const std::string & uri, const std::map<std::string,std::string> & opts)
	: m_connection(m_env, this, uri.c_str(), decodeTimeoutOption(opts), decodeRTPTransport(opts), rtc::LogMessage::GetLogToDebug()<=3)
{
	RTC_LOG(INFO) << "RTSPVideoCapturer" << uri ;
	m_h264 = h264_new();
}

RTSPVideoCapturer::~RTSPVideoCapturer()
{
	h264_free(m_h264);
}

bool RTSPVideoCapturer::onNewSession(const char* id,const char* media, const char* codec, const char* sdp)
{
	bool success = false;
	if (strcmp(media, "video") == 0) {
		RTC_LOG(INFO) << "RTSPVideoCapturer::onNewSession " << media << "/" << codec << " " << sdp;

		if (strcmp(codec, "H264") == 0)
		{
			m_codec = codec;
			const char* pattern="sprop-parameter-sets=";
			const char* sprop=strstr(sdp, pattern);
			if (sprop)
			{
				std::string sdpstr(sprop+strlen(pattern));
				size_t pos = sdpstr.find_first_of(" ;\r\n");
				if (pos != std::string::npos)
				{
					sdpstr.erase(pos);
				}
				webrtc::H264SpropParameterSets sprops;
				if (sprops.DecodeSprop(sdpstr))
				{
					struct timeval presentationTime;
					timerclear(&presentationTime);

					std::vector<uint8_t> sps;
					sps.insert(sps.end(), marker, marker+sizeof(marker));
					sps.insert(sps.end(), sprops.sps_nalu().begin(), sprops.sps_nalu().end());
					onData(id, sps.data(), sps.size(), presentationTime);

					std::vector<uint8_t> pps;
					pps.insert(pps.end(), marker, marker+sizeof(marker));
					pps.insert(pps.end(), sprops.pps_nalu().begin(), sprops.pps_nalu().end());
					onData(id, pps.data(), pps.size(), presentationTime);
				}
				else
				{
					RTC_LOG(WARNING) << "Cannot decode SPS:" << sprop;
				}
			}
			success = true;
		}
		else if (strcmp(codec, "JPEG") == 0)
		{
			m_codec = codec;
			success = true;
		}
	}
	return success;
}

bool RTSPVideoCapturer::onData(const char* id, unsigned char* buffer, ssize_t size, struct timeval presentationTime)
{
	int64_t ts = presentationTime.tv_sec;
	ts = ts*1000 + presentationTime.tv_usec/1000;
	RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData size:" << size << " ts:" << ts;
	int res = 0;

	if (m_codec == "H264") {
		int nal_start = 0;
		int nal_end   = 0;
		find_nal_unit(buffer, size, &nal_start, &nal_end);
		read_nal_unit(m_h264, &buffer[nal_start], nal_end - nal_start);
		if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_SPS) {
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SPS";
			m_cfg.clear();
			m_cfg.insert(m_cfg.end(), buffer, buffer+size);

			unsigned int width = ((m_h264->sps->pic_width_in_mbs_minus1 +1)*16) - m_h264->sps->frame_crop_left_offset*2 - m_h264->sps->frame_crop_right_offset*2;
			unsigned int height= ((2 - m_h264->sps->frame_mbs_only_flag)* (m_h264->sps->pic_height_in_map_units_minus1 +1) * 16) - (m_h264->sps->frame_crop_top_offset * 2) - (m_h264->sps->frame_crop_bottom_offset * 2);
			unsigned int fps=25;
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SPS set timing_info_present_flag:" << m_h264->sps->vui.timing_info_present_flag << " " << m_h264->sps->vui.time_scale << " " << m_h264->sps->vui.num_units_in_tick;
			if (m_decoder.get()) {
				if ( (GetCaptureFormat()->width != width) || (GetCaptureFormat()->height != height) )  {
					RTC_LOG(INFO) << "format changed => set format from " << GetCaptureFormat()->width << "x" << GetCaptureFormat()->height	 << " to " << width << "x" << height;
					m_decoder.reset(NULL);
				}
			}

			if (!m_decoder.get()) {
				RTC_LOG(INFO) << "RTSPVideoCapturer:onData SPS set format " << width << "x" << height << " fps:" << fps;
				cricket::VideoFormat videoFormat(width, height, cricket::VideoFormat::FpsToInterval(fps), cricket::FOURCC_I420);
				SetCaptureFormat(&videoFormat);

				m_decoder=m_factory.CreateVideoDecoder(webrtc::SdpVideoFormat(cricket::kH264CodecName));
				webrtc::VideoCodec codec_settings;
				codec_settings.codecType = webrtc::VideoCodecType::kVideoCodecH264;
				m_decoder->InitDecode(&codec_settings,2);
				m_decoder->RegisterDecodeCompleteCallback(this);
			}
		}
		else if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_PPS) {
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData PPS";
			m_cfg.insert(m_cfg.end(), buffer, buffer+size);
		}
		else if (m_decoder.get()) {
			std::vector<uint8_t> content;
			if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_CODED_SLICE_IDR) {
				RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData IDR";
				content.insert(content.end(), m_cfg.begin(), m_cfg.end());
			}
			else {
				RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SLICE NALU:" << m_h264->nal->nal_unit_type;
			}
			content.insert(content.end(), buffer, buffer+size);
			Frame frame(std::move(content), ts);
			{
				std::unique_lock<std::mutex> lock(m_queuemutex);
				m_queue.push(frame);
			}
			m_queuecond.notify_all();
		} else {
			RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData no decoder";
			res = -1;
		}
	} else if (m_codec == "JPEG") {
		int32_t width = 0;
		int32_t height = 0;
		if (libyuv::MJPGSize(buffer, size, &width, &height) == 0) {
			int stride_y = width;
			int stride_uv = (width + 1) / 2;

			rtc::scoped_refptr<webrtc::I420Buffer> I420buffer = webrtc::I420Buffer::Create(width, height, stride_y, stride_uv, stride_uv);
			const int conversionResult = libyuv::ConvertToI420((const uint8*)buffer, size,
							(uint8*)I420buffer->DataY(), I420buffer->StrideY(),
							(uint8*)I420buffer->DataU(), I420buffer->StrideU(),
							(uint8*)I420buffer->DataV(), I420buffer->StrideV(),
							0, 0,
							width, height,
							width, height,
							libyuv::kRotate0, ::libyuv::FOURCC_MJPG);

			if (conversionResult >= 0) {
				webrtc::VideoFrame frame(I420buffer, 0, ts, webrtc::kVideoRotation_0);
				this->Decoded(frame);
			} else {
				RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData decoder error:" << conversionResult;
				res = -1;
			}
		} else {
			RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData cannot JPEG dimension";
			res = -1;
		}

	}

	return (res == 0);
}

void RTSPVideoCapturer::DecoderThread()
{
	while (IsRunning()) {
		std::unique_lock<std::mutex> mlock(m_queuemutex);
		while (m_queue.empty())
		{
			m_queuecond.wait(mlock);
		}
		Frame frame = m_queue.front();
		m_queue.pop();
		mlock.unlock();

		RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:DecoderThread size:" << frame.m_content.size() << " ts:" << frame.m_timestamp_ms;
		uint8_t* data = frame.m_content.data();
		ssize_t size = frame.m_content.size();

		if (size) {
			size_t allocsize = size + webrtc::EncodedImage::GetBufferPaddingBytes(webrtc::VideoCodecType::kVideoCodecH264);
			uint8_t buf[allocsize];
			memcpy( buf, data, size );

			webrtc::EncodedImage input_image(buf, size, allocsize);
			input_image._timeStamp = frame.m_timestamp_ms; // store time in ms that overflow the 32bits
			m_decoder->Decode(input_image, false, NULL);
		}
	}
}

int32_t RTSPVideoCapturer::Decoded(webrtc::VideoFrame& decodedImage)
{
	int64_t ts = std::chrono::high_resolution_clock::now().time_since_epoch().count()/1000/1000;

	RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer::Decoded size:" << decodedImage.size()
				<< " decode ts:" << decodedImage.timestamp()
				<< " source ts:" << ts;

	// avoid to block the encoder that drop frames when sent too quickly
	static int64_t previmagets = 0;
	static int64_t prevts = 0;
	if (prevts != 0) {
		int64_t periodSource = decodedImage.timestamp() - previmagets;
		int64_t periodDecode = ts-prevts;

		RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer::Decoded interframe decode:" << periodDecode << " source:" << periodSource;
		int64_t delayms = periodSource-periodDecode;
		if ( (delayms > 0) && (delayms < 1000) ) {
			usleep(delayms*1000);
		}
	}

	this->OnFrame(decodedImage, decodedImage.height(), decodedImage.width());

	previmagets = decodedImage.timestamp();
	prevts = std::chrono::high_resolution_clock::now().time_since_epoch().count()/1000/1000;

	return true;
}

cricket::CaptureState RTSPVideoCapturer::Start(const cricket::VideoFormat& format)
{
	SetCaptureFormat(&format);
	SetCaptureState(cricket::CS_RUNNING);
	rtc::Thread::Start();
	m_decoderthread = std::thread(&RTSPVideoCapturer::DecoderThread, this);
	return cricket::CS_RUNNING;
}

void RTSPVideoCapturer::Stop()
{
	m_env.stop();
	rtc::Thread::Stop();
	SetCaptureFormat(NULL);
	SetCaptureState(cricket::CS_STOPPED);
	Frame frame;
	{
		std::unique_lock<std::mutex> lock(m_queuemutex);
		m_queue.push(frame);
	}
	m_queuecond.notify_all();
	m_decoderthread.join();
}

void RTSPVideoCapturer::Run()
{
	m_env.mainloop();
}

bool RTSPVideoCapturer::GetPreferredFourccs(std::vector<unsigned int>* fourccs)
{
	return true;
}
#endif
<commit_msg>fix #123: build with lastest WebRTC<commit_after>/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** rtspvideocapturer.cpp
**
** -------------------------------------------------------------------------*/

#ifdef HAVE_LIVE555

#include <chrono>

#include "rtc_base/timeutils.h"
#include "rtc_base/logging.h"

#include "modules/video_coding/h264_sprop_parameter_sets.h"
#include "api/video/i420_buffer.h"

#include "libyuv/video_common.h"
#include "libyuv/convert.h"

#include "rtspvideocapturer.h"

uint8_t marker[] = { 0, 0, 0, 1};

int decodeTimeoutOption(const std::map<std::string,std::string> & opts) {
	int timeout = 10;
	if (opts.find("timeout") != opts.end())
	{
		std::string timeoutString = opts.at("timeout");
		timeout = std::stoi(timeoutString);
	}
	return timeout;
}

int decodeRTPTransport(const std::map<std::string,std::string> & opts)
{
	int rtptransport = RTSPConnection::RTPUDPUNICAST;
	if (opts.find("rtptransport") != opts.end())
	{
		std::string rtpTransportString = opts.at("rtptransport");
		if (rtpTransportString == "tcp") {
			rtptransport = RTSPConnection::RTPOVERTCP;
		} else if (rtpTransportString == "http") {
			rtptransport = RTSPConnection::RTPOVERHTTP;
		} else if (rtpTransportString == "multicast") {
			rtptransport = RTSPConnection::RTPUDPMULTICAST;
		}
	}
	return rtptransport;
}

RTSPVideoCapturer::RTSPVideoCapturer(const std::string & uri, const std::map<std::string,std::string> & opts)
	: m_connection(m_env, this, uri.c_str(), decodeTimeoutOption(opts), decodeRTPTransport(opts), rtc::LogMessage::GetLogToDebug()<=3)
{
	RTC_LOG(INFO) << "RTSPVideoCapturer" << uri ;
	m_h264 = h264_new();
}

RTSPVideoCapturer::~RTSPVideoCapturer()
{
	h264_free(m_h264);
}

bool RTSPVideoCapturer::onNewSession(const char* id,const char* media, const char* codec, const char* sdp)
{
	bool success = false;
	if (strcmp(media, "video") == 0) {
		RTC_LOG(INFO) << "RTSPVideoCapturer::onNewSession " << media << "/" << codec << " " << sdp;

		if (strcmp(codec, "H264") == 0)
		{
			m_codec = codec;
			const char* pattern="sprop-parameter-sets=";
			const char* sprop=strstr(sdp, pattern);
			if (sprop)
			{
				std::string sdpstr(sprop+strlen(pattern));
				size_t pos = sdpstr.find_first_of(" ;\r\n");
				if (pos != std::string::npos)
				{
					sdpstr.erase(pos);
				}
				webrtc::H264SpropParameterSets sprops;
				if (sprops.DecodeSprop(sdpstr))
				{
					struct timeval presentationTime;
					timerclear(&presentationTime);

					std::vector<uint8_t> sps;
					sps.insert(sps.end(), marker, marker+sizeof(marker));
					sps.insert(sps.end(), sprops.sps_nalu().begin(), sprops.sps_nalu().end());
					onData(id, sps.data(), sps.size(), presentationTime);

					std::vector<uint8_t> pps;
					pps.insert(pps.end(), marker, marker+sizeof(marker));
					pps.insert(pps.end(), sprops.pps_nalu().begin(), sprops.pps_nalu().end());
					onData(id, pps.data(), pps.size(), presentationTime);
				}
				else
				{
					RTC_LOG(WARNING) << "Cannot decode SPS:" << sprop;
				}
			}
			success = true;
		}
		else if (strcmp(codec, "JPEG") == 0)
		{
			m_codec = codec;
			success = true;
		}
	}
	return success;
}

bool RTSPVideoCapturer::onData(const char* id, unsigned char* buffer, ssize_t size, struct timeval presentationTime)
{
	int64_t ts = presentationTime.tv_sec;
	ts = ts*1000 + presentationTime.tv_usec/1000;
	RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData size:" << size << " ts:" << ts;
	int res = 0;

	if (m_codec == "H264") {
		int nal_start = 0;
		int nal_end   = 0;
		find_nal_unit(buffer, size, &nal_start, &nal_end);
		read_nal_unit(m_h264, &buffer[nal_start], nal_end - nal_start);
		if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_SPS) {
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SPS";
			m_cfg.clear();
			m_cfg.insert(m_cfg.end(), buffer, buffer+size);

			unsigned int width = ((m_h264->sps->pic_width_in_mbs_minus1 +1)*16) - m_h264->sps->frame_crop_left_offset*2 - m_h264->sps->frame_crop_right_offset*2;
			unsigned int height= ((2 - m_h264->sps->frame_mbs_only_flag)* (m_h264->sps->pic_height_in_map_units_minus1 +1) * 16) - (m_h264->sps->frame_crop_top_offset * 2) - (m_h264->sps->frame_crop_bottom_offset * 2);
			unsigned int fps=25;
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SPS set timing_info_present_flag:" << m_h264->sps->vui.timing_info_present_flag << " " << m_h264->sps->vui.time_scale << " " << m_h264->sps->vui.num_units_in_tick;
			if (m_decoder.get()) {
				if ( (GetCaptureFormat()->width != width) || (GetCaptureFormat()->height != height) )  {
					RTC_LOG(INFO) << "format changed => set format from " << GetCaptureFormat()->width << "x" << GetCaptureFormat()->height	 << " to " << width << "x" << height;
					m_decoder.reset(NULL);
				}
			}

			if (!m_decoder.get()) {
				RTC_LOG(INFO) << "RTSPVideoCapturer:onData SPS set format " << width << "x" << height << " fps:" << fps;
				cricket::VideoFormat videoFormat(width, height, cricket::VideoFormat::FpsToInterval(fps), cricket::FOURCC_I420);
				SetCaptureFormat(&videoFormat);

				m_decoder=m_factory.CreateVideoDecoder(webrtc::SdpVideoFormat(cricket::kH264CodecName));
				webrtc::VideoCodec codec_settings;
				codec_settings.codecType = webrtc::VideoCodecType::kVideoCodecH264;
				m_decoder->InitDecode(&codec_settings,2);
				m_decoder->RegisterDecodeCompleteCallback(this);
			}
		}
		else if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_PPS) {
			RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData PPS";
			m_cfg.insert(m_cfg.end(), buffer, buffer+size);
		}
		else if (m_decoder.get()) {
			std::vector<uint8_t> content;
			if (m_h264->nal->nal_unit_type == NAL_UNIT_TYPE_CODED_SLICE_IDR) {
				RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData IDR";
				content.insert(content.end(), m_cfg.begin(), m_cfg.end());
			}
			else {
				RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:onData SLICE NALU:" << m_h264->nal->nal_unit_type;
			}
			content.insert(content.end(), buffer, buffer+size);
			Frame frame(std::move(content), ts);
			{
				std::unique_lock<std::mutex> lock(m_queuemutex);
				m_queue.push(frame);
			}
			m_queuecond.notify_all();
		} else {
			RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData no decoder";
			res = -1;
		}
	} else if (m_codec == "JPEG") {
		int32_t width = 0;
		int32_t height = 0;
		if (libyuv::MJPGSize(buffer, size, &width, &height) == 0) {
			int stride_y = width;
			int stride_uv = (width + 1) / 2;

			rtc::scoped_refptr<webrtc::I420Buffer> I420buffer = webrtc::I420Buffer::Create(width, height, stride_y, stride_uv, stride_uv);
			const int conversionResult = libyuv::ConvertToI420((const uint8*)buffer, size,
							(uint8*)I420buffer->DataY(), I420buffer->StrideY(),
							(uint8*)I420buffer->DataU(), I420buffer->StrideU(),
							(uint8*)I420buffer->DataV(), I420buffer->StrideV(),
							0, 0,
							width, height,
							width, height,
							libyuv::kRotate0, ::libyuv::FOURCC_MJPG);

			if (conversionResult >= 0) {
				webrtc::VideoFrame frame(I420buffer, 0, ts, webrtc::kVideoRotation_0);
				this->Decoded(frame);
			} else {
				RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData decoder error:" << conversionResult;
				res = -1;
			}
		} else {
			RTC_LOG(LS_ERROR) << "RTSPVideoCapturer:onData cannot JPEG dimension";
			res = -1;
		}

	}

	return (res == 0);
}

void RTSPVideoCapturer::DecoderThread()
{
	while (IsRunning()) {
		std::unique_lock<std::mutex> mlock(m_queuemutex);
		while (m_queue.empty())
		{
			m_queuecond.wait(mlock);
		}
		Frame frame = m_queue.front();
		m_queue.pop();
		mlock.unlock();

		RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer:DecoderThread size:" << frame.m_content.size() << " ts:" << frame.m_timestamp_ms;
		uint8_t* data = frame.m_content.data();
		ssize_t size = frame.m_content.size();

		if (size) {
			size_t allocsize = size + webrtc::EncodedImage::GetBufferPaddingBytes(webrtc::VideoCodecType::kVideoCodecH264);
			uint8_t buf[allocsize];
			memcpy( buf, data, size );

			webrtc::EncodedImage input_image(buf, size, allocsize);
			input_image._timeStamp = frame.m_timestamp_ms; // store time in ms that overflow the 32bits
			m_decoder->Decode(input_image, false, NULL,0);
		}
	}
}

int32_t RTSPVideoCapturer::Decoded(webrtc::VideoFrame& decodedImage)
{
	int64_t ts = std::chrono::high_resolution_clock::now().time_since_epoch().count()/1000/1000;

	RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer::Decoded size:" << decodedImage.size()
				<< " decode ts:" << decodedImage.timestamp()
				<< " source ts:" << ts;

	// avoid to block the encoder that drop frames when sent too quickly
	static int64_t previmagets = 0;
	static int64_t prevts = 0;
	if (prevts != 0) {
		int64_t periodSource = decodedImage.timestamp() - previmagets;
		int64_t periodDecode = ts-prevts;

		RTC_LOG(LS_VERBOSE) << "RTSPVideoCapturer::Decoded interframe decode:" << periodDecode << " source:" << periodSource;
		int64_t delayms = periodSource-periodDecode;
		if ( (delayms > 0) && (delayms < 1000) ) {
			usleep(delayms*1000);
		}
	}

	this->OnFrame(decodedImage, decodedImage.height(), decodedImage.width());

	previmagets = decodedImage.timestamp();
	prevts = std::chrono::high_resolution_clock::now().time_since_epoch().count()/1000/1000;

	return true;
}

cricket::CaptureState RTSPVideoCapturer::Start(const cricket::VideoFormat& format)
{
	SetCaptureFormat(&format);
	SetCaptureState(cricket::CS_RUNNING);
	rtc::Thread::Start();
	m_decoderthread = std::thread(&RTSPVideoCapturer::DecoderThread, this);
	return cricket::CS_RUNNING;
}

void RTSPVideoCapturer::Stop()
{
	m_env.stop();
	rtc::Thread::Stop();
	SetCaptureFormat(NULL);
	SetCaptureState(cricket::CS_STOPPED);
	Frame frame;
	{
		std::unique_lock<std::mutex> lock(m_queuemutex);
		m_queue.push(frame);
	}
	m_queuecond.notify_all();
	m_decoderthread.join();
}

void RTSPVideoCapturer::Run()
{
	m_env.mainloop();
}

bool RTSPVideoCapturer::GetPreferredFourccs(std::vector<unsigned int>* fourccs)
{
	return true;
}
#endif
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2012-2015 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 irlock.cpp
 * @author Michael Landes
 *
 * Driver for an IR-Lock and Pixy vision sensor connected via I2C.
 *
 * Created on: Nov 12, 2014
 **/

#include <string.h>

#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>

#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>

#include <uORB/Publication.hpp>
#include <uORB/topics/irlock_report.h>

/** Configuration Constants **/
#define IRLOCK_I2C_ADDRESS		0x54 /** 7-bit address (non shifted) **/
#define IRLOCK_CONVERSION_INTERVAL_US	20000U /** us = 20ms = 50Hz **/

#define IRLOCK_SYNC			0xAA55
#define IRLOCK_RESYNC		0x5500
#define IRLOCK_ADJUST		0xAA

#define IRLOCK_RES_X 320
#define IRLOCK_RES_Y 200

#define IRLOCK_CENTER_X				(IRLOCK_RES_X/2)			// the x-axis center pixel position
#define IRLOCK_CENTER_Y				(IRLOCK_RES_Y/2)			// the y-axis center pixel position

#define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f)
#define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f)

#define IRLOCK_TAN_HALF_FOV_X 0.57735026919f // tan(0.5 * 60 * pi/180)
#define IRLOCK_TAN_HALF_FOV_Y 0.31529878887f // tan(0.5 * 35 * pi/180)

#define IRLOCK_TAN_ANG_PER_PIXEL_X	(2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X)
#define IRLOCK_TAN_ANG_PER_PIXEL_Y	(2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y)

#define IRLOCK_OBJECTS_MAX	5	/** up to 5 objects can be detected/reported **/

struct irlock_target_s {
	uint16_t signature;	/** target signature **/
	float pos_x;	/** x-axis distance from center of image to center of target in units of tan(theta) **/
	float pos_y;	/** y-axis distance from center of image to center of target in units of tan(theta) **/
	float size_x;	/** size of target along x-axis in units of tan(theta) **/
	float size_y;	/** size of target along y-axis in units of tan(theta) **/
};

/** irlock_s structure returned from read calls **/
struct irlock_s {
	hrt_abstime timestamp; /** microseconds since system start **/
	uint8_t num_targets;
	struct irlock_target_s targets[IRLOCK_OBJECTS_MAX];
};

class IRLOCK : public device::I2C, public I2CSPIDriver<IRLOCK>
{
public:
	IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
	virtual ~IRLOCK();

	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
					     int runtime_instance);
	static void print_usage();

	void custom_method(const BusCLIArguments &cli) override;

	int init() override;
	int probe() override;
	void print_status() override;
	int test();

	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);

	/** read from device and schedule next read **/
	void		RunImpl();
private:

	/** start periodic reads from sensor **/
	void 		start();

	/** low level communication with sensor **/
	int 		read_device();
	bool 		sync_device();
	int 		read_device_word(uint16_t *word);
	int 		read_device_block(struct irlock_target_s *block);

	/** internal variables **/
	ringbuffer::RingBuffer *_reports;
	bool _sensor_ok;
	uint32_t _read_failures;

	int _orb_class_instance;
	uORB::Publication<irlock_report_s> _irlock_report_topic{ORB_ID(irlock_report)};
};

extern "C" __EXPORT int irlock_main(int argc, char *argv[]);

IRLOCK::IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
	I2C(DRV_SENS_DEVTYPE_IRLOCK, MODULE_NAME, bus, address, bus_frequency),
	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address),
	_reports(nullptr),
	_sensor_ok(false),
	_read_failures(0),
	_orb_class_instance(-1)
{
}

IRLOCK::~IRLOCK()
{
	if (_reports != nullptr) {
		delete _reports;
	}
}

/** initialise driver to communicate with sensor **/
int IRLOCK::init()
{
	/** initialise I2C bus **/
	int ret = I2C::init();

	if (ret != OK) {
		return ret;
	}

	/** allocate buffer storing values read from sensor **/
	_reports = new ringbuffer::RingBuffer(2, sizeof(struct irlock_s));

	if (_reports == nullptr) {
		return ENOTTY;

	} else {
		_sensor_ok = true;
		/** start work queue **/
		start();
		return OK;
	}
}

/** probe the device is on the I2C bus **/
int IRLOCK::probe()
{
	/*
	 * IRLock defaults to sending 0x00 when there is no block
	 * data to return, so really all we can do is check to make
	 * sure a transfer completes successfully.
	 **/
	uint8_t byte;

	if (transfer(nullptr, 0, &byte, 1) != OK) {
		return -EIO;
	}

	return OK;
}

void IRLOCK::print_status()
{
	/** display reports in queue **/
	if (_sensor_ok) {
		_reports->print_info("report queue: ");
		PX4_INFO("read errors:%lu", (unsigned long)_read_failures);

	} else {
		PX4_WARN("sensor is not healthy");
	}
}

/** test driver **/
int IRLOCK::test()
{
	/** exit immediately if sensor is not healty **/
	if (!_sensor_ok) {
		PX4_ERR("sensor is not healthy");
		return -1;
	}

	/** instructions to user **/
	PX4_INFO("searching for object for 10 seconds");

	/** read from sensor for 10 seconds **/
	struct irlock_s report;
	uint64_t start_time = hrt_absolute_time();

	while ((hrt_absolute_time() - start_time) < 10000000) {
		if (_reports->get(&report)) {
			/** output all objects found **/
			for (uint8_t i = 0; i < report.num_targets; i++) {
				PX4_INFO("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f",
					 (int)report.targets[i].signature,
					 (double)report.targets[i].pos_x,
					 (double)report.targets[i].pos_y,
					 (double)report.targets[i].size_x,
					 (double)report.targets[i].size_y);
			}
		}

		/** sleep for 0.05 seconds **/
		usleep(50000);
	}

	return OK;
}

/** start periodic reads from sensor **/
void IRLOCK::start()
{
	/** flush ring and reset state machine **/
	_reports->flush();

	/** start work queue cycle **/
	ScheduleNow();
}

void IRLOCK::RunImpl()
{
	/** ignoring failure, if we do, we will be back again right away... **/
	read_device();

	/** schedule the next cycle **/
	ScheduleDelayed(IRLOCK_CONVERSION_INTERVAL_US);
}

ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen)
{
	unsigned count = buflen / sizeof(struct irlock_s);
	struct irlock_s *rbuf = reinterpret_cast<struct irlock_s *>(buffer);
	int ret = 0;

	if (count < 1) {
		return -ENOSPC;
	}

	/** try to read **/
	while (count--) {
		if (_reports->get(rbuf)) {
			ret += sizeof(*rbuf);
			++rbuf;
		}
	}

	return ret ? ret : -EAGAIN;

	return ret;
}

/** sync device to ensure reading starts at new frame*/
bool IRLOCK::sync_device()
{
	uint8_t sync_byte;
	uint16_t sync_word;

	if (read_device_word(&sync_word) != OK) {
		return false;
	}

	if (sync_word == IRLOCK_RESYNC) {
		transfer(nullptr, 0, &sync_byte, 1);

		if (sync_byte == IRLOCK_ADJUST) {
			return true;
		}

	} else if (sync_word == IRLOCK_SYNC) {
		return true;
	}

	return false;
}

/** read all available frames from sensor **/
int IRLOCK::read_device()
{
	/** if we sync, then we are starting a new frame, else fail **/
	if (!sync_device()) {
		return -ENOTTY;
	}

	struct irlock_s report;

	report.timestamp = hrt_absolute_time();

	report.num_targets = 0;

	while (report.num_targets < IRLOCK_OBJECTS_MAX) {
		if (!sync_device() || read_device_block(&report.targets[report.num_targets]) != OK) {
			break;
		}

		report.num_targets++;
	}

	_reports->force(&report);

	// publish over uORB
	if (report.num_targets > 0) {
		irlock_report_s orb_report{};
		orb_report.timestamp = report.timestamp;
		orb_report.signature = report.targets[0].signature;
		orb_report.pos_x     = report.targets[0].pos_x;
		orb_report.pos_y     = report.targets[0].pos_y;
		orb_report.size_x    = report.targets[0].size_x;
		orb_report.size_y    = report.targets[0].size_y;

		_irlock_report_topic.publish(orb_report);
	}

	return OK;
}

/** read a word (two bytes) from sensor **/
int IRLOCK::read_device_word(uint16_t *word)
{
	uint8_t bytes[2];
	memset(bytes, 0, sizeof bytes);

	int status = transfer(nullptr, 0, &bytes[0], 2);
	*word = bytes[1] << 8 | bytes[0];

	return status;
}

/** read a single block (a full frame) from sensor **/
int IRLOCK::read_device_block(struct irlock_target_s *block)
{
	uint8_t bytes[12];
	memset(bytes, 0, sizeof bytes);

	int status = transfer(nullptr, 0, &bytes[0], 12);
	uint16_t checksum = bytes[1] << 8 | bytes[0];
	uint16_t signature = bytes[3] << 8 | bytes[2];
	uint16_t pixel_x = bytes[5] << 8 | bytes[4];
	uint16_t pixel_y = bytes[7] << 8 | bytes[6];
	uint16_t pixel_size_x = bytes[9] << 8 | bytes[8];
	uint16_t pixel_size_y = bytes[11] << 8 | bytes[10];

	/** crc check **/
	if (signature + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) {
		_read_failures++;
		return -EIO;
	}

	/** convert to angles **/
	block->signature = signature;
	block->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X;
	block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y;
	block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X;
	block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y;
	return status;
}

void
IRLOCK::print_usage()
{
	PRINT_MODULE_USAGE_NAME("irlock", "driver");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x54);
	PRINT_MODULE_USAGE_COMMAND("test");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

I2CSPIDriverBase *IRLOCK::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
				      int runtime_instance)
{
	IRLOCK *instance = new IRLOCK(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);

	if (instance == nullptr) {
		PX4_ERR("alloc failed");
		return nullptr;
	}

	if (instance->init() != PX4_OK) {
		delete instance;
		return nullptr;
	}

	return instance;
}
void
IRLOCK::custom_method(const BusCLIArguments &cli)
{
	test();
}

extern "C" __EXPORT int irlock_main(int argc, char *argv[])
{
	using ThisDriver = IRLOCK;
	BusCLIArguments cli{true, false};
	cli.i2c_address = IRLOCK_I2C_ADDRESS;
	cli.default_i2c_frequency = 400000;

	const char *verb = cli.parseDefaultArguments(argc, argv);

	if (!verb) {
		ThisDriver::print_usage();
		return -1;
	}

	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_SENS_DEVTYPE_IRLOCK);

	if (!strcmp(verb, "start")) {
		return ThisDriver::module_start(cli, iterator);
	}

	if (!strcmp(verb, "stop")) {
		return ThisDriver::module_stop(iterator);
	}

	if (!strcmp(verb, "status")) {
		return ThisDriver::module_status(iterator);
	}

	if (!strcmp(verb, "test")) {
		return ThisDriver::module_custom_method(cli, iterator, false);
	}

	ThisDriver::print_usage();
	return -1;
}
<commit_msg>irlock: remove ioctl(), read(), and RingBuffer<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2012-2015 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 irlock.cpp
 * @author Michael Landes
 *
 * Driver for an IR-Lock and Pixy vision sensor connected via I2C.
 *
 * Created on: Nov 12, 2014
 **/

#include <string.h>

#include <drivers/device/i2c.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/irlock_report.h>

/** Configuration Constants **/
#define IRLOCK_I2C_ADDRESS		0x54 /** 7-bit address (non shifted) **/
#define IRLOCK_CONVERSION_INTERVAL_US	20000U /** us = 20ms = 50Hz **/

#define IRLOCK_SYNC			0xAA55
#define IRLOCK_RESYNC		0x5500
#define IRLOCK_ADJUST		0xAA

#define IRLOCK_RES_X 320
#define IRLOCK_RES_Y 200

#define IRLOCK_CENTER_X				(IRLOCK_RES_X/2)			// the x-axis center pixel position
#define IRLOCK_CENTER_Y				(IRLOCK_RES_Y/2)			// the y-axis center pixel position

#define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f)
#define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f)

#define IRLOCK_TAN_HALF_FOV_X 0.57735026919f // tan(0.5 * 60 * pi/180)
#define IRLOCK_TAN_HALF_FOV_Y 0.31529878887f // tan(0.5 * 35 * pi/180)

#define IRLOCK_TAN_ANG_PER_PIXEL_X	(2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X)
#define IRLOCK_TAN_ANG_PER_PIXEL_Y	(2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y)

#define IRLOCK_OBJECTS_MAX	5	/** up to 5 objects can be detected/reported **/

struct irlock_target_s {
	uint16_t signature;	/** target signature **/
	float pos_x;	/** x-axis distance from center of image to center of target in units of tan(theta) **/
	float pos_y;	/** y-axis distance from center of image to center of target in units of tan(theta) **/
	float size_x;	/** size of target along x-axis in units of tan(theta) **/
	float size_y;	/** size of target along y-axis in units of tan(theta) **/
};

/** irlock_s structure returned from read calls **/
struct irlock_s {
	hrt_abstime timestamp; /** microseconds since system start **/
	uint8_t num_targets;
	irlock_target_s targets[IRLOCK_OBJECTS_MAX];
};

class IRLOCK : public device::I2C, public I2CSPIDriver<IRLOCK>
{
public:
	IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
	~IRLOCK() override = default;

	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
					     int runtime_instance);
	static void print_usage();

	int init() override;
	int probe() override;
	void print_status() override;

	/** read from device and schedule next read **/
	void		RunImpl();
private:

	/** low level communication with sensor **/
	int 		read_device();
	bool 		sync_device();
	int 		read_device_word(uint16_t *word);
	int 		read_device_block(struct irlock_target_s *block);

	/** internal variables **/
	uint32_t _read_failures{0};

	uORB::Publication<irlock_report_s> _irlock_report_topic{ORB_ID(irlock_report)};
};

IRLOCK::IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
	I2C(DRV_SENS_DEVTYPE_IRLOCK, MODULE_NAME, bus, address, bus_frequency),
	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
{
}

int IRLOCK::init()
{
	/** initialise I2C bus **/
	int ret = I2C::init();

	if (ret != OK) {
		return ret;
	}

	ScheduleNow();
	return OK;
}

int IRLOCK::probe()
{
	/*
	 * IRLock defaults to sending 0x00 when there is no block
	 * data to return, so really all we can do is check to make
	 * sure a transfer completes successfully.
	 **/
	uint8_t byte;

	if (transfer(nullptr, 0, &byte, 1) != OK) {
		return -EIO;
	}

	return OK;
}

void IRLOCK::print_status()
{
	PX4_INFO("read errors: %lu", (unsigned long)_read_failures);
}

void IRLOCK::RunImpl()
{
	/** ignoring failure, if we do, we will be back again right away... **/
	read_device();

	ScheduleDelayed(IRLOCK_CONVERSION_INTERVAL_US);
}

bool IRLOCK::sync_device()
{
	uint8_t sync_byte;
	uint16_t sync_word;

	if (read_device_word(&sync_word) != OK) {
		return false;
	}

	if (sync_word == IRLOCK_RESYNC) {
		transfer(nullptr, 0, &sync_byte, 1);

		if (sync_byte == IRLOCK_ADJUST) {
			return true;
		}

	} else if (sync_word == IRLOCK_SYNC) {
		return true;
	}

	return false;
}

int IRLOCK::read_device()
{
	/** if we sync, then we are starting a new frame, else fail **/
	if (!sync_device()) {
		return -ENOTTY;
	}

	irlock_s report{};
	report.timestamp = hrt_absolute_time();
	report.num_targets = 0;

	while (report.num_targets < IRLOCK_OBJECTS_MAX) {
		if (!sync_device() || read_device_block(&report.targets[report.num_targets]) != OK) {
			break;
		}

		report.num_targets++;
	}

	// publish over uORB
	if (report.num_targets > 0) {
		irlock_report_s orb_report{};
		orb_report.timestamp = report.timestamp;
		orb_report.signature = report.targets[0].signature;
		orb_report.pos_x     = report.targets[0].pos_x;
		orb_report.pos_y     = report.targets[0].pos_y;
		orb_report.size_x    = report.targets[0].size_x;
		orb_report.size_y    = report.targets[0].size_y;

		_irlock_report_topic.publish(orb_report);
	}

	return OK;
}

int IRLOCK::read_device_word(uint16_t *word)
{
	uint8_t bytes[2] {};

	int status = transfer(nullptr, 0, &bytes[0], 2);
	*word = bytes[1] << 8 | bytes[0];

	return status;
}

int IRLOCK::read_device_block(irlock_target_s *block)
{
	uint8_t bytes[12];
	memset(bytes, 0, sizeof bytes);

	int status = transfer(nullptr, 0, &bytes[0], 12);
	uint16_t checksum = bytes[1] << 8 | bytes[0];
	uint16_t signature = bytes[3] << 8 | bytes[2];
	uint16_t pixel_x = bytes[5] << 8 | bytes[4];
	uint16_t pixel_y = bytes[7] << 8 | bytes[6];
	uint16_t pixel_size_x = bytes[9] << 8 | bytes[8];
	uint16_t pixel_size_y = bytes[11] << 8 | bytes[10];

	/** crc check **/
	if (signature + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) {
		_read_failures++;
		return -EIO;
	}

	/** convert to angles **/
	block->signature = signature;
	block->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X;
	block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y;
	block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X;
	block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y;
	return status;
}

void IRLOCK::print_usage()
{
	PRINT_MODULE_USAGE_NAME("irlock", "driver");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x54);
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

I2CSPIDriverBase *IRLOCK::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
				      int runtime_instance)
{
	IRLOCK *instance = new IRLOCK(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);

	if (instance == nullptr) {
		PX4_ERR("alloc failed");
		return nullptr;
	}

	if (instance->init() != PX4_OK) {
		delete instance;
		return nullptr;
	}

	return instance;
}

extern "C" __EXPORT int irlock_main(int argc, char *argv[])
{
	using ThisDriver = IRLOCK;
	BusCLIArguments cli{true, false};
	cli.i2c_address = IRLOCK_I2C_ADDRESS;
	cli.default_i2c_frequency = 400000;

	const char *verb = cli.parseDefaultArguments(argc, argv);

	if (!verb) {
		ThisDriver::print_usage();
		return -1;
	}

	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_SENS_DEVTYPE_IRLOCK);

	if (!strcmp(verb, "start")) {
		return ThisDriver::module_start(cli, iterator);
	}

	if (!strcmp(verb, "stop")) {
		return ThisDriver::module_stop(iterator);
	}

	if (!strcmp(verb, "status")) {
		return ThisDriver::module_status(iterator);
	}

	ThisDriver::print_usage();
	return -1;
}
<|endoftext|>


<commit_before>#include "controlwindow.h"
#include "ui_controlwindow.h"

#include <QCoreApplication>
#include <QSound>
#include <iostream>

#define MapSize 1
#define VisionRange 2
#define NHexonInit 3
#define NObstacleInit 4

// Constructor for ControlWindow
// Generates interactive elements, and attaches signals
ControlWindow::ControlWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::ControlWindow)
{
    ui->setupUi(this);

    QPushButton * startButton = new QPushButton("Start",this);
    startButton->setGeometry(QRect(QPoint(100,100),QSize(50,25)));

    connect(startButton, SIGNAL(clicked()), this, SLOT(on_startButton_clicked()));
    //connect(&timer, SIGNAL(timeout()), &scene, SLOT(tick_update()));

    inputs[MapSize] = new InputWidget(this,100,150,10,999,20,"Map Radius");
    inputs[VisionRange] = new InputWidget(this,100,200,0,9,2,"Vision Range");
    inputs[NHexonInit] = new InputWidget(this,200,150,0,999,50,"Starting Hexons");
    inputs[NObstacleInit] = new InputWidget(this,200,200,0,999,100,"Starting Obstacles");
}

// Destructor
// Stops timer, and deletes UI before exiting
ControlWindow::~ControlWindow()
{
    timer.stop();
    delete ui;
}

// Slot for when start button is pressed
// Sets up view, hex_map, and timer
void ControlWindow::on_startButton_clicked()
{
    // Set Global variables - #from control window values
    set_map_size(inputs[MapSize]->value()); // radius of map
    set_visibility(inputs[VisionRange]->value()); // visibility range of Hexons
    int size = get_map_size();

    // Scaled size of scene
    scene.setSceneRect(-50*size, -50*size, 100*size, 100*size);

    scene.setItemIndexMethod(QGraphicsScene::NoIndex);

    srand(0);
    // Generate random Hexons
    for (int i = 0; i < inputs[NHexonInit]->value(); i++) // #from control window value
    {
        bool s = false;
        while (!s)
        {
            // generate random position in hexagon #make new function?
            int a = (rand() % (2*size+1)) - size;
            int b = (rand() % (2*size-std::abs(a)+1)) - size;
            if (a < 0)
            {
                b = b - a;
            }
            cube h = {a,b,-a-b};
            // add hexon to map
            s = hex_map.AddHexItem(new Hexon(h), &scene);
        }
    }

    for (int i = 0; i < inputs[NObstacleInit]->value(); i++) // #from control window value
    {
        bool s = false;
        while (!s)
        {
            // generate random position in hexagon #make new function?
            int a = (rand() % (2*size+1)) - size;
            int b = (rand() % (2*size-std::abs(a)+1)) - size;
            if (a < 0)
            {
                b = b - a;
            }
            cube h = {a,b,-a-b};
            // add obstacle to map
            s = hex_map.AddHexItem(new Obstacle(h), &scene);
        }
    }

    // attach scene to view, and adjust settings
    view.setScene(&scene);
    view.setRenderHint(QPainter::Antialiasing);
    view.setBackgroundBrush(Qt::black);

    view.setCacheMode(QGraphicsView::CacheBackground);
    view.setViewportUpdateMode(QGraphicsView::BoundingRectViewportUpdate); // FullViewportUpdate
    view.setDragMode(QGraphicsView::ScrollHandDrag);

    view.setWindowTitle(QT_TRANSLATE_NOOP(QGraphicsView, "Hexons"));
    view.resize(800, 800);

    // display view (new window)
    view.showMaximized();

    // play music
    QSound music("song_of_storms5.wav");
    //music.play("song_of_storms5.wav");
    music.play("lost_woods.wav");

    // connect timer to update function
    connect(&timer, SIGNAL(timeout()), this, SLOT(tick_update()));
    // start timer with msec milliseconds
    timer.start(1000/2);
}

// Slot for timer tick
// runs map update
void ControlWindow::tick_update()
{
    hex_map.map_update();
    return;
}
<commit_msg>Fixed music file paths<commit_after>#include "controlwindow.h"
#include "ui_controlwindow.h"

#include <QCoreApplication>
#include <QSound>
#include <iostream>

#define MapSize 1
#define VisionRange 2
#define NHexonInit 3
#define NObstacleInit 4

// Constructor for ControlWindow
// Generates interactive elements, and attaches signals
ControlWindow::ControlWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::ControlWindow)
{
    ui->setupUi(this);

    QPushButton * startButton = new QPushButton("Start",this);
    startButton->setGeometry(QRect(QPoint(100,100),QSize(50,25)));

    connect(startButton, SIGNAL(clicked()), this, SLOT(on_startButton_clicked()));
    //connect(&timer, SIGNAL(timeout()), &scene, SLOT(tick_update()));

    inputs[MapSize] = new InputWidget(this,100,150,10,999,20,"Map Radius");
    inputs[VisionRange] = new InputWidget(this,100,200,0,9,2,"Vision Range");
    inputs[NHexonInit] = new InputWidget(this,200,150,0,999,50,"Starting Hexons");
    inputs[NObstacleInit] = new InputWidget(this,200,200,0,999,100,"Starting Obstacles");
}

// Destructor
// Stops timer, and deletes UI before exiting
ControlWindow::~ControlWindow()
{
    timer.stop();
    delete ui;
}

// Slot for when start button is pressed
// Sets up view, hex_map, and timer
void ControlWindow::on_startButton_clicked()
{
    // Set Global variables - #from control window values
    set_map_size(inputs[MapSize]->value()); // radius of map
    set_visibility(inputs[VisionRange]->value()); // visibility range of Hexons
    int size = get_map_size();

    // Scaled size of scene
    scene.setSceneRect(-50*size, -50*size, 100*size, 100*size);

    scene.setItemIndexMethod(QGraphicsScene::NoIndex);

    srand(0);
    // Generate random Hexons
    for (int i = 0; i < inputs[NHexonInit]->value(); i++) // #from control window value
    {
        bool s = false;
        while (!s)
        {
            // generate random position in hexagon #make new function?
            int a = (rand() % (2*size+1)) - size;
            int b = (rand() % (2*size-std::abs(a)+1)) - size;
            if (a < 0)
            {
                b = b - a;
            }
            cube h = {a,b,-a-b};
            // add hexon to map
            s = hex_map.AddHexItem(new Hexon(h), &scene);
        }
    }

    for (int i = 0; i < inputs[NObstacleInit]->value(); i++) // #from control window value
    {
        bool s = false;
        while (!s)
        {
            // generate random position in hexagon #make new function?
            int a = (rand() % (2*size+1)) - size;
            int b = (rand() % (2*size-std::abs(a)+1)) - size;
            if (a < 0)
            {
                b = b - a;
            }
            cube h = {a,b,-a-b};
            // add obstacle to map
            s = hex_map.AddHexItem(new Obstacle(h), &scene);
        }
    }

    // attach scene to view, and adjust settings
    view.setScene(&scene);
    view.setRenderHint(QPainter::Antialiasing);
    view.setBackgroundBrush(Qt::black);

    view.setCacheMode(QGraphicsView::CacheBackground);
    view.setViewportUpdateMode(QGraphicsView::BoundingRectViewportUpdate); // FullViewportUpdate
    view.setDragMode(QGraphicsView::ScrollHandDrag);

    view.setWindowTitle(QT_TRANSLATE_NOOP(QGraphicsView, "Hexons"));
    view.resize(800, 800);

    // display view (new window)
    view.showMaximized();

    // play music
    QSound music("../Hexons/song_of_storms5.wav");
    //music.play("../Hexons/song_of_storms5.wav");
    music.play("../Hexons/lost_woods.wav");

    // connect timer to update function
    connect(&timer, SIGNAL(timeout()), this, SLOT(tick_update()));
    // start timer with msec milliseconds
    timer.start(1000/2);
}

// Slot for timer tick
// runs map update
void ControlWindow::tick_update()
{
    hex_map.map_update();
    return;
}
<|endoftext|>


<commit_before>// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_TEST_HMM_DISCRETIZATION_HH
#define DUNE_GDT_TEST_HMM_DISCRETIZATION_HH

#if !HAVE_DUNE_PDELAB
# error "This one requires dune-pdelab!"
#endif

#include <memory>
#include <vector>
#include <limits>
#include <complex>
#include <type_traits>

#include <dune/common/timer.hh>

#include <dune/stuff/common/memory.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/grid/information.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/functions/combined.hh>
#include <dune/stuff/functions/constant.hh>

#include <dune/gdt/spaces/nedelec/pdelab.hh>
#include <dune/gdt/localevaluation/hmm.hh>
#include <dune/gdt/localoperator/codim0.hh>
#include <dune/gdt/assembler/local/codim0.hh>
#include <dune/gdt/assembler/system.hh>
#include <dune/gdt/functionals/l2.hh>
#include <dune/gdt/spaces/constraints.hh>

template< class MacroGridViewType, class CellGridViewType, int polynomialOrder >
class HMMDiscretization
{
public:
  typedef typename MacroGridViewType::ctype MacroDomainFieldType;
  static const size_t                       dimDomain = MacroGridViewType::dimension;
  static const size_t                       dimRange = dimDomain;
  static const unsigned int                 polOrder = polynomialOrder;

  typedef Dune::Stuff::Grid::BoundaryInfoInterface< typename MacroGridViewType::Intersection >                                                               BoundaryInfoType;
  typedef Dune::Stuff::LocalizableFunctionInterface< typename MacroGridViewType::template Codim< 0 >::Entity, DomainFieldType, dimDomain, double, dimRange > Vectorfct;
  typedef Dune::Stuff::LocalizableFunctionInterface< typename MacroGridViewType::template Codim< 0 >::Entity, DomainFieldType, dimDomain, double, 1 >        ScalarFct;

  typedef Dune::Stuff::LA::Container< double, Dune::Stuff::LA::ChooseBackend::eigen_sparse>::MatrixType                 MatrixType;
  typedef Dune::Stuff::LA::Container< double, Dune::Stuff::LA::ChooseBackend::eigen_sparse>::VectorType                 VectorType;

  typedef Dune::GDT::Spaces::Nedelec::PdelabBased< MacroGridViewType, polOrder, double, dimRange > SpaceType;

  typedef Dune::GDT::ConstDiscreteFunction < SpaceType, VectorType > ConstDiscreteFunctionType;

  HMMDiscretization(const MacroGridViewType& macrogridview,
                    const CellGridViewType& cellgridview,
                    const BoundaryInfoType& info,
                    const ScalarFct& mu,
                    const ScalarFct& kappa,
                    const Vectorfct& source,
                    const ScalarFct& divparam)
    : space_(macrogridview)
    , cellgridview_(cellgridview)
    , bdry_info_(info)
    , mu_(mu)
    , kappa_(kappa)
    , source_(source)
    , divparam_(divparam)
    , is_assembled_(false)
    , system_matrix_(0,0)
    , rhs_vector_(0)
  {}

  const SpaceType& space() const
  {
    return space_;
  }

  VectorType create_vector() const
  {
    return VectorType(space_.mapper().size());
  }

  void assemble() const
  {
    using namespace Dune;
    using namespace Dune::GDT;
    if (!is_assembled_) {
      Stuff::LA::SparsityPatternDefault sparsity_pattern = space_.compute_volume_pattern();
      system_matrix_ = MatrixType(space_.mapper().size(), space_.mapper().size(), sparsity_pattern);
      rhs_vector_ = VectorType(space_.mapper().size());
      SystemAssembler< SpaceType > walker(space_);

      //rhs
      auto source_functional = Functionals::make_l2_volume(source_, rhs_vector_, space_);
      walker.add(*source_functional);

      //lhs
      typedef LocalOperator::Codim0Integral< LocalEvaluation::HMMCurlcurl< ScalarFct, CellGridViewType, polOrder > > HMMcurlOp;
      const HMMcurlOp hmmcurl(mu_, divparam_, cellgridview_);
      const LocalAssembler::Codim0Matrix< HMMcurlOp > curlassembler(hmmcurl);
      walker.add(curlassembler, system_matrix_);
      typedef LocalOperator::Codim0Integral< LocalEvaluation::HMMIdentity< ScalarFct, CellGridViewType, polOrder > > HMMidOp;
      const HMMidOp hmmid(kappa_, cellgridview_);
      const LocalAssembler::Codim0Matrix< HMMidOp > idassembler(hmmid);
      walker.add(idassembler, system_matrix_);

      //apply the homogenous (!) Dirichlet constraints on the macro grid
      Spaces::DirichletConstraints< typename MacroGridViewType::Intersection >
              dirichlet_constraints(bdry_info_, space_.mapper().size());
      walker.add(dirichlet_constraints);
      walker.assemble();
      dirichlet_constraints.apply(system_matrix_, rhs_vector_);

      is_assembled_ = true;
    }
  } //assemble

  bool assembled() const
  {
    return is_assembled_;
  }

  const MatrixType& system_matrix() const
  {
    return system_matrix_;
  }

  const VectorType& rhs_vector() const
  {
    return rhs_vector_;
  }

  void solve(VectorType& solution) const
  {
    if (!is_assembled_)
      assemble();

    Dune::Stuff::LA::Solver< MatrixType > solver(system_matrix_);
    solver.apply(rhs_vector_, solution, "bicgstab.diagonal");
  } //solve

private:
  const SpaceType& space_;
  const CellGridViewType& cellgridview_;
  const BoundaryInfoType& bdry_info_;
  const ScalarFct& mu_;
  const ScalarFct& kappa_;
  const Vectorfct& source_;
  const ScalarFct& divparam_;
  mutable bool is_assembled_;
  mutable MatrixType system_matrix_;
  mutable VectorType rhs_vector_;
};

#endif // DUNE_GDT_TEST_HMM_DISCRETIZATION_HH
<commit_msg>[test/hmm-discretization] minor updates, fix typos<commit_after>// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_TEST_HMM_DISCRETIZATION_HH
#define DUNE_GDT_TEST_HMM_DISCRETIZATION_HH

#if !HAVE_DUNE_PDELAB
# error "This one requires dune-pdelab!"
#endif

#include <memory>
#include <vector>
#include <limits>
#include <complex>
#include <type_traits>

#include <dune/common/timer.hh>

#include <dune/stuff/common/memory.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/grid/information.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/functions/combined.hh>
#include <dune/stuff/functions/constant.hh>

#include <dune/gdt/spaces/nedelec/pdelab.hh>
#include <dune/gdt/localevaluation/hmm.hh>
#include <dune/gdt/localoperator/codim0.hh>
#include <dune/gdt/assembler/local/codim0.hh>
#include <dune/gdt/assembler/system.hh>
#include <dune/gdt/functionals/l2.hh>
#include <dune/gdt/spaces/constraints.hh>

template< class MacroGridViewType, class CellGridViewType, int polynomialOrder >
class HMMDiscretization
{
public:
  typedef typename MacroGridViewType::ctype MacroDomainFieldType;
  static const size_t                       dimDomain = MacroGridViewType::dimension;
  static const size_t                       dimRange = dimDomain;
  static const unsigned int                 polOrder = polynomialOrder;

  typedef Dune::Stuff::Grid::BoundaryInfoInterface< typename MacroGridViewType::Intersection >                                                               BoundaryInfoType;
  typedef Dune::Stuff::LocalizableFunctionInterface< typename MacroGridViewType::template Codim< 0 >::Entity, MacroDomainFieldType, dimDomain, double, dimRange > Vectorfct;
  typedef Dune::Stuff::LocalizableFunctionInterface< typename MacroGridViewType::template Codim< 0 >::Entity, MacroDomainFieldType, dimDomain, double, 1 >        ScalarFct;

  typedef Dune::Stuff::LA::Container< double, Dune::Stuff::LA::ChooseBackend::eigen_sparse>::MatrixType                 MatrixType;
  typedef Dune::Stuff::LA::Container< double, Dune::Stuff::LA::ChooseBackend::eigen_sparse>::VectorType                 VectorType;

  typedef Dune::GDT::Spaces::Nedelec::PdelabBased< MacroGridViewType, polOrder, double, dimRange > SpaceType;

  typedef Dune::GDT::ConstDiscreteFunction < SpaceType, VectorType > ConstDiscreteFunctionType;

  HMMDiscretization(const MacroGridViewType& macrogridview,
                    const CellGridViewType& cellgridview,
                    const BoundaryInfoType& info,
                    const ScalarFct& mu,
                    const ScalarFct& kappa,
                    const Vectorfct& source,
                    const ScalarFct& divparam)
    : space_(macrogridview)
    , cellgridview_(cellgridview)
    , bdry_info_(info)
    , mu_(mu)
    , kappa_(kappa)
    , source_(source)
    , divparam_(divparam)
    , is_assembled_(false)
    , system_matrix_(0,0)
    , rhs_vector_(0)
  {}

  const SpaceType& space() const
  {
    return space_;
  }

  VectorType create_vector() const
  {
    return VectorType(space_.mapper().size());
  }

  void assemble() const
  {
    using namespace Dune;
    using namespace Dune::GDT;
    if (!is_assembled_) {
      Stuff::LA::SparsityPatternDefault sparsity_pattern = space_.compute_volume_pattern();
      system_matrix_ = MatrixType(space_.mapper().size(), space_.mapper().size(), sparsity_pattern);
      rhs_vector_ = VectorType(space_.mapper().size());
      SystemAssembler< SpaceType > walker(space_);

      //rhs
      auto source_functional = Functionals::make_l2_volume(source_, rhs_vector_, space_);
      walker.add(*source_functional);

      //lhs
      typedef LocalOperator::Codim0Integral< LocalEvaluation::HMMCurlcurl< ScalarFct, CellGridViewType, polOrder > > HMMcurlOp;
      const HMMcurlOp hmmcurl(mu_, divparam_, cellgridview_);
      const LocalAssembler::Codim0Matrix< HMMcurlOp > curlassembler(hmmcurl);
      walker.add(curlassembler, system_matrix_);
      typedef LocalOperator::Codim0Integral< LocalEvaluation::HMMIdentity< ScalarFct, CellGridViewType, polOrder > > HMMidOp;
      const HMMidOp hmmid(kappa_, cellgridview_);
      const LocalAssembler::Codim0Matrix< HMMidOp > idassembler(hmmid);
      walker.add(idassembler, system_matrix_);

      //apply the homogenous (!) Dirichlet constraints on the macro grid
      Spaces::DirichletConstraints< typename MacroGridViewType::Intersection >
              dirichlet_constraints(bdry_info_, space_.mapper().size());
      walker.add(dirichlet_constraints);
      walker.assemble();
      dirichlet_constraints.apply(system_matrix_, rhs_vector_);

      is_assembled_ = true;
    }
  } //assemble

  bool assembled() const
  {
    return is_assembled_;
  }

  const MatrixType& system_matrix() const
  {
    return system_matrix_;
  }

  const VectorType& rhs_vector() const
  {
    return rhs_vector_;
  }

  void solve(VectorType& solution) const
  {
    if (!is_assembled_)
      assemble();

    Dune::Stuff::LA::Solver< MatrixType > solver(system_matrix_);
    solver.apply(rhs_vector_, solution, "bicgstab.diagonal");
  } //solve

private:
  const SpaceType space_;
  const CellGridViewType& cellgridview_;
  const BoundaryInfoType& bdry_info_;
  const ScalarFct& mu_;
  const ScalarFct& kappa_;
  const Vectorfct& source_;
  const ScalarFct& divparam_;
  mutable bool is_assembled_;
  mutable MatrixType system_matrix_;
  mutable VectorType rhs_vector_;
};

#endif // DUNE_GDT_TEST_HMM_DISCRETIZATION_HH
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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 <cmath>
#include <iostream>

#include <QPainter>
#include <QPen>

#include "arrow.h"
#include "compositeitem.h"

constexpr double PI = 3.14159265358979323846264338327950288419716939937510;
constexpr double ArrowSize = 10;

Arrow::Arrow(CompositeItem *startItem, CompositeItem *endItem,
             QGraphicsItem *parent)
    : QGraphicsLineItem(parent),
      start_item_(startItem),
      end_item_(endItem),
      polygon_() {
  setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
}

QRectF Arrow::boundingRect() const {
  qreal extra = (pen().width() + 20) / 2.0;

  return QRectF(line().p1(), QSizeF(line().p2().x() - line().p1().x(),
                                    line().p2().y() - line().p1().y()))
      .normalized()
      .adjusted(-extra, -extra, extra, extra);
}

QPainterPath Arrow::shape() const {
  QPainterPath path = QGraphicsLineItem::shape();
  path.addPolygon(polygon_);
  return path;
}

void Arrow::updatePosition() {
  QLineF line(mapFromItem(start_item_, 0, 0), mapFromItem(end_item_, 0, 0));
  setLine(line);
}

QPointF Arrow::Intersect(const QPolygonF &polygon, const QPointF &point,
                         const QLineF &line) {
  QPointF p1 = polygon.first() + point;
  QPointF p2;
  QPointF intersectPoint(0, 0);
  QLineF polyLine;
  for (int i = 1; i < polygon.count(); ++i) {
    p2 = polygon.at(i) + point;
    polyLine = QLineF(p1, p2);
    QLineF::IntersectType intersectType =
        polyLine.intersect(line, &intersectPoint);
    if (intersectType == QLineF::BoundedIntersection) break;
    p1 = p2;
  }
  return intersectPoint;
}

void Arrow::paint(QPainter *painter, const QStyleOptionGraphicsItem *,
                  QWidget *) {
  if (start_item_->collidesWithItem(end_item_)) {
    return;
  }

  painter->setPen(pen());
  painter->setBrush(pen().color());

  QPointF startPoint =
      start_item_->pos() + QPointF(start_item_->boundingRect().width() / 2,
                                   start_item_->boundingRect().height() / 2);
  QPointF endPoint =
      end_item_->pos() + QPointF(end_item_->boundingRect().width() / 2,
                                 end_item_->boundingRect().height() / 2);

  QLineF centerLine(startPoint, endPoint);

  endPoint = Intersect(end_item_->polygon(), end_item_->pos(), centerLine);
  startPoint =
      Intersect(start_item_->polygon(), start_item_->pos(), centerLine);

  setLine(QLineF(endPoint, startPoint));

  double angle = std::acos(line().dx() / line().length());
  if (line().dy() >= 0) angle = (PI * 2) - angle;

  QPointF arrowP1 = line().p1() + QPointF(std::sin(angle + PI / 3) * ArrowSize,
                                          std::cos(angle + PI / 3) * ArrowSize);
  QPointF arrowP2 =
      line().p1() + QPointF(std::sin(angle + PI - PI / 3) * ArrowSize,
                            std::cos(angle + PI - PI / 3) * ArrowSize);

  polygon_.clear();
  polygon_ << line().p1() << arrowP1 << arrowP2;

  painter->drawLine(line());
  painter->drawPolygon(polygon_);
}
<commit_msg>Framework: fix issue<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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 <cmath>
#include <iostream>

#include <QPainter>
#include <QPen>

#include "arrow.h"
#include "composite_item.h"

constexpr double PI = 3.14159265358979323846264338327950288419716939937510;
constexpr double ArrowSize = 10;

Arrow::Arrow(CompositeItem *startItem, CompositeItem *endItem,
             QGraphicsItem *parent)
    : QGraphicsLineItem(parent),
      start_item_(startItem),
      end_item_(endItem),
      polygon_() {
  setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
}

QRectF Arrow::boundingRect() const {
  qreal extra = (pen().width() + 20) / 2.0;

  return QRectF(line().p1(), QSizeF(line().p2().x() - line().p1().x(),
                                    line().p2().y() - line().p1().y()))
      .normalized()
      .adjusted(-extra, -extra, extra, extra);
}

QPainterPath Arrow::shape() const {
  QPainterPath path = QGraphicsLineItem::shape();
  path.addPolygon(polygon_);
  return path;
}

void Arrow::updatePosition() {
  QLineF line(mapFromItem(start_item_, 0, 0), mapFromItem(end_item_, 0, 0));
  setLine(line);
}

QPointF Arrow::Intersect(const QPolygonF &polygon, const QPointF &point,
                         const QLineF &line) {
  QPointF p1 = polygon.first() + point;
  QPointF p2;
  QPointF intersectPoint(0, 0);
  QLineF polyLine;
  for (int i = 1; i < polygon.count(); ++i) {
    p2 = polygon.at(i) + point;
    polyLine = QLineF(p1, p2);
    QLineF::IntersectType intersectType =
        polyLine.intersect(line, &intersectPoint);
    if (intersectType == QLineF::BoundedIntersection) break;
    p1 = p2;
  }
  return intersectPoint;
}

void Arrow::paint(QPainter *painter, const QStyleOptionGraphicsItem *,
                  QWidget *) {
  if (start_item_->collidesWithItem(end_item_)) {
    return;
  }

  painter->setPen(pen());
  painter->setBrush(pen().color());

  QPointF startPoint =
      start_item_->pos() + QPointF(start_item_->boundingRect().width() / 2,
                                   start_item_->boundingRect().height() / 2);
  QPointF endPoint =
      end_item_->pos() + QPointF(end_item_->boundingRect().width() / 2,
                                 end_item_->boundingRect().height() / 2);

  QLineF centerLine(startPoint, endPoint);

  endPoint = Intersect(end_item_->polygon(), end_item_->pos(), centerLine);
  startPoint =
      Intersect(start_item_->polygon(), start_item_->pos(), centerLine);

  setLine(QLineF(endPoint, startPoint));

  double angle = std::acos(line().dx() / line().length());
  if (line().dy() >= 0) angle = (PI * 2) - angle;

  QPointF arrowP1 = line().p1() + QPointF(std::sin(angle + PI / 3) * ArrowSize,
                                          std::cos(angle + PI / 3) * ArrowSize);
  QPointF arrowP2 =
      line().p1() + QPointF(std::sin(angle + PI - PI / 3) * ArrowSize,
                            std::cos(angle + PI - PI / 3) * ArrowSize);

  polygon_.clear();
  polygon_ << line().p1() << arrowP1 << arrowP2;

  painter->drawLine(line());
  painter->drawPolygon(polygon_);
}
<|endoftext|>


<commit_before>using namespace std;
#include "models/c.h"
char testip[] = "float x;\nstatic int z;\nchar text[64] = \"hello\"; static int bob(char x, char *harry) { stuff  { inside } }";

/* Perform rangecheck on getChild before checking the type. Uses -1 as a sentinel
   for the type of an out of range child to simplify calling contexts.
*/
int getChildType(pANTLR3_BASE_TREE parent, int idx)
{
  int count = parent->getChildCount(parent);
  if( idx<0 || idx>=count )
    return -1;
  pANTLR3_BASE_TREE child = (pANTLR3_BASE_TREE)parent->getChild(parent,idx);
  return child->getType(child);
}

class TranslationU
{
public:
  list<Decl*> globals;
  list<FuncDef*> functions;
  TranslationU(pANTLR3_BASE_TREE root);
  void processTopLevel(pANTLR3_BASE_TREE node);
  void dump();
};

TranslationU::TranslationU(pANTLR3_BASE_TREE root)
{
  if( root->getType(root)==0 )
  {
    TokList tops = extractChildren(root, 0, -1);
    tmplForeach(list, pANTLR3_BASE_TREE, tok, tops)
      processTopLevel(tok);
    tmplEnd
  }
  else
    processTopLevel(root);
}

void TranslationU::processTopLevel(pANTLR3_BASE_TREE node)
{
int type  = node->getType(node);
int count = node->getChildCount(node);
  switch(type)
  {
    case PREPRO:
    //case HASHINCLUDE:
    //case HASHDEFINE:
    //case HASHUNDEF:
      break;
    case DECL:
      try {
        Decl::parse(node,globals);
      }
      catch(BrokenTree bt) {
        printf("ERROR(%u): %s\n", bt.blame->getLine(bt.blame), bt.explain);
        dumpTree(bt.blame,1);
      }
      break;
    case FUNC:
      try
      {
        FuncDef *f = new FuncDef;
        f->parse(node);
        functions.push_back(f);
      }
      catch(BrokenTree bt) {
        printf("ERROR(%u): %s\n", bt.blame->getLine(bt.blame), bt.explain);
        dumpTree(bt.blame,1);
      }
      break;
    case SEMI:
      break;
    default:
      printf("Unknown Type %u Children %u ", type, count);
      dumpTree(node,1);
      break;
  }
}

void TranslationU::dump()
{
  tmplForeach(list,Decl*,decl,globals)
    printf("Declation: %s is %s\n", decl->identifier, decl->type.str().c_str());
  tmplEnd
  tmplForeach(list,FuncDef*,f,functions)
    printf("Function: %s is ", f->identifier);
    printf("%s <- ", f->retType.str().c_str());
    tmplForeach(list,Decl*,p,f->args)
      printf("%s ", p->type.str().c_str());
      printf("%s  ", p->identifier);
    tmplEnd
    printf("\n");
  tmplEnd
}

int main(int argc, char **argv)
{
pANTLR3_INPUT_STREAM ip;
pANTLR3_COMMON_TOKEN_STREAM tokens;
pcInCLexer lex;
pcInCParser parser;
cInCParser_translationUnit_return retVal;

  if( argc==1 )
    ip = antlr3NewAsciiStringInPlaceStream((uint8_t*)testip, strlen(testip), NULL);
  else
    ip = antlr3AsciiFileStreamNew((pANTLR3_UINT8)argv[1]);
  // Todo: error checking for IO errors !
  lex = cInCLexerNew(ip);
  tokens = antlr3CommonTokenStreamSourceNew(ANTLR3_SIZE_HINT, TOKENSOURCE(lex));
  parser = cInCParserNew(tokens);
  retVal = parser->translationUnit(parser);

TranslationU model = TranslationU(retVal.tree);
  model.dump();
  dumpTree(retVal.tree,0);

}
<commit_msg>Missing seek interface.<commit_after>using namespace std;
#include "models/c.h"
char testip[] = "float x;\nstatic int z;\nchar text[64] = \"hello\"; static int bob(char x, char *harry) { stuff  { inside } }";

/* Perform rangecheck on getChild before checking the type. Uses -1 as a sentinel
   for the type of an out of range child to simplify calling contexts.
*/
int getChildType(pANTLR3_BASE_TREE parent, int idx)
{
  int count = parent->getChildCount(parent);
  if( idx<0 || idx>=count )
    return -1;
  pANTLR3_BASE_TREE child = (pANTLR3_BASE_TREE)parent->getChild(parent,idx);
  return child->getType(child);
}

class TranslationU
{
public:
  list<Decl*> globals;
  list<FuncDef*> functions;
  TranslationU(pANTLR3_BASE_TREE root);
  void processTopLevel(pANTLR3_BASE_TREE node);
  void dump();
};

TranslationU::TranslationU(pANTLR3_BASE_TREE root)
{
  if( root->getType(root)==0 )
  {
    TokList tops = extractChildren(root, 0, -1);
    tmplForeach(list, pANTLR3_BASE_TREE, tok, tops)
      processTopLevel(tok);
    tmplEnd
  }
  else
    processTopLevel(root);
}

void TranslationU::processTopLevel(pANTLR3_BASE_TREE node)
{
int type  = node->getType(node);
int count = node->getChildCount(node);
  switch(type)
  {
    case PREPRO:
    //case HASHINCLUDE:
    //case HASHDEFINE:
    //case HASHUNDEF:
      break;
    case DECL:
      try {
        Decl::parse(node,globals);
      }
      catch(BrokenTree bt) {
        printf("ERROR(%u): %s\n", bt.blame->getLine(bt.blame), bt.explain);
        dumpTree(bt.blame,1);
      }
      break;
    case FUNC:
      try
      {
        FuncDef *f = new FuncDef;
        f->parse(node);
        functions.push_back(f);
      }
      catch(BrokenTree bt) {
        printf("ERROR(%u): %s\n", bt.blame->getLine(bt.blame), bt.explain);
        dumpTree(bt.blame,1);
      }
      break;
    case SEMI:
      break;
    default:
      printf("Unknown Type %u Children %u ", type, count);
      dumpTree(node,1);
      break;
  }
}

void TranslationU::dump()
{
  tmplForeach(list,Decl*,decl,globals)
    printf("Declation: %s is %s\n", decl->identifier, decl->type.str().c_str());
  tmplEnd
  tmplForeach(list,FuncDef*,f,functions)
    printf("Function: %s is ", f->identifier);
    printf("%s <- ", f->retType.str().c_str());
    tmplForeach(list,Decl*,p,f->args)
      printf("%s ", p->type.str().c_str());
      printf("%s  ", p->identifier);
    tmplEnd
    printf("\n");
  tmplEnd
}

int main(int argc, char **argv)
{
pANTLR3_INPUT_STREAM ip;
pANTLR3_COMMON_TOKEN_STREAM tokens;
pcInCLexer lex;
pcInCParser parser;
cInCParser_translationUnit_return retVal;
cInCParser_declaration_return retVal2;

  if( argc==1 )
    ip = antlr3NewAsciiStringInPlaceStream((uint8_t*)testip, strlen(testip), NULL);
  else
    ip = antlr3AsciiFileStreamNew((pANTLR3_UINT8)argv[1]);
  // Todo: error checking for IO errors !
  lex = cInCLexerNew(ip);
  tokens = antlr3CommonTokenStreamSourceNew(ANTLR3_SIZE_HINT, TOKENSOURCE(lex));
  printf("%u\n",tokens->p);
  parser = cInCParserNew(tokens);
  retVal = parser->translationUnit(parser);
  tokens->p = 0;
  retVal2 = parser->declaration(parser);
  dumpTree(retVal2.tree,0);

TranslationU model = TranslationU(retVal.tree);
  model.dump();
  dumpTree(retVal.tree,0);

}
<|endoftext|>


<commit_before>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2016 Intel Corporation. All Rights Reserved.

#include <mutex>
#include <chrono>
#include <vector>
#include <iterator>
#include <cstddef>

#include "device.h"
#include "context.h"
#include "image.h"
#include "metadata-parser.h"

#include "ds5-nonmonochrome.h"
#include "ds5-private.h"
#include "ds5-options.h"
#include "ds5-timestamp.h"

namespace librealsense
{
    ds5_nonmonochrome::ds5_nonmonochrome(std::shared_ptr<context> ctx,
                                         const platform::backend_device_group& group)
        : device(ctx, group), ds5_device(ctx, group)
    {
        using namespace ds;

        auto pid = group.uvc_devices.front().pid;
        if ((_fw_version >= firmware_version("5.5.8.0")) && (pid != RS_USB2_PID))
        {
            get_depth_sensor().register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE,
                std::make_shared<uvc_xu_option<uint8_t>>(get_depth_sensor(),
                                                         depth_xu,
                                                         DS5_ENABLE_AUTO_WHITE_BALANCE,
                                                         "Enable Auto White Balance"));

            // RS400 rolling-shutter Skus allow to get low-quality color image from the same viewport as the depth
            get_depth_sensor().register_pixel_format(pf_uyvyl);
            get_depth_sensor().register_pixel_format(pf_rgb888);
            get_depth_sensor().register_pixel_format(pf_w10);
        }

        get_depth_sensor().unregister_option(RS2_OPTION_EMITTER_ON_OFF);

        if ((_fw_version >= firmware_version("5.9.13.6") &&
             _fw_version < firmware_version("5.9.15.1")))
        {
            get_depth_sensor().register_option(RS2_OPTION_INTER_CAM_SYNC_MODE,
                std::make_shared<external_sync_mode>(*_hw_monitor));
        }
    }
}
<commit_msg>Disable Auto WB depth control for D465<commit_after>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2016 Intel Corporation. All Rights Reserved.

#include <mutex>
#include <chrono>
#include <vector>
#include <iterator>
#include <cstddef>

#include "device.h"
#include "context.h"
#include "image.h"
#include "metadata-parser.h"

#include "ds5-nonmonochrome.h"
#include "ds5-private.h"
#include "ds5-options.h"
#include "ds5-timestamp.h"

namespace librealsense
{
    ds5_nonmonochrome::ds5_nonmonochrome(std::shared_ptr<context> ctx,
                                         const platform::backend_device_group& group)
        : device(ctx, group), ds5_device(ctx, group)
    {
        using namespace ds;

        auto pid = group.uvc_devices.front().pid;
        if ((_fw_version >= firmware_version("5.5.8.0")) && (!val_in_range(pid, { RS_USB2_PID, RS465_PID })))
        {
            get_depth_sensor().register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE,
                std::make_shared<uvc_xu_option<uint8_t>>(get_depth_sensor(),
                                                         depth_xu,
                                                         DS5_ENABLE_AUTO_WHITE_BALANCE,
                                                         "Enable Auto White Balance"));

            // RS400 rolling-shutter SKUs allow to get low-quality color image from the same viewport as the depth
            get_depth_sensor().register_pixel_format(pf_uyvyl);
            get_depth_sensor().register_pixel_format(pf_rgb888);
            get_depth_sensor().register_pixel_format(pf_w10);
        }

        get_depth_sensor().unregister_option(RS2_OPTION_EMITTER_ON_OFF);

        if ((_fw_version >= firmware_version("5.9.13.6") &&
             _fw_version < firmware_version("5.9.15.1")))
        {
            get_depth_sensor().register_option(RS2_OPTION_INTER_CAM_SYNC_MODE,
                std::make_shared<external_sync_mode>(*_hw_monitor));
        }
    }
}
<|endoftext|>