<commit_before>#ifndef IMAGE_MODEL_HPP
#define IMAGE_MODEL_HPP
#include <boost/thread.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include "gqi_process.hpp"
#include "image/image.hpp"

struct ImageModel
{
public:
    Voxel voxel;
    unsigned int thread_count;
    std::string file_name,error_msg;
    gz_mat_read mat_reader;
    std::vector<const unsigned short*> dwi_data;
    image::basic_image<float,3> dwi_sum;
    image::basic_image<unsigned char,3> mask;
public:
    // 0: x  1: y  2: z
    // 3: xy 4: yz 5: xz
    void flip(unsigned char type)
    {
        if(type < 3)
            for (unsigned int index = 0;index < voxel.bvectors.size();++index)
                voxel.bvectors[index][type] = -voxel.bvectors[index][type];
        else
            for (unsigned int index = 0;index < voxel.bvectors.size();++index)
                std::swap(voxel.bvectors[index][type%3],voxel.bvectors[index][(type+1)%3]);
        image::flip(dwi_sum,type);
        image::flip(mask,type);
        for (unsigned int index = 0;check_prog(index,dwi_data.size());++index)
        {
            image::pointer_image<unsigned short,3> I = image::make_image(voxel.dim,(unsigned short*)dwi_data[index]);
            image::flip(I,type);
        }
        voxel.dim = dwi_sum.geometry();
    }
    void rotate(const std::vector<float>& rotation_matrix)
    {
        image::resample(dwi_sum,rotation_matrix);
        image::resample(mask,rotation_matrix);
        for (unsigned int index = 0;check_prog(index,dwi_data.size());++index)
        {
            image::pointer_image<unsigned short,3> I = image::make_image(voxel.dim,(unsigned short*)dwi_data[index]);
            image::resample(I,rotation_matrix);
        }
    }

public:
    ImageModel(void):thread_count(1) {}
    bool load_from_file(const char* dwi_file_name)
    {
        file_name = dwi_file_name;
        if (!mat_reader.load_from_file(dwi_file_name))
        {
            error_msg = "Cannot open file";
            return false;
        }
        unsigned int row,col;

        const unsigned short* dim_ptr = 0;
        if (!mat_reader.read("dimension",row,col,dim_ptr))
        {
            error_msg = "Cannot find dimension matrix";
            return false;
        }
        const float* voxel_size = 0;
        if (!mat_reader.read("voxel_size",row,col,voxel_size))
        {
            //error_msg = "Cannot find voxel size matrix";
            //return false;
            std::fill(voxel.vs.begin(),voxel.vs.end(),3.0);
        }
        else
            std::copy(voxel_size,voxel_size+3,voxel.vs.begin());

        if (dim_ptr[0]*dim_ptr[1]*dim_ptr[2] <= 0)
        {
            error_msg = "Invalid dimension setting";
            return false;
        }
        voxel.dim[0] = dim_ptr[0];
        voxel.dim[1] = dim_ptr[1];
        voxel.dim[2] = dim_ptr[2];

        const float* table;
        if (!mat_reader.read("b_table",row,col,table))
        {
            error_msg = "Cannot find b_table matrix";
            return false;
        }
        voxel.bvalues.resize(col);
        voxel.bvectors.resize(col);
        for (unsigned int index = 0;index < col;++index)
        {
            voxel.bvalues[index] = table[0];
            voxel.bvectors[index][0] = table[1];
            voxel.bvectors[index][1] = table[2];
            voxel.bvectors[index][2] = table[3];
            table += 4;
        }

        voxel.q_count = col;
        dwi_data.resize(voxel.q_count);
        for (unsigned int index = 0;index < voxel.q_count;++index)
        {
            std::ostringstream out;
            out << "image" << index;
            mat_reader.read(out.str().c_str(),row,col,dwi_data[index]);
            if (!dwi_data[index])
            {
                error_msg = "Cannot find image matrix";
                return false;
            }
        }

        {
            // this grad_dev matrix is rotated
            const float* grad_dev = 0;
            if(mat_reader.read("grad_dev",row,col,grad_dev) && row*col == voxel.dim.size()*9)
            {
                for(unsigned int index = 0;index < 9;index++)
                    voxel.grad_dev.push_back(image::make_image(voxel.dim,grad_dev+index*voxel.dim.size()));
            }
        }

        // create mask;
        dwi_sum.clear();
        dwi_sum.resize(voxel.dim);
        for (unsigned int index = 0;index < voxel.q_count;++index)
            image::add(dwi_sum.begin(),dwi_sum.end(),dwi_data[index]);

        float max_value = *std::max_element(dwi_sum.begin(),dwi_sum.end());
        float min_value = max_value;
        for (unsigned int index = 0;index < dwi_sum.size();++index)
            if (dwi_sum[index] < min_value && dwi_sum[index] > 0)
                min_value = dwi_sum[index];

        ::set_title("creating mask");
        check_prog(0,3);
        image::minus_constant(dwi_sum,min_value);
        image::lower_threshold(dwi_sum,0.0f);
        image::normalize(dwi_sum,1.0);
        image::add_constant(dwi_sum,1.0);
        image::log(dwi_sum);
        image::divide_constant(dwi_sum,0.301);
        image::upper_threshold(dwi_sum,1.0f);

        const unsigned char* mask_ptr = 0;
        if(mat_reader.read("mask",row,col,mask_ptr))
        {
            mask.resize(voxel.dim);
            if(row*col == voxel.dim.size())
                std::copy(mask_ptr,mask_ptr+row*col,mask.begin());
        }
        else
        {
            image::threshold(dwi_sum,mask,image::segmentation::otsu_threshold(dwi_sum)*0.8,1,0);
            check_prog(1,3);
            image::morphology::recursive_smoothing(mask);
            check_prog(2,3);
            image::morphology::defragment(mask);
            image::morphology::recursive_smoothing(mask);
        }
        check_prog(3,3);
        return true;
    }
    void save_to_file(gz_mat_write& mat_writer)
    {

        set_title("saving");

        // dimension
        {
            short dim[3];
            dim[0] = voxel.dim[0];
            dim[1] = voxel.dim[1];
            dim[2] = voxel.dim[2];
            mat_writer.write("dimension",dim,1,3);
        }

        // voxel size
        mat_writer.write("voxel_size",&*voxel.vs.begin(),1,3);

        std::vector<float> float_data;
        std::vector<short> short_data;
        voxel.ti.save_to_buffer(float_data,short_data);
        mat_writer.write("odf_vertices",&*float_data.begin(),3,voxel.ti.vertices_count);
        mat_writer.write("odf_faces",&*short_data.begin(),3,voxel.ti.faces.size());

    }
public:
    template<typename CheckType>
    bool avaliable(void) const
    {
        return CheckType::check(voxel);
    }

    template<typename ProcessType>
    bool reconstruct(void)
    {
        voxel.image_model = this;
        voxel.CreateProcesses<ProcessType>();
        voxel.init(thread_count);
        boost::thread_group threads;
        for (unsigned int index = 1;index < thread_count;++index)
            threads.add_thread(new boost::thread(&Voxel::thread_run,&voxel,
                                                 index,thread_count,mask));
        voxel.thread_run(0,thread_count,mask);
        threads.join_all();
        return !prog_aborted();
    }

    template<typename ProcessType>
    bool reconstruct(const std::string& ext)
    {
        if (!reconstruct<ProcessType>())
            return false;
        std::string output_name = file_name;
        output_name += ext;
        gz_mat_write mat_writer(output_name.c_str());
        save_to_file(mat_writer);
        voxel.end(mat_writer);
        check_prog(0,0);
        return true;
    }

};

#endif//IMAGE_MODEL_HPP
<commit_msg>normalize bvector<commit_after>#ifndef IMAGE_MODEL_HPP
#define IMAGE_MODEL_HPP
#include <boost/thread.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include "gqi_process.hpp"
#include "image/image.hpp"

