<commit_before>/**
 * @file uncle.cpp
 * @brief Purpose: Contains uncle's methods.
 *
 * MIT License
 * Copyright (c) 2017 MindScape
 *
 * https://github.com/TecProg2017-2/mindscape/blob/master/LICENSE.md
 */

#include "../include/uncle.hpp"
#include "../include/platform.hpp"
#include "../include/little_girl.hpp"
#include <stdlib.h>

using namespace mindscape;

/**
 * @brief Initializes Uncle enemy.
 *
 * Constructor responsible to initialize Uncle in game.
 *
 * @param name Object's name.
 * @param position X and Y axis positions.
 * @param priority Rendering priority object.
 */

Uncle::Uncle(
  		std::string name,
  		std::pair<int, int> position,
  		int priority
	):Enemy(
    	name,
    	position,
    	priority,
    	100
  	){
        /* Initialize all the characterists of the Uncle's boss. */
    	initialize_state_map();
    	initialize_hitboxes();
    	initialize_animations();
    	initialize_audio_effects();
	};

/**
 * @brief Initialize uncle's audio effects.
 *
 * Not implemented.
 *
 * @return void.
 */

void Uncle::initialize_audio_effects() {
}

/**
 * @brief Initialize Uncle's animations.
 *
 * Define Uncle's movement and attack animation.
 *
 * @return void.
 */

void Uncle::initialize_animations() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing animations.");

  	engine::Animation* idle_animation = nullptr; /**< Animation.
    Animation that represents uncle's in a idle state.s */

    /* Creates uncle's idle animation. */
    idle_animation = create_animation(
      	"../assets/images/sprites/enemies/uncle/uncle_idle.png",
      	1, 4, 3.0, "LEFT"
    );

    /* Sets uncle's idle animation to define when its stopped. */
  	idle_animation->set_values(
      	std::make_pair(665, 484),
      	std::make_pair(665, 484),
      	std::make_pair(0, 0)
    );

    add_animation("idle_animation",idle_animation);

    idle_animation->activate();
  	set_actual_animation(idle_animation);

  	engine::Animation* attacking_animation = nullptr; /**< Animation.
    Animation that represents uncle's attacking. */

    /* Creates uncle's attack animation. */
    attacking_animation = create_animation(
     	"../assets/images/sprites/enemies/uncle/uncle_attacking.png",
      	1, 1, 3.0, "LEFT"
    );

    /* Sets uncle's attacking animation to define when its attacking. */
  	attacking_animation->set_values(
      	std::make_pair(719, 523),
      	std::make_pair(719, 523),
      	std::make_pair(0, 0)
    );

  	attacking_animation->in_loop = false;

 	add_animation("attacking_animation",attacking_animation);

    /* Ends debugger that represents the end of the method. */
    DEBUG("Animations initializated.");
}

/**
 * @brief Creates Uncle's animation.
 *
 * Creates an animation to Uncle enemy.
 *
 * @param path Path to the image which contains the sprite sheet.
 * @param sprite_lines Number of the lines on the sprite sheet.
 * @param sprite_colums Number of colums on the sprite sheet.
 * @param duration Time duration of animation.
 * @param direction Direction of animation.
 * @return This method returns Uncle's animation.
 */

engine::Animation* Uncle::create_animation(
	std::string path,
  	int sprite_lines,
  	int sprite_columns,
  	double duration,
  	std::string direction) {
    DEBUG("Creating animations.");

  	engine::Game& game = engine::Game::get_instance(); /**< Game.
    Gets an instance of a game just initializated. */

    engine::Animation* animation = nullptr; /**< Animation.
    Can represents all functions about uncle's animation. */

    /* Initializes uncle's animation object. */
    animation = new engine::Animation(
    	game.get_renderer(),
    	path,
    	false,
    	std::make_pair(0, 0),
    	1,
    	sprite_lines,
    	sprite_columns,
    	duration,
    	true,
    	direction
  	);

    /* Sets values to init a initial position of animation on the screen.*/
  	animation->set_values(
    	std::make_pair(320, 320),
    	std::make_pair(320, 320),
    	std::make_pair(0, 0)
  	);

    DEBUG("Animations created.");
  	return animation;
}

/**
 * @brief Initialize Uncle's hitboxes.
 *
 * Initializes Uncle instance hitboxes.
 *
 * @return void.
 */

void Uncle::initialize_hitboxes() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing uncle's hitbox.");

  	engine::Game& game = engine::Game::get_instance(); /**< Game.
    Gets an instance of a game just initializated. */

    engine::Hitbox* head_hitbox = nullptr; /**< Hitbox.
    Initialize uncle's head hitbox. */

    head_hitbox = new engine::Hitbox(
    	"head_hitbox",
    	this->get_position(),
    	std::make_pair(160, 380),
    	std::make_pair(180,20),
    	game.get_renderer()
  	);

    /* Add hitbox component into the game object. */
  	add_component(head_hitbox);

    /* Ends debugger that represents the end of the method. */
    DEBUG("Uncle's hitbox initialized");
}

/**
 * @brief Initialize Uncle's state map.
 *
 * Initialize all possibles states for the Uncle.
 *
 * @return void.
 */

void Uncle::initialize_state_map() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing uncle's state.");
  	states.set_state("ACTION_STATE","NORMAL");
    /* Ends debugger that represents the end of the method. */
    DEBUG("Uncle's state initialized.");
}

/**
 * @brief Register game events.
 *
 * Register an event to Uncle.
 *
 * @param game_event Triggering in-game events.
 * @return void.
 */

void Uncle::on_event(GameEvent game_event) {
  	std::string event_name = "";
    event_name = game_event.game_event_name;

  	if (event_name == "MOVE_LEFT"
  		&& !engine::GameObject::on_limit_of_level) {

    	set_position_x(get_position_x() + 10);
  	}
  	else if (event_name == "MOVE_RIGHT"
  		&& !engine::GameObject::on_limit_of_level) {

    	set_position_x(get_position_x() - 10);
  	}
}

/**
 * @brief Notify if the object is in the range.
 *
 * Notify if there is an object in the Uncle's range.
 *
 * @param game_object The game object passed in notification, in this case,
 * the girl.
 * @return void.
 */

void Uncle::notify(engine::Observable *game_object) {
  	LittleGirl* little_girl = nullptr;
    little_girl = dynamic_cast<LittleGirl *>(game_object);

    if (little_girl) {
    	attack(little_girl);
  	}
}

/**
 * @brief Uncle's attack.
 *
 * This method define Uncle's state while is attacking, is on attack or
 * while is dying.
 *
 * @param little_girl Object to be attacked.
 * @return void.
 */

void Uncle::attack(engine::GameObject* little_girl) {
    DEBUG("The uncle is attacking.");

  	std::string actual_action_state = ""; /**< String.
    Gets the actual state of the uncle. */
    actual_action_state = get_state("ACTION_STATE");

  	if (actual_action_state == "DYING") {
        INFO("The Uncle is dying.");
  		return;
  	}

  	if (actual_action_state == "ON_ATTACK"
  		|| actual_action_state == "ATTACKING") {

    	if (get_actual_animation()->is_finished) {
      		states.set_state("ACTION_STATE","NORMAL");
      		set_actual_animation(animations["idle_animation"]);
    	}
    	else {
      		return;
    	}
  	}
}

/**
 * @brief Define a basic attack.
 *
 * Method not implemented.
 *
 * @return void.
 */

void Uncle::basic_attack(){
}

/**
 * @brief Define action when is on attack.
 *
 * Event called when Uncle is attacking.
 *
 * @param game_object Object to be attacked.
 * @return void.
 */

void Uncle::on_attack(engine::GameObject *game_object) {
  	states.set_state("ACTION_STATE","ON_ATTACK");

  	hit(game_object, 1);
  	if (is_alive()) {
    	set_actual_animation(animations["on_attack_animation"]);
    	//play_song("hit_me");
  	}
}

/**
 * @brief Death method.
 *
 * This method define an animation when Uncle is dead.
 *
 * @param game_object Uncle.
 * @return void.
 */

