File content as of revision 12:f726eb78b54c:

#ifndef _MY_CAN_H_
#define _MY_CAN_H_

#include <mbed.h>
#include "odom.h"
#include "myRos.h"
#include "type.h"

/* CAN_GLOBAL DEFINE BEGIN */

typedef uint16_t FIXED_NUM_16;

typedef union{
    char array[6];
    
    struct{
        uint8_t frameName;
        FIXED_NUM_16 x;
        FIXED_NUM_16 y;
        uint8_t use_can_ack;
    }data;
}can_odom_xy_t;

typedef union{
    char array[6];
    
    struct{
        uint8_t frameName;
        uint8_t angle[4];
        uint8_t use_can_ack;
    }data;
}can_odom_angle_t;

typedef union{
    char array[3];
    
    struct{
        uint8_t frameName;
        uint8_t court_color : 1;
        uint8_t enable_oled : 1;
        uint8_t unuse : 6;
        uint8_t use_can_ack;
    }data;
}can_odom_config_t;

#define NORMAL_TYPE                 (0x00)
#define PING_TYPE                   (0x01)
#define ACK_TYPE                    (0x02)

#define ID_MAIN                     (0x00)
#define ID_ODOM                     (0x01)

#define FIXED_NUM_INT_MAX           (0x0f)
#define CAN_FILTER_SID_MASK         (0x07ff)
#define CAN_FILTER_TYP_MASK         (0x0007)
#define CAN_FILTER_SND_MASK         (0x0078)
#define CAN_FILTER_DST_MASK         (0x0780)

#define ODOM_DATA_XY                (0xa0)
#define ODOM_DATA_ANGLE             (0xa1)
#define ODOM_SET_INITIAL_XY         (0xa2)
#define ODOM_SET_INITIAL_ANGLE      (0xa3)
#define ODOM_SET_CONFIG             (0xa4)
#define ODOM_RESET                  (0xa5)
/* CAN_GLOBAL DEFINE END */

class My_Can : public Odom_Abstract, CAN
{
    private:
        FunctionPointer amcl_initialize_;
        FunctionPointer led_toggle_;
        
        Timer timer_;
        
        bool enable_send_odom_;
    
    public:
        My_Can(Odom *odom) : Odom_Abstract(odom), CAN(PB_8, PB_9, 1000000){
            filter(CreateSid(0, ID_MAIN, ID_ODOM), CAN_FILTER_SND_MASK | CAN_FILTER_DST_MASK, CANStandard);
            timer_.start();
            enable_send_odom_ = false;
        }
        
        void initialize_amcl_attach(My_Ros *object, void (My_Ros::*member)(void)){
            amcl_initialize_.attach(object, member);
        }
        
        void led_toggle_attach(void (*function)(void)){
            led_toggle_.attach(function);
        }
        
        void can_rx_it_cb(void (*function)(void)){
            attach(function);   
        }
        
        void test(){
//            led_toggle_.call();
        }
        
        void receive_cb();
        
    private:
    
        void check_initial_frame(uint8_t *data);
        void send_odom();
        
        FIXED_NUM_16 EncodeFixedNumber(float fn){
            uint16_t sign_part = 0;
            uint16_t int_part = 0;
            uint16_t frac_part = 0;
            
            //符号チェック
            if(fn < 0){
                sign_part = 1;
                fn = -fn;
            }
            
            //サイズチェック
            if(fn > FIXED_NUM_INT_MAX){
                //ERROR
                fn = FIXED_NUM_INT_MAX;
            }
            
            //整数部取得
            int_part = (uint16_t)fn;
            
            //小数以下取得
            float frac_tmp = fn - (float)int_part;
            
            //小数以下計算
            for(int i = 0; i < 11; i++){
                frac_tmp *= 2;
                if(frac_tmp >= 1){
                    frac_part |= (0x0400 >> i);    
                    frac_tmp = frac_tmp - (int)frac_tmp;
                }
            }
            
            FIXED_NUM_16 ret = (sign_part << 15) | (int_part << 11) | frac_part;
            return ret;
        }
        
        float DecodeFixedNumber(FIXED_NUM_16 fn){
            float output = 0;
        
            for(int i = 0; i < 11; i++){
                if((fn & 0x07ff) & (0x0400 >> i)){
                    output += 1.0f / (float)(1 << (i + 1));
                }
            }
            
            output += (float)((fn & 0x7800) >> 11);
            
            return (fn & 0x8000) ? -output : output;
        }
        
        /**
         * @brief float型データをCAN用のuint8_t型データに変換する関数
         * @param float_data
         * @param send_buf
         */
        void EncodeFloat(float float_data, uint8_t *send_buf) {
            union {
                uint8_t byte[4];
                float floating;
            } enc;
        
            enc.floating = float_data;
        
            send_buf[0] = enc.byte[0];
            send_buf[1] = enc.byte[1];
            send_buf[2] = enc.byte[2];
            send_buf[3] = enc.byte[3];
        }
        
        /**
         * @brief CAN用のuint8_t型データをfloat型に変換する関数
         * @param receive_buf
         * @return デコードされたデータ
         */
        float DecodeFloat(uint8_t *receive_buf) {
            union {
                uint8_t byte[4];
                float floating;
            } enc;
        
            enc.byte[0] = receive_buf[0];
            enc.byte[1] = receive_buf[1];
            enc.byte[2] = receive_buf[2];
            enc.byte[3] = receive_buf[3];
        
            return enc.floating;
        }
        
        /**
         * @brief CANのSIDを作成
         * (MSB:宛先(4bit)、送り主(4bit)、データ型(3bit):LSB)で格納
         * @param type
         * @param sender
         * @param destination
         * @return 作成したSID
         */
        uint16_t CreateSid(uint8_t type, uint8_t sender, uint8_t destination) {
            uint16_t msg = 0;
        
            type &= 0x07;
            sender &= 0x0f;
            destination &= 0x0f;
        
            msg = ((uint16_t) destination << 7);
            msg += ((uint16_t) sender << 3);
            msg += type;
            return msg;
        }
        
        /**
         * @brief 受信したフレームのSIDからTypeを取得
         * @param receive_msg 受信したSID
         * @return 抽出したType
         */
        uint8_t GetType(uint16_t receive_msg) {
            return (receive_msg & (0b0000000000000111));
        }
        
        /**
         * @brief 受信したフレームのSIDからSenderを取得
         * @param receive_msg 受信したSID
         * @return 抽出したSender
         */
        uint8_t GetSender(uint16_t receive_msg) {
            return ((receive_msg & (0b0000000001111000)) >> 3);
        }
        
        /**
         * @brief 受信したフレームのSIDからDestinationを取得
         * @param receive_msg 受信したSID
         * @return 抽出したDestination
         */
        uint8_t GetDestination(uint16_t receive_msg) {
            return ((receive_msg & (0b0000011110000000)) >> 7);
        }
        
        //Overlap function
        virtual void loop();

};

#endif