松靈機器人Scout代碼分析 --- scout_ros

公司採購了一款室外的四輪差速底盤，正在看通訊的代碼。

代碼的github: https://github.com/westonrobot/scout_ros

 

scout_base_node.cpp

1. 調用　ScoutBase.h　的　connetc() : 設置通訊方式（serial or can ）並　進行連接

2. scout_base/src/scout_messenger.h 的　SetupSubscription() : 發佈odom, scout_status topic, 設置訂閱　/cmd_vel, /scout_light_control 的回調函數, 頻率爲20hz

3. 發佈/scout_status，機器人狀態

int main(int argc, char **argv)
{
    // setup ROS node
    ros::init(argc, argv, "scout_odom");
    ros::NodeHandle node("scout_robot"), private_node("~");

    // instantiate a robot
    ScoutBase robot;
    ScoutROSMessenger messenger(&robot, node);

    std::string scout_can_port;
    private_node.param<std::string>("port_name", scout_can_port, std::string("can0"));
    private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
    private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_footprint"));
    private_node.param<bool>("simulated_robot", messenger.simulated_robot_, false);

    // connect to scout and setup ROS subscription
    robot.Connect(scout_can_port);
    messenger.SetupSubscription();

    // publish robot state at 20Hz while listening to twist commands
    ros::Rate rate_20hz(20); // 20Hz
    while (true)
    {
        messenger.PublishStateToROS();
        ros::spinOnce();
        rate_20hz.sleep();
    }
    return 0;
}


// resigeter callback function
void ScoutROSMessenger::SetupSubscription()
{
    // odometry publisher
    odom_publisher_ = nh_.advertise<nav_msgs::Odometry>(odom_frame_, 50);
    status_publisher_ = nh_.advertise<scout_msgs::ScoutStatus>("/scout_status", 10);

    // cmd subscriber
    motion_cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不啓用平滑包則訂閱“cmd_vel”
    light_cmd_subscriber_ = nh_.subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
}

1. 通訊連接並設置回調函數

scout_base/src/scout_sdk/src/scout_base/src/scout_base.cpp

1.1 自定義的消息類型

每次消息到來之後，首先轉換消息類型，轉成一個　ScoutStatusMessage　這個結構體，這個結構體每次只更新一項，由msg_type設置。

typedef struct 
{
    ScoutStatusMsgType msg_type;

    // only one of the following fields is updated, as specified by msg_type
    MotionStatusMessage motion_status_msg;
    LightStatusMessage light_status_msg;
    SystemStatusMessage system_status_msg;
    MotorDriverStatusMessage motor_driver_status_msg;
} ScoutStatusMessage;

// For convenience to access status/control message
typedef enum
{
    ScoutStatusNone = 0x00,
    ScoutMotionStatusMsg = 0x01,
    ScoutLightStatusMsg = 0x02,
    ScoutSystemStatusMsg = 0x03,
    ScoutMotorDriverStatusMsg = 0x04
} ScoutStatusMsgType;

typedef struct {
    union
    {
        struct
        {
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } linear_velocity;
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } angular_velocity;
            uint8_t reserved0;
            uint8_t reserved1;
            uint8_t count;
            uint8_t checksum;
        } status;
        uint8_t raw[8];
    } data;
} MotionStatusMessage;

typedef struct {
    union
    {
        struct
        {
            uint8_t light_ctrl_enable;
            uint8_t front_light_mode;
            uint8_t front_light_custom;
            uint8_t rear_light_mode;
            uint8_t rear_light_custom;
            uint8_t reserved0;
            uint8_t count;
            uint8_t checksum;
        } status;
        uint8_t raw[8];
    } data;
} LightStatusMessage;

typedef struct {
    union
    {
        struct
        {
            uint8_t base_state;
            uint8_t control_mode;
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } battery_voltage;
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } fault_code;
            uint8_t count;
            uint8_t checksum;
        } status;
        uint8_t raw[8];
    } data;
} SystemStatusMessage;

