embedded_components_0.2.0_37c39b23/src/motion/l3gd20/l3gd20.adb

------------------------------------------------------------------------------
--                                                                          --
--                    Copyright (C) 2015, AdaCore                           --
--                                                                          --
--  Redistribution and use in source and binary forms, with or without      --
--  modification, are permitted provided that the following conditions are  --
--  met:                                                                    --
--     1. Redistributions of source code must retain the above copyright    --
--        notice, this list of conditions and the following disclaimer.     --
--     2. Redistributions in binary form must reproduce the above copyright --
--        notice, this list of conditions and the following disclaimer in   --
--        the documentation and/or other materials provided with the        --
--        distribution.                                                     --
--     3. Neither the name of STMicroelectronics nor the names of its       --
--        contributors may be used to endorse or promote products derived   --
--        from this software without specific prior written permission.     --
--                                                                          --
--   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    --
--   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      --
--   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  --
--   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT   --
--   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, --
--   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT       --
--   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,  --
--   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY  --
--   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT    --
--   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE  --
--   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   --
--                                                                          --
--                                                                          --
--  This file is based on:                                                  --
--                                                                          --
--   @file    l3gd20.c                                                      --
--   @author  MCD Application Team                                          --
--   @version V1.1.0                                                        --
--   @date    19-June-2014                                                  --
--   @brief   This file provides a set of functions needed to manage the    --
--            L3GD20, ST MEMS motion sensor,  3-axis digital output         --
--            gyroscope.                                                    --
--                                                                          --
--   COPYRIGHT(c) 2014 STMicroelectronics                                   --
------------------------------------------------------------------------------

with Ada.Unchecked_Conversion;
with System;