void Uncle::die(engine::GameObject *game_object) {
    DEBUG("The uncle is dying.");
  	std::string actual_x_state = ""; /**< String.
    Gets the actual state of the uncle. */
    actual_x_state = get_state("X_STATE");

    /* Sets a state that represents when uncle is dead. */
    states.set_state("ACTION_STATE", "DYING");
    /* Initiates the animation that show the uncle dying.*/
  	set_actual_animation(animations["dying_animation"]);
  	//play_song("hit_me");
    DEBUG("The uncle died.");
}

/**
 * @brief Event for the collision.
 *
 * Method called everytime when two game objects collides.
 *
 * @param other Other game object that collides.
 * @param p_my_hitbox Hitbox that receives the collision.
 * @param p_other_hitbox Hitbox that collided.
 * @return void.
 */

void Uncle::on_collision(
	engine::GameObject* other,
	engine::Hitbox* p_my_hitbox,
	engine::Hitbox* p_other_hitbox) {

  	LittleGirl* little_girl = nullptr;
    little_girl = dynamic_cast<LittleGirl *>(other);

  	engine::Hitbox* my_hitbox = nullptr;
    my_hitbox = dynamic_cast<engine::Hitbox *>(p_my_hitbox);

  	engine::Hitbox* other_hitbox = nullptr;
    other_hitbox = dynamic_cast<engine::Hitbox *>(p_other_hitbox);

  	if (little_girl
  		&& little_girl->get_state("ACTION_STATE") == "ATTACKING"
  		&& my_hitbox->get_name() == "head_hitbox"
  		&& little_girl->get_actual_animation()->actual_column == 2
     	&& get_state("X_STATE") != little_girl->get_state("X_STATE")) {

      	if(get_state("ACTION_STATE") == "ON_ATTACK") {
      		return;
      	}
	    else {
	    	on_attack(other);
  		}
  	}
}
<commit_msg>[FIX BUGS] Fixes bugs in Uncle.cpp<commit_after>/**
 * @file uncle.cpp
 * @brief Purpose: Contains uncle's methods.
 *
 * MIT License
 * Copyright (c) 2017 MindScape
 *
 * https://github.com/TecProg2017-2/mindscape/blob/master/LICENSE.md
 */

#include "../include/uncle.hpp"
#include "../include/platform.hpp"
#include "../include/little_girl.hpp"
#include "../engine/include/log.hpp"
#include <stdlib.h>

using namespace mindscape;

/**
 * @brief Initializes Uncle enemy.
 *
 * Constructor responsible to initialize Uncle in game.
 *
 * @param name Object's name.
 * @param position X and Y axis positions.
 * @param priority Rendering priority object.
 */

Uncle::Uncle(
  		std::string name,
  		std::pair<int, int> position,
  		int priority
	):Enemy(
    	name,
    	position,
    	priority,
    	100
  	){
        /* Initialize all the characterists of the Uncle's boss. */
    	initialize_state_map();
    	initialize_hitboxes();
    	initialize_animations();
    	initialize_audio_effects();
	};

/**
 * @brief Initialize uncle's audio effects.
 *
 * Not implemented.
 *
 * @return void.
 */

void Uncle::initialize_audio_effects() {
}

/**
 * @brief Initialize Uncle's animations.
 *
 * Define Uncle's movement and attack animation.
 *
 * @return void.
 */

void Uncle::initialize_animations() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing animations.");

  	engine::Animation* idle_animation = nullptr; /**< Animation.
    Animation that represents uncle's in a idle state.s */

    /* Creates uncle's idle animation. */
    idle_animation = create_animation(
      	"../assets/images/sprites/enemies/uncle/uncle_idle.png",
      	1, 4, 3.0, "LEFT"
    );

    /* Sets uncle's idle animation to define when its stopped. */
  	idle_animation->set_values(
      	std::make_pair(665, 484),
      	std::make_pair(665, 484),
      	std::make_pair(0, 0)
    );

    add_animation("idle_animation",idle_animation);

    idle_animation->activate();
  	set_actual_animation(idle_animation);

  	engine::Animation* attacking_animation = nullptr; /**< Animation.
    Animation that represents uncle's attacking. */

    /* Creates uncle's attack animation. */
    attacking_animation = create_animation(
     	"../assets/images/sprites/enemies/uncle/uncle_attacking.png",
      	1, 1, 3.0, "LEFT"
    );

    /* Sets uncle's attacking animation to define when its attacking. */
  	attacking_animation->set_values(
      	std::make_pair(719, 523),
      	std::make_pair(719, 523),
      	std::make_pair(0, 0)
    );

  	attacking_animation->in_loop = false;

 	add_animation("attacking_animation",attacking_animation);

    /* Ends debugger that represents the end of the method. */
    DEBUG("Animations initializated.");
}

/**
 * @brief Creates Uncle's animation.
 *
 * Creates an animation to Uncle enemy.
 *
 * @param path Path to the image which contains the sprite sheet.
 * @param sprite_lines Number of the lines on the sprite sheet.
 * @param sprite_colums Number of colums on the sprite sheet.
 * @param duration Time duration of animation.
 * @param direction Direction of animation.
 * @return This method returns Uncle's animation.
 */

engine::Animation* Uncle::create_animation(
	std::string path,
  	int sprite_lines,
  	int sprite_columns,
  	double duration,
  	std::string direction) {
    DEBUG("Creating animations.");

  	engine::Game& game = engine::Game::get_instance(); /**< Game.
    Gets an instance of a game just initializated. */

    engine::Animation* animation = nullptr; /**< Animation.
    Can represents all functions about uncle's animation. */

    /* Initializes uncle's animation object. */
    animation = new engine::Animation(
    	game.get_renderer(),
    	path,
    	false,
    	std::make_pair(0, 0),
    	1,
    	sprite_lines,
    	sprite_columns,
    	duration,
    	true,
    	direction
  	);

    /* Sets values to init a initial position of animation on the screen.*/
  	animation->set_values(
    	std::make_pair(320, 320),
    	std::make_pair(320, 320),
    	std::make_pair(0, 0)
  	);

    DEBUG("Animations created.");
  	return animation;
}

/**
 * @brief Initialize Uncle's hitboxes.
 *
 * Initializes Uncle instance hitboxes.
 *
 * @return void.
 */

void Uncle::initialize_hitboxes() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing uncle's hitbox.");

  	engine::Game& game = engine::Game::get_instance(); /**< Game.
    Gets an instance of a game just initializated. */

    engine::Hitbox* head_hitbox = nullptr; /**< Hitbox.
    Initialize uncle's head hitbox. */

    head_hitbox = new engine::Hitbox(
    	"head_hitbox",
    	this->get_position(),
    	std::make_pair(160, 380),
    	std::make_pair(180,20),
    	game.get_renderer()
  	);

    /* Add hitbox component into the game object. */
  	add_component(head_hitbox);

    /* Ends debugger that represents the end of the method. */
    DEBUG("Uncle's hitbox initialized");
}

/**
 * @brief Initialize Uncle's state map.
 *
 * Initialize all possibles states for the Uncle.
 *
 * @return void.
 */

void Uncle::initialize_state_map() {
    /* Starts debugger to accompany the method's development. */
    DEBUG("Initializing uncle's state.");
  	states.set_state("ACTION_STATE","NORMAL");
    /* Ends debugger that represents the end of the method. */
    DEBUG("Uncle's state initialized.");
}

/**
 * @brief Register game events.
 *
 * Register an event to Uncle.
 *
 * @param game_event Triggering in-game events.
 * @return void.
 */

void Uncle::on_event(GameEvent game_event) {
  	std::string event_name = "";
    event_name = game_event.game_event_name;

  	if (event_name == "MOVE_LEFT"
  		&& !engine::GameObject::on_limit_of_level) {

    	set_position_x(get_position_x() + 10);
  	}
  	else if (event_name == "MOVE_RIGHT"
  		&& !engine::GameObject::on_limit_of_level) {

    	set_position_x(get_position_x() - 10);
  	}
}

/**
 * @brief Notify if the object is in the range.
 *
 * Notify if there is an object in the Uncle's range.
 *
 * @param game_object The game object passed in notification, in this case,
 * the girl.
 * @return void.
 */

void Uncle::notify(engine::Observable *game_object) {
  	LittleGirl* little_girl = nullptr;
    little_girl = dynamic_cast<LittleGirl *>(game_object);

    if (little_girl) {
    	attack(little_girl);
  	}
}

