private RegulatorData _getRegulatorData()
{
const decimal dec2fp = 256; // 8 fraction bits
RegulatorData data = new RegulatorData();
// Decimal to 16 bit 8.8 fixedpoint. 16 bit fixed point to 2 bytes.
data.Rate[0, 0] = (UInt16)(decimal.Round(rate_pkp.Value * dec2fp, 0));
data.Rate[0, 1] = (UInt16)(decimal.Round(rate_pki.Value * dec2fp, 0));
data.Rate[0, 2] = (UInt16)(decimal.Round(rate_pil.Value * dec2fp, 0));
data.Rate[1, 0] = (UInt16)(decimal.Round(rate_rkp.Value * dec2fp, 0));
data.Rate[1, 1] = (UInt16)(decimal.Round(rate_rki.Value * dec2fp, 0));
data.Rate[1, 2] = (UInt16)(decimal.Round(rate_ril.Value * dec2fp, 0));
data.Rate[2, 0] = (UInt16)(decimal.Round(rate_ykp.Value * dec2fp, 0));
data.Rate[2, 1] = (UInt16)(decimal.Round(rate_yki.Value * dec2fp, 0));
data.Rate[2, 2] = (UInt16)(decimal.Round(rate_yil.Value * dec2fp, 0));
data.Attitude[0, 0] = (UInt16)(decimal.Round(attitude_pkp.Value * dec2fp, 0));
data.Attitude[0, 1] = (UInt16)(decimal.Round(attitude_pki.Value * dec2fp, 0));
data.Attitude[0, 2] = (UInt16)(decimal.Round(attitude_pil.Value * dec2fp, 0));
data.Attitude[1, 0] = (UInt16)(decimal.Round(attitude_rkp.Value * dec2fp, 0));
data.Attitude[1, 1] = (UInt16)(decimal.Round(attitude_rki.Value * dec2fp, 0));
data.Attitude[1, 2] = (UInt16)(decimal.Round(attitude_ril.Value * dec2fp, 0));
data.Attitude[2, 0] = (UInt16)(decimal.Round(attitude_ykp.Value * dec2fp, 0));
data.Attitude[2, 1] = (UInt16)(decimal.Round(attitude_yki.Value * dec2fp, 0));
data.Attitude[2, 2] = (UInt16)(decimal.Round(attitude_yil.Value * dec2fp, 0));
return data;
}
public SetRegulatorData(RegulatorData data) : base(Command.SetRegulatorData)
{
_data = data;
}