struct ImageModel
{
public:
    Voxel voxel;
    unsigned int thread_count;
    std::string file_name,error_msg;
    gz_mat_read mat_reader;
    std::vector<const unsigned short*> dwi_data;
    image::basic_image<float,3> dwi_sum;
    image::basic_image<unsigned char,3> mask;
public:
    // 0: x  1: y  2: z
    // 3: xy 4: yz 5: xz
    void flip(unsigned char type)
    {
        if(type < 3)
            for (unsigned int index = 0;index < voxel.bvectors.size();++index)
                voxel.bvectors[index][type] = -voxel.bvectors[index][type];
        else
            for (unsigned int index = 0;index < voxel.bvectors.size();++index)
                std::swap(voxel.bvectors[index][type%3],voxel.bvectors[index][(type+1)%3]);
        image::flip(dwi_sum,type);
        image::flip(mask,type);
        for (unsigned int index = 0;check_prog(index,dwi_data.size());++index)
        {
            image::pointer_image<unsigned short,3> I = image::make_image(voxel.dim,(unsigned short*)dwi_data[index]);
            image::flip(I,type);
        }
        voxel.dim = dwi_sum.geometry();
    }
    void rotate(const std::vector<float>& rotation_matrix)
    {
        image::resample(dwi_sum,rotation_matrix);
        image::resample(mask,rotation_matrix);
        for (unsigned int index = 0;check_prog(index,dwi_data.size());++index)
        {
            image::pointer_image<unsigned short,3> I = image::make_image(voxel.dim,(unsigned short*)dwi_data[index]);
            image::resample(I,rotation_matrix);
        }
    }

public:
    ImageModel(void):thread_count(1) {}
    bool load_from_file(const char* dwi_file_name)
    {
        file_name = dwi_file_name;
        if (!mat_reader.load_from_file(dwi_file_name))
        {
            error_msg = "Cannot open file";
            return false;
        }
        unsigned int row,col;

        const unsigned short* dim_ptr = 0;
        if (!mat_reader.read("dimension",row,col,dim_ptr))
        {
            error_msg = "Cannot find dimension matrix";
            return false;
        }
        const float* voxel_size = 0;
        if (!mat_reader.read("voxel_size",row,col,voxel_size))
        {
            //error_msg = "Cannot find voxel size matrix";
            //return false;
            std::fill(voxel.vs.begin(),voxel.vs.end(),3.0);
        }
        else
            std::copy(voxel_size,voxel_size+3,voxel.vs.begin());

        if (dim_ptr[0]*dim_ptr[1]*dim_ptr[2] <= 0)
        {
            error_msg = "Invalid dimension setting";
            return false;
        }
        voxel.dim[0] = dim_ptr[0];
        voxel.dim[1] = dim_ptr[1];
        voxel.dim[2] = dim_ptr[2];

        const float* table;
        if (!mat_reader.read("b_table",row,col,table))
        {
            error_msg = "Cannot find b_table matrix";
            return false;
        }
        voxel.bvalues.resize(col);
        voxel.bvectors.resize(col);
        for (unsigned int index = 0;index < col;++index)
        {
            voxel.bvalues[index] = table[0];
            voxel.bvectors[index][0] = table[1];
            voxel.bvectors[index][1] = table[2];
            voxel.bvectors[index][2] = table[3];
            voxel.bvectors[index].normalize();
            table += 4;
        }

        voxel.q_count = col;
        dwi_data.resize(voxel.q_count);
        for (unsigned int index = 0;index < voxel.q_count;++index)
        {
            std::ostringstream out;
            out << "image" << index;
            mat_reader.read(out.str().c_str(),row,col,dwi_data[index]);
            if (!dwi_data[index])
            {
                error_msg = "Cannot find image matrix";
                return false;
            }
        }

        {
            // this grad_dev matrix is rotated
            const float* grad_dev = 0;
            if(mat_reader.read("grad_dev",row,col,grad_dev) && row*col == voxel.dim.size()*9)
            {
                for(unsigned int index = 0;index < 9;index++)
                    voxel.grad_dev.push_back(image::make_image(voxel.dim,grad_dev+index*voxel.dim.size()));
            }
        }

        // create mask;
        dwi_sum.clear();
        dwi_sum.resize(voxel.dim);
        for (unsigned int index = 0;index < voxel.q_count;++index)
            image::add(dwi_sum.begin(),dwi_sum.end(),dwi_data[index]);

        float max_value = *std::max_element(dwi_sum.begin(),dwi_sum.end());
        float min_value = max_value;
        for (unsigned int index = 0;index < dwi_sum.size();++index)
            if (dwi_sum[index] < min_value && dwi_sum[index] > 0)
                min_value = dwi_sum[index];

        ::set_title("creating mask");
        check_prog(0,3);
        image::minus_constant(dwi_sum,min_value);
        image::lower_threshold(dwi_sum,0.0f);
        image::normalize(dwi_sum,1.0);
        image::add_constant(dwi_sum,1.0);
        image::log(dwi_sum);
        image::divide_constant(dwi_sum,0.301);
        image::upper_threshold(dwi_sum,1.0f);

        const unsigned char* mask_ptr = 0;
        if(mat_reader.read("mask",row,col,mask_ptr))
        {
            mask.resize(voxel.dim);
            if(row*col == voxel.dim.size())
                std::copy(mask_ptr,mask_ptr+row*col,mask.begin());
        }
        else
        {
            image::threshold(dwi_sum,mask,image::segmentation::otsu_threshold(dwi_sum)*0.8,1,0);
            check_prog(1,3);
            image::morphology::recursive_smoothing(mask);
            check_prog(2,3);
            image::morphology::defragment(mask);
            image::morphology::recursive_smoothing(mask);
        }
        check_prog(3,3);
        return true;
    }
    void save_to_file(gz_mat_write& mat_writer)
    {

        set_title("saving");

        // dimension
        {
            short dim[3];
            dim[0] = voxel.dim[0];
            dim[1] = voxel.dim[1];
            dim[2] = voxel.dim[2];
            mat_writer.write("dimension",dim,1,3);
        }

        // voxel size
        mat_writer.write("voxel_size",&*voxel.vs.begin(),1,3);

        std::vector<float> float_data;
        std::vector<short> short_data;
        voxel.ti.save_to_buffer(float_data,short_data);
        mat_writer.write("odf_vertices",&*float_data.begin(),3,voxel.ti.vertices_count);
        mat_writer.write("odf_faces",&*short_data.begin(),3,voxel.ti.faces.size());

    }
public:
    template<typename CheckType>
    bool avaliable(void) const
    {
        return CheckType::check(voxel);
    }

    template<typename ProcessType>
    bool reconstruct(void)
    {
        voxel.image_model = this;
        voxel.CreateProcesses<ProcessType>();
        voxel.init(thread_count);
        boost::thread_group threads;
        for (unsigned int index = 1;index < thread_count;++index)
            threads.add_thread(new boost::thread(&Voxel::thread_run,&voxel,
                                                 index,thread_count,mask));
        voxel.thread_run(0,thread_count,mask);
        threads.join_all();
        return !prog_aborted();
    }

    template<typename ProcessType>
    bool reconstruct(const std::string& ext)
    {
        if (!reconstruct<ProcessType>())
            return false;
        std::string output_name = file_name;
        output_name += ext;
        gz_mat_write mat_writer(output_name.c_str());
        save_to_file(mat_writer);
        voxel.end(mat_writer);
        check_prog(0,0);
        return true;
    }

};

#endif//IMAGE_MODEL_HPP
<|endoftext|>


<commit_before><commit_msg>make "Styles & Formatting" preview HiDPI aware<commit_after><|endoftext|>


<commit_before><commit_msg>Change order of "New folder" entry in move menu.<commit_after><|endoftext|>


<commit_before>#ifndef CATOMIC_STACK
#define CATOMIC_STACK
#include<atomic>
#include<memory>	//shared_ptr, make_shared
#include<utility>	//forward, move

namespace nThread
{
	//use std::shared_ptr to implement
	template<class T>
	class CAtomic_stack
	{
	public:
		using value_type=T;
	private:
		struct Node
		{
			value_type data;
			std::shared_ptr<Node> next;
			template<class ... Args>
			Node(const std::shared_ptr<Node> &next_,Args &&...args)
				:data{std::forward<decltype(args)>(args)...},next{next_}{}
		};
		std::shared_ptr<Node> begin_;
	public:
		template<class ... Args>
		void emplace(Args &&...args)
		{
			std::shared_ptr<Node> node{std::make_shared<Node>(begin_,std::forward<decltype(args)>(args)...)};
			while(!std::atomic_compare_exchange_weak_explicit(&begin_,&node->next,node,std::memory_order_release,std::memory_order_relaxed))
				;
		}
		inline bool empty() const noexcept
		{
			return !begin_.operator bool();
		}
		inline bool is_lock_free() const noexcept
		{
			return std::atomic_is_lock_free(&begin_);
		}
		value_type pop()
		{
			std::shared_ptr<Node> node{std::atomic_load_explicit(&begin_,std::memory_order_relaxed)};
			while(!std::atomic_compare_exchange_weak_explicit(&begin_,&node,node->next,std::memory_order_acquire,std::memory_order_relaxed))
				;
			return std::move(node->data);
		}
	};
}

#endif<commit_msg>add comment<commit_after>#ifndef CATOMIC_STACK
#define CATOMIC_STACK
#include<atomic>
#include<memory>	//shared_ptr, make_shared
#include<utility>	//forward, move