/**
 * @brief Uncle's attack.
 *
 * This method define Uncle's state while is attacking, is on attack or
 * while is dying.
 *
 * @param little_girl Object to be attacked.
 * @return void.
 */

void Uncle::attack(engine::GameObject* little_girl) {
    DEBUG("The uncle is attacking.");

  	std::string actual_action_state = ""; /**< String.
    Gets the actual state of the uncle. */
    actual_action_state = get_state("ACTION_STATE");

  	if (actual_action_state == "DYING") {
        INFO("The Uncle is dying.");
  		return;
  	}

  	if (actual_action_state == "ON_ATTACK"
  		|| actual_action_state == "ATTACKING") {

    	if (get_actual_animation()->is_finished) {
      		states.set_state("ACTION_STATE","NORMAL");
      		set_actual_animation(animations["idle_animation"]);
    	}
    	else {
      		return;
    	}
  	}
}

/**
 * @brief Define a basic attack.
 *
 * Method not implemented.
 *
 * @return void.
 */

void Uncle::basic_attack(){
}

/**
 * @brief Define action when is on attack.
 *
 * Event called when Uncle is attacking.
 *
 * @param game_object Object to be attacked.
 * @return void.
 */

void Uncle::on_attack(engine::GameObject *game_object) {
  	states.set_state("ACTION_STATE","ON_ATTACK");

  	hit(game_object, 1);
  	if (is_alive()) {
    	set_actual_animation(animations["on_attack_animation"]);
    	//play_song("hit_me");
  	}
}

/**
 * @brief Death method.
 *
 * This method define an animation when Uncle is dead.
 *
 * @param game_object Uncle.
 * @return void.
 */

void Uncle::die(engine::GameObject *game_object) {
    DEBUG("The uncle is dying.");
  	std::string actual_x_state = ""; /**< String.
    Gets the actual state of the uncle. */
    actual_x_state = get_state("X_STATE");

    /* Sets a state that represents when uncle is dead. */
    states.set_state("ACTION_STATE", "DYING");
    /* Initiates the animation that show the uncle dying.*/
  	set_actual_animation(animations["dying_animation"]);
  	//play_song("hit_me");
    DEBUG("The uncle died.");
}

/**
 * @brief Event for the collision.
 *
 * Method called everytime when two game objects collides.
 *
 * @param other Other game object that collides.
 * @param p_my_hitbox Hitbox that receives the collision.
 * @param p_other_hitbox Hitbox that collided.
 * @return void.
 */

void Uncle::on_collision(
	engine::GameObject* other,
	engine::Hitbox* p_my_hitbox,
	engine::Hitbox* p_other_hitbox) {

  	LittleGirl* little_girl = nullptr;
    little_girl = dynamic_cast<LittleGirl *>(other);

  	engine::Hitbox* my_hitbox = nullptr;
    my_hitbox = dynamic_cast<engine::Hitbox *>(p_my_hitbox);

  	engine::Hitbox* other_hitbox = nullptr;
    other_hitbox = dynamic_cast<engine::Hitbox *>(p_other_hitbox);

  	if (little_girl
  		&& little_girl->get_state("ACTION_STATE") == "ATTACKING"
  		&& my_hitbox->get_name() == "head_hitbox"
  		&& little_girl->get_actual_animation()->actual_column == 2
     	&& get_state("X_STATE") != little_girl->get_state("X_STATE")) {

      	if(get_state("ACTION_STATE") == "ON_ATTACK") {
      		return;
      	}
	    else {
	    	on_attack(other);
  		}
  	}
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2019 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 AirspeedValidator.cpp
 * Estimates airspeed scale error (from indicated to calibrated airspeed), performes
 * checks on airspeed measurement input and reports airspeed valid or invalid.
 */

#include "AirspeedValidator.hpp"

void
AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
{
	// get indicated airspeed from input data (raw airspeed)
	_IAS = input_data.airspeed_indicated_raw;

	// to be able to detect missing data, save timestamp (used in data_missing check)
	if (input_data.airspeed_timestamp != _previous_airspeed_timestamp && input_data.airspeed_timestamp > 0) {
		_time_last_airspeed = input_data.timestamp;
		_previous_airspeed_timestamp = input_data.airspeed_timestamp;
	}

	update_CAS_scale();
	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
			      input_data.lpos_vy,
			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
	check_load_factor(input_data.accel_z);
	update_airspeed_valid_status(input_data.timestamp);
}

void
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
		float lpos_vx, float lpos_vy,
		float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
{
	bool att_valid = true; // att_valid could also be a input_data state

	_wind_estimator.update(time_now_usec);

	if (lpos_valid && att_valid && _in_fixed_wing_flight) {

		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
		Quatf q(att_q);

		// airspeed fusion (with raw TAS)
		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)});

		// sideslip fusion
		_wind_estimator.fuse_beta(time_now_usec, vI, q);

	}
}

// this function returns the current states of the wind estimator to be published in the airspeed module
wind_estimate_s
AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
{
	wind_estimate_s wind_est = {};

	wind_est.timestamp = timestamp;
	float wind[2];
	_wind_estimator.get_wind(wind);
	wind_est.windspeed_north = wind[0];
	wind_est.windspeed_east = wind[1];
	float wind_cov[2];
	_wind_estimator.get_wind_var(wind_cov);
	wind_est.variance_north = wind_cov[0];
	wind_est.variance_east = wind_cov[1];
	wind_est.tas_innov = _wind_estimator.get_tas_innov();
	wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
	wind_est.beta_innov = _wind_estimator.get_beta_innov();
	wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
	wind_est.tas_scale = _wind_estimator.get_tas_scale();
	return wind_est;
}

void
AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
{
	_airspeed_scale_manual = airspeed_scale_manual;
	_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
}

void
AirspeedValidator::update_CAS_scale()
{
	if (_wind_estimator.is_estimate_valid()) {
		_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);

	} else {
		_CAS_scale = _airspeed_scale_manual;
	}

}

void
AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
{
	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
	_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
}

void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
		float estimator_status_mag_test_ratio)
{
	// Check normalised innovation levels with requirement for continuous data and use of hysteresis
	// to prevent false triggering.

	if (_wind_estimator.get_wind_estimator_reset()) {
		_time_wind_estimator_initialized = time_now;
	}

	/* Reset states if we are not flying */
	if (!_in_fixed_wing_flight) {
		// not in a flight condition that enables use of this check, thus pass check
		_innovations_check_failed = false;
		_time_last_tas_pass = time_now;
		_time_last_tas_fail = 0;
		_airspeed_valid = true;
		_time_last_aspd_innov_check = time_now;

	} else {
		float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz

		if (dt_s < 1.0f) {
			// Compute the ratio of innovation to gate size
			float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
					       / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.0f) * _wind_estimator.get_tas_innov_var());

			if (tas_test_ratio <= _tas_innov_threshold) {
				// record pass and reset integrator used to trigger
				_time_last_tas_pass = time_now;
				_apsd_innov_integ_state = 0.0f;

			} else {
				// integrate exceedance
				_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold);
			}

			if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) {
				// nav velocity data is likely good so airspeed innovations are able to be used
				if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) {
					_time_last_tas_fail = time_now;
				}
			}

			if (!_innovations_check_failed) {
				_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;

			} else {
				_innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100)
							    || (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100;
			}
		}

		_time_last_aspd_innov_check = time_now;
	}
}

