bluepill mbed os can bus problem

add_nanolib.py

Import("env")
env.Append(LINKFLAGS=["--specs=nano.specs"])

CANMsg.h

#ifndef CANMSG_H
#define CANMSG_H

/* CAN message container.
 * Provides "<<" (append) and ">>" (extract) operators to simplyfy
 * adding/getting data items to/from a CAN message.
 * Usage is similar to the C++ io-stream operators.
 * Data length of CAN message is automatically updated when using "<<" or ">>" operators.
 *
 * See Wiki page <https://developer.mbed.org/users/hudakz/code/CAN_Hello/> for demo.
 */

#include "CAN.h"

class CANMsg : public mbed::CANMessage
{
public:
    /** Creates empty CAN message.
     */
    CANMsg() :
        CANMessage(){ }

    /** Creates CAN message with specific content.
     */
    CANMsg(int _id, const char *_data, char _len = 8, CANType _type = CANData, CANFormat _format = CANStandard) :
        CANMessage(_id, _data, _len, _type, _format){ }

    /** Creates CAN remote message.
     */
    CANMsg(int _id, CANFormat _format = CANStandard) :
        CANMessage(_id, _format){ }

    /** Clears CAN message content
     */
    void clear(void) {
        len    = 0;
        type   = CANData;
        format = CANStandard;
        id     = 0;
        memset(data, 0, 8);
    };

    /** Append operator: Appends data (value) to CAN message
     */
    template<class T>
    CANMsg &operator<<(const T val) {
        MBED_ASSERT(len + sizeof(T) <= 8);
        memcpy(&data[len], &val, sizeof(T));
        len += sizeof(T);
        return *this;
    }

    /** Extract operator: Extracts data (value) from CAN message
     */
    template<class T>
    CANMsg &operator>>(T& val) {
        MBED_ASSERT(sizeof(T) <= len);
        if (sizeof(T) > len) {
            memcpy(&val, data, len);
            len = 0;
        }
        else {
            memcpy(&val, data, sizeof(T));
            len -= sizeof(T);
        }
        memcpy(data, data + sizeof(T), len);
        return *this;
    }
};

#endif // CANMSG_H

main.cpp

#include <mbed.h>
#include "CANMsg.h"

#define USE_PLL_HSE_XTAL 1
#define CAN_DELAY 15

CAN can1(PA_11, PA_12);

static bool can_status = false;
DigitalOut led(PC_13);

static uint16_t can_period = 125;
uint64_t can_inits = 0;

static int can_errors = 0;

Thread led_thread(osPriorityLow, 1000, NULL, NULL);
Thread tx_thread(osPriorityHigh, 5000, NULL, NULL);

void blink(int count, int total, int on) {
    for (int i = 0; i < count; i++) {
        led = 0;
        ThisThread::sleep_for(on);
        led = 1;
        ThisThread::sleep_for(total - on);
    }
}

void onCanReceived(void) {
    CANMsg rxMsg;
    can1.read(rxMsg);
}

void init_can() {
    can_status = false;
    if (can_inits > 65000) {
        can_inits = 0;
    }
    can_inits++;
    can1.reset();
    can1.frequency(1000000);
    can1.mode(CAN::Normal);
    can1.attach(onCanReceived);
    can_errors = 0;
    can_status = true;
}

void handle_can_tx_error() {
    can_errors++;
    ThisThread::sleep_for(can_period);
    if ( can_errors >= 5) {
        can_status = false;
        init_can();
        ThisThread::sleep_for(can_period);
    }
}

void tx_task() {
    while (can_status != true) {
        ThisThread::sleep_for(100);
    }
    int iterator = 0;
    while (true) {
        CANMsg data;

        data.clear();
        data.id = 0x320;
        data << (int32_t)320;

        if (can1.write(data) == 0) {
            handle_can_tx_error();
        };
        ThisThread::sleep_for(CAN_DELAY);

        data.clear();
        data.id = 0x321;
        data << (int32_t)321;

        if (can1.write(data) == 0) {
            handle_can_tx_error();
        };
        ThisThread::sleep_for(CAN_DELAY);

        if (iterator >= 10) {
            data.clear();
            data.id = 0x328;
            data << (uint64_t)328;
            if (can1.write(data) == 0) {
                handle_can_tx_error();
            };
            ThisThread::sleep_for(CAN_DELAY);

            data.clear();
            data.id = 0x329;
            data << (uint32_t)329;
            if (can1.write(data) == 0) {
                handle_can_tx_error();
            };
            ThisThread::sleep_for(CAN_DELAY);

            data.clear();
            data.id = 0x327;
            data << (uint32_t)327;
            if (can1.write(data) == 0) {
                handle_can_tx_error();
            };
            ThisThread::sleep_for(CAN_DELAY);

            data.clear();
            data.id = 0x199;
            data << can_inits;
            if (can1.write(data) == 0) {
                handle_can_tx_error();
            };
            ThisThread::sleep_for(CAN_DELAY);

            iterator = 0;
        } else {
            iterator++;
        }
        ThisThread::sleep_for(can_period);
    }
}

void led_task() {
    while (true) {
        if (can_status == true) {
            blink(1, 1000, 200);
        } else  {
            blink(2, 500, 100);
        }
        ThisThread::sleep_for(1000);
    }
}

int main() {
    init_can();
    led_thread.start(led_task);
    tx_thread.start(tx_task);

    while (true) {
        ThisThread::sleep_for(125);
    }
}

platformio.ini

; PlatformIO Project Configuration File
;
;   Build options: build flags, source filter
;   Upload options: custom upload port, speed and extra flags
;   Library options: dependencies, extra library storages
;   Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html

[env:bluepill_f103c8]
platform = ststm32
board = bluepill_f103c8
framework = mbed
build_flags = -DPIO_FRAMEWORK_MBED_RTOS_PRESENT
board_build.f_cpu = 64000000L
board_config.clock_source = USE_PLL_HSE_XTAL
lib_ignore = mbed-net, mbed-rpc
extra_scripts = add_nanolib.py