// Motor Driver Feedback
typedef struct
{
    uint8_t motor_id;
    union {
        struct
        {
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } current;
            struct
            {
                uint8_t high_byte;
                uint8_t low_byte;
            } rpm;
            int8_t temperature;
            uint8_t reserved0;
            uint8_t count;
            uint8_t checksum;
        } status;
        uint8_t raw[8];
    } data;
} MotorDriverStatusMessage;

 

// choose serial or Can communication
void ScoutBase::Connect(std::string dev_name, int32_t baud_rate)
{
    if (baud_rate == 0)
    {
        ConfigureCANBus(dev_name);
    }
    else
    {
        ConfigureSerial(dev_name, baud_rate);

        if (!serial_connected_)
            std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
    }
}

void ScoutBase::ConfigureCANBus(const std::string &can_if_name)
{
    can_if_ = std::make_shared<ASyncCAN>(can_if_name);

    can_if_->set_receive_callback(std::bind(&ScoutBase::ParseCANFrame, this, std::placeholders::_1));

    can_connected_ = true;
}

void ScoutBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
{
    serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
    serial_if_->open();

    if (serial_if_->is_open())
        serial_connected_ = true;

    serial_if_->set_receive_callback(std::bind(&ScoutBase::ParseUARTBuffer, this,
                                               std::placeholders::_1,
                                               std::placeholders::_2,
                                               std::placeholders::_3));
}

 

2 讀取數據　及　更新狀態數據

void ScoutBase::ParseCANFrame(can_frame *rx_frame)
{
    // validate checksum, discard frame if fails
    if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc))
    {
        std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl;
        return;
    }

    // otherwise, update robot state with new frame
    ScoutStatusMessage status_msg;
    DecodeScoutStatusMsgFromCAN(rx_frame, &status_msg);
    NewStatusMsgReceivedCallback(status_msg);
}


void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
    // std::cout << "bytes received from serial: " << bytes_received << std::endl;
    ScoutStatusMessage status_msg;
    for (int i = 0; i < bytes_received; ++i)
    {
        if (DecodeScoutStatusMsgFromUART(buf[i], &status_msg))
            NewStatusMsgReceivedCallback(status_msg);
    }
}


void ScoutBase::NewStatusMsgReceivedCallback(const ScoutStatusMessage &msg)
{
    // std::cout << "new status msg received" << std::endl;
    std::lock_guard<std::mutex> guard(scout_state_mutex_);
    UpdateScoutState(msg, scout_state_);
}




void ScoutBase::UpdateScoutState(const ScoutStatusMessage &status_msg, ScoutState &state)
{
    switch (status_msg.msg_type)
    {
    case ScoutMotionStatusMsg:
    {
        // std::cout << "motion control feedback received" << std::endl;
        const MotionStatusMessage &msg = status_msg.motion_status_msg;
        state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0;
        state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0;
        break;
    }
    case ScoutLightStatusMsg:
    {
        // std::cout << "light control feedback received" << std::endl;
        const LightStatusMessage &msg = status_msg.light_status_msg;
        if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
            state.light_control_enabled = false;
        else
            state.light_control_enabled = true;
        state.front_light_state.mode = msg.data.status.front_light_mode;
        state.front_light_state.custom_value = msg.data.status.front_light_custom;
        state.rear_light_state.mode = msg.data.status.rear_light_mode;
        state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
        break;
    }
    case ScoutSystemStatusMsg:
    {
        // std::cout << "system status feedback received" << std::endl;
        const SystemStatusMessage &msg = status_msg.system_status_msg;
        state.control_mode = msg.data.status.control_mode;
        state.base_state = msg.data.status.base_state;
        state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0;
        state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
        break;
    }
    case ScoutMotorDriverStatusMsg:
    {
        // std::cout << "motor 1 driver feedback received" << std::endl;
        const MotorDriverStatusMessage &msg = status_msg.motor_driver_status_msg;
        for (int i = 0; i < 4; ++i)
        {
            state.motor_states[status_msg.motor_driver_status_msg.motor_id].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
            state.motor_states[status_msg.motor_driver_status_msg.motor_id].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
            state.motor_states[status_msg.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature;
        }
        break;
    }
    }
}

 

3. 發送指令

3.1 設置  線速度，角速度。

訂閱 /cmd_vel ，之後將收到的線速度，角速度　除以　最大值　再乘以　100，得到　線速度，角速度的百分比。

motion_cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);

// scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (!simulated_robot_)
    {
        scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
    }
    else
    {
        std::lock_guard<std::mutex> guard(twist_mutex_);
        current_twist_ = *msg.get();
    }
    // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
}


void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag)
{
    // make sure cmd thread is started before attempting to send commands
    if (!cmd_thread_started_)
        StartCmdThread();

    if (linear_vel < ScoutMotionCmd::min_linear_velocity)
        linear_vel = ScoutMotionCmd::min_linear_velocity;
    if (linear_vel > ScoutMotionCmd::max_linear_velocity)
        linear_vel = ScoutMotionCmd::max_linear_velocity;
    if (angular_vel < ScoutMotionCmd::min_angular_velocity)
        angular_vel = ScoutMotionCmd::min_angular_velocity;
    if (angular_vel > ScoutMotionCmd::max_angular_velocity)
        angular_vel = ScoutMotionCmd::max_angular_velocity;

    std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
    current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
    current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
    current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}

 

3.2 內置的消息類型

// Motion Control
typedef struct {
    union
    {
        struct
        {
            uint8_t control_mode;
            uint8_t fault_clear_flag;
            int8_t linear_velocity_cmd;
            int8_t angular_velocity_cmd;
            uint8_t reserved0;
            uint8_t reserved1;
            uint8_t count;
            uint8_t checksum;
        } cmd;
        uint8_t raw[8];
    } data;
} MotionControlMessage;

3.3 定時向底層發送數據(100hz)

// 開啓 速度　cmd 線程 ScoutBase::ControlLoop
void ScoutBase::StartCmdThread()
{
    current_motion_cmd_.linear_velocity = 0;
    current_motion_cmd_.angular_velocity = 0;
    current_motion_cmd_.fault_clear_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT;

    // cmd thread runs at 100Hz (10ms) by default ; cmd_thread_period_ms_ = 10
    cmd_thread_ = std::thread(std::bind(&ScoutBase::ControlLoop, this, cmd_thread_period_ms_));
    cmd_thread_started_ = true;
}

void ScoutBase::ControlLoop(int32_t period_ms)
{
    StopWatch ctrl_sw;
    uint8_t cmd_count = 0;
    uint8_t light_cmd_count = 0;
    while (true)
    {
        ctrl_sw.tic();

        // motion control message
        SendMotionCmd(cmd_count++);

        // check if there is request for light control
        if (light_ctrl_requested_)
            SendLightCmd(light_cmd_count++);

        ctrl_sw.sleep_until_ms(period_ms);
        // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
    }
}

void ScoutBase::SendMotionCmd(uint8_t count)
{
    // motion control message
    MotionControlMessage m_msg;

    if (can_connected_)
        m_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
    else if (serial_connected_)
        m_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;

    motion_cmd_mutex_.lock();
    m_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
    m_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
    m_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
    motion_cmd_mutex_.unlock();

    m_msg.data.cmd.reserved0 = 0;
    m_msg.data.cmd.reserved1 = 0;
    m_msg.data.cmd.count = count;

    if (can_connected_)
        m_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, m_msg.data.raw, 8);
    // serial_connected_: checksum will be calculated later when packed into a complete serial frame

    if (can_connected_)
    {
        // send to can bus
        can_frame m_frame;
        EncodeScoutMotionControlMsgToCAN(&m_msg, &m_frame);
        can_if_->send_frame(m_frame);
    }
    else
    {
        // send to serial port
        EncodeMotionControlMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
        serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
    }
}

 

4 燈光控制

4.1 ROS 下的消息類型

uint8 LIGHT_CONST_OFF = 0
uint8 LIGHT_CONST_ON = 1
uint8 LIGHT_BREATH = 2
uint8 LIGHT_CUSTOM = 3

bool enable_cmd_light_control
uint8 front_mode
uint8 front_custom_value
uint8 rear_mode
uint8 rear_custom_value

4.2 自定義消息類型