void
AirspeedValidator::check_load_factor(float accel_z)
{
	// Check if the airpeed reading is lower than physically possible given the load factor

	const bool bad_number_fail = false; // disable this for now

	if (_in_fixed_wing_flight) {

		if (!bad_number_fail) {
			float max_lift_ratio = fmaxf(_CAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
			max_lift_ratio *= max_lift_ratio;

			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio;
			_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
			_load_factor_check_failed = (_load_factor_ratio > 1.1f);

		} else {
			_load_factor_check_failed = true; // bad number fail
		}

	} else {

		_load_factor_ratio = 0.5f; // reset if not in fixed-wing flight (and not in takeoff condition)
	}

}

void
AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
{

	const bool bad_number_fail = false; // disable this for now

	// Check if sensor data is missing - assume a minimum 5Hz data rate.
	const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms;

	// Declare data stopped if not received for longer than 1 second
	_data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s;

	if (_innovations_check_failed || _load_factor_check_failed || data_missing || bad_number_fail) {
		// either innovation, load factor or data missing check failed, so declare airspeed failing and record timestamp
		_time_checks_failed = timestamp;
		_airspeed_failing = true;

	} else if (!_innovations_check_failed && !_load_factor_check_failed && !data_missing && !bad_number_fail) {
		// All checks must pass to declare airspeed good
		_time_checks_passed = timestamp;
		_airspeed_failing = false;

	}

	if (_airspeed_valid) {
		// A simultaneous load factor and innovaton check fail makes it more likely that a large
		// airspeed measurement fault has developed, so a fault should be declared immediately
		const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed);

		// Because the innovation, load factor and data missing checks are subject to short duration false positives
		// a timeout period is applied.
		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;

		if (_data_stopped_failed || both_checks_failed || single_check_fail_timeout || bad_number_fail) {

			_airspeed_valid = false;
		}

	} else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
		_airspeed_valid = true;
	}
}
<commit_msg>AirspeedValidator: improve readability<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2019 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 AirspeedValidator.cpp
 * Estimates airspeed scale error (from indicated to calibrated airspeed), performes
 * checks on airspeed measurement input and reports airspeed valid or invalid.
 */

#include "AirspeedValidator.hpp"

void
AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
{
	// get indicated airspeed from input data (raw airspeed)
	_IAS = input_data.airspeed_indicated_raw;

	// to be able to detect missing data, save timestamp (used in data_missing check)
	if (input_data.airspeed_timestamp != _previous_airspeed_timestamp && input_data.airspeed_timestamp > 0) {
		_time_last_airspeed = input_data.timestamp;
		_previous_airspeed_timestamp = input_data.airspeed_timestamp;
	}

	update_CAS_scale();
	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
			      input_data.lpos_vy,
			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
	check_load_factor(input_data.accel_z);
	update_airspeed_valid_status(input_data.timestamp);
}

void
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
		float lpos_vx, float lpos_vy,
		float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
{
	_wind_estimator.update(time_now_usec);

	if (lpos_valid && _in_fixed_wing_flight) {

		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
		Quatf q(att_q);

		// airspeed fusion (with raw TAS)
		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)});

		// sideslip fusion
		_wind_estimator.fuse_beta(time_now_usec, vI, q);
	}
}

// this function returns the current states of the wind estimator to be published in the airspeed module
wind_estimate_s
AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
{
	wind_estimate_s wind_est = {};

	wind_est.timestamp = timestamp;
	float wind[2];
	_wind_estimator.get_wind(wind);
	wind_est.windspeed_north = wind[0];
	wind_est.windspeed_east = wind[1];
	float wind_cov[2];
	_wind_estimator.get_wind_var(wind_cov);
	wind_est.variance_north = wind_cov[0];
	wind_est.variance_east = wind_cov[1];
	wind_est.tas_innov = _wind_estimator.get_tas_innov();
	wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
	wind_est.beta_innov = _wind_estimator.get_beta_innov();
	wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
	wind_est.tas_scale = _wind_estimator.get_tas_scale();
	return wind_est;
}

void
AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
{
	_airspeed_scale_manual = airspeed_scale_manual;
	_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
}

void
AirspeedValidator::update_CAS_scale()
{
	if (_wind_estimator.is_estimate_valid()) {
		_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);

	} else {
		_CAS_scale = _airspeed_scale_manual;
	}
}

void
AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
{
	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
	_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
}

void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
		float estimator_status_mag_test_ratio)
{
	// Check normalised innovation levels with requirement for continuous data and use of hysteresis
	// to prevent false triggering.

	if (_wind_estimator.get_wind_estimator_reset()) {
		_time_wind_estimator_initialized = time_now;
	}

	// reset states if we are not flying
	if (!_in_fixed_wing_flight) {
		_innovations_check_failed = false;
		_time_last_tas_pass = time_now;
		_time_last_tas_fail = 0;
		_airspeed_valid = true;
		_time_last_aspd_innov_check = time_now;

	} else {
		const float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz

		if (dt_s < 1.0f) {
			// compute the ratio of innovation to gate size
			float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
					       / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.0f) * _wind_estimator.get_tas_innov_var());

			if (tas_test_ratio <= _tas_innov_threshold) {
				// record pass and reset integrator used to trigger
				_time_last_tas_pass = time_now;
				_apsd_innov_integ_state = 0.0f;

			} else {
				// integrate exceedance
				_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold);
			}

			if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) {
				// nav velocity data is likely good so airspeed innovations are able to be used
				if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) {
					_time_last_tas_fail = time_now;
				}
			}

			if (!_innovations_check_failed) {
				_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;

			} else {
				_innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100)
							    || (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100;
			}
		}

		_time_last_aspd_innov_check = time_now;
	}
}

void
AirspeedValidator::check_load_factor(float accel_z)
{
	// Check if the airpeed reading is lower than physically possible given the load factor

	if (_in_fixed_wing_flight) {

		float max_lift_ratio = fmaxf(_CAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
		max_lift_ratio *= max_lift_ratio;
		_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio;
		_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
		_load_factor_check_failed = (_load_factor_ratio > 1.1f);

	} else {
		_load_factor_ratio = 0.5f; // reset if not in fixed-wing flight (and not in takeoff condition)
	}
}

void
AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
{
	// Check if sensor data is missing - assume a minimum 5Hz data rate.
	const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms;

	// Declare data stopped if not received for longer than 1 second
	_data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s;

	if (_innovations_check_failed || _load_factor_check_failed || data_missing) {
		// either innovation, load factor or data missing check failed, so declare airspeed failing and record timestamp
		_time_checks_failed = timestamp;
		_airspeed_failing = true;

	} else if (!_innovations_check_failed && !_load_factor_check_failed && !data_missing) {
		// All checks must pass to declare airspeed good
		_time_checks_passed = timestamp;
		_airspeed_failing = false;
	}

	if (_airspeed_valid) {
		// A simultaneous load factor and innovaton check fail makes it more likely that a large
		// airspeed measurement fault has developed, so a fault should be declared immediately
		const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed);

		// Because the innovation, load factor and data missing checks are subject to short duration false positives
		// a timeout period is applied.
		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;

		if (_data_stopped_failed || both_checks_failed || single_check_fail_timeout) {

			_airspeed_valid = false;
		}

	} else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
		_airspeed_valid = true;
	}
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013 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 ecl_pitch_controller.cpp
 * Implementation of a simple orthogonal pitch PID controller.
 *
 * Authors and acknowledgements in header.
 */

#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>

ECL_PitchController::ECL_PitchController() :
	_last_run(0),
	_tc(0.1f),
	_k_p(0.0f),
	_k_i(0.0f),
	_k_ff(0.0f),
	_integrator_max(0.0f),
	_max_rate_pos(0.0f),
	_max_rate_neg(0.0f),
	_roll_ff(0.0f),
	_last_output(0.0f),
	_integrator(0.0f),
	_rate_error(0.0f),
	_rate_setpoint(0.0f),
	_bodyrate_setpoint(0.0f),
	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
{
}

ECL_PitchController::~ECL_PitchController()
{
	perf_free(_nonfinite_input_perf);
}

float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
		perf_count(_nonfinite_input_perf);
		warnx("not controlling pitch");
		return _rate_setpoint;
	}

	/* flying inverted (wings upside down) ? */
	bool inverted = false;

	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */

		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (roll > 0.0f) {
			/* right hemisphere */
			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
		} else {
			/* left hemisphere */
			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* calculate the offset in the rate resulting from rolling  */
	//xxx needs explanation and conversion to body angular rates or should be removed
	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
				tanf(roll) * sinf(roll)) * _roll_ff;
	if (inverted)
		turn_offset = -turn_offset;

	/* Calculate the error */
	float pitch_error = pitch_setpoint - pitch;

	/*  Apply P controller: rate setpoint from current error and time constant */
	_rate_setpoint =  pitch_error / _tc;

	/* add turn offset */
	_rate_setpoint += turn_offset;

	/* limit the rate */ //XXX: move to body angluar rates
	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
		if (_rate_setpoint > 0.0f) {
			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
		} else {
			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
		}

	}

	return _rate_setpoint;
}

