//=========================================================================
// 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(short source, float roll, float pitch, float yaw,
	       float rollRate, float pitchRate, float yawRate, int updateTime) :
    MMNavigationAttitude(roll,pitch,yaw,updateTime)
{
    m_source = source;
    setRollRate(rollRate);
    setPitchRate(pitchRate);
    setYawRate(yawRate);
}

const char* MMAhrs::pcRead(const char* sourceBuffer)
{
    const char* pcEnd = ::pcRead(sourceBuffer, &m_source);
    pcEnd = ::pcRead(pcEnd, &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_source);
    pcEnd = ::pcWrite(pcEnd, 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());
}