diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
new file mode 100644
index 0000000000..540eab907e
--- /dev/null
+++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
@@ -0,0 +1,272 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include
+
+#if HAL_WITH_UAVCAN
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "AP_ToshibaCAN.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)
+
+// data format for messages from flight controller
+static const uint8_t COMMAND_STOP = 0x0;
+static const uint8_t COMMAND_LOCK = 0x10;
+static const uint8_t COMMAND_REQUEST_DATA = 0x20;
+static const uint8_t COMMAND_MOTOR3 = 0x3B;
+static const uint8_t COMMAND_MOTOR2 = 0x3D;
+static const uint8_t COMMAND_MOTOR1 = 0x3F;
+
+// data format for messages from ESC
+static const uint8_t MOTOR_DATA1 = 0x40;
+static const uint8_t MOTOR_DATA2 = 0x50;
+static const uint8_t MOTOR_DATA3 = 0x60;
+static const uint8_t MOTOR_DATA5 = 0x80;
+
+// processing definitions
+static const uint16_t TOSHIBACAN_OUTPUT_MIN = 6300;
+static const uint16_t TOSHIBACAN_OUTPUT_MAX = 32000;
+static const uint16_t TOSHIBACAN_SEND_TIMEOUT_US = 500;
+static const uint8_t CAN_IFACE_INDEX = 0;
+
+AP_ToshibaCAN::AP_ToshibaCAN()
+{
+ debug_can(2, "ToshibaCAN: constructed\n\r");
+}
+
+AP_ToshibaCAN *AP_ToshibaCAN::get_tcan(uint8_t driver_index)
+{
+ if (driver_index >= AP::can().get_num_drivers() ||
+ AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_ToshibaCAN) {
+ return nullptr;
+ }
+ return static_cast(AP::can().get_driver(driver_index));
+}
+
+// initialise ToshibaCAN bus
+void AP_ToshibaCAN::init(uint8_t driver_index)
+{
+ _driver_index = driver_index;
+
+ debug_can(2, "ToshibaCAN: starting init\n\r");
+
+ if (_initialized) {
+ debug_can(1, "ToshibaCAN: already initialized\n\r");
+ return;
+ }
+
+ AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
+
+ if (can_mgr == nullptr) {
+ debug_can(1, "ToshibaCAN: no mgr for this driver\n\r");
+ return;
+ }
+
+ if (!can_mgr->is_initialized()) {
+ debug_can(1, "ToshibaCAN: mgr not initialized\n\r");
+ return;
+ }
+
+ _can_driver = can_mgr->get_driver();
+
+ if (_can_driver == nullptr) {
+ debug_can(1, "ToshibaCAN: no CAN driver\n\r");
+ return;
+ }
+
+ // start calls to loop in separate thread
+ if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ToshibaCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_MAIN, 1)) {
+ debug_can(1, "ToshibaCAN: couldn't create thread\n\r");
+ return;
+ }
+
+ _initialized = true;
+
+ debug_can(2, "ToshibaCAN: init done\n\r");
+
+ return;
+}
+
+// loop to send output to ESCs in background thread
+void AP_ToshibaCAN::loop()
+{
+ uavcan::MonotonicTime timeout;
+ const uint32_t timeout_us = MIN(AP::scheduler().get_loop_period_us(), TOSHIBACAN_SEND_TIMEOUT_US);
+
+ while (true) {
+ if (!_initialized) {
+ // if not initialised wait 2ms
+ debug_can(2, "ToshibaCAN: not initialized\n\r");
+ hal.scheduler->delay_microseconds(2000);
+ continue;
+ }
+
+ // check for updates
+ if (update_count == update_count_sent) {
+ hal.scheduler->delay_microseconds(50);
+ continue;
+ }
+
+ // prepare commands and frames
+ if (send_stage == 0) {
+ motor_lock_cmd_t unlock_cmd = {};
+ motor_rotation_cmd_t mot_rot_cmd1;
+ motor_rotation_cmd_t mot_rot_cmd2;
+ motor_rotation_cmd_t mot_rot_cmd3;
+ {
+ // take semaphore to read scaled motor outputs
+ WITH_SEMAPHORE(_rc_out_sem);
+
+ // prepare command to lock or unlock motors
+ unlock_cmd.motor1 = (_scaled_output[0] == 0) ? 2 : 1;
+ unlock_cmd.motor2 = (_scaled_output[1] == 0) ? 2 : 1;
+ unlock_cmd.motor3 = (_scaled_output[2] == 0) ? 2 : 1;
+ unlock_cmd.motor4 = (_scaled_output[3] == 0) ? 2 : 1;
+ unlock_cmd.motor5 = (_scaled_output[4] == 0) ? 2 : 1;
+ unlock_cmd.motor6 = (_scaled_output[5] == 0) ? 2 : 1;
+ unlock_cmd.motor7 = (_scaled_output[6] == 0) ? 2 : 1;
+ unlock_cmd.motor8 = (_scaled_output[7] == 0) ? 2 : 1;
+ unlock_cmd.motor9 = (_scaled_output[8] == 0) ? 2 : 1;
+ unlock_cmd.motor10 = (_scaled_output[9] == 0) ? 2 : 1;
+ unlock_cmd.motor11 = (_scaled_output[10] == 0) ? 2 : 1;
+ unlock_cmd.motor12 = (_scaled_output[11] == 0) ? 2 : 1;
+
+ // prepare command to spin motors in bank1
+ mot_rot_cmd1.motor1 = htobe16(_scaled_output[0]);
+ mot_rot_cmd1.motor2 = htobe16(_scaled_output[1]);
+ mot_rot_cmd1.motor3 = htobe16(_scaled_output[2]);
+ mot_rot_cmd1.motor4 = htobe16(_scaled_output[3]);
+
+ // prepare message to spin motors in bank2
+ mot_rot_cmd2.motor1 = htobe16(_scaled_output[4]);
+ mot_rot_cmd2.motor2 = htobe16(_scaled_output[5]);
+ mot_rot_cmd2.motor3 = htobe16(_scaled_output[6]);
+ mot_rot_cmd2.motor4 = htobe16(_scaled_output[7]);
+
+ // prepare message to spin motors in bank3
+ mot_rot_cmd3.motor1 = htobe16(_scaled_output[8]);
+ mot_rot_cmd3.motor2 = htobe16(_scaled_output[9]);
+ mot_rot_cmd3.motor3 = htobe16(_scaled_output[10]);
+ mot_rot_cmd3.motor4 = htobe16(_scaled_output[11]);
+
+ // copy update time
+ update_count_buffered = update_count;
+ }
+ unlock_frame = {(uint8_t)COMMAND_LOCK, unlock_cmd.data, sizeof(unlock_cmd.data)};
+ mot_rot_frame1 = {((uint8_t)COMMAND_MOTOR1 & uavcan::CanFrame::MaskStdID), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};
+ mot_rot_frame2 = {((uint8_t)COMMAND_MOTOR2 & uavcan::CanFrame::MaskStdID), mot_rot_cmd2.data, sizeof(mot_rot_cmd2.data)};
+ mot_rot_frame3 = {((uint8_t)COMMAND_MOTOR3 & uavcan::CanFrame::MaskStdID), mot_rot_cmd3.data, sizeof(mot_rot_cmd3.data)};
+
+ // advance to next stage
+ send_stage++;
+ }
+
+ // send unlock command
+ if (send_stage == 1) {
+ timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
+ if (!write_frame(unlock_frame, timeout)) {
+ continue;
+ }
+ send_stage++;
+ }
+
+ // send output to motor bank3
+ if (send_stage == 2) {
+ timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
+ if (!write_frame(mot_rot_frame3, timeout)) {
+ continue;
+ }
+ send_stage++;
+ }
+
+ // send output to motor bank2
+ if (send_stage == 3) {
+ timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
+ if (!write_frame(mot_rot_frame2, timeout)) {
+ continue;
+ }
+ send_stage++;
+ }
+
+ // send output to motor bank1
+ if (send_stage == 4) {
+ timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
+ if (!write_frame(mot_rot_frame1, timeout)) {
+ continue;
+ }
+ }
+
+ // success!
+ send_stage = 0;
+
+ // record success so we don't send this frame again
+ update_count_sent = update_count_buffered;
+ }
+}
+
+// write frame on CAN bus
+bool AP_ToshibaCAN::write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout)
+{
+ // wait for space in buffer to send unlock command
+ uavcan::CanSelectMasks inout_mask;
+ do {
+ inout_mask.read = 0;
+ inout_mask.write = 1 << CAN_IFACE_INDEX;
+ _select_frames[CAN_IFACE_INDEX] = &out_frame;
+ _can_driver->select(inout_mask, _select_frames, timeout);
+
+ // delay if no space is available to send
+ if (!inout_mask.write) {
+ hal.scheduler->delay_microseconds(50);
+ }
+ } while (!inout_mask.write);
+
+ // send frame and return success
+ return (_can_driver->getIface(CAN_IFACE_INDEX)->send(out_frame, timeout, 0) == 1);
+}
+
+// called from SRV_Channels
+void AP_ToshibaCAN::update()
+{
+ // take semaphore and update outputs
+ WITH_SEMAPHORE(_rc_out_sem);
+ for (uint8_t i = 0; i < MIN(TOSHIBACAN_MAX_NUM_ESCS, 16); i++) {
+ SRV_Channel *c = SRV_Channels::srv_channel(i);
+ if (c == nullptr) {
+ _scaled_output[i] = 0;
+ } else {
+ uint16_t pwm_out = c->get_output_pwm();
+ if (pwm_out <= 1000) {
+ _scaled_output[i] = 0;
+ } else if (pwm_out >= 2000) {
+ _scaled_output[i] = TOSHIBACAN_OUTPUT_MAX;
+ } else {
+ _scaled_output[i] = TOSHIBACAN_OUTPUT_MIN + (pwm_out - 1000) * 0.001f * (TOSHIBACAN_OUTPUT_MAX - TOSHIBACAN_OUTPUT_MIN);
+ }
+ }
+ }
+ update_count++;
+}
+
+#endif // HAL_WITH_UAVCAN
diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h
new file mode 100644
index 0000000000..9e277f2f6c
--- /dev/null
+++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h
@@ -0,0 +1,98 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+#define TOSHIBACAN_MAX_NUM_ESCS 12
+
+class AP_ToshibaCAN : public AP_HAL::CANProtocol {
+public:
+ AP_ToshibaCAN();
+ ~AP_ToshibaCAN();
+
+ /* Do not allow copies */
+ AP_ToshibaCAN(const AP_ToshibaCAN &other) = delete;
+ AP_ToshibaCAN &operator=(const AP_ToshibaCAN&) = delete;
+
+ // Return ToshibaCAN from @driver_index or nullptr if it's not ready or doesn't exist
+ static AP_ToshibaCAN *get_tcan(uint8_t driver_index);
+
+ // initialise ToshibaCAN bus
+ void init(uint8_t driver_index) override;
+
+ // called from SRV_Channels
+ void update();
+
+private:
+
+ // loop to send output to ESCs in background thread
+ void loop();
+
+ // write frame on CAN bus, returns true on success
+ bool write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout);
+
+ bool _initialized;
+ char _thread_name[9];
+ uint8_t _driver_index;
+ uavcan::ICanDriver* _can_driver;
+ const uavcan::CanFrame* _select_frames[uavcan::MaxCanIfaces] { };
+
+ // PWM output
+ HAL_Semaphore _rc_out_sem;
+ uint16_t _scaled_output[TOSHIBACAN_MAX_NUM_ESCS];
+ uint16_t update_count; // counter increments each time main thread updates outputs
+ uint16_t update_count_buffered; // counter when outputs copied to buffer before before sending to ESCs
+ uint16_t update_count_sent; // counter of outputs successfully sent
+ uint8_t send_stage; // stage of sending algorithm (each stage sends one frame to ESCs)
+
+ // structure for sending motor lock command to ESC
+ union motor_lock_cmd_t {
+ struct PACKED {
+ uint8_t motor2:4;
+ uint8_t motor1:4;
+ uint8_t motor4:4;
+ uint8_t motor3:4;
+ uint8_t motor6:4;
+ uint8_t motor5:4;
+ uint8_t motor8:4;
+ uint8_t motor7:4;
+ uint8_t motor10:4;
+ uint8_t motor9:4;
+ uint8_t motor12:4;
+ uint8_t motor11:4;
+ };
+ uint8_t data[6];
+ };
+
+ // structure for sending turn rate command to ESC
+ union motor_rotation_cmd_t {
+ struct PACKED {
+ uint16_t motor4;
+ uint16_t motor3;
+ uint16_t motor2;
+ uint16_t motor1;
+ };
+ uint8_t data[8];
+ };
+
+ // frames to be sent
+ uavcan::CanFrame unlock_frame;
+ uavcan::CanFrame mot_rot_frame1;
+ uavcan::CanFrame mot_rot_frame2;
+ uavcan::CanFrame mot_rot_frame3;
+};