float ECL_PitchController::control_bodyrate(float roll, float pitch,
		float pitch_rate, float yaw_rate,
		float yaw_rate_setpoint,
		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
				isfinite(airspeed_max) && isfinite(scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	if (dt_micros > 500000)
		lock_integrator = true;

	/* input conditioning */
	if (!isfinite(airspeed)) {
		/* airspeed is NaN, +- INF or not available, pick center of band */
		airspeed = 0.5f * (airspeed_min + airspeed_max);
	} else if (airspeed < airspeed_min) {
		airspeed = airspeed_min;
	}

	/* Transform setpoint to body angular rates */
	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian

	/* Transform estimation to body angular rates */
	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian

	_rate_error = _bodyrate_setpoint - pitch_bodyrate;

	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {

		float id = _rate_error * dt * scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);
		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * scaler +
		_rate_error * _k_p * scaler * scaler
		+ integrator_constrained;  //scaler is proportional to 1/airspeed
//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
//	warnx("roll: _last_output %.4f", (double)_last_output);
	return math::constrain(_last_output, -1.0f, 1.0f);
}

void ECL_PitchController::reset_integrator()
{
	_integrator = 0.0f;
}
<commit_msg>Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now)<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013 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 ecl_pitch_controller.cpp
 * Implementation of a simple orthogonal pitch PID controller.
 *
 * Authors and acknowledgements in header.
 */

#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>

#ifdef CONFIG_ARCH_ARM
#include <systemlib/err.h>
#else
#include<ros_error.h>
#include <cmath>
#define isfinite std::isfinite
#endif

ECL_PitchController::ECL_PitchController() :
	_last_run(0),
	_tc(0.1f),
	_k_p(0.0f),
	_k_i(0.0f),
	_k_ff(0.0f),
	_integrator_max(0.0f),
	_max_rate_pos(0.0f),
	_max_rate_neg(0.0f),
	_roll_ff(0.0f),
	_last_output(0.0f),
	_integrator(0.0f),
	_rate_error(0.0f),
	_rate_setpoint(0.0f),
	_bodyrate_setpoint(0.0f)
	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
{
}

ECL_PitchController::~ECL_PitchController()
{
	perf_free(_nonfinite_input_perf);
}

float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
		//perf_count(_nonfinite_input_perf);
		warnx("not controlling pitch");
		return _rate_setpoint;
	}

	/* flying inverted (wings upside down) ? */
	bool inverted = false;

	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */

		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (roll > 0.0f) {
			/* right hemisphere */
			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
		} else {
			/* left hemisphere */
			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* calculate the offset in the rate resulting from rolling  */
	//xxx needs explanation and conversion to body angular rates or should be removed
	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
				tanf(roll) * sinf(roll)) * _roll_ff;
	if (inverted)
		turn_offset = -turn_offset;

	/* Calculate the error */
	float pitch_error = pitch_setpoint - pitch;

	/*  Apply P controller: rate setpoint from current error and time constant */
	_rate_setpoint =  pitch_error / _tc;

	/* add turn offset */
	_rate_setpoint += turn_offset;

	/* limit the rate */ //XXX: move to body angluar rates
	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
		if (_rate_setpoint > 0.0f) {
			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
		} else {
			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
		}

	}

	return _rate_setpoint;
}

float ECL_PitchController::control_bodyrate(float roll, float pitch,
		float pitch_rate, float yaw_rate,
		float yaw_rate_setpoint,
		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
				isfinite(airspeed_max) && isfinite(scaler))) {
		//perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	if (dt_micros > 500000)
		lock_integrator = true;

	/* input conditioning */
	if (!isfinite(airspeed)) {
		/* airspeed is NaN, +- INF or not available, pick center of band */
		airspeed = 0.5f * (airspeed_min + airspeed_max);
	} else if (airspeed < airspeed_min) {
		airspeed = airspeed_min;
	}

	/* Transform setpoint to body angular rates */
	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian

	/* Transform estimation to body angular rates */
	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian

	_rate_error = _bodyrate_setpoint - pitch_bodyrate;

	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {

		float id = _rate_error * dt * scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);
		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * scaler +
		_rate_error * _k_p * scaler * scaler
		+ integrator_constrained;  //scaler is proportional to 1/airspeed
//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
//	warnx("roll: _last_output %.4f", (double)_last_output);
	return math::constrain(_last_output, -1.0f, 1.0f);
}

void ECL_PitchController::reset_integrator()
{
	_integrator = 0.0f;
}
<|endoftext|>


<commit_before>#pragma once

#include <string>
#include <functional>
#include <json.hpp>

namespace utils
{
	inline bool TryDumpJson(nlohmann::json const &data, std::string &dest)
	{
		try
		{
			dest = data.dump();
		}
		catch (const json::type_error &e)
		{
			dest = e.what();
			return false;
		}
		return true;
	}

	inline bool IsValidJson(nlohmann::json const &data)
	{
		(void)data; // unused
		return true;
	}

	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		const char *subkey, Ty&&... other);

	// useful when the key can be both Snowflake and null (e.g. voice channel on leave)
	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		nlohmann::json::value_t type, nlohmann::json::value_t type2, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && (jit->type() == type || jit->type() == type2))
			return IsValidJson(data, std::forward<Ty>(other)...);

		return false;
	}

	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		nlohmann::json::value_t type, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == type)
			return IsValidJson(data, std::forward<Ty>(other)...);

		return false;
	}

	// for json objects
	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		const char *subkey, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == nlohmann::json::value_t::object)
			return IsValidJson(*jit, subkey, std::forward<Ty>(other)...);

		return false;
	}

	inline bool TryGetJsonValue(nlohmann::json const &data, bool &dest)
	{
		if (data.type() != nlohmann::json::value_t::boolean)
			return false;

		dest = data.get<bool>();
		return true;
	}

	inline bool TryGetJsonValue(nlohmann::json const &data, std::string &dest)
	{
		if (data.type() != nlohmann::json::value_t::string)
			return false;

		dest = data.get<std::string>();
		return true;
	}

	inline bool TryGetJsonValue(nlohmann::json const &data, float &dest)
	{
		if (data.type() != nlohmann::json::value_t::number_float)
			return false;

		dest = data.get<float>();
		return true;
	}

	template<typename Td>
	typename std::enable_if<std::is_integral<Td>::value, bool>::type
		TryGetJsonValue(nlohmann::json const &data, Td &dest)
	{
		if (data.type() != nlohmann::json::value_t::number_unsigned && data.type() != nlohmann::json::value_t::number_integer)
			return false;

		dest = data.get<Td>();
		return true;
	}

	template<typename... Ty, typename Td>
	bool TryGetJsonValue(nlohmann::json const &data, Td &dest, const char *key)
	{
		auto jit = data.find(key);
		if (jit != data.end())
			return TryGetJsonValue(*jit, dest);

		return false;
	}

	// for json object
	template<typename... Ty, typename Td>
	bool TryGetJsonValue(nlohmann::json const &data, Td &dest, const char *key,
		const char *subkey, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == nlohmann::json::value_t::object)
		{
			return TryGetJsonValue(*jit, dest, subkey,
				std::forward<Ty>(other)...);
		}

		return false;
	}
}
<commit_msg>Refactor & shorten getting JSON values<commit_after>#pragma once

#include <string>
#include <functional>
#include <json.hpp>

namespace utils
{
	inline bool TryDumpJson(nlohmann::json const &data, std::string &dest)
	{
		try
		{
			dest = data.dump();
		}
		catch (const json::type_error &e)
		{
			dest = e.what();
			return false;
		}
		return true;
	}