namespace nThread
{
	//use std::shared_ptr to implement
	template<class T>
	class CAtomic_stack
	{
	public:
		using value_type=T;
	private:
		struct Node
		{
			value_type data;
			std::shared_ptr<Node> next;
			template<class ... Args>
			Node(const std::shared_ptr<Node> &next_,Args &&...args)
				:data{std::forward<decltype(args)>(args)...},next{next_}{}
		};
		std::shared_ptr<Node> begin_;
	public:
		template<class ... Args>
		void emplace(Args &&...args)
		{
			std::shared_ptr<Node> node{std::make_shared<Node>(begin_,std::forward<decltype(args)>(args)...)};
			while(!std::atomic_compare_exchange_weak_explicit(&begin_,&node->next,node,std::memory_order_release,std::memory_order_relaxed))
				;
		}
		inline bool empty() const noexcept
		{
			return !begin_.operator bool();
		}
		inline bool is_lock_free() const noexcept
		{
			return std::atomic_is_lock_free(&begin_);
		}
		//if constructor or assignment operator you use here is not noexcept, it may not be exception safety
		value_type pop()
		{
			std::shared_ptr<Node> node{std::atomic_load_explicit(&begin_,std::memory_order_relaxed)};
			while(!std::atomic_compare_exchange_weak_explicit(&begin_,&node,node->next,std::memory_order_acquire,std::memory_order_relaxed))
				;
			return std::move(node->data);
		}
	};
}

#endif<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 ekf.cpp
 * Core functions for ekf attitude and position estimator.
 *
 * @author Roman Bast <bapstroman@gmail.com>
 *
 */

#include "ekf.h"
#include <drivers/drv_hrt.h>

Ekf::Ekf():
	_filter_initialised(false),
	_earth_rate_initialised(false),
	_fuse_height(false),
	_fuse_pos(false),
	_fuse_vel(false),
	_mag_fuse_index(0),
	_time_last_fake_gps(0)
{
	_earth_rate_NED.setZero();
	_R_prev = matrix::Dcm<float>();
	_delta_angle_corr.setZero();
	_delta_vel_corr.setZero();
	_vel_corr.setZero();
}

Ekf::~Ekf()
{

}

bool Ekf::update()
{
	bool ret = false;	// indicates if there has been an update
	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();
		if (!_filter_initialised) {
			return false;
		}
	}
	printStates();
	//printStatesFast();
	// prediction
	if (_imu_updated) {
		ret = true;
		predictState();
		predictCovariance();
	}

	// measurement updates

	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		fuseHeading();
		//fuseMag(_mag_fuse_index);
		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
	}

	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
		_fuse_height = true;
	}

	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
		_fuse_pos = true;
		_fuse_vel = true;
	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
		_fuse_vel = true;
		_gps_sample_delayed.vel.setZero();
	}

	if (_fuse_height || _fuse_pos || _fuse_vel) {
		fuseVelPosHeight();
		_fuse_vel = _fuse_pos = _fuse_height = false;
	}

	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
		fuseRange();
	}

	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
		fuseAirspeed();
	}

	calculateOutputStates();

	return ret;
}

bool Ekf::initialiseFilter(void)
{
	_state.ang_error.setZero();
	_state.vel.setZero();
	_state.pos.setZero();
	_state.gyro_bias.setZero();
	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
	_state.accel_z_bias = 0.0f;
	_state.mag_I.setZero();
	_state.mag_B.setZero();
	_state.wind_vel.setZero();

	// get initial attitude estimate from accel vector, assuming vehicle is static
	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;

	float pitch = 0.0f;
	float roll = 0.0f;

	if (accel_init.norm() > 0.001f) {
		accel_init.normalize();

		pitch = asinf(accel_init(0));
		roll = -asinf(accel_init(1) / cosf(pitch));
	}

	magSample mag_init = _mag_buffer.get_newest();
	if (mag_init.time_us == 0) {
		return false;
	}

	float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0));

	matrix::Euler<float> euler_init(roll, pitch, yaw_init);
	_state.quat_nominal = Quaternion(euler_init);

	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init.mag;

	resetVelocity();
	resetPosition();

	initialiseCovariance();

	return true;
}

void Ekf::predictState()
{
	if (!_earth_rate_initialised) {
		if (_gps_initialised) {
			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad );
			_earth_rate_initialised = true;
		}
	}

	// attitude error state prediciton
	matrix::Dcm<float> R_to_earth(_state.quat_nominal);	// transformation matrix from body to world frame
	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
	Quaternion dq;	// delta quaternion since last update
	dq.from_axis_angle(corrected_delta_ang);
	_state.quat_nominal = dq * _state.quat_nominal;
	_state.quat_nominal.normalize();

	_R_prev = R_to_earth.transpose();

	Vector3f vel_last = _state.vel;

	// predict velocity states
	_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;

	// predict position states via trapezoidal integration of velocity
	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;

	constrainStates();
}

void Ekf::calculateOutputStates()
{
	imuSample imu_new = _imu_sample_new;
	Vector3f delta_angle;
	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);

	delta_angle -= _state.gyro_bias;

	Vector3f delta_vel = imu_new.delta_vel;
	delta_vel(2) -= _state.accel_z_bias;

	delta_angle += _delta_angle_corr;
	Quaternion dq;
	dq.from_axis_angle(delta_angle);

	_output_new.time_us = imu_new.time_us;
	_output_new.quat_nominal = dq * _output_new.quat_nominal;
	_output_new.quat_nominal.normalize();

	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);

	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;

	Vector3f vel_last = _output_new.vel;

	_output_new.vel += delta_vel_NED;

	_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;

	if (_imu_updated) {
		_output_buffer.push(_output_new);
		_imu_updated = false;
	}

	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed))
	{
		return;
	}

	Quaternion quat_inv = _state.quat_nominal.inversed();
	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;
	q_error.normalize();
	Vector3f delta_ang_error;

	float scalar;

	if (q_error(0) >= 0.0f) {
		scalar = -2.0f;
	} else {
		scalar = 2.0f;
	}

	delta_ang_error(0) = scalar * q_error(1);
	delta_ang_error(1) = scalar * q_error(2);
	delta_ang_error(2) = scalar * q_error(3);

	_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;

	_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;

	_vel_corr = (_state.pos - _output_sample_delayed.pos);

}

void Ekf::fuseAirspeed()
{

}

void Ekf::fuseRange()
{

}

void Ekf::printStates()
{
	static int counter = 0;

	if (counter % 50 == 0) {
		printf("quaternion\n");
		for(int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
		}

		matrix::Euler<float> euler(_state.quat_nominal);
		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));

		printf("vel\n");
		for(int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_state.vel(i));
		}

		printf("pos\n");
		for(int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_state.pos(i));
		}

		printf("gyro_scale\n");
		for(int i = 0; i < 3; i++) {
			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
		}

		printf("mag earth\n");
		for(int i = 0; i < 3; i++) {
			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
		}

		printf("mag bias\n");
		for(int i = 0; i < 3; i++) {
			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
		}
		counter = 0;
	}
	counter++;

}

void Ekf::printStatesFast()
{
	static int counter_fast = 0;

	if (counter_fast % 50 == 0) {
		printf("quaternion\n");
		for(int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
		}

		printf("vel\n");
		for(int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
		}

		printf("pos\n");
		for(int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
		}

		counter_fast = 0;
	}
	counter_fast++;
}
<commit_msg>Do not print states by default<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 ekf.cpp
 * Core functions for ekf attitude and position estimator.
 *
 * @author Roman Bast <bapstroman@gmail.com>
 *
 */

#include "ekf.h"
#include <drivers/drv_hrt.h>

Ekf::Ekf():
	_filter_initialised(false),
	_earth_rate_initialised(false),
	_fuse_height(false),
	_fuse_pos(false),
	_fuse_vel(false),
	_mag_fuse_index(0),
	_time_last_fake_gps(0)
{
	_earth_rate_NED.setZero();
	_R_prev = matrix::Dcm<float>();
	_delta_angle_corr.setZero();
	_delta_vel_corr.setZero();
	_vel_corr.setZero();
}

Ekf::~Ekf()
{

}

bool Ekf::update()
{
	bool ret = false;	// indicates if there has been an update
	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();
		if (!_filter_initialised) {
			return false;
		}
	}
	//printStates();
	//printStatesFast();
	// prediction
	if (_imu_updated) {
		ret = true;
		predictState();
		predictCovariance();
	}

	// measurement updates

	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		fuseHeading();
		//fuseMag(_mag_fuse_index);
		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
	}

	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
		_fuse_height = true;
	}

	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
		_fuse_pos = true;
		_fuse_vel = true;
	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
		_fuse_vel = true;
		_gps_sample_delayed.vel.setZero();
	}

	if (_fuse_height || _fuse_pos || _fuse_vel) {
		fuseVelPosHeight();
		_fuse_vel = _fuse_pos = _fuse_height = false;
	}

	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
		fuseRange();
	}

	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
		fuseAirspeed();
	}

	calculateOutputStates();

	return ret;
}

bool Ekf::initialiseFilter(void)
{
	_state.ang_error.setZero();
	_state.vel.setZero();
	_state.pos.setZero();
	_state.gyro_bias.setZero();
	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
	_state.accel_z_bias = 0.0f;
	_state.mag_I.setZero();
	_state.mag_B.setZero();
	_state.wind_vel.setZero();

	// get initial attitude estimate from accel vector, assuming vehicle is static
	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;

	float pitch = 0.0f;
	float roll = 0.0f;

	if (accel_init.norm() > 0.001f) {
		accel_init.normalize();

		pitch = asinf(accel_init(0));
		roll = -asinf(accel_init(1) / cosf(pitch));
	}

	magSample mag_init = _mag_buffer.get_newest();
	if (mag_init.time_us == 0) {
		return false;
	}

	float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0));

	matrix::Euler<float> euler_init(roll, pitch, yaw_init);
	_state.quat_nominal = Quaternion(euler_init);

	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init.mag;

	resetVelocity();
	resetPosition();

	initialiseCovariance();

	return true;
}

