Micolink Protocol Definition and Parsing

#pragma once

#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#define MICOLINK_MSG_HEAD            0xEF
#define MICOLINK_MAX_PAYLOAD_LEN     64
#define MICOLINK_MAX_LEN             MICOLINK_MAX_PAYLOAD_LEN + 7

/*
    Message ID definitions
*/
enum
{
    MICOLINK_MSG_ID_RANGE_SENSOR = 0x51,     // Range sensor
};

/*
    Message structure definition
*/
typedef struct
{
    uint8_t head;                      
    uint8_t dev_id;                          
    uint8_t sys_id;						
    uint8_t msg_id;                        
    uint8_t seq;                          
    uint8_t len;                               
    uint8_t payload[MICOLINK_MAX_PAYLOAD_LEN]; 
    uint8_t checksum;                          

    uint8_t status;                           
    uint8_t payload_cnt;                       
} MICOLINK_MSG_t;

/*
    Payload definitions
*/
#pragma pack (1)
// Range sensor
typedef struct
{
    uint32_t  time_ms;			    // System time, ms
    uint32_t  distance;			    // Distance (mm); minimum valid value is 10, and 0 means unavailable
    uint8_t   strength;	            // Signal strength
    uint8_t   precision;	        // Precision
    uint8_t   tof_status;	        // Status
    uint8_t  reserved1;			    // Reserved
    int16_t   flow_vel_x;	        // Optical-flow velocity, X axis
    int16_t   flow_vel_y;	        // Optical-flow velocity, Y axis
    uint8_t   flow_quality;	        // Optical-flow quality
    uint8_t   flow_status;	        // Optical-flow status
    uint16_t  reserved2;	        // Reserved
} MICOLINK_PAYLOAD_RANGE_SENSOR_t;
#pragma pack ()

#include "mtf01.h"

/*
Usage: Use micolink_decode as the serial-data processing function.

The minimum valid distance is 10 mm; a value of 0 means that the distance is unavailable.
Optical-flow velocity unit: cm/s at 1 m
In the flight controller, multiply the optical-flow velocity by altitude to obtain the actual horizontal velocity.
Formula: actual velocity (cm/s) = optical-flow velocity × altitude (m)
*/

bool micolink_parse_char(MICOLINK_MSG_t* msg, uint8_t data);

void micolink_decode(uint8_t data)
{
    static MICOLINK_MSG_t msg;

    if(micolink_parse_char(&msg, data) == false)
        return;
    
    switch(msg.msg_id)
    {
        case MICOLINK_MSG_ID_RANGE_SENSOR:
        {
            MICOLINK_PAYLOAD_RANGE_SENSOR_t payload;
            memcpy(&payload, msg.payload, msg.len);

            /*
                Sensor data is available here:
            
                Distance               = payload.distance;
                Strength               = payload.strength;
                Precision              = payload.precision;
                Distance status        = payload.tof_status;
                Optical-flow velocity X = payload.flow_vel_x;
                Optical-flow velocity Y = payload.flow_vel_y;
                Optical-flow quality   = payload.flow_quality;	
                Optical-flow status    = payload.flow_status;
            */
            break;
        } 

        default:
            break;
        }
}

bool micolink_check_sum(MICOLINK_MSG_t* msg)
{
    uint8_t length = msg->len + 6;
    uint8_t temp[MICOLINK_MAX_LEN];
    uint8_t checksum = 0;

    memcpy(temp, msg, length);

    for(uint8_t i=0; i<length; i++)
    {
        checksum += temp[i];
    }

    if(checksum == msg->checksum)
        return true;
    else
        return false;
}

bool micolink_parse_char(MICOLINK_MSG_t* msg, uint8_t data)
{
    switch(msg->status)
    {
    case 0:     // Frame header
        if(data == MICOLINK_MSG_HEAD)
        {
            msg->head = data;
            msg->status++;
        }
        break;
        
    case 1:     // Device ID
        msg->dev_id = data;
        msg->status++;
        break;
    
    case 2:     // System ID
        msg->sys_id = data;
        msg->status++;
        break;
    
    case 3:     // Message ID
        msg->msg_id = data;
        msg->status++;
        break;
    
    case 4:     // Packet sequence
        msg->seq = data;
        msg->status++;
        break;
    
    case 5:     // Payload length
        msg->len = data;
        if(msg->len == 0)
            msg->status += 2;
        else if(msg->len > MICOLINK_MAX_PAYLOAD_LEN)
            msg->status = 0;
        else
            msg->status++;
        break;
        
    case 6:     // Receive payload
        msg->payload[msg->payload_cnt++] = data;
        if(msg->payload_cnt == msg->len)
        {
            msg->payload_cnt = 0;
            msg->status++;
        }
        break;
        
    case 7:     // Frame checksum
        msg->checksum = data;
        msg->status = 0;
        if(micolink_check_sum(msg))
        {
            return true;
        }
        
    default:
        msg->status = 0;
        msg->payload_cnt = 0;
        break;
    }

    return false;
}