	inline bool IsValidJson(nlohmann::json const &data)
	{
		(void)data; // unused
		return true;
	}

	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		const char *subkey, Ty&&... other);

	// useful when the key can be both Snowflake and null (e.g. voice channel on leave)
	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		nlohmann::json::value_t type, nlohmann::json::value_t type2, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && (jit->type() == type || jit->type() == type2))
			return IsValidJson(data, std::forward<Ty>(other)...);

		return false;
	}

	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		nlohmann::json::value_t type, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == type)
			return IsValidJson(data, std::forward<Ty>(other)...);

		return false;
	}

	// for json objects
	template<typename... Ty>
	bool IsValidJson(nlohmann::json const &data, const char *key,
		const char *subkey, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == nlohmann::json::value_t::object)
			return IsValidJson(*jit, subkey, std::forward<Ty>(other)...);

		return false;
	}

	template <typename T>
	inline bool TryGetJsonValue(nlohmann::json const &data, T &dest)
	{
		try
		{
			dest = data.get<T>();
		}
		catch (nlohmann::json::type_error)
		{
			return false;
		}
		return true;
	}

	template<typename... Ty, typename Td>
	inline bool TryGetJsonValue(nlohmann::json const &data, Td &dest, const char *key)
	{
		auto jit = data.find(key);
		if (jit != data.end())
			return TryGetJsonValue(*jit, dest);

		return false;
	}

	// for json object
	template<typename... Ty, typename Td>
	inline bool TryGetJsonValue(nlohmann::json const &data, Td &dest, const char *key, const char *subkey, Ty&&... other)
	{
		auto jit = data.find(key);
		if (jit != data.end() && jit->type() == nlohmann::json::value_t::object)
		{
			return TryGetJsonValue(*jit, dest, subkey,
				std::forward<Ty>(other)...);
		}
		return false;
	}
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2016-2017 mvs developers
 *
 * This file is part of metaverse-explorer.
 *
 * metaverse-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <metaverse/explorer/json_helper.hpp>
#include <metaverse/explorer/dispatch.hpp>
#include <metaverse/explorer/extensions/commands/listtxs.hpp>
#include <metaverse/explorer/extensions/command_extension_func.hpp>
#include <metaverse/explorer/extensions/command_assistant.hpp>
#include <metaverse/explorer/extensions/exception.hpp>

namespace libbitcoin {
namespace explorer {
namespace commands {
using namespace bc::explorer::config;

class BC_API tx_block_info
{
public:
    tx_block_info(uint64_t height, uint32_t timestamp, hash_digest hash):
        height_(height), timestamp_(timestamp), hash_(hash)
    {}
    uint64_t get_height() {
        return height_;
    }
    uint32_t get_timestamp() {
        return timestamp_;
    }
    hash_digest get_hash(){
        return hash_;
    }
    bool operator<(const tx_block_info & rinfo) const
    {
        return hash_ < const_cast<tx_block_info&>(rinfo).get_hash();
    }
    bool operator==(const tx_block_info& rinfo) const
    {
        return hash_ == const_cast<tx_block_info&>(rinfo).get_hash();
    }

private:
    uint64_t height_;
    uint32_t timestamp_;
    hash_digest hash_;
};

/************************ listtxs *************************/

console_result listtxs::invoke (Json::Value& jv_output,
         libbitcoin::server::server_node& node)
{
    using namespace libbitcoin::config; // for hash256
    auto& blockchain = node.chain_impl();
    blockchain.is_account_passwd_valid(auth_.name, auth_.auth);
    // address option check
    if (!argument_.address.empty() && !blockchain.is_valid_address(argument_.address))
        throw address_invalid_exception{"invalid address parameter!"};
    // height check
    if(option_.height.first()
        && option_.height.second()
        && (option_.height.first() >= option_.height.second())) {
        throw block_height_exception{"invalid height option!"};
    }
    // symbol check
    if(!argument_.symbol.empty()) {
        blockchain.uppercase_symbol(argument_.symbol);
        if(!blockchain.get_issued_asset(argument_.symbol))
            throw asset_symbol_notfound_exception{argument_.symbol + std::string(" not exist!")};
    }

    auto& aroot = jv_output;
    Json::Value balances;

    auto sort_by_height = [](const tx_block_info &lhs, const tx_block_info &rhs)->bool {
        return const_cast<tx_block_info&>(lhs).get_height() > const_cast<tx_block_info&>(rhs).get_height();
    };

    auto sh_txs = std::make_shared<std::vector<tx_block_info>>();
    auto sh_addr_vec = std::make_shared<std::vector<std::string>>();

    // collect address
    if(argument_.address.empty()) {
        auto pvaddr = blockchain.get_account_addresses(auth_.name);
        if(!pvaddr)
            throw address_invalid_exception{"nullptr for address list"};

        for (auto& elem: *pvaddr) {
            sh_addr_vec->push_back(elem.get_address());
        }
    } else { // address exist in command
        sh_addr_vec->push_back(argument_.address);
    }

    // scan all addresses business record
    for (auto& each: *sh_addr_vec) {
        auto sh_vec = blockchain.get_address_business_record(each, argument_.symbol,
                option_.height.first(), option_.height.second(), 0, 0);
        for(auto& elem : *sh_vec)
            sh_txs->push_back(tx_block_info(elem.height, elem.data.get_timestamp(), elem.point.hash));
    }
    std::sort (sh_txs->begin(), sh_txs->end());
    sh_txs->erase(std::unique(sh_txs->begin(), sh_txs->end()), sh_txs->end());
    std::sort (sh_txs->begin(), sh_txs->end(), sort_by_height);

    // page limit & page index paramenter check
    if(!argument_.index)
        throw argument_legality_exception{"page index parameter must not be zero"};
    if(!argument_.limit)
        throw argument_legality_exception{"page record limit parameter must not be zero"};
    if(argument_.limit > 100)
        throw argument_legality_exception{"page record limit must not be bigger than 100."};

    uint64_t start, end, total_page, tx_count;
    if(argument_.index && argument_.limit) {
        start = (argument_.index - 1)*argument_.limit;
        end = (argument_.index)*argument_.limit;
        if(start >= sh_txs->size() || !sh_txs->size())
            throw argument_legality_exception{"no record in this page"};

        total_page = sh_txs->size() % argument_.limit ? (sh_txs->size()/argument_.limit + 1) : (sh_txs->size()/argument_.limit);
        tx_count = end >=sh_txs->size()? (sh_txs->size() - start) : argument_.limit ;

    } else if(!argument_.index && !argument_.limit) { // all tx records
        start = 0;
        tx_count = sh_txs->size();
        argument_.index = 1;
        total_page = 1;
    } else {
        throw argument_legality_exception{"invalid limit or index parameter"};
    }

    // sort by height
    std::vector<tx_block_info> result(sh_txs->begin() + start, sh_txs->begin() + start + tx_count);

    // fetch tx according its hash
    std::vector<std::string> vec_ip_addr; // input addr
    chain::transaction tx;
    uint64_t tx_height;
    //hash_digest trans_hash;
    for (auto& each: result){
        //decode_hash(trans_hash, each.hash);
        if(!blockchain.get_transaction(each.get_hash(), tx, tx_height))
            continue;

        Json::Value tx_item;
        tx_item["hash"] = encode_hash(each.get_hash());
        if (get_api_version() == 1) {
            tx_item["height"] += each.get_height();
            tx_item["timestamp"] += each.get_timestamp();
        } else {
            tx_item["height"] = each.get_height();
            tx_item["timestamp"] = each.get_timestamp();
        }
        tx_item["direction"] = "send";

        // set inputs content
        Json::Value input_addrs;
        for(auto& input : tx.inputs) {
            Json::Value input_addr;

            auto&& script_address = payment_address::extract(input.script);
            if (script_address) {
                auto&& temp_addr = script_address.encoded();
                input_addr["address"] = temp_addr;
                // add input address
                vec_ip_addr.push_back(temp_addr);
            } else {
                // empty input address : coin base tx;
                if (get_api_version() == 1)
                    input_addr["address"] = "";
                else
                    input_addr["address"] = Json::nullValue;
            }

            input_addr["script"] = input.script.to_string(1);
            input_addrs.append(input_addr);

        }

        if (get_api_version() == 1 && input_addrs.isNull()) { // compatible for v1
            tx_item["inputs"] = "";
        } else {
            tx_item["inputs"] = input_addrs;
        }

        // set outputs content
        Json::Value pt_outputs;
        uint64_t lock_height = 0;
        for(auto& op : tx.outputs) {
            Json::Value pt_output;

            auto&& address = payment_address::extract(op.script);
            if (address) {
                auto&& temp_addr = address.encoded();
                auto ret = blockchain.get_account_address(auth_.name, temp_addr);
                if(get_api_version() == 1)
                    pt_output["own"] = ret ? "true" : "false";
                else
                    pt_output["own"] = ret ? true : false;

            } else {
                // empty output address ? unbelievable.
                if(get_api_version() == 1)
                    pt_output["address"] = "";
                else
                    pt_output["address"] = Json::nullValue;
            }

            pt_output["script"] = op.script.to_string(1);
            lock_height = 0;
            if(chain::operation::is_pay_key_hash_with_lock_height_pattern(op.script.operations))
                lock_height = chain::operation::get_lock_height_from_pay_key_hash_with_lock_height(op.script.operations);

            if (get_api_version() == 1) {
                pt_output["locked_height_range"] += lock_height;
                pt_output["etp-value"] += op.value;
            } else {
                pt_output["locked_height_range"] = lock_height;
                pt_output["etp-value"] = op.value;
            }

            auto attach_data = op.attach_data;
            Json::Value tree;
            if(attach_data.get_type() == ETP_TYPE) {
                tree["type"] = "etp";
            } else if(attach_data.get_type() == ASSET_TYPE) {
                auto asset_info = boost::get<bc::chain::asset>(attach_data.get_attach());
                if(asset_info.get_status() == ASSET_DETAIL_TYPE) {
                    tree["type"] = "asset-issue";
                    auto detail_info = boost::get<bc::chain::asset_detail>(asset_info.get_data());
                    tree["symbol"] = detail_info.get_symbol();
                    if (get_api_version() == 1) {
                        tree["maximum_supply"] += detail_info.get_maximum_supply();
                        tree["decimal_number"] += detail_info.get_decimal_number();
                    } else {
                        tree["maximum_supply"] = detail_info.get_maximum_supply();
                        tree["decimal_number"] = detail_info.get_decimal_number();
                    }
                    tree["issuer"] = detail_info.get_issuer();
                    tree["address"] = detail_info.get_address();
                    tree["description"] = detail_info.get_description();
                }
                if(asset_info.get_status() == ASSET_TRANSFERABLE_TYPE) {
                    tree["type"] = "asset-transfer";
                    auto trans_info = boost::get<bc::chain::asset_transfer>(asset_info.get_data());
                    tree["symbol"] = trans_info.get_address();
                    tree["quantity"] = trans_info.get_quantity();
                    auto symbol = trans_info.get_address();
                    auto issued_asset = blockchain.get_issued_asset(symbol);
                    if(issued_asset && get_api_version() == 1) {
                        tree["decimal_number"] += issued_asset->get_decimal_number();
                    }
                    if(issued_asset && get_api_version() == 2) {
                        tree["decimal_number"] = issued_asset->get_decimal_number();
                    }
                }
            } else if(attach_data.get_type() == MESSAGE_TYPE) {
                tree["type"] = "message";
                auto msg_info = boost::get<bc::chain::blockchain_message>(attach_data.get_attach());
                tree["content"] = msg_info.get_content();
            } else {
                tree["type"] = "unknown business";
            }
            pt_output["attachment"] = tree;
            ////////////////////////////////////////////////////////////

            pt_outputs.append(pt_output);

        }

        if (get_api_version() == 1 && pt_outputs.isNull()) { // compatible for v1
            tx_item["outputs"] = "";
        } else {
            tx_item["outputs"] = pt_outputs;
        }

        // set tx direction
        // 1. receive check
        auto pos = std::find_if(vec_ip_addr.begin(), vec_ip_addr.end(), [&](const std::string& i){
                return blockchain.get_account_address(auth_.name, i) != nullptr;
                });

        if (pos == vec_ip_addr.end()){
            tx_item["direction"] = "receive";
        }
        // 2. transfer check
    #if 0
        auto is_ip_intern = true;
        auto is_op_intern = true;

        if(vec_ip_addr.empty())
            is_ip_intern = false;
        for(auto& each : vec_ip_addr) {
            if(!blockchain.get_account_address(auth_.name, each))
                is_ip_intern = false;
        }

        for(auto& each : vec_op_addr) {
            if(!blockchain.get_account_address(auth_.name, each))
                is_op_intern = false;
        }

        if (is_ip_intern && is_ip_intern){
            tx_item["direction"] = "transfer";
        }
    #endif
        // 3. all address clear
        vec_ip_addr.clear();
        balances.append(tx_item);
    }

    if (get_api_version() == 1) {
        aroot["total_page"] += total_page;
        aroot["current_page"] += argument_.index;
        aroot["transaction_count"] += tx_count;
    } else {
        aroot["total_page"] = total_page;
        aroot["current_page"] = argument_.index;
        aroot["transaction_count"] = tx_count;
    }

    if (get_api_version() == 1 && balances.isNull()) { // compatible for v1
        aroot["transactions"] = "";
    } else {
        aroot["transactions"] = balances;
    }

    return console_result::okay;
}
} // namespace commands
} // namespace explorer
} // namespace libbitcoin
<commit_msg>fix listtxs quantity/output address<commit_after>/**
 * Copyright (c) 2016-2017 mvs developers
 *
 * This file is part of metaverse-explorer.
 *
 * metaverse-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <metaverse/explorer/json_helper.hpp>
#include <metaverse/explorer/dispatch.hpp>
#include <metaverse/explorer/extensions/commands/listtxs.hpp>
#include <metaverse/explorer/extensions/command_extension_func.hpp>
#include <metaverse/explorer/extensions/command_assistant.hpp>
#include <metaverse/explorer/extensions/exception.hpp>

namespace libbitcoin {
namespace explorer {
namespace commands {
using namespace bc::explorer::config;

class BC_API tx_block_info
{
public:
    tx_block_info(uint64_t height, uint32_t timestamp, hash_digest hash):
        height_(height), timestamp_(timestamp), hash_(hash)
    {}
    uint64_t get_height() {
        return height_;
    }
    uint32_t get_timestamp() {
        return timestamp_;
    }
    hash_digest get_hash(){
        return hash_;
    }
    bool operator<(const tx_block_info & rinfo) const
    {
        return hash_ < const_cast<tx_block_info&>(rinfo).get_hash();
    }
    bool operator==(const tx_block_info& rinfo) const
    {
        return hash_ == const_cast<tx_block_info&>(rinfo).get_hash();
    }

private:
    uint64_t height_;
    uint32_t timestamp_;
    hash_digest hash_;
};

/************************ listtxs *************************/