void Ekf::predictState()
{
	if (!_earth_rate_initialised) {
		if (_gps_initialised) {
			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad );
			_earth_rate_initialised = true;
		}
	}

	// attitude error state prediciton
	matrix::Dcm<float> R_to_earth(_state.quat_nominal);	// transformation matrix from body to world frame
	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
	Quaternion dq;	// delta quaternion since last update
	dq.from_axis_angle(corrected_delta_ang);
	_state.quat_nominal = dq * _state.quat_nominal;
	_state.quat_nominal.normalize();

	_R_prev = R_to_earth.transpose();

	Vector3f vel_last = _state.vel;

	// predict velocity states
	_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;

	// predict position states via trapezoidal integration of velocity
	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;

	constrainStates();
}

void Ekf::calculateOutputStates()
{
	imuSample imu_new = _imu_sample_new;
	Vector3f delta_angle;
	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);

	delta_angle -= _state.gyro_bias;

	Vector3f delta_vel = imu_new.delta_vel;
	delta_vel(2) -= _state.accel_z_bias;

	delta_angle += _delta_angle_corr;
	Quaternion dq;
	dq.from_axis_angle(delta_angle);

	_output_new.time_us = imu_new.time_us;
	_output_new.quat_nominal = dq * _output_new.quat_nominal;
	_output_new.quat_nominal.normalize();

	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);

	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;

	Vector3f vel_last = _output_new.vel;

	_output_new.vel += delta_vel_NED;

	_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;

	if (_imu_updated) {
		_output_buffer.push(_output_new);
		_imu_updated = false;
	}

	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed))
	{
		return;
	}

	Quaternion quat_inv = _state.quat_nominal.inversed();
	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;
	q_error.normalize();
	Vector3f delta_ang_error;

	float scalar;

	if (q_error(0) >= 0.0f) {
		scalar = -2.0f;
	} else {
		scalar = 2.0f;
	}

	delta_ang_error(0) = scalar * q_error(1);
	delta_ang_error(1) = scalar * q_error(2);
	delta_ang_error(2) = scalar * q_error(3);

	_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;

	_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;

	_vel_corr = (_state.pos - _output_sample_delayed.pos);

}

void Ekf::fuseAirspeed()
{

}

void Ekf::fuseRange()
{

}

void Ekf::printStates()
{
	static int counter = 0;

	if (counter % 50 == 0) {
		printf("quaternion\n");
		for(int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
		}

		matrix::Euler<float> euler(_state.quat_nominal);
		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));

		printf("vel\n");
		for(int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_state.vel(i));
		}

		printf("pos\n");
		for(int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_state.pos(i));
		}

		printf("gyro_scale\n");
		for(int i = 0; i < 3; i++) {
			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
		}

		printf("mag earth\n");
		for(int i = 0; i < 3; i++) {
			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
		}

		printf("mag bias\n");
		for(int i = 0; i < 3; i++) {
			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
		}
		counter = 0;
	}
	counter++;

}

void Ekf::printStatesFast()
{
	static int counter_fast = 0;

	if (counter_fast % 50 == 0) {
		printf("quaternion\n");
		for(int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
		}

		printf("vel\n");
		for(int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
		}

		printf("pos\n");
		for(int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
		}

		counter_fast = 0;
	}
	counter_fast++;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 ekf.cpp
 * Core functions for ekf attitude and position estimator.
 *
 * @author Roman Bast <bapstroman@gmail.com>
 *
 */

#include "ekf.h"
#include <drivers/drv_hrt.h>

Ekf::Ekf():
	_filter_initialised(false),
	_earth_rate_initialised(false),
	_fuse_height(false),
	_fuse_pos(false),
	_fuse_hor_vel(false),
	_fuse_vert_vel(false),
	_mag_fuse_index(0),
	_time_last_fake_gps(0)
{
	_earth_rate_NED.setZero();
	_R_prev = matrix::Dcm<float>();
	_delta_angle_corr.setZero();
	_delta_vel_corr.setZero();
	_vel_corr.setZero();
}

Ekf::~Ekf()
{

}

bool Ekf::update()
{
	bool ret = false;	// indicates if there has been an update

	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();

		if (!_filter_initialised) {
			return false;
		}
	}

	//printStates();
	//printStatesFast();
	// prediction
	if (_imu_updated) {
		ret = true;
		predictState();
		predictCovariance();
	}

	// measurement updates

	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		fuseHeading();
		//fuseMag(_mag_fuse_index);
		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
	}

	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
		_fuse_height = true;
	}

	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
		_fuse_pos = true;
		_fuse_vert_vel = true;
		_fuse_hor_vel = true;

	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
		// if gps does not update then fake position and horizontal veloctiy measurements
		_fuse_hor_vel = true;	// we only fake horizontal velocity because we still have baro
		_gps_sample_delayed.vel.setZero();
	}

	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
		fuseVelPosHeight();
		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
	}

	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
		fuseRange();
	}

	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
		fuseAirspeed();
	}

	calculateOutputStates();

	return ret;
}

bool Ekf::initialiseFilter(void)
{
	_state.ang_error.setZero();
	_state.vel.setZero();
	_state.pos.setZero();
	_state.gyro_bias.setZero();
	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
	_state.accel_z_bias = 0.0f;
	_state.mag_I.setZero();
	_state.mag_B.setZero();
	_state.wind_vel.setZero();

	// get initial roll and pitch estimate from accel vector, assuming vehicle is static
	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;

	float pitch = 0.0f;
	float roll = 0.0f;

	if (accel_init.norm() > 0.001f) {
		accel_init.normalize();

		pitch = asinf(accel_init(0));
		roll = -asinf(accel_init(1) / cosf(pitch));
	}

	matrix::Euler<float> euler_init(roll, pitch, 0.0f);

	// Get the latest magnetic field measurement.
	// If we don't have a measurement then we cannot initialise the filter
	magSample mag_init = _mag_buffer.get_newest();

	if (mag_init.time_us == 0) {
		return false;
	}

	// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
	// TODO use declination if available
	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
	float declination = 0.0f;
	euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));

	// calculate initial quaternion states
	_state.quat_nominal = Quaternion(euler_init);
	_output_new.quat_nominal = _state.quat_nominal;

	// calculate initial earth magnetic field states
	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init.mag;

	resetVelocity();
	resetPosition();

	// initialize vertical position with newest baro measurement
	baroSample baro_init = _baro_buffer.get_newest();

	if (baro_init.time_us == 0) {
		return false;
	}

	_state.pos(2) = -baro_init.hgt;

	initialiseCovariance();

	return true;
}

void Ekf::predictState()
{
	if (!_earth_rate_initialised) {
		if (_gps_initialised) {
			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
			_earth_rate_initialised = true;
		}
	}

	// attitude error state prediciton
	matrix::Dcm<float> R_to_earth(_state.quat_nominal);	// transformation matrix from body to world frame
	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
				       _imu_sample_delayed.delta_ang_dt;
	Quaternion dq;	// delta quaternion since last update
	dq.from_axis_angle(corrected_delta_ang);
	_state.quat_nominal = dq * _state.quat_nominal;
	_state.quat_nominal.normalize();

	_R_prev = R_to_earth.transpose();

	Vector3f vel_last = _state.vel;

	// predict velocity states
	_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;

	// predict position states via trapezoidal integration of velocity
	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;

	constrainStates();
}

void Ekf::calculateOutputStates()
{
	imuSample imu_new = _imu_sample_new;
	Vector3f delta_angle;
	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);

	delta_angle -= _state.gyro_bias;

	Vector3f delta_vel = imu_new.delta_vel;
	delta_vel(2) -= _state.accel_z_bias;

	delta_angle += _delta_angle_corr;
	Quaternion dq;
	dq.from_axis_angle(delta_angle);

	_output_new.time_us = imu_new.time_us;
	_output_new.quat_nominal = dq * _output_new.quat_nominal;
	_output_new.quat_nominal.normalize();

	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);

	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;

	Vector3f vel_last = _output_new.vel;

	_output_new.vel += delta_vel_NED;

	_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;

	if (_imu_updated) {
		_output_buffer.push(_output_new);
		_imu_updated = false;
	}

	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
		return;
	}

	Quaternion quat_inv = _state.quat_nominal.inversed();
	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;
	q_error.normalize();
	Vector3f delta_ang_error;

	float scalar;

	if (q_error(0) >= 0.0f) {
		scalar = -2.0f;

	} else {
		scalar = 2.0f;
	}

	delta_ang_error(0) = scalar * q_error(1);
	delta_ang_error(1) = scalar * q_error(2);
	delta_ang_error(2) = scalar * q_error(3);

	_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;

	_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;

	_vel_corr = (_state.pos - _output_sample_delayed.pos);

}

