Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 0:072c4589bc62, committed 2018-02-03
- Comitter:
- surpace0924
- Date:
- Sat Feb 03 15:12:59 2018 +0000
- Commit message:
- 1.1.0; ??????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SBUS/SBUS.cpp Sat Feb 03 15:12:59 2018 +0000 @@ -0,0 +1,103 @@ +#include "mbed.h" +#include "SBUS.hpp" + +SBUS::SBUS(PinName tx, PinName rx) : sbus_(tx, rx) +{ + sbusIndex = 0; + for (int i = 0; i < 18; i++) + rcChannel[i] = 0; + + failSafe = true; + + sbus_.baud(100000); + sbus_.format(8, Serial::Even, 2); + sbus_.attach(this, &SBUS::sbusIrqRx, Serial::RxIrq); +} + +void SBUS::sbusIrqRx () +{ + static uint8_t sbus[25] = {0}; + + int val = sbus_.getc(); + + + if (sbusIndex == 0 && val != 0x0F) { + return; + } + + sbus[sbusIndex] = val; + sbusIndex++; + + + if (sbusIndex == 25) { + sbusIndex = 0; + + if (sbus[24] != 0x0) { + rcChannel[0] = ((sbus[1] | sbus[2] << 8) & 0x07FF); + rcChannel[1] = ((sbus[2] >> 3 | sbus[3] << 5) & 0x07FF); + rcChannel[2] = ((sbus[3] >> 6 | sbus[4] << 2 | sbus[5] << 10) & 0x07FF); + rcChannel[3] = ((sbus[5] >> 1 | sbus[6] << 7) & 0x07FF); + rcChannel[4] = ((sbus[6] >> 4 | sbus[7] << 4) & 0x07FF); + rcChannel[5] = ((sbus[7] >> 7 | sbus[8] << 1 | sbus[9] << 9) & 0x07FF); + rcChannel[6] = ((sbus[9] >> 2 | sbus[10] << 6) & 0x07FF); + rcChannel[7] = ((sbus[10] >> 5 | sbus[11] << 3) & 0x07FF); + rcChannel[8] = ((sbus[12] | sbus[13] << 8) & 0x07FF); + rcChannel[9] = ((sbus[13] >> 3 | sbus[14] << 5) & 0x07FF); + rcChannel[10] = ((sbus[14] >> 6 | sbus[15] << 2 | sbus[16] << 10) & 0x07FF); + rcChannel[11] = ((sbus[16] >> 1 | sbus[17] << 7) & 0x07FF); + rcChannel[12] = ((sbus[17] >> 4 | sbus[18] << 4) & 0x07FF); + rcChannel[13] = ((sbus[18] >> 7 | sbus[19] << 1 | sbus[20] << 9) & 0x07FF); + rcChannel[14] = ((sbus[20] >> 2 | sbus[21] << 6) & 0x07FF); + rcChannel[15] = ((sbus[21] >> 5 | sbus[22] << 3) & 0x07FF); + + if ((sbus[23] >> 3) & 0x0001) { + // コントローラがOFF状態 + failSafe = true; + } else { + failSafe = false; + } + } + } +} + + + +int16_t SBUS::getData(uint8_t ch) +{ + return rcChannel[ch]; +} + +int SBUS::getStickVal(int axis) +{ + if(!failSafe) { + switch (axis) { + case 0: + return SBUS::map(getData(3), 368, 1680, -255, 255); + case 1: + return SBUS::map(getData(1), 368, 1680, 255, -255); + case 2: + return SBUS::map(getData(0), 368, 1680, -255, 255); + case 3: + return SBUS::map(getData(2), 368, 1680, -255, 255); + default: + return 0; + } + } else return 0; +} + +int SBUS::getSwitchVal(int parm) +{ + if(!failSafe) { + if (parm == 0) + return SBUS::map(getData(4), 144, 1904, 0, 2); + else if (parm <= 4) + return SBUS::map(getData(parm + 5), 144, 1904, 0, 2); + else + return 0; + } else return 3; +} + +long SBUS::map(long x, long in_min, long in_max, long out_min, long out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SBUS/SBUS.hpp Sat Feb 03 15:12:59 2018 +0000 @@ -0,0 +1,24 @@ +#ifndef _SBUS_ +#define _SBUS_ +#define _SBUS_ + +#include "mbed.h" + +class SBUS +{ +public: + SBUS(PinName tx, PinName rx); + int16_t getData(uint8_t ch); + int getStickVal(int axis); + int getSwitchVal(int parm); + bool failSafe; + +private: + RawSerial sbus_; + void sbusIrqRx (); + int sbusIndex; + unsigned int rcChannel[18]; + long map(long x, long in_min, long in_max, long out_min, long out_max); // From:Arduino.h +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Feb 03 15:12:59 2018 +0000 @@ -0,0 +1,24 @@ +#include "mbed.h" +#include "SBUS.hpp" +SBUS propo(PC_10, PC_11); +Serial pc(USBTX, USBRX); + +int main() +{ + pc.baud(57600); + while(1) { + if(propo.failSafe == true) + pc.printf("[NG]\t"); + else + pc.printf("[OK]\t"); + + + for (int i = 0; i < 4; i++) + pc.printf("%d\t", propo.getStickVal(i)); + + for (int i = 0; i < 5; i++) + pc.printf("%d\t", propo.getSwitchVal(i)); + + pc.printf("\n"); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Feb 03 15:12:59 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file