aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.h
- Committer:
- nakedt555
- Date:
- 2018-12-18
- Revision:
- 6:20a32baeff79
- Parent:
- 5:e678f1ac6cdc
- Child:
- 7:b240464868e8
File content as of revision 6:20a32baeff79:
#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[8]; struct{ uint8_t frameName; uint8_t id; FIXED_NUM_16 x; FIXED_NUM_16 y; uint32_t unuse : 16; }data; }can_odom_xy_t; //pose initializeとフレームを共用にするためcourt_colorを入れた //通常のデータ送信では使わない typedef union{ char array[8]; struct{ uint8_t frameName; uint8_t id; uint8_t angle[4]; uint8_t court_color; uint32_t unuse : 8; }data; }can_odom_angle_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_RESET (0xa4) /* CAN_GLOBAL DEFINE END */ class My_Can : public Odom_Abstract, CAN { private: FunctionPointer amcl_initialize_; FunctionPointer led_toggle_; Timer timer_; 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(); } 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); 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