//=========================================================================
// Summary  :
// Filename : MMNavigation.h
// Author   : paley
// Project  :
// Revision : 1
// Created  : 2001/05/01
// Modified : 2001/05/14
//=========================================================================
// Description :  Navigation message
//=========================================================================

#include <stdio.h>
#include "MMAhrs.h"

MMAhrs::MMAhrs(const ModemMessage *pMM)
{
	Assert(pMM);
	Assert(pMM->getType()==tGetMessageType());
	const char* pcEnd = pcRead(pMM->getBuffer());
	Assert(pcEnd==pMM->getBuffer()+pMM->getBufferSize());
}

MMAhrs::MMAhrs(float roll, float pitch, float yaw,
		float rollRate, float pitchRate, float yawRate, int updateTime) :
		MMNavigationAttitude(roll,pitch,yaw,updateTime)
{
	setRollRate(rollRate);
	setPitchRate(pitchRate);
	setYawRate(yawRate);
}

const char* MMAhrs::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_roll);
	pcEnd = ::pcRead(pcEnd, &m_pitch);
	pcEnd = ::pcRead(pcEnd, &m_yaw);
	pcEnd = ::pcRead(pcEnd, &m_rollRate);
	pcEnd = ::pcRead(pcEnd, &m_pitchRate);
	pcEnd = ::pcRead(pcEnd, &m_yawRate);
	pcEnd = ::pcRead(pcEnd, &m_updateTime);
	return pcEnd;
}

char* MMAhrs::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_roll);
	pcEnd = ::pcWrite(pcEnd,m_pitch);
	pcEnd = ::pcWrite(pcEnd,m_yaw);
	pcEnd = ::pcWrite(pcEnd,m_rollRate);
	pcEnd = ::pcWrite(pcEnd,m_pitchRate);
	pcEnd = ::pcWrite(pcEnd,m_yawRate);
	pcEnd = ::pcWrite(pcEnd,m_updateTime);
	return pcEnd;
}

void MMAhrs::toString(char* str)
{
	sprintf(str,"tMMAhrs: roll=%f, pitch=%f, yaw=%f, rollRate=%f, pitchRate=%f,"
		"yawRate=%f, time=%d",getRoll(),getPitch(),getYaw(),getRollRate(),getPitchRate(),
		getYawRate(),getUpdateTime());
}