void Ekf::fuseAirspeed()
{

}

void Ekf::fuseRange()
{

}

void Ekf::printStates()
{
	static int counter = 0;

	if (counter % 50 == 0) {
		printf("quaternion\n");

		for (int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
		}

		matrix::Euler<float> euler(_state.quat_nominal);
		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));

		printf("vel\n");

		for (int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_state.vel(i));
		}

		printf("pos\n");

		for (int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_state.pos(i));
		}

		printf("gyro_scale\n");

		for (int i = 0; i < 3; i++) {
			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
		}

		printf("mag earth\n");

		for (int i = 0; i < 3; i++) {
			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
		}

		printf("mag bias\n");

		for (int i = 0; i < 3; i++) {
			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
		}

		counter = 0;
	}

	counter++;

}

void Ekf::printStatesFast()
{
	static int counter_fast = 0;

	if (counter_fast % 50 == 0) {
		printf("quaternion\n");

		for (int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
		}

		printf("vel\n");

		for (int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
		}

		printf("pos\n");

		for (int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
		}

		counter_fast = 0;
	}

	counter_fast++;
}
<commit_msg>use full mag fusion<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 ekf.cpp
 * Core functions for ekf attitude and position estimator.
 *
 * @author Roman Bast <bapstroman@gmail.com>
 *
 */

#include "ekf.h"
#include <drivers/drv_hrt.h>

Ekf::Ekf():
	_filter_initialised(false),
	_earth_rate_initialised(false),
	_fuse_height(false),
	_fuse_pos(false),
	_fuse_hor_vel(false),
	_fuse_vert_vel(false),
	_mag_fuse_index(0),
	_time_last_fake_gps(0)
{
	_earth_rate_NED.setZero();
	_R_prev = matrix::Dcm<float>();
	_delta_angle_corr.setZero();
	_delta_vel_corr.setZero();
	_vel_corr.setZero();
}

Ekf::~Ekf()
{

}

bool Ekf::update()
{
	bool ret = false;	// indicates if there has been an update

	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();

		if (!_filter_initialised) {
			return false;
		}
	}

	//printStates();
	//printStatesFast();
	// prediction
	if (_imu_updated) {
		ret = true;
		predictState();
		predictCovariance();
	}

	// measurement updates

	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		//fuseHeading();
		fuseMag(_mag_fuse_index);
		_mag_fuse_index = (_mag_fuse_index + 1) % 3;
	}

	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
		_fuse_height = true;
	}

	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
		_fuse_pos = true;
		_fuse_vert_vel = true;
		_fuse_hor_vel = true;

	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
		// if gps does not update then fake position and horizontal veloctiy measurements
		_fuse_hor_vel = true;	// we only fake horizontal velocity because we still have baro
		_gps_sample_delayed.vel.setZero();
	}

	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
		fuseVelPosHeight();
		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
	}

	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
		fuseRange();
	}

	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
		fuseAirspeed();
	}

	calculateOutputStates();

	return ret;
}

bool Ekf::initialiseFilter(void)
{
	_state.ang_error.setZero();
	_state.vel.setZero();
	_state.pos.setZero();
	_state.gyro_bias.setZero();
	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
	_state.accel_z_bias = 0.0f;
	_state.mag_I.setZero();
	_state.mag_B.setZero();
	_state.wind_vel.setZero();

	// get initial roll and pitch estimate from accel vector, assuming vehicle is static
	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;

	float pitch = 0.0f;
	float roll = 0.0f;

	if (accel_init.norm() > 0.001f) {
		accel_init.normalize();

		pitch = asinf(accel_init(0));
		roll = -asinf(accel_init(1) / cosf(pitch));
	}

	matrix::Euler<float> euler_init(roll, pitch, 0.0f);

	// Get the latest magnetic field measurement.
	// If we don't have a measurement then we cannot initialise the filter
	magSample mag_init = _mag_buffer.get_newest();

	if (mag_init.time_us == 0) {
		return false;
	}

	// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
	// TODO use declination if available
	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
	float declination = 0.0f;
	euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));

	// calculate initial quaternion states
	_state.quat_nominal = Quaternion(euler_init);
	_output_new.quat_nominal = _state.quat_nominal;

	// calculate initial earth magnetic field states
	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init.mag;

	resetVelocity();
	resetPosition();

	// initialize vertical position with newest baro measurement
	baroSample baro_init = _baro_buffer.get_newest();

	if (baro_init.time_us == 0) {
		return false;
	}

	_state.pos(2) = -baro_init.hgt;

	initialiseCovariance();

	return true;
}

void Ekf::predictState()
{
	if (!_earth_rate_initialised) {
		if (_gps_initialised) {
			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
			_earth_rate_initialised = true;
		}
	}

	// attitude error state prediciton
	matrix::Dcm<float> R_to_earth(_state.quat_nominal);	// transformation matrix from body to world frame
	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
				       _imu_sample_delayed.delta_ang_dt;
	Quaternion dq;	// delta quaternion since last update
	dq.from_axis_angle(corrected_delta_ang);
	_state.quat_nominal = dq * _state.quat_nominal;
	_state.quat_nominal.normalize();

	_R_prev = R_to_earth.transpose();

	Vector3f vel_last = _state.vel;

	// predict velocity states
	_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;

	// predict position states via trapezoidal integration of velocity
	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;

	constrainStates();
}

void Ekf::calculateOutputStates()
{
	imuSample imu_new = _imu_sample_new;
	Vector3f delta_angle;
	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);

	delta_angle -= _state.gyro_bias;

	Vector3f delta_vel = imu_new.delta_vel;
	delta_vel(2) -= _state.accel_z_bias;

	delta_angle += _delta_angle_corr;
	Quaternion dq;
	dq.from_axis_angle(delta_angle);

	_output_new.time_us = imu_new.time_us;
	_output_new.quat_nominal = dq * _output_new.quat_nominal;
	_output_new.quat_nominal.normalize();

	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);

	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;

	Vector3f vel_last = _output_new.vel;

	_output_new.vel += delta_vel_NED;

	_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;

	if (_imu_updated) {
		_output_buffer.push(_output_new);
		_imu_updated = false;
	}

	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
		return;
	}

	Quaternion quat_inv = _state.quat_nominal.inversed();
	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;
	q_error.normalize();
	Vector3f delta_ang_error;

	float scalar;

	if (q_error(0) >= 0.0f) {
		scalar = -2.0f;

	} else {
		scalar = 2.0f;
	}

	delta_ang_error(0) = scalar * q_error(1);
	delta_ang_error(1) = scalar * q_error(2);
	delta_ang_error(2) = scalar * q_error(3);

	_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;

	_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;

	_vel_corr = (_state.pos - _output_sample_delayed.pos);

}

void Ekf::fuseAirspeed()
{

}

void Ekf::fuseRange()
{

}

void Ekf::printStates()
{
	static int counter = 0;

	if (counter % 50 == 0) {
		printf("quaternion\n");

		for (int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
		}

		matrix::Euler<float> euler(_state.quat_nominal);
		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));

		printf("vel\n");

		for (int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_state.vel(i));
		}

		printf("pos\n");

		for (int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_state.pos(i));
		}

		printf("gyro_scale\n");

		for (int i = 0; i < 3; i++) {
			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
		}

		printf("mag earth\n");

		for (int i = 0; i < 3; i++) {
			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
		}

		printf("mag bias\n");

		for (int i = 0; i < 3; i++) {
			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
		}

		counter = 0;
	}

	counter++;

}

void Ekf::printStatesFast()
{
	static int counter_fast = 0;

	if (counter_fast % 50 == 0) {
		printf("quaternion\n");

		for (int i = 0; i < 4; i++) {
			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
		}

		printf("vel\n");

		for (int i = 0; i < 3; i++) {
			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
		}

		printf("pos\n");

		for (int i = 0; i < 3; i++) {
			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
		}

		counter_fast = 0;
	}

	counter_fast++;
}
<|endoftext|>


<commit_before>/* vim:ts=2 sw=2 sts=2 */
/*
 * EspEspWiFi - William Durand <will@drnd.me> - MIT License
 */
#include <Arduino.h>
#include <SoftwareSerial.h>

#include "EspWiFi.h"

EspWiFi::EspWiFi(
  HardwareSerial & serial,
  const unsigned int resetPin,
  const unsigned long baud
) : serial_(& serial), resetPin_(resetPin), baud_(baud), begun_(false)
{
}

#ifdef DEBUG
EspWiFi::EspWiFi(
  HardwareSerial & serial,
  const unsigned int resetPin,
  SoftwareSerial & debug,
  const unsigned long baud
) :  serial_(& serial), resetPin_(resetPin), debug_(& debug), baud_(baud), begun_(false)
{
}
#endif

