示例#1
0
        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;
 }