console_result listtxs::invoke (Json::Value& jv_output,
         libbitcoin::server::server_node& node)
{
    using namespace libbitcoin::config; // for hash256
    auto& blockchain = node.chain_impl();
    blockchain.is_account_passwd_valid(auth_.name, auth_.auth);
    // address option check
    if (!argument_.address.empty() && !blockchain.is_valid_address(argument_.address))
        throw address_invalid_exception{"invalid address parameter!"};
    // height check
    if(option_.height.first()
        && option_.height.second()
        && (option_.height.first() >= option_.height.second())) {
        throw block_height_exception{"invalid height option!"};
    }
    // symbol check
    if(!argument_.symbol.empty()) {
        blockchain.uppercase_symbol(argument_.symbol);
        if(!blockchain.get_issued_asset(argument_.symbol))
            throw asset_symbol_notfound_exception{argument_.symbol + std::string(" not exist!")};
    }

    auto& aroot = jv_output;
    Json::Value balances;

    auto sort_by_height = [](const tx_block_info &lhs, const tx_block_info &rhs)->bool {
        return const_cast<tx_block_info&>(lhs).get_height() > const_cast<tx_block_info&>(rhs).get_height();
    };

    auto sh_txs = std::make_shared<std::vector<tx_block_info>>();
    auto sh_addr_vec = std::make_shared<std::vector<std::string>>();

    // collect address
    if(argument_.address.empty()) {
        auto pvaddr = blockchain.get_account_addresses(auth_.name);
        if(!pvaddr)
            throw address_invalid_exception{"nullptr for address list"};

        for (auto& elem: *pvaddr) {
            sh_addr_vec->push_back(elem.get_address());
        }
    } else { // address exist in command
        sh_addr_vec->push_back(argument_.address);
    }

    // scan all addresses business record
    for (auto& each: *sh_addr_vec) {
        auto sh_vec = blockchain.get_address_business_record(each, argument_.symbol,
                option_.height.first(), option_.height.second(), 0, 0);
        for(auto& elem : *sh_vec)
            sh_txs->push_back(tx_block_info(elem.height, elem.data.get_timestamp(), elem.point.hash));
    }
    std::sort (sh_txs->begin(), sh_txs->end());
    sh_txs->erase(std::unique(sh_txs->begin(), sh_txs->end()), sh_txs->end());
    std::sort (sh_txs->begin(), sh_txs->end(), sort_by_height);

    // page limit & page index paramenter check
    if(!argument_.index)
        throw argument_legality_exception{"page index parameter must not be zero"};
    if(!argument_.limit)
        throw argument_legality_exception{"page record limit parameter must not be zero"};
    if(argument_.limit > 100)
        throw argument_legality_exception{"page record limit must not be bigger than 100."};

    uint64_t start, end, total_page, tx_count;
    if(argument_.index && argument_.limit) {
        start = (argument_.index - 1)*argument_.limit;
        end = (argument_.index)*argument_.limit;
        if(start >= sh_txs->size() || !sh_txs->size())
            throw argument_legality_exception{"no record in this page"};

        total_page = sh_txs->size() % argument_.limit ? (sh_txs->size()/argument_.limit + 1) : (sh_txs->size()/argument_.limit);
        tx_count = end >=sh_txs->size()? (sh_txs->size() - start) : argument_.limit ;

    } else if(!argument_.index && !argument_.limit) { // all tx records
        start = 0;
        tx_count = sh_txs->size();
        argument_.index = 1;
        total_page = 1;
    } else {
        throw argument_legality_exception{"invalid limit or index parameter"};
    }

    // sort by height
    std::vector<tx_block_info> result(sh_txs->begin() + start, sh_txs->begin() + start + tx_count);

    // fetch tx according its hash
    std::vector<std::string> vec_ip_addr; // input addr
    chain::transaction tx;
    uint64_t tx_height;
    //hash_digest trans_hash;
    for (auto& each: result){
        //decode_hash(trans_hash, each.hash);
        if(!blockchain.get_transaction(each.get_hash(), tx, tx_height))
            continue;

        Json::Value tx_item;
        tx_item["hash"] = encode_hash(each.get_hash());
        if (get_api_version() == 1) {
            tx_item["height"] += each.get_height();
            tx_item["timestamp"] += each.get_timestamp();
        } else {
            tx_item["height"] = each.get_height();
            tx_item["timestamp"] = each.get_timestamp();
        }
        tx_item["direction"] = "send";

        // set inputs content
        Json::Value input_addrs;
        for(auto& input : tx.inputs) {
            Json::Value input_addr;

            auto&& script_address = payment_address::extract(input.script);
            if (script_address) {
                auto&& temp_addr = script_address.encoded();
                input_addr["address"] = temp_addr;
                // add input address
                vec_ip_addr.push_back(temp_addr);
            } else {
                // empty input address : coin base tx;
                if (get_api_version() == 1)
                    input_addr["address"] = "";
                else
                    input_addr["address"] = Json::nullValue;
            }

            input_addr["script"] = input.script.to_string(1);
            input_addrs.append(input_addr);

        }

        if (get_api_version() == 1 && input_addrs.isNull()) { // compatible for v1
            tx_item["inputs"] = "";
        } else {
            tx_item["inputs"] = input_addrs;
        }

        // set outputs content
        Json::Value pt_outputs;
        uint64_t lock_height = 0;
        for(auto& op : tx.outputs) {
            Json::Value pt_output;

            auto&& address = payment_address::extract(op.script);
            if (address) {
                auto&& temp_addr = address.encoded();
                pt_output["address"] = temp_addr;
                auto ret = blockchain.get_account_address(auth_.name, temp_addr);
                if(get_api_version() == 1)
                    pt_output["own"] = ret ? "true" : "false";
                else
                    pt_output["own"] = ret ? true : false;

            } else {
                // empty output address ? unbelievable.
                if(get_api_version() == 1)
                    pt_output["address"] = "";
                else
                    pt_output["address"] = Json::nullValue;
            }

            pt_output["script"] = op.script.to_string(1);
            lock_height = 0;
            if(chain::operation::is_pay_key_hash_with_lock_height_pattern(op.script.operations))
                lock_height = chain::operation::get_lock_height_from_pay_key_hash_with_lock_height(op.script.operations);

            if (get_api_version() == 1) {
                pt_output["locked_height_range"] += lock_height;
                pt_output["etp-value"] += op.value;
            } else {
                pt_output["locked_height_range"] = lock_height;
                pt_output["etp-value"] = op.value;
            }

            auto attach_data = op.attach_data;
            Json::Value tree;
            if(attach_data.get_type() == ETP_TYPE) {
                tree["type"] = "etp";
            } else if(attach_data.get_type() == ASSET_TYPE) {
                auto asset_info = boost::get<bc::chain::asset>(attach_data.get_attach());
                if(asset_info.get_status() == ASSET_DETAIL_TYPE) {
                    tree["type"] = "asset-issue";
                    auto detail_info = boost::get<bc::chain::asset_detail>(asset_info.get_data());
                    tree["symbol"] = detail_info.get_symbol();
                    if (get_api_version() == 1) {
                        tree["maximum_supply"] += detail_info.get_maximum_supply();
                        tree["decimal_number"] += detail_info.get_decimal_number();
                    } else {
                        tree["maximum_supply"] = detail_info.get_maximum_supply();
                        tree["decimal_number"] = detail_info.get_decimal_number();
                    }
                    tree["issuer"] = detail_info.get_issuer();
                    tree["address"] = detail_info.get_address();
                    tree["description"] = detail_info.get_description();
                }
                if(asset_info.get_status() == ASSET_TRANSFERABLE_TYPE) {

                    tree["type"] = "asset-transfer";
                    auto trans_info = boost::get<bc::chain::asset_transfer>(asset_info.get_data());
                    tree["symbol"] = trans_info.get_address();

                    if (get_api_version() == 1) {
                        tree["quantity"] += trans_info.get_quantity();
                    } else {
                        tree["quantity"] = trans_info.get_quantity();
                    }

                    auto symbol = trans_info.get_address();
                    auto issued_asset = blockchain.get_issued_asset(symbol);

                    if(issued_asset && get_api_version() == 1) {
                        tree["decimal_number"] += issued_asset->get_decimal_number();
                    }
                    if(issued_asset && get_api_version() == 2) {
                        tree["decimal_number"] = issued_asset->get_decimal_number();
                    }
                }
            } else if(attach_data.get_type() == MESSAGE_TYPE) {
                tree["type"] = "message";
                auto msg_info = boost::get<bc::chain::blockchain_message>(attach_data.get_attach());
                tree["content"] = msg_info.get_content();
            } else {
                tree["type"] = "unknown business";
            }
            pt_output["attachment"] = tree;
            ////////////////////////////////////////////////////////////

            pt_outputs.append(pt_output);

        }

        if (get_api_version() == 1 && pt_outputs.isNull()) { // compatible for v1
            tx_item["outputs"] = "";
        } else {
            tx_item["outputs"] = pt_outputs;
        }

        // set tx direction
        // 1. receive check
        auto pos = std::find_if(vec_ip_addr.begin(), vec_ip_addr.end(), [&](const std::string& i){
                return blockchain.get_account_address(auth_.name, i) != nullptr;
                });

        if (pos == vec_ip_addr.end()){
            tx_item["direction"] = "receive";
        }
        // 3. all address clear
        vec_ip_addr.clear();
        balances.append(tx_item);
    }

    if (get_api_version() == 1) {
        aroot["total_page"] += total_page;
        aroot["current_page"] += argument_.index;
        aroot["transaction_count"] += tx_count;
    } else {
        aroot["total_page"] = total_page;
        aroot["current_page"] = argument_.index;
        aroot["transaction_count"] = tx_count;
    }

    if (get_api_version() == 1 && balances.isNull()) { // compatible for v1
        aroot["transactions"] = "";
    } else {
        aroot["transactions"] = balances;
    }

    return console_result::okay;
}
} // namespace commands
} // namespace explorer
} // namespace libbitcoin
<|endoftext|>