pios_hmc5883.c

Go to the documentation of this file.

/* 
 * This program is free software; you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation; either version 3 of the License, or 
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
 * for more details.
 * 
 * You should have received a copy of the GNU General Public License along 
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */

/* Project Includes */
#include "pios.h"
#include "pios_hmc5883_priv.h"

#if defined(PIOS_INCLUDE_HMC5883)

#include "pios_semaphore.h"
#include "pios_thread.h"
#include "pios_queue.h"

/* Private constants */
#define HMC5883_TASK_PRIORITY        PIOS_THREAD_PRIO_HIGHEST
#define HMC5883_TASK_STACK_BYTES     512
#define PIOS_HMC5883_MAX_DOWNSAMPLE  1

/* Global Variables */

/* Local Types */
enum pios_hmc5883_dev_magic {
        PIOS_HMC5883_DEV_MAGIC = 0x3d8e17ab,
};

struct hmc5883_dev {
        pios_i2c_t i2c_id;
        const struct pios_hmc5883_cfg *cfg;
        struct pios_queue *queue;
        struct pios_thread *task;
        struct pios_semaphore *data_ready_sema;
        enum pios_hmc5883_dev_magic magic;
        enum pios_hmc5883_orientation orientation;
};

/* Local Variables */
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg);
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len);
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer);
static void PIOS_HMC5883_Task(void *parameters);

static struct hmc5883_dev *dev;

static struct hmc5883_dev * PIOS_HMC5883_alloc(void)
{
        struct hmc5883_dev *hmc5883_dev;
        
        hmc5883_dev = (struct hmc5883_dev *)PIOS_malloc(sizeof(*hmc5883_dev));
        if (!hmc5883_dev) return (NULL);
        
        hmc5883_dev->magic = PIOS_HMC5883_DEV_MAGIC;
        
        hmc5883_dev->queue = PIOS_Queue_Create(PIOS_HMC5883_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
        if (hmc5883_dev->queue == NULL) {
                PIOS_free(hmc5883_dev);
                return NULL;
        }

        return hmc5883_dev;
}

static int32_t PIOS_HMC5883_Validate(struct hmc5883_dev *dev)
{
        if (dev == NULL) 
                return -1;
        if (dev->magic != PIOS_HMC5883_DEV_MAGIC)
                return -2;
        if (dev->i2c_id == 0)
                return -3;
        return 0;
}

int32_t PIOS_HMC5883_Init(pios_i2c_t i2c_id, const struct pios_hmc5883_cfg *cfg)
{
        dev = (struct hmc5883_dev *) PIOS_HMC5883_alloc();
        if (dev == NULL)
                return -1;

        dev->cfg = cfg;
        dev->i2c_id = i2c_id;
        dev->orientation = cfg->Default_Orientation;

        /* check if we are using an irq line */
#ifndef PIOS_HMC5883_NO_EXTI
        if (cfg->exti_cfg != NULL) {
                PIOS_EXTI_Init(cfg->exti_cfg);

                dev->data_ready_sema = PIOS_Semaphore_Create();
                PIOS_Assert(dev->data_ready_sema != NULL);
        }
        else {
                dev->data_ready_sema = NULL;
        }
#endif

        if (PIOS_HMC5883_Config(cfg) != 0)
                return -2;

        PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->queue);

        dev->task = PIOS_Thread_Create(PIOS_HMC5883_Task, "pios_hmc5883", HMC5883_TASK_STACK_BYTES, NULL, HMC5883_TASK_PRIORITY);

        PIOS_Assert(dev->task != NULL);

        return 0;
}

int32_t PIOS_HMC5883_SetOrientation(enum pios_hmc5883_orientation orientation)
{
        if (PIOS_HMC5883_Validate(dev) != 0)
                return -1;

        dev->orientation = orientation;

        return 0;
}

