diff --git a/app/src/routes/peripherals/imu/imu.svelte b/app/src/routes/peripherals/imu/imu.svelte
index 38b75d0..9a066ac 100644
--- a/app/src/routes/peripherals/imu/imu.svelte
+++ b/app/src/routes/peripherals/imu/imu.svelte
@@ -18,10 +18,12 @@
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
     let altitudeChartElement: HTMLCanvasElement
+    let magnetometerChartElement: HTMLCanvasElement
 
     let angleChart: Chart
     let tempChart: Chart
     let altitudeChart: Chart
+    let magnetometerChart: Chart
 
     const getChartColors = () => {
         const style = getComputedStyle(document.body)
@@ -171,6 +173,37 @@
                 }
             }
         })
+
+        magnetometerChart = new Chart(magnetometerChartElement, {
+            type: 'line',
+            data: {
+                datasets: [
+                    {
+                        label: 'Heading',
+                        borderColor: colors.primary,
+                        backgroundColor: colors.primary,
+                        borderWidth: 2,
+                        data: $imu.heading,
+                        yAxisID: 'y'
+                    }
+                ]
+            },
+            options: {
+                ...baseConfig,
+                scales: {
+                    ...baseConfig.scales,
+                    y: {
+                        ...baseConfig.scales.y,
+                        title: {
+                            display: true,
+                            text: 'Heading [°]',
+                            color: colors.background,
+                            font: { size: 16, weight: 'bold' }
+                        }
+                    }
+                }
+            }
+        })
     }
 
     const updateChartData = (chart: Chart, data: number[]) => {
@@ -194,6 +227,10 @@
             angleChart.update('none')
         }
 
+        if ($features.mag) {
+            updateChartData(magnetometerChart, $imu.heading)
+        }
+
         if ($features.bmp) {
             updateChartData(tempChart, $imu.bmp_temp)
             updateChartData(altitudeChart, $imu.altitude)
@@ -235,6 +272,17 @@
         </div>
     {/if}
 
+    {#if $features.mag}
+        <div class="w-full overflow-x-auto">
+            <div
+                class="flex w-full flex-col space-y-1 h-60"
+                transition:slide|local={{ duration: 300, easing: cubicOut }}
+            >
+                <canvas bind:this={magnetometerChartElement}></canvas>
+            </div>
+        </div>
+    {/if}
+
     {#if $features.bmp}
         <div class="w-full overflow-x-auto">
             <div
diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index cdd5731..bfade6c 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -106,6 +106,7 @@ class IMU : public SensorBase<IMUAnglesMsg> {
     _imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
     if (_imu->status != ICM_20948_Stat_Ok){ return false; }
     // TODO: Setup low pass filter config
+    _msg.success = true;
 #endif
         return _msg.success;
     }
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index 27bf046..d2eecca 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -53,6 +53,7 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
             
             _mag->startupMagnetometer();
             if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _msg.success = true;
         #elif FT_ENABLED(USE_HMC5883)
             _msg.success = _mag.begin();
         #endif