package body L3GD20 is

   --  the following are per Table 4 of the L3GD20 Datasheet, pg 9.
   --  the values are millidegrees per second, so we scale accordingly
   Sensitivity_250dps  : constant := 8.75 * 0.001; -- mdps/digit
   Sensitivity_500dps  : constant := 17.5 * 0.001; -- mdps/digit
   Sensitivity_2000dps : constant := 70.0 * 0.001; -- mdps/digit

   ReadWrite_CMD : constant := 16#80#;
   MultiUInt8_CMD       : constant := 16#40#;

   --  bit definitions for the CTRL_REG3 register

   INT1_Interrupt_Enable            : constant := 2#1000_0000#;
   Interrupt_Pin_Mode_Bit           : constant := 2#0001_0000#;
   INT2_Data_Ready_Interrupt_Enable : constant := 2#0000_1000#;
   INT2_Watermark_Interrupt_Enable  : constant := 2#0000_0100#;
   INT2_Overrun_Interrupt_Enable    : constant := 2#0000_0010#;
   INT2_FIFO_Empty_Interrupt_Enable : constant := 2#0000_0001#;

   --  bit definitions for the CTRL_REG4 register

   Endian_Selection_Mask    : constant := 2#0100_0000#;
   Fullscale_Selection_Bits : constant := 2#0011_0000#;

   --  bit definitions for the CTRL_REG5 register

   Boot_Bit               : constant := 2#1000_0000#;
   FIFO_Enable_Bit        : constant := 2#0100_0000#;
   HighPass_Filter_Enable : constant := 2#0001_0000#;
   LowPass_Filter_Enable  : constant := 2#0000_0010#;

   --  bit definitions for the FIFO_CTRL register

   FIFO_Mode_Bits           : constant := 2#1110_0000#;
   Watermark_Threshold_Bits : constant := 2#0001_1111#;

   --  bit definitions for the FIFO_SRC register

   Watermark_Status_Bit : constant := 2#1000_0000#;
   FIFO_Overrun_Bit     : constant := 2#0100_0000#;
   FIFO_Empty_Bit       : constant := 2#0010_0000#;
   FIFO_Depth_Bits      : constant := 2#0001_1111#;

   --  bit definitions for the INT1_CFG register

   Logically_And_Or_Events_Bit : constant := 2#1000_0000#;
   Int1_Latch_Enable_Bit       : constant := 2#0100_0000#;

   Axes_Interrupt_Enablers : constant array (Axes_Interrupts) of UInt8 :=
     (Z_High_Interrupt => 2#0010_0000#,
      Z_Low_Interrupt  => 2#0001_0000#,
      Y_High_Interrupt => 2#0000_1000#,
      Y_Low_Interrupt  => 2#0000_0100#,
      X_High_Interrupt => 2#0000_0010#,
      X_Low_Interrupt  => 2#0000_0001#);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => High_Pass_Filter_Mode, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => High_Pass_Cutoff_Frequency, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Power_Mode_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Output_Data_Rate_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Axes_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Bandwidth_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Block_Data_Update_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Endian_Data_Selection, Target => UInt8);

   function As_UInt8 is new Ada.Unchecked_Conversion
     (Source => Full_Scale_Selection, Target => UInt8);

   type Angle_Rate_Pointer is access all Angle_Rate with Storage_Size => 0;

   function As_Angle_Rate_Pointer is new Ada.Unchecked_Conversion
     (Source => System.Address, Target => Angle_Rate_Pointer);
   --  So that we can treat the address of a UInt8 as a pointer to a two-UInt8
   --  sequence representing a signed integer quantity.

   procedure Swap2 (Location : System.Address) with Inline;
   --  Swaps the two UInt8s at Location and Location+1

   procedure SPI_Mode (This : Three_Axis_Gyroscope; Enabled : Boolean);
   --  Enable or disable SPI mode communication with the device. This is named
   --  "chip select" in other demos/examples.

   ----------------
   -- Initialize --
   ----------------

   procedure Initialize
     (This        : in out Three_Axis_Gyroscope;
      Port        : Any_SPI_Port;
      Chip_Select : Any_GPIO_Point)
   is
   begin
      This.Port := Port;
      This.CS := Chip_Select;

      SPI_Mode (This, Enabled => False);
   end Initialize;

   -----------------
   -- SPI_Mode --
   -----------------

   procedure SPI_Mode (This : Three_Axis_Gyroscope; Enabled : Boolean) is
      --  When the pin is low (cleared), the device is in SPI mode.
      --  When the pin is high (set), the device is in I2C mode.
      --  We want SPI mode communication, so Enabled, when True,
      --  means we must drive the pin low.
   begin
      if Enabled then
         This.CS.Clear;
      else
         This.CS.Set;
      end if;
   end SPI_Mode;

   -----------
   -- Write --
   -----------

   procedure Write (This : Three_Axis_Gyroscope; Addr : Register;  Data : UInt8)
   is
      Status : SPI_Status;
   begin
      SPI_Mode (This, Enabled => True);

      This.Port.Transmit (SPI_Data_8b'(1 => UInt8 (Addr)), Status);
      if Status /= Ok then
         raise Program_Error;
      end if;

      This.Port.Transmit (SPI_Data_8b'(1 => Data), Status);
      if Status /= Ok then
         raise Program_Error;
      end if;

      SPI_Mode (This, Enabled => False);
   end Write;

   ----------
   -- Read --
   ----------

   procedure Read
     (This : Three_Axis_Gyroscope;
      Addr : Register;
      Data : out UInt8)
   is
      Status : SPI_Status;
      Tmp_Data : SPI_Data_8b (1 .. 1);
   begin
      SPI_Mode (This, Enabled => True);

      This.Port.Transmit (SPI_Data_8b'(1 => UInt8 (Addr) or ReadWrite_CMD),
                         Status);
      if Status /= Ok then
         raise Program_Error;
      end if;

      This.Port.Receive (Tmp_Data, Status);
      if Status /= Ok then
         raise Program_Error;
      end if;
      Data := Tmp_Data (Tmp_Data'First);

      SPI_Mode (This, Enabled => False);
   end Read;

   ----------------
   -- Read_UInt8s --
   ----------------

   procedure Read_UInt8s
     (This   : Three_Axis_Gyroscope;
      Addr   : Register;
      Buffer : out SPI_Data_8b;
      Count  : Natural)
   is
      Index : Natural := Buffer'First;
      Status : SPI_Status;
      Tmp_Data : SPI_Data_8b (1 .. 1);
   begin
      SPI_Mode (This, Enabled => True);

      This.Port.Transmit
        (SPI_Data_8b'(1 => UInt8 (Addr) or ReadWrite_CMD or MultiUInt8_CMD),
         Status);

      if Status /= Ok then
         raise Program_Error;
      end if;

      for K in 1 .. Count loop
         This.Port.Receive (Tmp_Data, Status);
         if Status /= Ok then
            raise Program_Error;
         end if;
         Buffer (Index) := Tmp_Data (Tmp_Data'First);
         Index := Index + 1;
      end loop;

      SPI_Mode (This, Enabled => False);
   end Read_UInt8s;

   ---------------
   -- Configure --
   ---------------

   procedure Configure
     (This             : in out Three_Axis_Gyroscope;
      Power_Mode       : Power_Mode_Selection;
      Output_Data_Rate : Output_Data_Rate_Selection;
      Axes_Enable      : Axes_Selection;
      Bandwidth        : Bandwidth_Selection;
      BlockData_Update : Block_Data_Update_Selection;
      Endianness       : Endian_Data_Selection;
      Full_Scale       : Full_Scale_Selection)
   is
      Ctrl1 : UInt8;
      Ctrl4 : UInt8;
   begin
      Ctrl1 := As_UInt8 (Power_Mode)       or
               As_UInt8 (Output_Data_Rate) or
               As_UInt8 (Axes_Enable)      or
               As_UInt8 (Bandwidth);

      Ctrl4 := As_UInt8 (BlockData_Update) or
               As_UInt8 (Endianness)       or
               As_UInt8 (Full_Scale);

      Write (This, CTRL_REG1, Ctrl1);
      Write (This, CTRL_REG4, Ctrl4);
   end Configure;

   -----------
   -- Sleep --
   -----------

   procedure Sleep (This : in out Three_Axis_Gyroscope) is
      Ctrl1      : UInt8;
      Sleep_Mode : constant := 2#1000#;  -- per the Datasheet, Table 22, pg 32
   begin
      Read (This, CTRL_REG1, Ctrl1);
      Ctrl1 := Ctrl1 or Sleep_Mode;
      Write (This, CTRL_REG1, Ctrl1);
   end Sleep;

   --------------------------------
   -- Configure_High_Pass_Filter --
   --------------------------------

   procedure Configure_High_Pass_Filter
     (This             : in out Three_Axis_Gyroscope;
      Mode_Selection   : High_Pass_Filter_Mode;
      Cutoff_Frequency : High_Pass_Cutoff_Frequency)
   is
      Ctrl2 : UInt8;
   begin
      --  note that the two high-order bits must remain zero, per the datasheet
      Ctrl2 := As_UInt8 (Mode_Selection) or As_UInt8 (Cutoff_Frequency);
      Write (This, CTRL_REG2, Ctrl2);
   end Configure_High_Pass_Filter;

   -----------------------------
   -- Enable_High_Pass_Filter --
   -----------------------------

   procedure Enable_High_Pass_Filter (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      --  set HPen bit
      Ctrl5 := Ctrl5 or HighPass_Filter_Enable;
      Write (This, CTRL_REG5, Ctrl5);
   end Enable_High_Pass_Filter;

   ------------------------------
   -- Disable_High_Pass_Filter --
   ------------------------------

   procedure Disable_High_Pass_Filter (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      --  clear HPen bit
      Ctrl5 := Ctrl5 and (not HighPass_Filter_Enable);
      Write (This, CTRL_REG5, Ctrl5);
   end Disable_High_Pass_Filter;

   ----------------------------
   -- Enable_Low_Pass_Filter --
   ----------------------------

   procedure Enable_Low_Pass_Filter (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      Ctrl5 := Ctrl5 or LowPass_Filter_Enable;
      Write (This, CTRL_REG5, Ctrl5);
   end Enable_Low_Pass_Filter;

   -----------------------------
   -- Disable_Low_Pass_Filter --
   -----------------------------

   procedure Disable_Low_Pass_Filter (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      --  clear HPen bit
      Ctrl5 := Ctrl5 and (not LowPass_Filter_Enable);
      Write (This, CTRL_REG5, Ctrl5);
   end Disable_Low_Pass_Filter;

   ---------------------
   -- Reference_Value --
   ---------------------

   function Reference_Value (This : Three_Axis_Gyroscope) return UInt8 is
      Result : UInt8;
   begin
      Read (This, Reference, Result);
      return Result;
   end Reference_Value;

   -------------------
   -- Set_Reference --
   -------------------

   procedure Set_Reference (This : in out Three_Axis_Gyroscope; Value : UInt8) is
   begin
      Write (This, Reference, Value);
   end Set_Reference;

   -----------------
   -- Data_Status --
   -----------------

   function Data_Status (This : Three_Axis_Gyroscope) return Gyro_Data_Status is
      Result : UInt8;
      function As_Gyro_Data_Status is
        new Ada.Unchecked_Conversion (Source => UInt8,
                                      Target => Gyro_Data_Status);
   begin
      Read (This, Status, Result);
      return As_Gyro_Data_Status (Result);
   end Data_Status;

   ---------------
   -- Device_Id --
   ---------------

   function Device_Id (This : Three_Axis_Gyroscope) return UInt8 is
      Result : UInt8;
   begin
      Read (This, Who_Am_I, Result);
      return Result;
   end Device_Id;

   -----------------
   -- Temperature --
   -----------------

   function Temperature (This : Three_Axis_Gyroscope) return UInt8 is
      Result : UInt8;
   begin
      Read (This, OUT_Temp, Result);
      return Result;
   end Temperature;

   ------------
   -- Reboot --
   ------------

   procedure Reboot (This : Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      --  set the boot bit
      Ctrl5 := Ctrl5 or Boot_Bit;
      Write (This, CTRL_REG5, Ctrl5);
   end Reboot;

   ----------------------------
   -- Full_Scale_Sensitivity --
   ----------------------------

   function Full_Scale_Sensitivity (This : Three_Axis_Gyroscope) return Float is
      Ctrl4  : UInt8;
      Result : Float;
      Fullscale_Selection : UInt8;
   begin
      Read (This, CTRL_REG4, Ctrl4);

      Fullscale_Selection := Ctrl4 and Fullscale_Selection_Bits;

      if Fullscale_Selection = L3GD20_Fullscale_250'Enum_Rep then
         Result := Sensitivity_250dps;
      elsif Fullscale_Selection = L3GD20_Fullscale_500'Enum_Rep then
         Result := Sensitivity_500dps;
      else
         Result := Sensitivity_2000dps;
      end if;

      return Result;
   end Full_Scale_Sensitivity;

   -------------------------
   -- Get_Raw_Angle_Rates --
   -------------------------

   procedure Get_Raw_Angle_Rates
     (This  : Three_Axis_Gyroscope;
      Rates : out Angle_Rates)
   is

      Ctrl4 : UInt8;

      UInt8s_To_Read : constant Integer := 6;  -- ie, three 2-UInt8 integers
      --  the number of UInt8s in an Angle_Rates record object

      Received : SPI_Data_8b (1 .. UInt8s_To_Read);

   begin
      Read (This, CTRL_REG4, Ctrl4);

      Read_UInt8s (This, OUT_X_L, Received, UInt8s_To_Read);
      --  The above has the effect of separate reads, as follows:
      --    Read (This, OUT_X_L, Received (1));
      --    Read (This, OUT_X_H, Received (2));
      --    Read (This, OUT_Y_L, Received (3));
      --    Read (This, OUT_Y_H, Received (4));
      --    Read (This, OUT_Z_L, Received (5));
      --    Read (This, OUT_Z_H, Received (6));

      if (Ctrl4 and Endian_Selection_Mask) = L3GD20_Big_Endian'Enum_Rep then
         Swap2 (Received (1)'Address);
         Swap2 (Received (3)'Address);
         Swap2 (Received (5)'Address);
      end if;

      Rates.X := As_Angle_Rate_Pointer (Received (1)'Address).all;
      Rates.Y := As_Angle_Rate_Pointer (Received (3)'Address).all;
      Rates.Z := As_Angle_Rate_Pointer (Received (5)'Address).all;
   end Get_Raw_Angle_Rates;

   --------------------------
   -- Configure_Interrupt1 --
   --------------------------

   procedure Configure_Interrupt1
     (This           : in out Three_Axis_Gyroscope;
      Triggers       : Threshold_Event_List;
      Latched        : Boolean := False;
      Active_Edge    : Interrupt1_Active_Edge := L3GD20_Interrupt1_High_Edge;
      Combine_Events : Boolean := False;
      Sample_Count   : Sample_Counter := 0)
   is
      Config : UInt8;
      Ctrl3  : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and 16#DF#;
      Ctrl3 := Ctrl3 or Active_Edge'Enum_Rep;
      Ctrl3 := Ctrl3 or INT1_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);

      if Sample_Count > 0 then
         Set_Duration_Counter (This, Sample_Count);
      end if;

      Config := 0;

      if Latched then
         Config := Config or Int1_Latch_Enable_Bit;
      end if;

      if Combine_Events then
         Config := Config or Logically_And_Or_Events_Bit;
      end if;

      for Event of Triggers loop
         Config := Config or Axes_Interrupt_Enablers (Event.Axis);
      end loop;

      Write (This, INT1_CFG, Config);

      for Event of Triggers loop
         Set_Threshold (This, Event.Axis, Event.Threshold);
      end loop;
   end Configure_Interrupt1;

   ----------------------------
   -- Set_Interrupt_Pin_Mode --
   ----------------------------

   procedure Set_Interrupt_Pin_Mode
     (This : in out Three_Axis_Gyroscope;
      Mode : Pin_Modes)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not Interrupt_Pin_Mode_Bit);
      Ctrl3 := Ctrl3 or Mode'Enum_Rep;
      Write (This, CTRL_REG3, Ctrl3);
   end Set_Interrupt_Pin_Mode;

   -----------------------
   -- Enable_Interrupt1 --
   -----------------------

   procedure Enable_Interrupt1 (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 or INT1_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);
   end Enable_Interrupt1;

   ------------------------
   -- Disable_Interrupt1 --
   ------------------------

   procedure Disable_Interrupt1 (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not INT1_Interrupt_Enable);
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_Interrupt1;

   -----------------------
   -- Interrupt1_Status --
   -----------------------

   function Interrupt1_Source (This : Three_Axis_Gyroscope) return Interrupt1_Sources is
      Result : UInt8;
      function As_Interrupt_Source is new Ada.Unchecked_Conversion
        (Source => UInt8, Target => Interrupt1_Sources);
   begin
      Read (This, INT1_SRC, Result);
      return As_Interrupt_Source (Result);
   end Interrupt1_Source;

   --------------------------
   -- Set_Duration_Counter --
   --------------------------

   procedure Set_Duration_Counter
     (This  : in out Three_Axis_Gyroscope;
      Value : Sample_Counter)
   is
      Wait_Bit : constant := 2#1000_0000#;
   begin
      Write (This, INT1_Duration, UInt8 (Value) or Wait_Bit);
   end Set_Duration_Counter;

   ------------------
   -- Enable_Event --
   ------------------

   procedure Enable_Event
     (This  : in out Three_Axis_Gyroscope;
      Event : Axes_Interrupts)
   is
      Config : UInt8;
   begin
      Read (This, INT1_CFG, Config);
      Config := Config or Axes_Interrupt_Enablers (Event);
      Write (This, INT1_CFG, Config);
   end Enable_Event;

   -------------------
   -- Disable_Event --
   -------------------

   procedure Disable_Event
     (This  : in out Three_Axis_Gyroscope;
      Event : Axes_Interrupts)
   is
      Config : UInt8;
   begin
      Read (This, INT1_CFG, Config);
      Config := Config and (not Axes_Interrupt_Enablers (Event));
      Write (This, INT1_CFG, Config);
   end Disable_Event;

   -------------------
   -- Set_Threshold --
   -------------------

   procedure Set_Threshold
     (This  : in out Three_Axis_Gyroscope;
      Event : Axes_Interrupts;
      Value : Axis_Sample_Threshold)
   is
   begin
      case Event is
         when Z_High_Interrupt | Z_Low_Interrupt =>
            Write (This, INT1_TSH_ZL, UInt8 (Value));
            Write (This, INT1_TSH_ZH, UInt8 (Shift_Right (Value, 8)));

         when Y_High_Interrupt | Y_Low_Interrupt =>
            Write (This, INT1_TSH_YL, UInt8 (Value));
            Write (This, INT1_TSH_YH, UInt8 (Shift_Right (Value, 8)));

         when X_High_Interrupt | X_Low_Interrupt =>
            Write (This, INT1_TSH_XL, UInt8 (Value));
            Write (This, INT1_TSH_XH, UInt8 (Shift_Right (Value, 8)));
      end case;
   end Set_Threshold;

   -------------------
   -- Set_FIFO_Mode --
   -------------------

   procedure Set_FIFO_Mode
     (This : in out Three_Axis_Gyroscope;
      Mode : FIFO_Modes)
   is
      FIFO  : UInt8;
   begin
      Read (This, FIFO_CTRL, FIFO);
      FIFO := FIFO and (not FIFO_Mode_Bits);  -- clear the current bits
      FIFO := FIFO or Mode'Enum_Rep;
      Write (This, FIFO_CTRL, FIFO);
   end Set_FIFO_Mode;

   -----------------
   -- Enable_FIFO --
   -----------------

   procedure Enable_FIFO (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      Ctrl5 := Ctrl5 or FIFO_Enable_Bit;
      Write (This, CTRL_REG5, Ctrl5);
   end Enable_FIFO;

   ------------------
   -- Disable_FIFO --
   ------------------

   procedure Disable_FIFO (This : in out Three_Axis_Gyroscope) is
      Ctrl5 : UInt8;
   begin
      Read (This, CTRL_REG5, Ctrl5);
      Ctrl5 := Ctrl5 and (not FIFO_Enable_Bit);
      Write (This, CTRL_REG5, Ctrl5);
   end Disable_FIFO;

   ------------------------
   -- Set_FIFO_Watermark --
   ------------------------

   procedure Set_FIFO_Watermark
     (This  : in out Three_Axis_Gyroscope;
      Level : FIFO_Level)
   is
      Value : UInt8;
   begin
      Read (This, FIFO_CTRL, Value);
      Value := Value and (not Watermark_Threshold_Bits); -- clear the bits
      Value := Value or UInt8 (Level);
      Write (This, FIFO_CTRL, Value);
   end Set_FIFO_Watermark;

   ------------------------------
   -- Get_Raw_Angle_Rates_FIFO --
   ------------------------------

   procedure Get_Raw_Angle_Rates_FIFO
     (This   : in out Three_Axis_Gyroscope;
      Buffer : out Angle_Rates_FIFO_Buffer)
   is
      Ctrl4 : UInt8;

      Angle_Rate_Size : constant Integer := 6;  -- UInt8s
      UInt8s_To_Read   : constant Integer := Buffer'Length * Angle_Rate_Size;
      Received        : SPI_Data_8b (0 .. UInt8s_To_Read - 1)
        with Alignment => 2;
   begin
      Read (This, CTRL_REG4, Ctrl4);

      Read_UInt8s (This, OUT_X_L, Received, UInt8s_To_Read);

      if (Ctrl4 and Endian_Selection_Mask) = L3GD20_Big_Endian'Enum_Rep then
         declare
            J : Integer := 0;
         begin
            for K in Received'First .. Received'Last / 2 loop
               Swap2 (Received (J)'Address);
               J := J + 2;
            end loop;
         end;
      end if;

      declare
         J : Integer := 0;
      begin
         for K in Buffer'Range loop
            Buffer (K).X := As_Angle_Rate_Pointer (Received (J)'Address).all;
            J := J + 2;
            Buffer (K).Y := As_Angle_Rate_Pointer (Received (J)'Address).all;
            J := J + 2;
            Buffer (K).Z := As_Angle_Rate_Pointer (Received (J)'Address).all;
            J := J + 2;
         end loop;
      end;
   end Get_Raw_Angle_Rates_FIFO;

   ------------------------
   -- Current_FIFO_Depth --
   ------------------------

   function Current_FIFO_Depth (This : Three_Axis_Gyroscope) return FIFO_Level is
      Result : UInt8;
   begin
      Read (This, FIFO_SRC, Result);
      Result := Result and FIFO_Depth_Bits;
      return FIFO_Level (Result);
   end Current_FIFO_Depth;

   --------------------------
   -- FIFO_Below_Watermark --
   --------------------------

   function FIFO_Below_Watermark (This : Three_Axis_Gyroscope) return Boolean is
      Result : UInt8;
   begin
      Read (This, FIFO_SRC, Result);
      Result := Result and Watermark_Status_Bit;
      return Result = 0;
   end FIFO_Below_Watermark;

   ----------------
   -- FIFO_Empty --
   ----------------

   function FIFO_Empty (This : Three_Axis_Gyroscope) return Boolean is
      Result : UInt8;
   begin
      Read (This, FIFO_SRC, Result);
      Result := Result and FIFO_Empty_Bit;
      return Result = FIFO_Empty_Bit;
   end FIFO_Empty;

   ------------------
   -- FIFO_Overrun --
   ------------------

   function FIFO_Overrun (This : Three_Axis_Gyroscope) return Boolean is
      Result : UInt8;
   begin
      Read (This, FIFO_SRC, Result);
      Result := Result and FIFO_Overrun_Bit;
      return Result = FIFO_Overrun_Bit;
   end FIFO_Overrun;

   ---------------------------------
   -- Enable_Data_Ready_Interrupt --
   ---------------------------------

   procedure Enable_Data_Ready_Interrupt (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 or INT2_Data_Ready_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);
   end Enable_Data_Ready_Interrupt;

   ----------------------------------
   -- Disable_Data_Ready_Interrupt --
   ----------------------------------

   procedure Disable_Data_Ready_Interrupt (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not INT2_Data_Ready_Interrupt_Enable);
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_Data_Ready_Interrupt;

   -------------------------------------
   -- Enable_FIFO_Watermark_Interrupt --
   -------------------------------------

   procedure Enable_FIFO_Watermark_Interrupt (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 or INT2_Watermark_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);
   end Enable_FIFO_Watermark_Interrupt;

   -----------------------------
   -- Disable_Int1_Interrupts --
   -----------------------------

   procedure Disable_Int1_Interrupts (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and 2#0001_1111#;
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_Int1_Interrupts;

   -----------------------------
   -- Disable_Int2_Interrupts --
   -----------------------------

   procedure Disable_Int2_Interrupts (This : in out Three_Axis_Gyroscope) is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and 2#1111_0000#;
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_Int2_Interrupts;

   --------------------------------------
   -- Disable_FIFO_Watermark_Interrupt --
   --------------------------------------

   procedure Disable_FIFO_Watermark_Interrupt
     (This : in out Three_Axis_Gyroscope)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not INT2_Watermark_Interrupt_Enable);
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_FIFO_Watermark_Interrupt;

   -----------------------------------
   -- Enable_FIFO_Overrun_Interrupt --
   -----------------------------------

   procedure Enable_FIFO_Overrun_Interrupt
     (This : in out Three_Axis_Gyroscope)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 or INT2_Overrun_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);
   end Enable_FIFO_Overrun_Interrupt;

   ------------------------------------
   -- Disable_FIFO_Overrun_Interrupt --
   ------------------------------------

   procedure Disable_FIFO_Overrun_Interrupt
     (This : in out Three_Axis_Gyroscope)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not INT2_Overrun_Interrupt_Enable);
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_FIFO_Overrun_Interrupt;

   ---------------------------------
   -- Enable_FIFO_Empty_Interrupt --
   ---------------------------------

   procedure Enable_FIFO_Empty_Interrupt
     (This : in out Three_Axis_Gyroscope)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 or INT2_FIFO_Empty_Interrupt_Enable;
      Write (This, CTRL_REG3, Ctrl3);
   end Enable_FIFO_Empty_Interrupt;

   ----------------------------------
   -- Disable_FIFO_Empty_Interrupt --
   ----------------------------------

   procedure Disable_FIFO_Empty_Interrupt
     (This : in out Three_Axis_Gyroscope)
   is
      Ctrl3 : UInt8;
   begin
      Read (This, CTRL_REG3, Ctrl3);
      Ctrl3 := Ctrl3 and (not INT2_FIFO_Empty_Interrupt_Enable);
      Write (This, CTRL_REG3, Ctrl3);
   end Disable_FIFO_Empty_Interrupt;

   -----------
   -- Swap2 --
   -----------

   procedure Swap2 (Location : System.Address) is
      X : UInt16;
      for X'Address use Location;
      function Bswap_16 (X : UInt16) return UInt16;
      pragma Import (Intrinsic, Bswap_16, "__builtin_bswap16");
   begin
      X := Bswap_16 (X);
   end Swap2;

   -----------
   -- Reset --
   -----------

   procedure Reset (This : in out Three_Axis_Gyroscope) is
   begin
      --  per the Datasheet, Table 17, pg 29
      Write (This, CTRL_REG1, 2#0000_0111#);
      Write (This, CTRL_REG2, 0);
      Write (This, CTRL_REG3, 0);
      Write (This, CTRL_REG4, 0);
      Write (This, CTRL_REG5, 0);
      Write (This, Reference, 0);
      Write (This, FIFO_CTRL, 0);
      Write (This, INT1_CFG, 0);
      Write (This, INT1_TSH_ZL, 0);
      Write (This, INT1_TSH_ZH, 0);
      Write (This, INT1_TSH_YL, 0);
      Write (This, INT1_TSH_YH, 0);
      Write (This, INT1_TSH_XL, 0);
      Write (This, INT1_TSH_XH, 0);
      Write (This, INT1_Duration, 0);
   end Reset;

end L3GD20;