1212// See the License for the specific language governing permissions and
1313// limitations under the License.
1414
15- #include " ../hardware_map.hpp"
15+ #include < cmath>
16+
1617#include < libhal-icm/icm20948.hpp>
1718#include < libhal-util/serial.hpp>
1819#include < libhal-util/steady_clock.hpp>
1920
21+ #include " ../hardware_map.hpp"
22+
23+ float compute_heading (float x, float y, float offset = 0.0 )
24+ {
25+ float angle = 360 - (atan2 (y, x) * (180.0 / std::numbers::pi));
26+ angle += offset; // Apply offset
27+ if (angle < 0 ) {
28+ angle += 360 ;
29+ } else if (angle >= 360 ) {
30+ angle -= 360 ;
31+ }
32+ return angle;
33+ }
34+
2035hal::status application (hardware_map& p_map)
2136{
2237 using namespace std ::chrono_literals;
@@ -27,32 +42,33 @@ hal::status application(hardware_map& p_map)
2742 auto & i2c = *p_map.i2c ;
2843
2944 hal::print (console, " icm Application Starting...\n\n " );
30- (void )hal::delay (clock, 200ms);
31- auto icm_device = HAL_CHECK (hal::icm::icm20948::create (i2c, 0x69 ));
32- (void )hal::delay (clock, 200ms);
33- icm_device.auto_offsets ();
34- (void )hal::delay (clock, 100ms);
45+ hal::delay (clock, 200ms);
46+ auto icm_device = HAL_CHECK (hal::icm::icm20948::create (i2c));
3547
48+ hal::delay (clock, 200ms);
49+ icm_device.init_mag ();
50+ hal::delay (clock, 100ms);
51+
52+ icm_device.auto_offsets ();
3653
3754 while (true ) {
38- hal::print (console, " \n\n ================Reading IMU================\n " );
3955
40- (void )hal::delay (clock, 500ms);
4156 auto accel = HAL_CHECK (icm_device.read_acceleration ());
42- ( void ) hal::delay (clock, 10ms);
57+ hal::delay (clock, 10ms);
4358 auto gyro = HAL_CHECK (icm_device.read_gyroscope ());
44- ( void ) hal::delay (clock, 10ms);
59+ hal::delay (clock, 10ms);
4560 auto temp = HAL_CHECK (icm_device.read_temperature ());
46- (void )hal::delay (clock, 10ms);
47-
61+ hal::delay (clock, 10ms);
62+ auto mag = HAL_CHECK (icm_device.read_magnetometer ());
63+ hal::delay (clock, 10ms);
64+ hal::print (console, " \n\n ================Reading IMU================\n " );
4865
4966 hal::print<128 >(console,
5067 " \n\n G-Accel Values: x = %fg, y = %fg, z = %fg" ,
5168 accel.x ,
5269 accel.y ,
5370 accel.z );
5471
55-
5672 hal::print<128 >(console,
5773 " \n\n Gyro Values: x = %f, y = %f, z = %f" ,
5874 gyro.x ,
@@ -61,6 +77,15 @@ hal::status application(hardware_map& p_map)
6177
6278 hal::print<128 >(console, " \n\n Current Temperature: %f°C" , temp.temp );
6379
80+ hal::print<128 >(console,
81+ " \n\n Magnetometer Values: x = %f, y = %f, z = %f" ,
82+ mag.x ,
83+ mag.y ,
84+ mag.z );
85+
86+ float heading = compute_heading (mag.x , mag.y , 0.0 );
87+ hal::print<128 >(console, " \n\n Heading: %f°" , heading);
88+
6489 hal::print (console, " \n\n ===========================================\n " );
6590 }
6691 return hal::success ();
0 commit comments