void EspWiFi::begin()
{
  serial_->begin(baud_);

  serial_->println(F("AT"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT");
    return;
  }

  begun_ = true;
}

bool EspWiFi::connect(String ssid, String password)
{
  if (! begun_) {
    begin();
  }

  // 1 = client
  serial_->println(F("AT+CWMODE=1"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWMODE");
    return false;
  }

  if (! reset()) {
    debug("Error reset() in connect()");
    return false;
  }

  serial_->println(F("AT+CWJAP?"));

  if (! check_esp_response(1000, ssid)) {
    serial_->print(F("AT+CWJAP=\""));
    serial_->print(ssid);
    serial_->print(F("\",\""));
    serial_->print(password);
    serial_->println(F("\""));

    if (! check_esp_response(5000, F("OK"))) {
      debug("Error AT+CWJAP");
      return false;
    }
  }

  return true;
}

bool EspWiFi::startAccessPoint(String ssid, String password)
{
  if (! begun_) {
    begin();
  }

  // 3 = client + AP
  serial_->println(F("AT+CWMODE=3"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWMODE");
    return false;
  }

  if (! reset()) {
    debug("Error reset() in startAccessPoint()");
    return false;
  }

  serial_->print(F("AT+CWSAP=\""));
  serial_->print(ssid);
  serial_->print(F("\",\""));
  serial_->print(password);
  serial_->println(F("\",3,0"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWSAP");
    return false;
  }

  return true;
}

bool EspWiFi::listen(int port)
{
  serial_->println(F("AT+CIPMUX=1"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CIPMUX");
    return false;
  }

  serial_->print(F("AT+CIPSERVER=1,"));
  serial_->println(port);

  if (! check_esp_response(1000, F("OK"), F("no change"))) {
    debug("Error AT+CIPSERVER");
    return false;
  }

  return true;
}

String EspWiFi::read(unsigned int & mux_id, int & length, unsigned long timeout)
{
  String data;

  bool isBlank    = false;
  unsigned long t = millis();

  char c;
  while (millis() - t < timeout) {
    if (serial_->available()) {
      c = serial_->read();

      if (c == '\n' && isBlank) {
        break;
      }

      if (c == '\n') {
        isBlank = true;
      }
      else if (c != '\r') {
        isBlank = false;
      }

      data += c;
    }
  }

  if (data.startsWith("+IPD,")) {
    // request: +IPD,ch,len:data
    sscanf(data.substring(5, data.indexOf(":")).c_str(), "%d,%d", & mux_id, & length);
    data = data.substring(data.indexOf(":") + 1);

    t = millis();
    while (data.length() < length && millis() - t < timeout) {
      if (serial_->available()) {
        c = serial_->read();
        data += c;
      }
    }

    data = data.substring(0, length);

    // empty buffer
    while (serial_->available()) {
      serial_->read();
    }

    return data;
  }

  return "";
}

bool EspWiFi::write(const unsigned int mux_id, String data)
{
  serial_->print(F("AT+CIPSEND="));
  serial_->print(mux_id);
  serial_->print(",");
  serial_->println(data.length());

  if (check_esp_response(2000, F(">"))) {
    serial_->print(data);

    return check_esp_response(1000, F("SEND OK"));
  }

  return close(mux_id);
}

bool EspWiFi::close(const unsigned int mux_id)
{
  serial_->print(F("AT+CIPCLOSE="));
  serial_->println(mux_id);

  return check_esp_response(1000, F("OK"));
}

void EspWiFi::end()
{
  serial_->end();
  begun_ = false;
}

bool EspWiFi::reset()
{
  serial_->println(F("AT+RST"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+RST");
    return false;
  }

  unsigned long t = millis();
  while (millis() - t < 10000) {
    serial_->println(F("AT"));

    if (check_esp_response(1000, F("OK"))) {
      delay(1000);
      return true;
    }

    delay(500);
  }

  return false;
}

void EspWiFi::hardReset()
{
  end();

  digitalWrite(resetPin_, LOW);
  delay(1000);
  digitalWrite(resetPin_, HIGH);
  delay(3000);
}

bool EspWiFi::check_esp_response(unsigned long timeout, String response)
{
  char c;
  String buffer;
  bool found = false;
  unsigned long t = millis();

  while (millis() - t < timeout) {
    if (serial_->available() > 0) {
      c = serial_->read();
      buffer += c;

      if (buffer.indexOf(response) != -1 || buffer.equals(response)) {
        found = true;

        break;
      }
    }
  }

  while(serial_->available() > 0) {
    c = serial_->read();
    buffer += c;
  }

  debug(buffer);

  return found;
}

bool EspWiFi::check_esp_response(unsigned long timeout, String response, String altResponse)
{
  char c;
  String buffer;
  bool found = false;
  unsigned long t = millis();

  while (millis() - t < timeout) {
    if (serial_->available() > 0) {
      c = serial_->read();
      buffer += c;

      if (buffer.indexOf(response) != -1 || buffer.equals(response)
        || buffer.indexOf(altResponse) != -1 || buffer.equals(altResponse)) {
        found = true;

        break;
      }
    }
  }

  while(serial_->available() > 0) {
    c = serial_->read();
    buffer += c;
  }

  debug(buffer);

  return found;
}

void EspWiFi::debug(String content)
{
#ifdef DEBUG
  debug_->println(content);
  delay(100);
#endif
}
<commit_msg>Include stdio.h<commit_after>/* vim:ts=2 sw=2 sts=2 */
/*
 * EspEspWiFi - William Durand <will@drnd.me> - MIT License
 */
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <stdio.h>

#include "EspWiFi.h"

EspWiFi::EspWiFi(
  HardwareSerial & serial,
  const unsigned int resetPin,
  const unsigned long baud
) : serial_(& serial), resetPin_(resetPin), baud_(baud), begun_(false)
{
}

#ifdef DEBUG
EspWiFi::EspWiFi(
  HardwareSerial & serial,
  const unsigned int resetPin,
  SoftwareSerial & debug,
  const unsigned long baud
) :  serial_(& serial), resetPin_(resetPin), debug_(& debug), baud_(baud), begun_(false)
{
}
#endif

void EspWiFi::begin()
{
  serial_->begin(baud_);

  serial_->println(F("AT"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT");
    return;
  }

  begun_ = true;
}

bool EspWiFi::connect(String ssid, String password)
{
  if (! begun_) {
    begin();
  }

  // 1 = client
  serial_->println(F("AT+CWMODE=1"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWMODE");
    return false;
  }

  if (! reset()) {
    debug("Error reset() in connect()");
    return false;
  }

  serial_->println(F("AT+CWJAP?"));

  if (! check_esp_response(1000, ssid)) {
    serial_->print(F("AT+CWJAP=\""));
    serial_->print(ssid);
    serial_->print(F("\",\""));
    serial_->print(password);
    serial_->println(F("\""));

    if (! check_esp_response(5000, F("OK"))) {
      debug("Error AT+CWJAP");
      return false;
    }
  }

  return true;
}

bool EspWiFi::startAccessPoint(String ssid, String password)
{
  if (! begun_) {
    begin();
  }

  // 3 = client + AP
  serial_->println(F("AT+CWMODE=3"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWMODE");
    return false;
  }

  if (! reset()) {
    debug("Error reset() in startAccessPoint()");
    return false;
  }

  serial_->print(F("AT+CWSAP=\""));
  serial_->print(ssid);
  serial_->print(F("\",\""));
  serial_->print(password);
  serial_->println(F("\",3,0"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CWSAP");
    return false;
  }

  return true;
}

bool EspWiFi::listen(int port)
{
  serial_->println(F("AT+CIPMUX=1"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+CIPMUX");
    return false;
  }

  serial_->print(F("AT+CIPSERVER=1,"));
  serial_->println(port);

  if (! check_esp_response(1000, F("OK"), F("no change"))) {
    debug("Error AT+CIPSERVER");
    return false;
  }

  return true;
}

String EspWiFi::read(unsigned int & mux_id, int & length, unsigned long timeout)
{
  String data;

  bool isBlank    = false;
  unsigned long t = millis();

  char c;
  while (millis() - t < timeout) {
    if (serial_->available()) {
      c = serial_->read();

      if (c == '\n' && isBlank) {
        break;
      }

      if (c == '\n') {
        isBlank = true;
      }
      else if (c != '\r') {
        isBlank = false;
      }

      data += c;
    }
  }

  if (data.startsWith("+IPD,")) {
    // request: +IPD,ch,len:data
    sscanf(data.substring(5, data.indexOf(":")).c_str(), "%d,%d", & mux_id, & length);
    data = data.substring(data.indexOf(":") + 1);

    t = millis();
    while (data.length() < length && millis() - t < timeout) {
      if (serial_->available()) {
        c = serial_->read();
        data += c;
      }
    }

    data = data.substring(0, length);

    // empty buffer
    while (serial_->available()) {
      serial_->read();
    }

    return data;
  }

  return "";
}

bool EspWiFi::write(const unsigned int mux_id, String data)
{
  serial_->print(F("AT+CIPSEND="));
  serial_->print(mux_id);
  serial_->print(",");
  serial_->println(data.length());

  if (check_esp_response(2000, F(">"))) {
    serial_->print(data);

    return check_esp_response(1000, F("SEND OK"));
  }

  return close(mux_id);
}

bool EspWiFi::close(const unsigned int mux_id)
{
  serial_->print(F("AT+CIPCLOSE="));
  serial_->println(mux_id);

  return check_esp_response(1000, F("OK"));
}

void EspWiFi::end()
{
  serial_->end();
  begun_ = false;
}

bool EspWiFi::reset()
{
  serial_->println(F("AT+RST"));

  if (! check_esp_response(1000, F("OK"))) {
    debug("Error AT+RST");
    return false;
  }

  unsigned long t = millis();
  while (millis() - t < 10000) {
    serial_->println(F("AT"));

    if (check_esp_response(1000, F("OK"))) {
      delay(1000);
      return true;
    }

    delay(500);
  }

  return false;
}

void EspWiFi::hardReset()
{
  end();

  digitalWrite(resetPin_, LOW);
  delay(1000);
  digitalWrite(resetPin_, HIGH);
  delay(3000);
}

bool EspWiFi::check_esp_response(unsigned long timeout, String response)
{
  char c;
  String buffer;
  bool found = false;
  unsigned long t = millis();

  while (millis() - t < timeout) {
    if (serial_->available() > 0) {
      c = serial_->read();
      buffer += c;

      if (buffer.indexOf(response) != -1 || buffer.equals(response)) {
        found = true;

        break;
      }
    }
  }

  while(serial_->available() > 0) {
    c = serial_->read();
    buffer += c;
  }

  debug(buffer);

  return found;
}

bool EspWiFi::check_esp_response(unsigned long timeout, String response, String altResponse)
{
  char c;
  String buffer;
  bool found = false;
  unsigned long t = millis();

  while (millis() - t < timeout) {
    if (serial_->available() > 0) {
      c = serial_->read();
      buffer += c;

      if (buffer.indexOf(response) != -1 || buffer.equals(response)
        || buffer.indexOf(altResponse) != -1 || buffer.equals(altResponse)) {
        found = true;

        break;
      }
    }
  }

  while(serial_->available() > 0) {
    c = serial_->read();
    buffer += c;
  }

  debug(buffer);

  return found;
}

void EspWiFi::debug(String content)
{
#ifdef DEBUG
  debug_->println(content);
  delay(100);
#endif
}
<|endoftext|>


<commit_before> /*
  * Copyright 2016 Google Inc.
  *
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE file.
  */

#include "Test.h"

#include "SkBitmap.h"
#include "SkImage.h"
#include "SkImageFilter.h"
#include "SkImageFilterCacheKey.h"
#include "SkMatrix.h"
#include "SkSpecialImage.h"

static const int kSmallerSize = 10;
static const int kPad = 3;
static const int kFullSize = kSmallerSize + 2 * kPad;

static SkBitmap create_bm() {
    SkBitmap bm;
    bm.allocN32Pixels(kFullSize, kFullSize, true);
    bm.eraseColor(SK_ColorTRANSPARENT);
    return bm;
}

// Ensure the cache can return a cached image
static void test_find_existing(skiatest::Reporter* reporter,
                               SkSpecialImage* image,
                               SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(0, SkMatrix::I(), clip, subset->uniqueID(), subset->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);

    SkIPoint foundOffset;

    SkSpecialImage* foundImage = cache->get(key1, &foundOffset);
    REPORTER_ASSERT(reporter, foundImage);
    REPORTER_ASSERT(reporter, offset == foundOffset);

    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
}

// If either id is different or the clip or the matrix are different the
// cached image won't be found. Even if it is caching the same bitmap.
static void test_dont_find_if_diff_key(skiatest::Reporter* reporter,
                                       SkSpecialImage* image,
                                       SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip1 = SkIRect::MakeWH(100, 100);
    SkIRect clip2 = SkIRect::MakeWH(200, 200);
    SkImageFilter::Cache::Key key0(0, SkMatrix::I(), clip1, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key1(1, SkMatrix::I(), clip1, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(0, SkMatrix::MakeTrans(5, 5), clip1,
                                   image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key3(0, SkMatrix::I(), clip2, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key4(0, SkMatrix::I(), clip1, subset->uniqueID(), subset->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key0, image, offset);

    SkIPoint foundOffset;
    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key3, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key4, &foundOffset));
}

// Test purging when the max cache size is exceeded
static void test_internal_purge(skiatest::Reporter* reporter, SkSpecialImage* image) {
    SkASSERT(image->getSize());
    static const size_t kCacheSize = image->getSize() + 10;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(1, SkMatrix::I(), clip, image->uniqueID(), image->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);

    SkIPoint foundOffset;

    REPORTER_ASSERT(reporter, cache->get(key1, &foundOffset));

    // This should knock the first one out of the cache
    cache->set(key2, image, offset);

    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
}

// Exercise the purgeByKeys and purge methods
static void test_explicit_purging(skiatest::Reporter* reporter,
                                  SkSpecialImage* image,
                                  SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(1, SkMatrix::I(), clip, subset->uniqueID(), image->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);
    cache->set(key2, image, offset);

    SkIPoint foundOffset;

    REPORTER_ASSERT(reporter, cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));

    cache->purgeByKeys(&key1, 1);

    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));

    cache->purge();

    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
}

DEF_TEST(ImageFilterCache_RasterBacked, reporter) {
    SkBitmap srcBM = create_bm();

    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromRaster(nullptr, full, srcBM));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromRaster(nullptr, subset, srcBM));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}

DEF_TEST(ImageFilterCache_ImageBacked, reporter) {
    SkBitmap srcBM = create_bm();

    SkAutoTUnref<SkImage> srcImage(SkImage::NewFromBitmap(srcBM));

    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromImage(full, srcImage));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromImage(subset, srcImage));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}

#if SK_SUPPORT_GPU
#include "GrContext.h"

DEF_GPUTEST_FOR_RENDERING_CONTEXTS(ImageFilterCache_GPUBacked, reporter, context) {
    SkBitmap srcBM = create_bm();

    GrSurfaceDesc desc;
    desc.fConfig = kSkia8888_GrPixelConfig;
    desc.fFlags  = kNone_GrSurfaceFlags;
    desc.fWidth  = kFullSize;
    desc.fHeight = kFullSize;

    SkAutoTUnref<GrTexture> srcTexture(context->textureProvider()->createTexture(desc, false,
                                                                                srcBM.getPixels(),
                                                                                0));
    if (!srcTexture) {
        return;
    }

    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromGpu(
                                                                nullptr, full,
                                                                kNeedNewImageUniqueID_SpecialImage,
                                                                srcTexture));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromGpu(
                                                                nullptr, subset,
                                                                kNeedNewImageUniqueID_SpecialImage,
                                                                srcTexture));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}
#endif

<commit_msg>Add GPU-backed SkImage case to ImageFilterCache unit test<commit_after> /*
  * Copyright 2016 Google Inc.
  *
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE file.
  */

#include "Test.h"

#include "SkBitmap.h"
#include "SkImage.h"
#include "SkImageFilter.h"
#include "SkImageFilterCacheKey.h"
#include "SkMatrix.h"
#include "SkSpecialImage.h"

static const int kSmallerSize = 10;
static const int kPad = 3;
static const int kFullSize = kSmallerSize + 2 * kPad;

static SkBitmap create_bm() {
    SkBitmap bm;
    bm.allocN32Pixels(kFullSize, kFullSize, true);
    bm.eraseColor(SK_ColorTRANSPARENT);
    return bm;
}

// Ensure the cache can return a cached image
static void test_find_existing(skiatest::Reporter* reporter,
                               SkSpecialImage* image,
                               SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(0, SkMatrix::I(), clip, subset->uniqueID(), subset->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);

    SkIPoint foundOffset;

    SkSpecialImage* foundImage = cache->get(key1, &foundOffset);
    REPORTER_ASSERT(reporter, foundImage);
    REPORTER_ASSERT(reporter, offset == foundOffset);

    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
}

// If either id is different or the clip or the matrix are different the
// cached image won't be found. Even if it is caching the same bitmap.
static void test_dont_find_if_diff_key(skiatest::Reporter* reporter,
                                       SkSpecialImage* image,
                                       SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip1 = SkIRect::MakeWH(100, 100);
    SkIRect clip2 = SkIRect::MakeWH(200, 200);
    SkImageFilter::Cache::Key key0(0, SkMatrix::I(), clip1, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key1(1, SkMatrix::I(), clip1, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(0, SkMatrix::MakeTrans(5, 5), clip1,
                                   image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key3(0, SkMatrix::I(), clip2, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key4(0, SkMatrix::I(), clip1, subset->uniqueID(), subset->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key0, image, offset);

    SkIPoint foundOffset;
    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key3, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key4, &foundOffset));
}

// Test purging when the max cache size is exceeded
static void test_internal_purge(skiatest::Reporter* reporter, SkSpecialImage* image) {
    SkASSERT(image->getSize());
    static const size_t kCacheSize = image->getSize() + 10;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(1, SkMatrix::I(), clip, image->uniqueID(), image->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);

    SkIPoint foundOffset;

    REPORTER_ASSERT(reporter, cache->get(key1, &foundOffset));

    // This should knock the first one out of the cache
    cache->set(key2, image, offset);

    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
}

// Exercise the purgeByKeys and purge methods
static void test_explicit_purging(skiatest::Reporter* reporter,
                                  SkSpecialImage* image,
                                  SkSpecialImage* subset) {
    static const size_t kCacheSize = 1000000;
    SkAutoTUnref<SkImageFilter::Cache> cache(SkImageFilter::Cache::Create(kCacheSize));

    SkIRect clip = SkIRect::MakeWH(100, 100);
    SkImageFilter::Cache::Key key1(0, SkMatrix::I(), clip, image->uniqueID(), image->subset());
    SkImageFilter::Cache::Key key2(1, SkMatrix::I(), clip, subset->uniqueID(), image->subset());

    SkIPoint offset = SkIPoint::Make(3, 4);
    cache->set(key1, image, offset);
    cache->set(key2, image, offset);

    SkIPoint foundOffset;

    REPORTER_ASSERT(reporter, cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));

    cache->purgeByKeys(&key1, 1);

    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, cache->get(key2, &foundOffset));

    cache->purge();

    REPORTER_ASSERT(reporter, !cache->get(key1, &foundOffset));
    REPORTER_ASSERT(reporter, !cache->get(key2, &foundOffset));
}

DEF_TEST(ImageFilterCache_RasterBacked, reporter) {
    SkBitmap srcBM = create_bm();

    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromRaster(nullptr, full, srcBM));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromRaster(nullptr, subset, srcBM));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}

// Shared test code for both the raster and gpu-backed image cases
static void test_image_backed(skiatest::Reporter* reporter, SkImage* srcImage) {
    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromImage(full, srcImage));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromImage(subset, srcImage));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}

