pios_px4flow.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 "coordinate_conversions.h"
#include "physical_constants.h"

#if defined(PIOS_INCLUDE_PX4FLOW)
#include "pios_px4flow_priv.h"

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

/* Private constants */
#define PX4FLOW_TASK_PRIORITY        PIOS_THREAD_PRIO_HIGH
#define PX4FLOW_TASK_STACK_BYTES     512
#define PX4FLOW_SAMPLE_PERIOD_MS     5
#define PIOS_PX4FLOW_MAX_DOWNSAMPLE  1

/* Global Variables */

/* Local Types */
enum pios_px4flow_dev_magic {
        PIOS_PX4FLOW_DEV_MAGIC = 0x1dbef871, // md5 hash of the string "PIOS_PX4FLOW_DEV_MAGIC"
};

struct px4flow_dev {
        pios_i2c_t i2c_id;
        const struct pios_px4flow_cfg *cfg;
        struct pios_queue *optical_flow_queue;
        struct pios_queue *rangefinder_queue;
        struct pios_thread *task;
        struct pios_semaphore *data_ready_sema;
        enum pios_px4flow_dev_magic magic;
        float Rsb[3][3];
};

/* Local Variables */
static int32_t PIOS_PX4Flow_Config(const struct pios_px4flow_cfg * cfg);
static void PIOS_PX4Flow_Task(void *parameters);

static struct px4flow_dev *dev;

static struct px4flow_dev * PIOS_PX4Flow_alloc(void)
{
        struct px4flow_dev *px4flow_dev;
        
        px4flow_dev = (struct px4flow_dev *)PIOS_malloc(sizeof(*px4flow_dev));
        if (!px4flow_dev) return (NULL);
        
        px4flow_dev->magic = PIOS_PX4FLOW_DEV_MAGIC;
        
        px4flow_dev->optical_flow_queue = PIOS_Queue_Create(PIOS_PX4FLOW_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_optical_flow_data));
        px4flow_dev->rangefinder_queue = PIOS_Queue_Create(PIOS_PX4FLOW_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_rangefinder_data));
        if (px4flow_dev->optical_flow_queue == NULL || px4flow_dev->rangefinder_queue == NULL) {
                PIOS_free(px4flow_dev);
                return NULL;
        }

        return px4flow_dev;
}

static int32_t PIOS_PX4Flow_Validate(struct px4flow_dev *dev)
{
        if (dev == NULL)
                return -1;
        if (dev->magic != PIOS_PX4FLOW_DEV_MAGIC)
                return -2;
        if (dev->i2c_id == 0)
                return -3;
        return 0;
}

int32_t PIOS_PX4Flow_Init(const struct pios_px4flow_cfg *cfg, pios_i2c_t i2c_id)
{
        dev = (struct px4flow_dev *) PIOS_PX4Flow_alloc();
        if (dev == NULL)
                return -1;

        dev->cfg = cfg;
        dev->i2c_id = i2c_id;
        PIOS_PX4Flow_SetRotation(cfg->rotation);

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

        PIOS_SENSORS_Register(PIOS_SENSOR_OPTICAL_FLOW, dev->optical_flow_queue);
        PIOS_SENSORS_Register(PIOS_SENSOR_RANGEFINDER, dev->rangefinder_queue);

        dev->task = PIOS_Thread_Create(PIOS_PX4Flow_Task, "pios_px4flow", PX4FLOW_TASK_STACK_BYTES, NULL, PX4FLOW_TASK_PRIORITY);

        PIOS_Assert(dev->task != NULL);

        return 0;
}

int32_t PIOS_PX4Flow_SetRotation(const struct Rotation rotation)
{
        if (PIOS_PX4Flow_Validate(dev) != 0)
                return -1;

        float rpy_R[3]  = {rotation.roll_D100/100.0f * DEG2RAD, rotation.pitch_D100/100.0f * DEG2RAD, rotation.yaw_D100/100.0f * DEG2RAD};

        Euler2R(rpy_R, dev->Rsb);


        return 0;
}

static int32_t PIOS_PX4Flow_Config(const struct pios_px4flow_cfg * cfg)
{
        // This function intentionally left blank.

        return 0;
}