struct ScoutLightCmd
{
    enum class LightMode
    {
        CONST_OFF = 0x00,
        CONST_ON = 0x01,
        BREATH = 0x02,
        CUSTOM = 0x03
    };

    ScoutLightCmd() = default;
    ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value),
                                                                                          rear_mode(r_mode), rear_custom_value(r_value) {}

    LightMode front_mode;
    uint8_t front_custom_value;
    LightMode rear_mode;
    uint8_t rear_custom_value;
};


// Light Control
typedef struct {
    union
    {
        struct
        {
            uint8_t light_ctrl_enable;
            uint8_t front_light_mode;
            uint8_t front_light_custom;
            uint8_t rear_light_mode;
            uint8_t rear_light_custom;
            uint8_t reserved0;
            uint8_t count;
            uint8_t checksum;
        } cmd;
        uint8_t raw[8];
    } data;
} LightControlMessage;

 

4.3 設置燈光

light_cmd_subscriber_ = nh_.subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);

// scout_->SetLightCommand(cmd);
void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
{
    if (!simulated_robot_)
    {
        if (msg->enable_cmd_light_control)
        {
            ScoutLightCmd cmd;

            switch (msg->front_mode)
            {
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
                cmd.front_custom_value = msg->front_custom_value;
                break;
            }
            }

            switch (msg->rear_mode)
            {
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
                cmd.rear_custom_value = msg->rear_custom_value;
                break;
            }
            }

            scout_->SetLightCommand(cmd);
        }
        else
        {
            scout_->DisableLightCmdControl();
        }
    }
    else
    {
        std::cout << "simulated robot received light control cmd" << std::endl;
    }
}


void ScoutBase::SetLightCommand(ScoutLightCmd cmd)
{
    if (!cmd_thread_started_)
        StartCmdThread();

    std::lock_guard<std::mutex> guard(light_cmd_mutex_);
    current_light_cmd_ = cmd;
    light_ctrl_enabled_ = true;
    light_ctrl_requested_ = true;
}

4.4 發送燈光控制指令

void ScoutBase::SendLightCmd(uint8_t count)
{
    LightControlMessage l_msg;

    light_cmd_mutex_.lock();
    if (light_ctrl_enabled_)
    {
        l_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;

        l_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
        l_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
        l_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
        l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
    }
    else
    {
        l_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;

        l_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
        l_msg.data.cmd.front_light_custom = 0;
        l_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
        l_msg.data.cmd.rear_light_custom = 0;
    }
    light_ctrl_requested_ = false;
    light_cmd_mutex_.unlock();

    l_msg.data.cmd.reserved0 = 0;
    l_msg.data.cmd.count = count;

    if (can_connected_)
        l_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.data.raw, 8);
    // serial_connected_: checksum will be calculated later when packed into a complete serial frame

    if (can_connected_)
    {
        // send to can bus
        can_frame l_frame;
        EncodeScoutLightControlMsgToCAN(&l_msg, &l_frame);

        can_if_->send_frame(l_frame);
    }
    else
    {
        // send to serial port
        EncodeLightControlMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
        serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
    }

}

5 通訊協議解析

5.1 串口通訊

typedef union {
    ScoutStatusMessage status_msg;
    ScoutControlMessage control_msg;
} ScoutDecodedMessage;

// c爲讀取的字節
bool DecodeScoutStatusMsgFromUART(uint8_t c, ScoutStatusMessage *msg)
{
    static ScoutDecodedMessage decoded_msg;

    bool result = ParseChar(c, &decoded_msg);
    if (result)
        *msg = decoded_msg.status_msg;
    return result;
}