DEF_TEST(ImageFilterCache_ImageBackedRaster, reporter) {
    SkBitmap srcBM = create_bm();

    SkAutoTUnref<SkImage> srcImage(SkImage::NewFromBitmap(srcBM));

    test_image_backed(reporter, srcImage);
}

#if SK_SUPPORT_GPU
#include "GrContext.h"

static GrTexture* create_texture(GrContext* context) {
    SkBitmap srcBM = create_bm();

    GrSurfaceDesc desc;
    desc.fConfig = kSkia8888_GrPixelConfig;
    desc.fFlags  = kNone_GrSurfaceFlags;
    desc.fWidth  = kFullSize;
    desc.fHeight = kFullSize;

    return context->textureProvider()->createTexture(desc, false, srcBM.getPixels(), 0);
}

DEF_GPUTEST_FOR_RENDERING_CONTEXTS(ImageFilterCache_ImageBackedGPU, reporter, context) {
    SkAutoTUnref<GrTexture> srcTexture(create_texture(context));
    if (!srcTexture) {
        return;
    }

    GrBackendTextureDesc backendDesc;
    backendDesc.fConfig = kSkia8888_GrPixelConfig;
    backendDesc.fFlags = kNone_GrBackendTextureFlag;
    backendDesc.fWidth = kFullSize;
    backendDesc.fHeight = kFullSize;
    backendDesc.fSampleCnt = 0;
    backendDesc.fTextureHandle = srcTexture->getTextureHandle();
    SkAutoTUnref<SkImage> srcImage(SkImage::NewFromTexture(context, backendDesc,
                                                           kPremul_SkAlphaType));
    if (!srcImage) {
        return;
    }

    test_image_backed(reporter, srcImage);
}

