File content as of revision 27:61876b34ded4:

#include "mbed.h"
#include "sbus.h"


SBUS::SBUS(PinName tx, PinName rx)
    :
    rawserial_p(new RawSerial(tx,rx)),
    rawserial(*rawserial_p)
{} 

SBUS::~SBUS()
{
    if(rawserial_p != NULL) delete rawserial_p;
}

void SBUS::initialize(void){
    flg_ch_update = false;
    failsafe_status = SBUS_SIGNAL_FAILSAFE;
    cnt_sbus = 0;
    lastfunc = NULL;
  for (uint8_t i = 0; i < 25; i++)  buf_sbus[i] = 0;

  rawserial.baud(100000);
  rawserial.format(8, Serial::Even, 2);
}

void SBUS::startInterrupt(void){
  rawserial.attach(this, &SBUS::func_interrupt, RawSerial::RxIrq);  //fromSBUStoPWMの前に＆が必要?
}

//SBUS::stopInterrupt(void){
//}

void SBUS::setLastfuncPoint(void (*func_p)(void)){
    lastfunc = func_p;
}

//シリアル割り込みで実行する関数
void SBUS::func_interrupt(void){
    if(catchSerial()){
        unpackSerialdata();
        inputPwm();
        if(lastfunc != NULL)    (*lastfunc)();  //setLastfuncPointで設定した関数を最後に実行
    }
    return;
}

int8_t SBUS::catchSerial(void){
    buf_sbus[cnt_sbus] = rawserial.getc();
  if(buf_sbus[0] == 0x0f)   cnt_sbus ++;
  if(cnt_sbus >=25){  
    if(buf_sbus[24] == 0x00){
      return(1);     //calculatePwm実行
    }else{
      cnt_sbus = 0;
    }
  }
    return(0);
}

void SBUS::unpackSerialdata(void){
/*  for (uint8_t i=0; i<25; i++){
        pc.printf("%x ", buf_sbus[i]);
        }pc.printf("\n"); 
*/
    // clear channels[]
    for (uint8_t i=0; i<18; i++) {channels[i] = 0;}

    // reset counters
    byte_in_sbus = 1;
    bit_in_sbus = 0;
    ch = 0;
    bit_in_channel = 0;

    // process actual sbus data
    for (uint8_t i=0; i<176; i++) {
        if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
            channels[ch] |= (1<<bit_in_channel);
        }
        bit_in_sbus++;
        bit_in_channel++;

        if (bit_in_sbus == 8) {
            bit_in_sbus =0;
            byte_in_sbus++;
        }
        if (bit_in_channel == 11) {
            bit_in_channel =0;
            ch++;
        }
    }
    // DigiChannel 1
    if (buf_sbus[23] & (1<<0)) {
        channels[16] = 1;
    }else{
        channels[16] = 0;
    }
    // DigiChannel 2
    if (buf_sbus[23] & (1<<1)) {
        channels[17] = 1;
    }else{
        channels[17] = 0;
    }
    // Failsafe
    failsafe_status = SBUS_SIGNAL_OK;
    if (buf_sbus[23] & (1<<2)) {
        failsafe_status = SBUS_SIGNAL_LOST;
    }
    if (buf_sbus[23] & (1<<3)) {
        failsafe_status = SBUS_SIGNAL_FAILSAFE;
    }
    //channels[i] = channels[i]>>1;
    for (uint8_t i=0; i<25; i++) {buf_sbus[i] = 0;}
    cnt_sbus = 0;
    flg_ch_update = true;
    
    return;
}

void SBUS::inputPwm(void){
  for(uint8_t i=0; i<8; i++){
    manualpwm[i] = (channels[i]>>1) + 1000;
  }
}