static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg)
{
        // CRTL_REGA
        if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, cfg->M_ODR | cfg->Meas_Conf) != 0)
                return -1;
        
        // CRTL_REGB
        if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, cfg->Gain) != 0)
                return -1;
        
        // Mode register
        if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, cfg->Mode) != 0)
                return -1;
        
        return 0;
}

static uint16_t PIOS_HMC5883_Config_GetSensitivity()
{
        switch (dev->cfg->Gain) {
        case PIOS_HMC5883_GAIN_0_88:
                return PIOS_HMC5883_Sensitivity_0_88Ga;
        case PIOS_HMC5883_GAIN_1_3:
                return PIOS_HMC5883_Sensitivity_1_3Ga;
        case PIOS_HMC5883_GAIN_1_9:
                return PIOS_HMC5883_Sensitivity_1_9Ga;
        case PIOS_HMC5883_GAIN_2_5:
                return PIOS_HMC5883_Sensitivity_2_5Ga;
        case PIOS_HMC5883_GAIN_4_0:
                return PIOS_HMC5883_Sensitivity_4_0Ga;
        case PIOS_HMC5883_GAIN_4_7:
                return PIOS_HMC5883_Sensitivity_4_7Ga;
        case PIOS_HMC5883_GAIN_5_6:
                return PIOS_HMC5883_Sensitivity_5_6Ga;
        case PIOS_HMC5883_GAIN_8_1:
                return PIOS_HMC5883_Sensitivity_8_1Ga;
        }

        return 0;
}

static int32_t PIOS_HMC5883_ReadMag(struct pios_sensor_mag_data *mag_data)
{
        if (PIOS_HMC5883_Validate(dev) != 0)
                return -1;

        /* don't use PIOS_HMC5883_Read and PIOS_HMC5883_Write here because the task could be
         * switched out of context in between which would give the sensor less time to capture
         * the next sample.
         */
        uint8_t addr_read = PIOS_HMC5883_DATAOUT_XMSB_REG;
        uint8_t buffer_read[6];

        // PIOS_HMC5883_MODE_CONTINUOUS: This should not be necessary but for some reason it is coming out of continuous conversion mode
        // PIOS_HMC5883_MODE_SINGLE: This triggers the next measurement
        uint8_t buffer_write[2] = {
                PIOS_HMC5883_MODE_REG,
                dev->cfg->Mode
        };

        const struct pios_i2c_txn txn_list[] = {
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_WRITE,
                        .len = sizeof(addr_read),
                        .buf = &addr_read,
                },
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_READ,
                        .len = sizeof(buffer_read),
                        .buf = buffer_read,
                },
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_WRITE,
                        .len = sizeof(buffer_write),
                        .buf = buffer_write,
                },
        };

        if (PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list)) != 0)
                return -1;

        int16_t mag_x, mag_y, mag_z;
        uint16_t sensitivity = PIOS_HMC5883_Config_GetSensitivity();
        mag_x = ((int16_t) ((uint16_t)buffer_read[0] << 8) + buffer_read[1]) * 1000 / sensitivity;
        mag_z = ((int16_t) ((uint16_t)buffer_read[2] << 8) + buffer_read[3]) * 1000 / sensitivity;
        mag_y = ((int16_t) ((uint16_t)buffer_read[4] << 8) + buffer_read[5]) * 1000 / sensitivity;

        // Define "0" when the fiducial is in the front left of the board
        switch (dev->orientation) {
                case PIOS_HMC5883_TOP_0DEG:
                        mag_data->x = -mag_x;
                        mag_data->y = mag_y;
                        mag_data->z = -mag_z;
                        break;
                case PIOS_HMC5883_TOP_90DEG:
                        mag_data->x = -mag_y;
                        mag_data->y = -mag_x;
                        mag_data->z = -mag_z;
                        break;
                case PIOS_HMC5883_TOP_180DEG:
                        mag_data->x = mag_x;
                        mag_data->y = -mag_y;
                        mag_data->z = -mag_z;
                        break;
                case PIOS_HMC5883_TOP_270DEG:
                        mag_data->x = mag_y;
                        mag_data->y = mag_x;
                        mag_data->z = -mag_z;
                        break;
                case PIOS_HMC5883_BOTTOM_0DEG:
                        mag_data->x = -mag_x;
                        mag_data->y = -mag_y;
                        mag_data->z = mag_z;
                        break;
                case PIOS_HMC5883_BOTTOM_90DEG:
                        mag_data->x = mag_y;
                        mag_data->y = -mag_x;
                        mag_data->z = mag_z;
                        break;
                case PIOS_HMC5883_BOTTOM_180DEG:
                        mag_data->x = mag_x;
                        mag_data->y = mag_y;
                        mag_data->z = mag_z;
                        break;
                case PIOS_HMC5883_BOTTOM_270DEG:
                        mag_data->x = -mag_y;
                        mag_data->y = mag_x;
                        mag_data->z = mag_z;
                        break;
        }
        
        return 0;
}


static uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
{
        uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
        out[3] = '\0';
        return retval;
}


static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len)
{
        if(PIOS_HMC5883_Validate(dev) != 0)
                return -1;

        uint8_t addr_buffer[] = {
                address,
        };
        
        const struct pios_i2c_txn txn_list[] = {
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_WRITE,
                        .len = sizeof(addr_buffer),
                        .buf = addr_buffer,
                }
                ,
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_READ,
                        .len = len,
                        .buf = buffer,
                }
        };
        
        return PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list));
}

static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
{
        if(PIOS_HMC5883_Validate(dev) != 0)
                return -1;

        uint8_t data[] = {
                address,
                buffer,
        };
        
        const struct pios_i2c_txn txn_list[] = {
                {
                        .info = __func__,
                        .addr = PIOS_HMC5883_I2C_ADDR,
                        .rw = PIOS_I2C_TXN_WRITE,
                        .len = sizeof(data),
                        .buf = data,
                }
                ,
        };

        return PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list));
}

int32_t PIOS_HMC5883_Test(void)
{
        /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
        char id[4];
        PIOS_HMC5883_ReadID((uint8_t *)id);
        if((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) // Expect H43
                return -1;

        return 0;
}

#ifndef PIOS_HMC5883_NO_EXTI
bool PIOS_HMC5883_IRQHandler(void)
{
        if (PIOS_HMC5883_Validate(dev) != 0)
                return false;

        bool woken = false;
        PIOS_Semaphore_Give_FromISR(dev->data_ready_sema, &woken);

        return woken;
}
#endif

static void PIOS_HMC5883_Task(void *parameters)
{
        while (PIOS_HMC5883_Validate(dev) != 0) {
                PIOS_Thread_Sleep(100);
        }

        uint32_t sample_delay;

        switch (dev->cfg->M_ODR) {
        case PIOS_HMC5883_ODR_0_75:
                sample_delay = 1000 / 0.75f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_1_5:
                sample_delay = 1000 / 1.5f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_3:
                sample_delay = 1000 / 3.0f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_7_5:
                sample_delay = 1000 / 7.5f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_15:
                sample_delay = 1000 / 15.0f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_30:
                sample_delay = 1000 / 30.0f + 0.99999f;
                break;
        case PIOS_HMC5883_ODR_75:
        default:
                sample_delay = 1000 / 75.0f + 0.99999f;
                break;
        }

        uint32_t now = PIOS_Thread_Systime();

        while (1) {
                if ((dev->data_ready_sema != NULL) && (dev->cfg->Mode == PIOS_HMC5883_MODE_CONTINUOUS)) {
                        if (PIOS_Semaphore_Take(dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true) {
                                PIOS_Thread_Sleep(100);
                                continue;
                        }
                } else {
                        PIOS_Thread_Sleep_Until(&now, sample_delay);
                }

                struct pios_sensor_mag_data mag_data;
                if (PIOS_HMC5883_ReadMag(&mag_data) == 0)
                        PIOS_Queue_Send(dev->queue, &mag_data, 0);
        }
}

#endif /* PIOS_INCLUDE_HMC5883 */