<commit_before>/*
 *	pAngle.cpp
 *	
 *	Author: petel__
 *	Copyright (c) 2014-2016 HKUST SmartCar Team
 *	Refer to LICENSE for details
 *	
 */

#include "pAngle.h"

using namespace libsc;
using namespace std;

Mma8451q::Config getAccelConfig(void)
{
	Mma8451q::Config config;
	config.id = 0;
	config.power_mode = Mma8451q::Config::PowerMode::kHighResolution;
	return config;
}

Mpu6050::Config getGyroConfig(void)
{
	Mpu6050::Config config;
	config.accel_range = Mpu6050::Config::Range::kLarge;
	config.gyro_range = Mpu6050::Config::Range::kSmall;
	config.cal_drift = true;
	return config;
}

pAngle::pAngle(pAngle::Config config)
:
	Mma8451q(getAccelConfig()),
	Mpu6050(getGyroConfig()),
	m_param(config),
	m_lastSpeed(0),
	m_lastAngle(0),
	m_lastYawOmega(0),
	m_gyroAngle(0),
	m_gyroOffset(0),
	m_lastTime(0)
{
	System::Init();
}

void pAngle::update(void)
{
	if (m_lastTime)
	{
		this->Mma8451q::UpdateF();
		this->Mpu6050::UpdateF();

		m_lastAccel = this->Mma8451q::GetAccelF();
		m_lastOmega = GetOmegaF();

		Timer::TimerInt dt = (System::Time() - m_lastTime) / 1000;

		m_gyroAngle -= m_lastOmega[0] * dt;
		m_gyroOffset += m_param.accelTrustValue * (asin(inRange(-1.0f, m_lastAccel[0], 1.0f)) * RadToDeg - m_lastAngle);
		m_lastSpeed = (m_gyroAngle + m_gyroOffset - m_lastAngle) / dt * m_param.cgHeight;
		m_lastAngle = m_gyroAngle + m_gyroOffset;
		m_lastYawOmega = m_lastOmega[2] * sin(m_lastAngle * DegToRad);
	}
	else
	{
		this->Mma8451q::UpdateF();
		m_lastAccel = this->Mma8451q::GetAccelF();
		m_lastAngle = m_gyroOffset = asin(inRange(-1.0f, m_lastAccel[0], 1.0f)) * RadToDeg;
	}

	m_lastTime = System::Time();
}

float pAngle::getAngle(void) const
{
	return m_lastAngle;
}

float pAngle::getSpeed(void) const
{
	return m_lastSpeed;<commit_msg>Fix: dt in pAngle was always zero<commit_after>/*
 *	pAngle.cpp
 *	
 *	Author: petel__
 *	Copyright (c) 2014-2016 HKUST SmartCar Team
 *	Refer to LICENSE for details
 *	
 */

#include "pAngle.h"

using namespace libsc;
using namespace std;

Mma8451q::Config getAccelConfig(void)
{
	Mma8451q::Config config;
	config.id = 0;
	config.power_mode = Mma8451q::Config::PowerMode::kHighResolution;
	return config;
}

Mpu6050::Config getGyroConfig(void)
{
	Mpu6050::Config config;
	config.accel_range = Mpu6050::Config::Range::kLarge;
	config.gyro_range = Mpu6050::Config::Range::kSmall;
	config.cal_drift = true;
	return config;
}

pAngle::pAngle(pAngle::Config config)
:
	Mma8451q(getAccelConfig()),
	Mpu6050(getGyroConfig()),
	m_param(config),
	m_lastSpeed(0),
	m_lastAngle(0),
	m_lastYawOmega(0),
	m_gyroAngle(0),
	m_gyroOffset(0),
	m_lastTime(0)
{
	System::Init();
}

void pAngle::update(void)
{
	if (m_lastTime)
	{
		this->Mma8451q::UpdateF();
		this->Mpu6050::UpdateF();

		m_lastAccel = this->Mma8451q::GetAccelF();
		m_lastOmega = GetOmegaF();

		float dt = (System::Time() - m_lastTime) / 1000.0f;

		m_gyroAngle -= m_lastOmega[0] * dt;
		m_gyroOffset += m_param.accelTrustValue * (asin(inRange(-1.0f, m_lastAccel[0], 1.0f)) * RadToDeg - m_lastAngle);
		m_lastSpeed = (m_gyroAngle + m_gyroOffset - m_lastAngle) / dt * m_param.cgHeight;
		m_lastAngle = m_gyroAngle + m_gyroOffset;
		m_lastYawOmega = m_lastOmega[2] * sin(m_lastAngle * DegToRad);
	}
	else
	{
		this->Mma8451q::UpdateF();
		m_lastAccel = this->Mma8451q::GetAccelF();
		m_lastAngle = m_gyroOffset = asin(inRange(-1.0f, m_lastAccel[0], 1.0f)) * RadToDeg;
	}

	m_lastTime = System::Time();
}

float pAngle::getAngle(void) const
{
	return m_lastAngle;
}

float pAngle::getSpeed(void) const
{
	return m_lastSpeed;
{'fraction_non_alphanumeric': 0.1505802434191905, 'fraction_numerical': 0.04953297480894424, 'mean_word_length': 5.356115107913669, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1045770', 'n_tokens_mistral': 1643, 'n_tokens_neox': 1511, 'n_words': 239}