Unable to get proper bearing values

Question

AIM: Read in filtered bearing values

CONCERN: The output is not accurate. Values returned are either all 0 or all 180.

CODE STATUS: Compiled successfully and running without any compiler warnings.

SENSOR : QMC5883L

DETAILS :

1. The magnetometer is functional. Verified with the help of a standalone python script.

2. Values are faulty even without the kalman filter.

3. Have to execute in c++ as the latter part of the project is done in c++

following is my code and the related header files.

compassvalscpp.cpp

#include <iostream>
#include "mycompass.h"
#include "mykalman.h"

int main(){
    bool init = true;
    kalman_state state;
compass * compass_sensor = new compass();
compass_sensor->start();
int i=0;
while(1){
    compass_sensor->update();
    if(init){       
        state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
        init = false;
        }
    kalman_update(&state, compass_sensor->bearing);
    float filtered_bearing = state.x;
    std::cout<<"filtered bearing = "<<filtered_bearing<<std::endl;
    }
delete compass_sensor;
return 0;}

mycompass.h

#ifndef __COMPASS_H__  
#define __COMPASS_H__
#define QMC5883L_I2C_ADDR 0x0D
#define M_PI = 3.1415926535
#include <iostream>
#include <errno.h>
#include <unistd.h>
#include "mycompass.h"
#include <cmath>
#include <bits/stdc++.h>
#include <stdlib.h>
#include <stdio.h>
#include <cstring>
#include <sys/socket.h>
#include <stropts.h>
#include <stddef.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
bool init;
class compass{
private:
    int i2c_fd;
    bool select_i2c_device(int fd, int addr);
    bool write_to_i2c(int fd, int reg, int val);
public:
    float bearing;
    int start();
    int update();
};
bool compass::select_i2c_device(int fd, int addr){
    if (ioctl(fd,I2C_SLAVE, addr) < 0) {
        return false;}
    return true;}
bool compass::write_to_i2c(int fd, int reg, int val){
    char buf[2];
    buf[0]=reg;
    buf[1]=val;
    if (write(fd, buf, 2) != 2){
        return false;}
    return true;}
int compass::start(){
    init = true;
    if ((i2c_fd = open("/dev/i2c-1", O_RDWR)) < 0){
        return 0;}
    select_i2c_device(i2c_fd, QMC5883L_I2C_ADDR);
    write_to_i2c(i2c_fd, 0x0A, 0b10000000);
    write_to_i2c(i2c_fd, 0x0A, 0b00000001);
    write_to_i2c(i2c_fd, 0x0B, 0x01);
    write_to_i2c(i2c_fd, 0x09, 0b00000001);
    return 1;
}
int compass::update(){
    float angle = 0;
    unsigned char i2c_buf[16];
    i2c_buf[0] = 0x03;
    if ((write(i2c_fd, i2c_buf, 1)) != 1){
        return 0;}
    if (read(i2c_fd, i2c_buf, 6) != 6){
        return 0;} 
    else{
        short x = (i2c_buf[1] << 8) | i2c_buf[0];
        short y = (i2c_buf[3] << 8) | i2c_buf[2];
    x = x*0.00092;
    y = y*0.00092;
        angle = atan2(y, x) * 180 / M_PI;}
    bearing = angle;
    return 1;
}
#endif

mykalman.h

#ifndef __KALMAN_H__  
#define __KALMAN_H__

typedef struct {
    float q; //process noise covariance
    float r; //measurement noise covariance
    float x; //value
    float p; //estimation error covariance
    float k; //kalman gain
} kalman_state;

kalman_state kalman_init(float q, float r, float p, float x);

void kalman_update(kalman_state *s, float m);

kalman_state kalman_init(float q, float r, float p, float x) {
    kalman_state s;
    s.q = q;
    s.r = r;
    s.p = p;
    s.x = x;

    return s;
}

void kalman_update(kalman_state * s, float m) {
    //prediction update
    //omit x = x
    s->p = s->p + s->q;

    //measurement update
    s->k = s->p / (s->p + s->r);
    s->x = s->x + s->k * (m - s->x);
    s->p = (1 - s->k) * s->p;
}

#endif // __KALMAN_H__