DEF_GPUTEST_FOR_RENDERING_CONTEXTS(ImageFilterCache_GPUBacked, reporter, context) {

    SkAutoTUnref<GrTexture> srcTexture(create_texture(context));
    if (!srcTexture) {
        return;
    }

    const SkIRect& full = SkIRect::MakeWH(kFullSize, kFullSize);

    SkAutoTUnref<SkSpecialImage> fullImg(SkSpecialImage::NewFromGpu(
                                                                nullptr, full,
                                                                kNeedNewImageUniqueID_SpecialImage,
                                                                srcTexture));

    const SkIRect& subset = SkIRect::MakeXYWH(kPad, kPad, kSmallerSize, kSmallerSize);

    SkAutoTUnref<SkSpecialImage> subsetImg(SkSpecialImage::NewFromGpu(
                                                                nullptr, subset,
                                                                kNeedNewImageUniqueID_SpecialImage,
                                                                srcTexture));

    test_find_existing(reporter, fullImg, subsetImg);
    test_dont_find_if_diff_key(reporter, fullImg, subsetImg);
    test_internal_purge(reporter, fullImg);
    test_explicit_purging(reporter, fullImg, subsetImg);
}
#endif

<|endoftext|>


<commit_before>/*
    Queue World is copyright (c) 2014 Ross Bencina

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/
#include "QwMpscFifoQueue.h"

#include "catch.hpp"

namespace {

    struct TestNode{
        TestNode *links_[2];
        enum { LINK_INDEX_1, LINK_COUNT };

        int value;

        TestNode()
            : value( 0 )
        {
            for( int i=0; i < LINK_COUNT; ++i )
                links_[i] = 0;
        }
    };

    typedef QwMPSCFifoQueue<TestNode*, TestNode::LINK_INDEX_1> mpsc_fifo_queue_t;

} // end anonymous namespace

TEST_CASE( "qw/mpsc_fifo_queue", "QwMPSCFifoQueue single threaded test" ) {

    TestNode nodes[3];
    TestNode *a = &nodes[0];
    TestNode *b = &nodes[1];
    TestNode *c = &nodes[2];

    mpsc_fifo_queue_t q;

    REQUIRE( q.consumer_empty() == true );

    // void push( node_ptr_type n )
    // bool consumer_empty() const
    // node_ptr_type pop()

    q.push( a );
    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );
    REQUIRE( q.consumer_empty() == true );

    q.push( a );
    q.push( b );
    q.push( c );

    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );
    REQUIRE( q.pop() == b );
    REQUIRE( q.pop() == c );
    REQUIRE( q.consumer_empty() == true );

    // void push( node_ptr_type n, bool& wasconsumer_empty )

    bool wasconsumer_empty = false;
    q.push( a, wasconsumer_empty );
    REQUIRE( wasconsumer_empty == true );

    q.push( b, wasconsumer_empty );
    REQUIRE( wasconsumer_empty == false );

    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );

    q.push( c, wasconsumer_empty );
    // REQUIRE( wasconsumer_empty == false ); // <--- KNOWNBUG this fails.

    REQUIRE( q.pop() == b );
    REQUIRE( q.pop() == c );
    REQUIRE( q.consumer_empty() == true );
}
<commit_msg>undo find and replace havoc<commit_after>/*
    Queue World is copyright (c) 2014 Ross Bencina

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/
#include "QwMpscFifoQueue.h"

#include "catch.hpp"

namespace {

    struct TestNode{
        TestNode *links_[2];
        enum { LINK_INDEX_1, LINK_COUNT };

        int value;

        TestNode()
            : value( 0 )
        {
            for( int i=0; i < LINK_COUNT; ++i )
                links_[i] = 0;
        }
    };

    typedef QwMPSCFifoQueue<TestNode*, TestNode::LINK_INDEX_1> mpsc_fifo_queue_t;

} // end anonymous namespace

TEST_CASE( "qw/mpsc_fifo_queue", "QwMPSCFifoQueue single threaded test" ) {

    TestNode nodes[3];
    TestNode *a = &nodes[0];
    TestNode *b = &nodes[1];
    TestNode *c = &nodes[2];

    mpsc_fifo_queue_t q;

    REQUIRE( q.consumer_empty() == true );
    REQUIRE( q.pop() == 0 );

    // void push( node_ptr_type n )
    // bool consumer_empty() const
    // node_ptr_type pop()

    q.push( a );
    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );
    REQUIRE( q.consumer_empty() == true );

    q.push( a );
    q.push( b );
    q.push( c );

    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );
    REQUIRE( q.pop() == b );
    REQUIRE( q.pop() == c );
    REQUIRE( q.consumer_empty() == true );

    // void push( node_ptr_type n, bool& wasEmpty )

    bool wasEmpty = false;
    q.push( a, wasEmpty );
    REQUIRE( wasEmpty == true );

    q.push( b, wasEmpty );
    REQUIRE( wasEmpty == false );

    REQUIRE( q.consumer_empty() == false );
    REQUIRE( q.pop() == a );

    q.push( c, wasEmpty );
    // REQUIRE( wasEmpty == false ); // <--- KNOWNBUG this fails.

    REQUIRE( q.pop() == b );
    REQUIRE( q.pop() == c );
    REQUIRE( q.consumer_empty() == true );
}
<|endoftext|>