static int32_t PIOS_PX4Flow_ReadData()
{
        if (PIOS_PX4Flow_Validate(dev) != 0) {
                return -1;
        }

        /* don't use PIOS_PX4Flow_Read and PIOS_PX4Flow_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_PX4FLOW_FRAMECOUNTER_LSB
        };

        /* Optical flow structs */
        struct I2C_Frame
        {
                uint16_t frame_count; // counts created I2C frames [#frames]
                int16_t pixel_flow_x_sum_px10; // latest x flow measurement in pixels*10 [pixels]
                int16_t pixel_flow_y_sum_px10; // latest y flow measurement in pixels*10 [pixels]
                int16_t flow_comp_x_m1000; // x velocity*1000 [meters/sec]
                int16_t flow_comp_y_m1000; // y velocity*1000 [meters/sec]
                uint16_t qual; // Optical flow solution quality [0: bad, 255: maximum quality]
                int16_t gyro_x_R; // latest gyro x rate [rad/sec]
                int16_t gyro_y_R; // latest gyro y rate [rad/sec]
                int16_t gyro_z_R; // latest gyro z rate [rad/sec]
                uint8_t gyro_range; // gyro range [0 .. 7] equals [50 deg/sec .. 2000 deg/sec]
                uint8_t sonar_timestamp; // time since last sonar update [milliseconds]
                int16_t ground_distance_m1000; // Ground distance in meters*1000 [meters]. Positive value: distance known. Negative value: Unknown distance
        } __attribute__((packed)) i2c_frame;

        const struct pios_i2c_txn txn_list[] = {
                {
                        .info = __func__,
                        .addr = PIOS_PX4FLOW_I2C_7_BIT_ADDR,
                        .rw = PIOS_I2C_TXN_WRITE,
                        .len = 1,
                        .buf = addr_read,
                },
                {
                        .info = __func__,
                        .addr = PIOS_PX4FLOW_I2C_7_BIT_ADDR,
                        .rw = PIOS_I2C_TXN_READ,
                        .len = sizeof(i2c_frame),
                        .buf = (uint8_t *)&i2c_frame,
                }
        };

        // Perform transfer, and return error if TX fails
        if (PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list)) != 0)
                return -1;


        // PX4Flow docs state that `quality` is an int16, but that it only takes values
        // from 0..255. This provides a useful check for corruption.
        if (i2c_frame.qual > 255) {
                return -1;
        }

        struct pios_sensor_optical_flow_data optical_flow_data = { 0 };
        struct pios_sensor_rangefinder_data rangefinder_data = { 0 };

        // Only update rangefinder queue if data is fresh XXX this conditional looks
        // wrong
        if (i2c_frame.sonar_timestamp > 0) {
                rangefinder_data.range = i2c_frame.ground_distance_m1000 / 1000.0f;

                if (rangefinder_data.range >= 0) {
                        rangefinder_data.range_status = true;
                }

                PIOS_Queue_Send(dev->rangefinder_queue, &rangefinder_data, 0);
        }

        /* Rotate the flow from the sensor frame into the body frame. It's not
          * good to set the z-axis velocity to 0, but the optical flow doesn't return
          * any data along that direction, and moreover if the sensor is always
          * mounted vertically then the sensor board mounting yaw angle will not
          * affect the vertical results.
          */
        float flow_sensor_frame[3] = {i2c_frame.flow_comp_x_m1000 / 1000.0f, i2c_frame.flow_comp_y_m1000 / 1000.0f, 0 / 1000.0f};
        float flow_rotated[3];
        rot_mult(dev->Rsb, flow_sensor_frame, flow_rotated, true);
        optical_flow_data.x_dot = flow_rotated[0];
        optical_flow_data.y_dot = flow_rotated[1];
        optical_flow_data.z_dot = flow_rotated[2];

        optical_flow_data.quality = i2c_frame.qual;

        PIOS_Queue_Send(dev->optical_flow_queue, &optical_flow_data, 0);

        return 0;
}


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

        uint32_t now = PIOS_Thread_Systime();

        while (1) {
                PIOS_Thread_Sleep_Until(&now, PX4FLOW_SAMPLE_PERIOD_MS);

                PIOS_PX4Flow_ReadData();
        }
}

#endif /* PIOS_INCLUDE_PX4FLOW */