bool ParseChar(uint8_t c, ScoutDecodedMessage *msg)
{
    static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;

    bool new_frame_parsed = false;
    switch (decode_state)
    {
    case WAIT_FOR_SOF1:
    {
        if (c == FRAME_SOF1)
        {
            frame_id = FRAME_NONE_ID;
            frame_type = 0;
            frame_len = 0;
            frame_cnt = 0;
            frame_checksum = 0;
            internal_checksum = 0;
            payload_data_pos = 0;
            memset(payload_buffer, 0, PAYLOAD_BUFFER_SIZE);

            decode_state = WAIT_FOR_SOF2;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "found sof1" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "found sof1\n");
#endif
        }
        break;
    }
    case WAIT_FOR_SOF2:
    {
        if (c == FRAME_SOF2)
        {
            decode_state = WAIT_FOR_FRAME_LEN;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "found sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "found sof2\n");
#endif
        }
        else
        {
            decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "failed to find sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "failed to find sof2\n");
#endif
        }
        break;
    }
    case WAIT_FOR_FRAME_LEN:
    {
        frame_len = c;
        decode_state = WAIT_FOR_FRAME_TYPE;
#ifdef PRINT_CPP_DEBUG_INFO
        std::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
        JLinkRTTPrintf(0, "frame len: %d\n", frame_len);
#endif
        break;
    }
    case WAIT_FOR_FRAME_TYPE:
    {
        switch (c)
        {
        case FRAME_TYPE_CONTROL:
        {
            frame_type = FRAME_TYPE_CONTROL;
            decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "control type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "control type frame received\n");
#endif
            break;
        }
        case FRAME_TYPE_STATUS:
        {
            frame_type = FRAME_TYPE_STATUS;
            decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "status type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "status type frame received\n");
#endif
            break;
        }
        default:
        {
#ifdef PRINT_CPP_DEBUG_INFO
            std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n");
#endif
            decode_state = WAIT_FOR_SOF1;
        }
        }
        break;
    }
    case WAIT_FOR_FRAME_ID:
    {
        switch (c)
        {
        case UART_FRAME_SYSTEM_STATUS_ID:
        case UART_FRAME_MOTION_STATUS_ID:
        case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
        case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
        case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
        case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
        case UART_FRAME_LIGHT_STATUS_ID:
        {
            frame_id = c;
            decode_state = WAIT_FOR_PAYLOAD;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkRTTPrintf(0, "frame id: %d\n", frame_id);
#endif
            break;
        }
        default:
        {
#ifdef PRINT_CPP_DEBUG_INFO
            std::cerr << "ERROR: Unknown frame id" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "ERROR: Unknown frame id\n");
#endif
            decode_state = WAIT_FOR_SOF1;
        }
        }
        break;
    }
    case WAIT_FOR_PAYLOAD:
    {
        payload_buffer[payload_data_pos++] = c;
#ifdef PRINT_CPP_DEBUG_INFO
        std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
        JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif
        if (payload_data_pos == (frame_len - FRAME_FIXED_FIELD_LEN))
            decode_state = WAIT_FOR_FRAME_COUNT;
        break;
    }
    case WAIT_FOR_FRAME_COUNT:
    {
        frame_cnt = c;
        decode_state = WAIT_FOR_CHECKSUM;
#ifdef PRINT_CPP_DEBUG_INFO
        std::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
        JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt);
#endif
        break;
    }
    case WAIT_FOR_CHECKSUM:
    {
        frame_checksum = c;
        internal_checksum = CalcBufferedFrameChecksum();
        new_frame_parsed = true;
        decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFO
        std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
        std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
        JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum);
        JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum);
#endif
        break;
    }
    default:
        break;
    }

    if (new_frame_parsed)
    {
        if (frame_checksum == internal_checksum)
        {
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "checksum correct" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "checksum correct\n");
#endif
            if (frame_type == FRAME_TYPE_STATUS)
                ConstructStatusMessage(&(msg->status_msg));
            else if (frame_type == FRAME_TYPE_CONTROL)
                ConstructControlMessage(&(msg->control_msg));
            ++frame_parsed;
        }
        else
        {
            ++frame_with_wrong_checksum;
#ifdef PRINT_CPP_DEBUG_INFO
            std::cout << "checksum is NOT correct" << std::endl;
            std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl;
            std::cout << "payload: ";
            for (int i = 0; i < payload_data_pos; ++i)
                std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " ";
            std::cout << std::endl;
            std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
            std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
            JLinkWriteString(0, "checksum is NOT correct\n");
#endif
        }
    }

    return new_frame_parsed;
}

 

5.2 can通訊