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

/**
 * @file voted_sensors_update.cpp
 *
 * @author <NAME> <<EMAIL>>
 */

#include "voted_sensors_update.h"

#include <systemlib/mavlink_log.h>

#include <conversion/rotation.h>

#define MAG_ROT_VAL_INTERNAL		-1
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"


using namespace sensors;
using namespace DriverFramework;


const double VotedSensorsUpdate::_msl_pressure = 101.325f;

VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
	: _parameters(parameters)
{
	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
	memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp));
	memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp));
	memset(&_accel_diff, 0, sizeof(_accel_diff));
	memset(&_gyro_diff, 0, sizeof(_gyro_diff));

	// initialise the corrections
	memset(&_corrections, 0, sizeof(_corrections));

	for (unsigned i = 0; i < 3; i++) {
		_corrections.gyro_scale_0[i] = 1.0f;
		_corrections.accel_scale_0[i] = 1.0f;
		_corrections.gyro_scale_1[i] = 1.0f;
		_corrections.accel_scale_1[i] = 1.0f;
		_corrections.gyro_scale_2[i] = 1.0f;
		_corrections.accel_scale_2[i] = 1.0f;

		for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
			_accel_scale[j][i] = 1.0f;
			_gyro_scale[j][i] = 1.0f;
			_baro_scale[j] = 1.0f;
		}
	}

	_corrections.baro_scale_0 = 1.0f;
	_corrections.baro_scale_1 = 1.0f;
	_corrections.baro_scale_2 = 1.0f;

	memset(&_accel_offset, 0, sizeof(_accel_offset));
	memset(&_gyro_offset, 0, sizeof(_gyro_offset));
	memset(&_baro_offset, 0, sizeof(_baro_offset));

	_baro.voter.set_timeout(300000);
	_mag.voter.set_timeout(300000);
	_mag.voter.set_equal_value_threshold(1000);
}

int VotedSensorsUpdate::init(sensor_combined_s &raw)
{
	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.timestamp = 0;

	initialize_sensors();

	_corrections_changed = true; //make sure to initially publish the corrections topic

	return 0;
}

void VotedSensorsUpdate::initialize_sensors()
{
	init_sensor_class(ORB_ID(sensor_gyro), _gyro);
	init_sensor_class(ORB_ID(sensor_mag), _mag);
	init_sensor_class(ORB_ID(sensor_accel), _accel);
	init_sensor_class(ORB_ID(sensor_baro), _baro);
}

void VotedSensorsUpdate::deinit()
{
	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
		orb_unsubscribe(_gyro.subscription[i]);
	}

	for (unsigned i = 0; i < _accel.subscription_count; i++) {
		orb_unsubscribe(_accel.subscription[i]);
	}

	for (unsigned i = 0; i < _mag.subscription_count; i++) {
		orb_unsubscribe(_mag.subscription[i]);
	}

	for (unsigned i = 0; i < _baro.subscription_count; i++) {
		orb_unsubscribe(_baro.subscription[i]);
	}
}

void VotedSensorsUpdate::parameters_update()
{
	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
	/* fine tune board offset */
	math::Matrix<3, 3> board_rotation_offset;
	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
					 M_DEG_TO_RAD_F * _parameters.board_offset[1],
					 M_DEG_TO_RAD_F * _parameters.board_offset[2]);

	_board_rotation = board_rotation_offset * _board_rotation;

	/* Load & apply the sensor calibrations.
	 * IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
	 */

	/* temperature compensation */
	_temperature_compensation.parameters_update();

	for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) {

		/* gyro */
		if (topic_instance < _gyro.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct gyro_report report;

			if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
				if (_temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance) < 0) {
					PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
				}
			}
		}

		/* accel */
		if (topic_instance < _accel.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct accel_report report;

			if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
				if (_temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance) < 0) {
					PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
				}
			}
		}

		/* baro */
		if (topic_instance < _baro.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct baro_report report;

			if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
				if (_temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance) < 0) {
					PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
				}
			}
		}
	}


	/* set offset parameters to new values */
	bool failed;
	char str[30];
	unsigned mag_count = 0;
	unsigned gyro_count = 0;
	unsigned accel_count = 0;
	unsigned gyro_cal_found_count = 0;
	unsigned accel_cal_found_count = 0;

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

/**
 * @file voted_sensors_update.cpp
 *
 * @author <NAME> <<EMAIL>>
 */

#include "voted_sensors_update.h"

#include <systemlib/mavlink_log.h>

#include <conversion/rotation.h>

#define MAG_ROT_VAL_INTERNAL		-1
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"


using namespace sensors;
using namespace DriverFramework;


const double VotedSensorsUpdate::_msl_pressure = 101.325f;

VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
	: _parameters(parameters)
{
	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
	memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp));
	memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp));
	memset(&_accel_diff, 0, sizeof(_accel_diff));
	memset(&_gyro_diff, 0, sizeof(_gyro_diff));

	// initialise the corrections
	memset(&_corrections, 0, sizeof(_corrections));

	for (unsigned i = 0; i < 3; i++) {
		_corrections.gyro_scale_0[i] = 1.0f;
		_corrections.accel_scale_0[i] = 1.0f;
		_corrections.gyro_scale_1[i] = 1.0f;
		_corrections.accel_scale_1[i] = 1.0f;
		_corrections.gyro_scale_2[i] = 1.0f;
		_corrections.accel_scale_2[i] = 1.0f;

		for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
			_accel_scale[j][i] = 1.0f;
			_gyro_scale[j][i] = 1.0f;
			_baro_scale[j] = 1.0f;
		}
	}

	_corrections.baro_scale_0 = 1.0f;
	_corrections.baro_scale_1 = 1.0f;
	_corrections.baro_scale_2 = 1.0f;

	memset(&_accel_offset, 0, sizeof(_accel_offset));
	memset(&_gyro_offset, 0, sizeof(_gyro_offset));
	memset(&_baro_offset, 0, sizeof(_baro_offset));

	_baro.voter.set_timeout(300000);
	_mag.voter.set_timeout(300000);
	_mag.voter.set_equal_value_threshold(1000);
}

int VotedSensorsUpdate::init(sensor_combined_s &raw)
{
	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
	raw.timestamp = 0;

	initialize_sensors();

	_corrections_changed = true; //make sure to initially publish the corrections topic

	return 0;
}

void VotedSensorsUpdate::initialize_sensors()
{
	init_sensor_class(ORB_ID(sensor_gyro), _gyro);
	init_sensor_class(ORB_ID(sensor_mag), _mag);
	init_sensor_class(ORB_ID(sensor_accel), _accel);
	init_sensor_class(ORB_ID(sensor_baro), _baro);
}

void VotedSensorsUpdate::deinit()
{
	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
		orb_unsubscribe(_gyro.subscription[i]);
	}

	for (unsigned i = 0; i < _accel.subscription_count; i++) {
		orb_unsubscribe(_accel.subscription[i]);
	}

	for (unsigned i = 0; i < _mag.subscription_count; i++) {
		orb_unsubscribe(_mag.subscription[i]);
	}

	for (unsigned i = 0; i < _baro.subscription_count; i++) {
		orb_unsubscribe(_baro.subscription[i]);
	}
}

void VotedSensorsUpdate::parameters_update()
{
	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
	/* fine tune board offset */
	math::Matrix<3, 3> board_rotation_offset;
	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
					 M_DEG_TO_RAD_F * _parameters.board_offset[1],
					 M_DEG_TO_RAD_F * _parameters.board_offset[2]);

	_board_rotation = board_rotation_offset * _board_rotation;

	/* Load & apply the sensor calibrations.
	 * IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
	 */

	/* temperature compensation */
	_temperature_compensation.parameters_update();

	for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) {

		/* gyro */
		if (topic_instance < _gyro.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct gyro_report report;

			if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
				int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance);
				if (temp < 0) {
					PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
					_corrections.gyro_mapping[topic_instance] =  0;

				} else {
					_corrections.gyro_mapping[topic_instance] =  temp;

				}
			}
		}

		/* accel */
		if (topic_instance < _accel.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct accel_report report;

			if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
				int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance);
				if (temp < 0) {
					PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
					_corrections.accel_mapping[topic_instance] =  0;

				} else {
					_corrections.accel_mapping[topic_instance] =  temp;

				}
			}
		}
		/* baro */
		if (topic_instance < _baro.subscription_count) {
			// valid subscription, so get the driver id by getting the published sensor data
			struct baro_report report;

			if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
				int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance);
				if (temp < 0) {
					PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i",
						report.device_id, topic_instance);
					_corrections.baro_mapping[topic_instance] =  0;

				} else {
					_corrections.baro_mapping[topic_instance] =  temp;

				}
			}
		}
	}


	/* set offset parameters to new values */
	bool failed;
	char str[30];
	unsigned mag_count = 0;
	unsigned gyro_count = 0;
	unsigned accel_count = 0;
	unsigned gyro_cal_found_count = 0;
	unsigned accel_cal_found_count = 0;

	/* run through all gyro sensors */
	for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
{'fraction_non_alphanumeric': 0.11791064740502943, 'fraction_numerical': 0.01451310861423221, 'mean_word_length': 4.950258654994031, 'pattern_counts': {'":': 0, '<': 43, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5516917', 'n_tokens_mistral': 5462, 'n_tokens_neox': 4863, 'n_words': 1272}