WIP: AP_HAL: add CANDriver

jschall · jschall · commit db3ed06f258d · 2016-12-07T18:01:22.000-08:00
diff --git a/libraries/AP_HAL/CAN.cpp b/libraries/AP_HAL/CAN.cpp
@@ -0,0 +1,22 @@
+#include "CAN.h"
+
+void AP_HAL::CANDriver::register_receiver(uint8_t bus_mask, ReceiveCb cb) {
+    if (_num_receivers == AP_HAL_CAN_MAX_RECEIVERS) {
+        return;
+    }
+
+    _receivers[_num_receivers].bus_mask = bus_mask;
+    _receivers[_num_receivers].cb = cb;
+
+    _num_receivers++;
+}
+
+void AP_HAL::CANDriver::process_message(uint32_t recv_time_us, const struct message_s& message)
+{
+    for (uint8_t i=0; i<_num_receivers; i++) {
+        if (_receivers[i].bus_mask & (1<<message.bus_id)) {
+            _receivers[i].cb(recv_time_us, message);
+        }
+    }
+}
+
diff --git a/libraries/AP_HAL/CAN.h b/libraries/AP_HAL/CAN.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <stdint.h>
+#include "AP_HAL_Namespace.h"
+#include "utility/functor.h"
+
+#define AP_HAL_CAN_MAX_RECEIVERS 8
+
+class AP_HAL::CANDriver {
+public:
+    FUNCTOR_TYPEDEF(ReceiveCb, void, uint32_t, const struct message_s&);
+    struct message_s {
+        uint8_t bus_id;
+        uint32_t id;
+        uint8_t rtr : 1;
+        uint8_t extid : 1;
+        uint8_t length : 4;
+        uint8_t data[8];
+    };
+
+    void register_receiver(uint8_t bus_mask, ReceiveCb cb);
+
+    virtual bool transmit(const struct message_s& message) = 0;
+
+protected:
+    void process_message(uint32_t recv_time_us, const struct message_s& message);
+
+private:
+    struct receiver_s {
+        uint8_t bus_mask;
+        ReceiveCb cb;
+    };
+
+    uint8_t _num_receivers;
+    struct receiver_s _receivers[AP_HAL_CAN_MAX_RECEIVERS];
+};
diff --git a/libraries/CAN_Servo/CAN_Servo.cpp b/libraries/CAN_Servo/CAN_Servo.cpp
@@ -0,0 +1,31 @@
+#include "CAN_Servo.h"
+
+extern const AP_HAL::HAL& hal;
+
+CAN_Servo::CAN_Servo(uint8_t bus, uint32_t angle_command_id, uint32_t angle_feedback_id) :
+_angle_command_id(angle_command_id),
+_angle_feedback_id(angle_feedback_id),
+_bus(bus)
+{
+    hal.can->register_receiver(1<<bus, FUNCTOR_BIND_MEMBER(can_recv, void, uint32_t, const struct AP_HAL::CAN::message_s&));
+}
+
+void CAN_Servo::send_angle_target(float angle) {
+    // TODO populate the message struct in AP_HAL::CAN::transmit, not here
+
+    struct AP_HAL::CAN::message_s msg;
+    msg.bus_id = _bus;
+    msg.id = _angle_command_id;
+    msg.rtr = 0;
+    msg.extid = 0;
+    msg.length = 4;
+    memcpy(msg.data, &angle, 4);
+    hal.can->transmit(msg);
+}
+
+void CAN_Servo::can_recv(uint32_t time_us, const struct AP_HAL::CAN::message_s& msg)
+{
+    if (msg.id == _angle_feedback_id && msg.length == 4) {
+        memcpy(&_angle, msg.data, 4);
+    }
+}
diff --git a/libraries/CAN_Servo/CAN_Servo.h b/libraries/CAN_Servo/CAN_Servo.h
@@ -0,0 +1,13 @@
+#include <AP_HAL/AP_HAL.h>
+
+class CAN_Servo {
+public:
+    CAN_Servo(uint8_t bus, uint32_t angle_command_id, uint32_t angle_feedback_id);
+    void send_angle_target(float angle);
+private:
+    void can_recv(uint32_t time_us, const struct AP_HAL::CAN::message_s& msg);
+    float _angle;
+    uint32_t _angle_command_id;
+    uint32_t _angle_feedback_id;
+    uint8_t _bus;
+};
