1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108 | -- SPDX-FileCopyrightText: 2024 Max Reznik <reznikmm@gmail.com>
--
-- SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
----------------------------------------------------------------
with Ada.Real_Time;
with Ada.Text_IO;
with Ravenscar_Time;
with STM32.Board;
with STM32.Device;
with STM32.Setup;
with HAL.I2C;
with QMC5883.Sensor;
procedure Main is
use type Ada.Real_Time.Time;
package QMC5883_I2C is new QMC5883.Sensor
(I2C_Port => STM32.Device.I2C_1'Access);
Ok : Boolean := False;
Vector : array (1 .. 16) of QMC5883.Magnetic_Field_Vector;
Prev : Ada.Real_Time.Time;
Spin : Natural;
begin
STM32.Board.Initialize_LEDs;
STM32.Setup.Setup_I2C_Master
(Port => STM32.Device.I2C_1,
SDA => STM32.Device.PB9,
SCL => STM32.Device.PB8,
SDA_AF => STM32.Device.GPIO_AF_I2C1_4,
SCL_AF => STM32.Device.GPIO_AF_I2C1_4,
Clock_Speed => 400_000);
declare
Status : HAL.I2C.I2C_Status;
begin
-- Workaround for STM32 I2C driver bug
STM32.Device.I2C_1.Master_Transmit
(Addr => 16#1A#, -- 0D * 2
Data => (1 => 16#0D#), -- Chip ID for QMC5883L
Status => Status);
end;
-- Look for QMC5883L chip
if not QMC5883_I2C.Check_Chip_Id then
Ada.Text_IO.Put_Line ("QMC5883L not found.");
raise Program_Error;
end if;
-- Reset QMC5883L
QMC5883_I2C.Reset (Ok);
pragma Assert (Ok);
-- Set QMC5883L up
QMC5883_I2C.Configure
((Over_Sample => 512,
Data_Rate => 10,
Full_Range => 2,
Mode => QMC5883.Continuous),
Ok);
pragma Assert (Ok);
loop
Prev := Ada.Real_Time.Clock;
Spin := 0;
STM32.Board.Toggle (STM32.Board.D1_LED);
for J in Vector'Range loop
while not QMC5883_I2C.Is_Data_Ready loop
Spin := Spin + 1;
end loop;
-- Read scaled values from the sensor
QMC5883_I2C.Read_Measurement (Vector (J), Ok);
pragma Assert (Ok);
end loop;
-- Printing...
declare
Now : constant Ada.Real_Time.Time := Ada.Real_Time.Clock;
Diff : constant Duration := Ada.Real_Time.To_Duration (Now - Prev);
begin
Ada.Text_IO.New_Line;
Ada.Text_IO.New_Line;
Ada.Text_IO.Put_Line
("Time=" & Diff'Image & "/16 spin=" & Spin'Image);
for Value of Vector loop
declare
X : constant String := Value.X'Image;
Y : constant String := Value.Y'Image;
Z : constant String := Value.Z'Image;
begin
Ada.Text_IO.Put_Line ("X=" & X & " Y=" & Y & " Z=" & Z);
end;
end loop;
Ada.Text_IO.Put_Line ("Sleeping 2s...");
Ravenscar_Time.Delays.Delay_Seconds (2);
end;
end loop;
end Main;
|