/** * Sylphide Autopilot DSP Program * * * */ //#define ORCA_SPECIAL //#define MOGURA_SPECIAL #define IMU_IS_E0 #define SERVO_TEST #include #if defined(SIM_MODE) #include "sylphide_simcfg.h" #elif defined(ROM_TARGET) #include "sylphide_ROMcfg.h" #else #include "sylphidecfg.h" #endif // Chip support Library #include #include #include #include #include #include #ifdef DEBUG_HST_USE //#undef DEBUG_HST_USE #endif //#define MCBSP_BUFFER_IN_IRAM /* Local software delay function */ static void _plldelay(Uint32 Count){ volatile int i = Count; while(i--); } static void init_EMIF(){ EMIF_Config emifCfg0 = { 0x60 | EMIF_FMKS(GBLCTL, NOHOLD, DISABLE) | EMIF_FMKS(GBLCTL, CLK1EN, DISABLE) | EMIF_FMKS(GBLCTL, CLK2EN, ENABLE), EMIF_FMKS(CECTL, WRSETUP, DEFAULT) | EMIF_FMKS(CECTL, WRSTRB, DEFAULT) | EMIF_FMKS(CECTL, WRHLD, DEFAULT) | EMIF_FMKS(CECTL, RDSETUP, DEFAULT) | EMIF_FMKS(CECTL, TA, OF(2)) | EMIF_FMKS(CECTL, RDSTRB, DEFAULT) | EMIF_FMKS(CECTL, MTYPE, SDRAM16) | EMIF_FMKS(CECTL, RDHLD, DEFAULT), EMIF_FMKS(CECTL, WRSETUP, OF(0)) | EMIF_FMKS(CECTL, WRSTRB, OF(8)) | EMIF_FMKS(CECTL, WRHLD, OF(2)) | EMIF_FMKS(CECTL, RDSETUP, OF(0)) | EMIF_FMKS(CECTL, TA, OF(2)) | EMIF_FMKS(CECTL, RDSTRB, OF(8)) | EMIF_FMKS(CECTL, MTYPE, ASYNC16) | EMIF_FMKS(CECTL, RDHLD, OF(2)), EMIF_FMKS(CECTL, WRSETUP, OF(2)) | EMIF_FMKS(CECTL, WRSTRB, OF(10)) | EMIF_FMKS(CECTL, WRHLD, OF(2)) | EMIF_FMKS(CECTL, RDSETUP, OF(2)) | EMIF_FMKS(CECTL, TA, OF(2)) | EMIF_FMKS(CECTL, RDSTRB, OF(10)) | EMIF_FMKS(CECTL, MTYPE, ASYNC32) | EMIF_FMKS(CECTL, RDHLD, OF(2)), EMIF_FMKS(CECTL, WRSETUP, OF(2)) | EMIF_FMKS(CECTL, WRSTRB, OF(10)) | EMIF_FMKS(CECTL, WRHLD, OF(2)) | EMIF_FMKS(CECTL, RDSETUP, OF(2)) | EMIF_FMKS(CECTL, TA, OF(2)) | EMIF_FMKS(CECTL, RDSTRB, OF(10)) | EMIF_FMKS(CECTL, MTYPE, ASYNC32) | EMIF_FMKS(CECTL, RDHLD, OF(2)), EMIF_FMKS(SDCTL, SDBSZ, 4BANKS) | EMIF_FMKS(SDCTL, SDRSZ, 12ROW) | EMIF_FMKS(SDCTL, SDCSZ, 8COL) | EMIF_FMKS(SDCTL, RFEN, ENABLE) | EMIF_FMKS(SDCTL, INIT, YES) | EMIF_FMKS(SDCTL, TRCD, OF(1)) | EMIF_FMKS(SDCTL, TRP, OF(1)) | EMIF_FMKS(SDCTL, TRC, OF(4)), EMIF_FMKS(SDTIM, CNTR, OF(0)) | EMIF_FMKS(SDTIM, PERIOD, OF(1250)), EMIF_FMKS(SDEXT, WR2RD, OF(0)) | EMIF_FMKS(SDEXT, WR2DEAC, OF(2)) | EMIF_FMKS(SDEXT, WR2WR, OF(1)) | EMIF_FMKS(SDEXT, R2WDQM, OF(1)) | EMIF_FMKS(SDEXT, RD2WR, OF(0)) | EMIF_FMKS(SDEXT, RD2DEAC, OF(1)) | EMIF_FMKS(SDEXT, RD2RD, OF(0)) | EMIF_FMKS(SDEXT, THZP, OF(2)) | EMIF_FMKS(SDEXT, TWR, OF(1)) | EMIF_FMKS(SDEXT, TRRD, OF(0)) | EMIF_FMKS(SDEXT, TRAS, OF(2)) | EMIF_FMKS(SDEXT, TCL, OF(0)) }; /* Initialize all of the EMIF */ EMIF_config(&emifCfg0); } static void init_PLL(){ /* Put PLL in bypass */ PLL_bypass(); _plldelay(20); /* Reset the PLL */ PLL_reset(); _plldelay(20); /* Set main multiplier/divisor */ PLL_RSET(PLLM, 8); // 50MHz x 8 = 400MHz PLL_RSET(PLLDIV0, PLL_PLLDIV0_RMK(1, 0)); // 400MHz / 1 = 400MHz PLL_RSET(OSCDIV1, PLL_OSCDIV1_RMK(1, 4)); // 50MHz / 5 = 10Mhz /* Set DSP clock */ PLL_RSET(PLLDIV1, PLL_PLLDIV1_RMK(1, 1)); // 400MHz / 2 = 200MHz _plldelay(20); /* Set peripheral clock */ PLL_RSET(PLLDIV2, PLL_PLLDIV2_RMK(1, 3)); // 400MHz / 4 = 100MHz _plldelay(20); /* Set EMIF clock */ PLL_RSET(PLLDIV3, PLL_PLLDIV3_RMK(1, 4)); // 400MHz / 5 = 80MHz _plldelay(20); /* Take PLL out of reset */ PLL_deassert(); _plldelay(1500); /* Enable PLL */ PLL_enable(); _plldelay(20); } MCBSP_Handle hMcBSP0; static void init_McBSP0(){ // McBSP0をSPI Slaveにする(エレメントは32bits) MCBSP_Config MCBSP0_Config = { // Serial Port Control Register (SPCR) MCBSP_SPCR_RMK( MCBSP_SPCR_FREE_DEFAULT, MCBSP_SPCR_SOFT_DEFAULT, MCBSP_SPCR_FRST_YES, MCBSP_SPCR_GRST_YES, MCBSP_SPCR_XINTM_XRDY, MCBSP_SPCR_XSYNCERR_NO, MCBSP_SPCR_XRST_YES, MCBSP_SPCR_DLB_OFF, MCBSP_SPCR_RJUST_RZF, MCBSP_SPCR_CLKSTP_DELAY, MCBSP_SPCR_DXENA_OFF, MCBSP_SPCR_RINTM_RRDY, MCBSP_SPCR_RSYNCERR_NO, MCBSP_SPCR_RRST_YES ), // Receive Control Register (RCR) MCBSP_RCR_RMK( MCBSP_RCR_RPHASE_SINGLE, MCBSP_RCR_RFRLEN2_DEFAULT, MCBSP_RCR_RWDLEN2_DEFAULT, MCBSP_RCR_RCOMPAND_MSB, MCBSP_RCR_RFIG_NO, MCBSP_RCR_RDATDLY_0BIT, MCBSP_RCR_RFRLEN1_OF(0), MCBSP_RCR_RWDLEN1_32BIT, MCBSP_RCR_RWDREVRS_DISABLE ), // Transmit Control Register (XCR) MCBSP_XCR_RMK( MCBSP_XCR_XPHASE_SINGLE, MCBSP_XCR_XFRLEN2_DEFAULT, MCBSP_XCR_XWDLEN2_DEFAULT, MCBSP_XCR_XCOMPAND_MSB, MCBSP_XCR_XFIG_NO, MCBSP_XCR_XDATDLY_0BIT, MCBSP_XCR_XFRLEN1_OF(0), MCBSP_XCR_XWDLEN1_32BIT, MCBSP_XCR_XWDREVRS_DISABLE ), //serial port sample rate generator register(SRGR) MCBSP_SRGR_RMK( MCBSP_SRGR_GSYNC_FREE, MCBSP_SRGR_CLKSP_RISING, MCBSP_SRGR_CLKSM_INTERNAL, MCBSP_SRGR_FSGM_DXR2XSR, MCBSP_SRGR_FPER_DEFAULT, MCBSP_SRGR_FWID_DEFAULT, MCBSP_SRGR_CLKGDV_OF(1) ), MCBSP_MCR_DEFAULT, MCBSP_RCER_DEFAULT, MCBSP_XCER_DEFAULT, // serial port pin control register(PCR) MCBSP_PCR_RMK( MCBSP_PCR_XIOEN_SP, MCBSP_PCR_RIOEN_SP, MCBSP_PCR_FSXM_EXTERNAL, MCBSP_PCR_FSRM_DEFAULT, MCBSP_PCR_CLKXM_INPUT, MCBSP_PCR_CLKRM_DEFAULT, MCBSP_PCR_CLKSSTAT_0, MCBSP_PCR_DXSTAT_0, MCBSP_PCR_FSXP_ACTIVELOW, MCBSP_PCR_FSRP_ACTIVELOW, MCBSP_PCR_CLKXP_FALLING, MCBSP_PCR_CLKRP_FALLING ) }; hMcBSP0 = MCBSP_open(MCBSP_DEV0, MCBSP_OPEN_RESET); MCBSP_config(hMcBSP0, &MCBSP0_Config); MCBSP_enableSrgr(hMcBSP0); #ifndef SIM_MODE for(volatile unsigned i = 0; i < 0xFFFF; i++); #endif IRQ_map(MCBSP_getRcvEventId(hMcBSP0), 11); //IRQ_map(MCBSP_getXmtEventId(hMcBSP0), 9); } EDMA_Handle hEDMA_NULL; static void init_EDMA(){ EDMA_reset(hEDMA_NULL); EDMA_clearPram(0x00000000); // Dynamically allocates PaRAM RAM table // Dummy or Terminating Table in PaRAM hEDMA_NULL = EDMA_allocTable(-1); IRQ_map(IRQ_EVT_EDMAINT, 8); IRQ_enable(IRQ_EVT_EDMAINT); } extern "C" { /* Initialize the board APIs */ void Sylphide_init(){ /* * NOTE: If running out of SDRAM make sure the SDRAM refresh counter * is set such that it will not miss refreshes when the new * frequency is adopted. */ CSL_init(); #ifndef SIM_MODE //init_PLL(); #endif //init_EMIF(); //CACHE_setL2Mode(CACHE_64KCACHE); // Sets the Caching mode to use 64k of dsp ram. //CACHE_enableCaching(CACHE_CE00); // Enable caching for range CACHE_CE00(0x80000000 to 0x80FFFFFF) init_McBSP0(); init_EDMA(); IRQ_nmiEnable(); } } // 標準C/C++ヘッダ #include #include #include // DSP BIOS ヘッダ #include #include #include #include #define ENABLE_EXCEPTION_THROW 0 #define ENABLE_IOSTREAM 0 #include "note/070423/SylphideStream.h" #include "note/070423/SylphideProcessor.h" extern "C"{ #include #include #include #include #include #include //#include //#include //#include //#include } #include "matrix.h" #include "vector3.h" #include "quarternion.h" #include "dsp67x_helper.h" using namespace std; #include "util/fifo.h" #include "util/crc.h" #define BUFFER_SIZE (PAGE_SIZE * 1) // 32 #define OBSERVER_SIZE (PAGE_SIZE * 32) // 1024 #define EVENT_EDMA_MCBSP0_RX 13 #define EVENT_EDMA_MCBSP0_TX 12 EDMA_Handle hEDMA_McBSP0_RX; EDMA_Config cfgEDMA_McBSP0_RX = { EDMA_OPT_RMK( EDMA_OPT_PRI_HIGH, // High priority EDMA EDMA_OPT_ESIZE_32BIT, // Element size 32 bits EDMA_OPT_2DS_NO, EDMA_OPT_SUM_NONE, EDMA_OPT_2DD_NO, EDMA_OPT_DUM_INC, // Destination increment by element size EDMA_OPT_TCINT_YES, // Enable Transfer Complete Interrupt EDMA_OPT_TCC_OF(EVENT_EDMA_MCBSP0_RX), // TCCINT = 0xD, REVT0 EDMA_OPT_LINK_YES, // Enable linking to NULL table EDMA_OPT_FS_NO ), EDMA_SRC_RMK(0), // src to DRR0 EDMA_CNT_RMK(0, 0), // no. of elements EDMA_DST_RMK(0), // dst addr to edmaInbuff EDMA_IDX_RMK(0, 0), EDMA_RLD_RMK(0, 0) }; EDMA_Handle hEDMA_McBSP0_TX; EDMA_Config cfgEDMA_McBSP0_TX = { EDMA_OPT_RMK( EDMA_OPT_PRI_HIGH, // High priority EDMA EDMA_OPT_ESIZE_32BIT, // Element size 32 bits EDMA_OPT_2DS_NO, EDMA_OPT_SUM_INC, // Source increment by element size EDMA_OPT_2DD_NO, EDMA_OPT_DUM_NONE, EDMA_OPT_TCINT_YES, // Enable Transfer Complete Interrupt EDMA_OPT_TCC_OF(EVENT_EDMA_MCBSP0_TX), // TCCINT = 0xC, XEVT0 EDMA_OPT_LINK_YES, // Enable linking to NULL table EDMA_OPT_FS_NO ), EDMA_SRC_RMK(0), // src to edmaOutbuff EDMA_CNT_RMK(0, 0), // set no. of elements EDMA_DST_RMK(0), // dst addr to DXR0 EDMA_IDX_RMK(0, 0), EDMA_RLD_RMK(0, 0) }; static void init_McBSP0_RX_TX_handler(){ IRQ_disable(IRQ_EVT_EDMAINT); IRQ_clear(IRQ_EVT_EDMAINT); IRQ_enable(IRQ_EVT_EDMAINT); // McBSP0を使ったEDMA受信 hEDMA_McBSP0_RX = EDMA_open(EDMA_CHA_REVT0, EDMA_OPEN_RESET); cfgEDMA_McBSP0_RX.src = MCBSP_ADDR(DRR0); //MCBSP_getRcvAddr(hMcBSP0); // Terminate EDMA transfers by linking to this NULL table cfgEDMA_McBSP0_RX.rld = EDMA_getTableAddress(hEDMA_NULL) & 0xFFFF; //EDMA_config(hEDMA_McBSP0_RX, &cfgEDMA_McBSP0_RX); EDMA_enableChannel(hEDMA_McBSP0_RX); EDMA_intEnable(EVENT_EDMA_MCBSP0_RX); // McBSP0を使ったEDMA送信 hEDMA_McBSP0_TX = EDMA_open(EDMA_CHA_XEVT0, EDMA_OPEN_RESET); cfgEDMA_McBSP0_TX.dst = MCBSP_ADDR(DXR0); //MCBSP_getXmtAddr(hMcBSP0); // Terminate EDMA transfers by linking to this NULL table cfgEDMA_McBSP0_TX.rld = EDMA_getTableAddress(hEDMA_NULL) & 0xFFFF; //EDMA_config(hEDMA_McBSP0_TX, &cfgEDMA_McBSP0_TX); EDMA_enableChannel(hEDMA_McBSP0_TX); EDMA_intEnable(EVENT_EDMA_MCBSP0_TX); #ifndef DEBUG_HST_USE MCBSP_start(hMcBSP0, MCBSP_RCV_START | MCBSP_XMIT_START, 0x0); //MCBSP_start(hMcBSP0, MCBSP_SRGR_FRAMESYNC, 0xFFFF); #endif // Fool cache... #ifndef SIM_MODE for(volatile unsigned i = 0; i < 0xFFFFFF; i++); #endif } typedef union{ struct { char payload[3]; unsigned char header; } bytes; Uint32 raw; } mcbsp_element_t; struct mcbsp_frame_t{ mcbsp_element_t *content; Uint32 elements; Uint32 max_elements; mcbsp_frame_t() : content(NULL), elements(0), max_elements(0) {} mcbsp_frame_t(const Uint32 &_elements) : content(new mcbsp_element_t[_elements]), elements(0), max_elements(_elements) {} mcbsp_frame_t(const Uint32 &_elements, mcbsp_element_t *_content) : content(_content), elements(0), max_elements(_elements) {} Uint32 encode(char *buf, Uint32 bytes){ elements = std::min(((bytes + 3) / 4 + 1), (max_elements - 1)); content[0].raw = bytes; for(Uint32 element_index = 1; element_index < elements; element_index++){ Uint32 element = *reinterpret_cast(buf); #ifdef DEBUG_HST_USE content[element_index].raw = element; #else content[element_index].raw = swap_endian(element); #endif buf += sizeof(Uint32); } return elements; } Uint32 decode(char *buf, Uint32 max_size){ char *buf_orig(buf); if(content){ bool previous_header_has_start_flag(true); for(Uint32 i(0), j(elements); i < elements; i++, j--){ // check header structure bool header_has_start_flag(content[i].bytes.header & 0x80); // 残りのエレメント数が降順になっていない // または、途中でフレームのスタートフラグが再び立ちだす // 以上の状況はおかしい if(((content[i].bytes.header & 0x1F) != j) || (header_has_start_flag && !(previous_header_has_start_flag))){ buf = buf_orig; break; } previous_header_has_start_flag = header_has_start_flag; char *src_addres(&(content[i].bytes.payload[2])); unsigned int copy_bytes((content[i].bytes.header & 0x60) >> 5); copy_bytes = std::min(copy_bytes, max_size); max_size -= copy_bytes; for(; copy_bytes > 0; copy_bytes--){ // endian swap *(buf++) = *(src_addres--); } } } return buf - buf_orig; } ~mcbsp_frame_t(){delete [] content;} }; template class mbx_handler_t { private: MBX_Handle _handle; Uns _size; public: mbx_handler_t(const Uns &size) : _handle(MBX_create(sizeof(T), size, NULL)), _size(size){ } ~mbx_handler_t(){ if(_handle){MBX_delete(_handle);} } Uns size() const {return _size;} Uns push(const T *value, Uns timeout = 0){ return MBX_post(_handle, (Ptr)value, timeout) ? 1 : 0; } Uns pop(T *buffer, Uns timeout = 0){ return MBX_pend(_handle, (Ptr)buffer, timeout) ? 1 : 0; } Uns stored() const { return SEM_count(&(_handle->dataSem)); } Bool is_empty() const { return QUE_empty(&(_handle->dataQue)); } T &head() const { return *(T *)QUE_head(&(_handle->dataQue)); } }; typedef mbx_handler_t Buffer_McBSP0_RX; typedef mbx_handler_t Buffer_McBSP0_TX; #ifndef MCBSP_BUFFER_IN_IRAM #define MCBSP0_BUF_RX_SIZE 128 #define MCBSP0_BUF_TX_SIZE 16 #else #define MCBSP0_BUF_RX_SIZE 24 #define MCBSP0_BUF_TX_SIZE 8 #endif Buffer_McBSP0_RX buffer_McBSP0_RX_free(MCBSP0_BUF_RX_SIZE); Buffer_McBSP0_RX buffer_McBSP0_RX(MCBSP0_BUF_RX_SIZE); Buffer_McBSP0_TX buffer_McBSP0_TX_free(MCBSP0_BUF_TX_SIZE); Buffer_McBSP0_TX buffer_McBSP0_TX(MCBSP0_BUF_TX_SIZE); mcbsp_frame_t *frame_McBSP0_RX(NULL); mcbsp_frame_t *frame_McBSP0_TX(NULL); static const Uint32 rx_sync_frame_pattern = 0x55555500; static const Uint32 tx_sync_frame_pattern = 0xAAAAAA00; volatile Uint32 sync_frame_RX(rx_sync_frame_pattern); volatile Uint32 sync_frame_TX(tx_sync_frame_pattern); mcbsp_element_t mcbsp0_rx_head; volatile unsigned int mcbsp0_rfull_count = 0; extern "C" { void isr_McBSP0_RX(){ IRQ_disable(IRQ_EVT_RINT0); IRQ_clear(IRQ_EVT_RINT0); mcbsp0_rx_head.raw = MCBSP_RGET(DRR0); // <= config bus, MCBSP_read(hMcBSP0); <= data bus if(mcbsp0_rx_head.bytes.header & 0x80){ Uint32 remain_elements; Uint32 sync_frame; if(remain_elements = (mcbsp0_rx_head.bytes.header & 0x1F)){ // receive if((--remain_elements) > 0){ // まだフレームがある場合はEDMAによる受信開始に切り替える cfgEDMA_McBSP0_RX.cnt = EDMA_FMK(CNT, ELECNT, remain_elements); CACHE_wbInvL2((void *)cfgEDMA_McBSP0_RX.dst, sizeof(Uint32) * remain_elements, CACHE_NOWAIT); EDMA_config(hEDMA_McBSP0_RX, &cfgEDMA_McBSP0_RX); // notify for master sync_frame = sync_frame_RX; }else{ // フレーム数が1の場合はこれで終わり mcbsp_frame_t *frame_McBSP0_RX_next; if(buffer_McBSP0_RX_free.pop(&frame_McBSP0_RX_next)){ // 次の空きバッファが見つからない場合は再利用 frame_McBSP0_RX->content[0] = mcbsp0_rx_head; frame_McBSP0_RX->elements = 1; buffer_McBSP0_RX.push(&frame_McBSP0_RX); frame_McBSP0_RX = frame_McBSP0_RX_next; } cfgEDMA_McBSP0_RX.dst = (Uint32)&(frame_McBSP0_RX->content[1]); // 再びMcBSP0の受信割り込みを許可する IRQ_enable(IRQ_EVT_RINT0); } }else{ if(frame_McBSP0_TX && mcbsp0_rx_head.bytes.payload[2]){ // EDMAによる送信 CACHE_wbL2((void *)cfgEDMA_McBSP0_TX.src, sizeof(Uint32) * cfgEDMA_McBSP0_TX.cnt, CACHE_NOWAIT); EDMA_config(hEDMA_McBSP0_TX, &cfgEDMA_McBSP0_TX); sync_frame = 0; }else{ sync_frame = sync_frame_TX; IRQ_enable(IRQ_EVT_RINT0); } } MCBSP_RSET(DXR0, sync_frame); // <= config bus, MCBSP_write(hMcBSP0, sync_frame); <= data bus }else{ IRQ_enable(IRQ_EVT_RINT0); } mcbsp0_rfull_count = 0; } } extern "C" { void isr_EDMA(){ bool mcbsp0_rx_complete(EDMA_intTest(EVENT_EDMA_MCBSP0_RX)); bool mcbsp0_tx_complete(EDMA_intTest(EVENT_EDMA_MCBSP0_TX)); if(mcbsp0_rx_complete){ IRQ_disable(IRQ_EVT_RINT0); // 解放と次のバッファの準備 mcbsp_frame_t *frame_McBSP0_RX_next; if(buffer_McBSP0_RX_free.pop(&frame_McBSP0_RX_next)){ // 次の空きバッファが見つからない場合は再利用 frame_McBSP0_RX->content[0] = mcbsp0_rx_head; frame_McBSP0_RX->elements = (mcbsp0_rx_head.bytes.header & 0x1F); buffer_McBSP0_RX.push(&frame_McBSP0_RX); frame_McBSP0_RX = frame_McBSP0_RX_next; } cfgEDMA_McBSP0_RX.dst = (Uint32)&(frame_McBSP0_RX->content[1]); // EDMAによる受信が完了したので、再びMcBSP0の受信割り込みを許可する EDMA_intClear(EVENT_EDMA_MCBSP0_RX); IRQ_clear(IRQ_EVT_RINT0); IRQ_enable(IRQ_EVT_RINT0); } if(mcbsp0_tx_complete){ IRQ_disable(IRQ_EVT_RINT0); // 解放と次のバッファの準備 buffer_McBSP0_TX_free.push(&frame_McBSP0_TX); if(buffer_McBSP0_TX.pop(&frame_McBSP0_TX)){ cfgEDMA_McBSP0_TX.src = (Uint32)frame_McBSP0_TX->content; cfgEDMA_McBSP0_TX.cnt = EDMA_FMK(CNT, ELECNT, frame_McBSP0_TX->elements); sync_frame_TX = frame_McBSP0_TX->content->raw; }else{ frame_McBSP0_TX = NULL; sync_frame_TX = tx_sync_frame_pattern; } sync_frame_RX = (buffer_McBSP0_TX.stored() & 0xFF) | rx_sync_frame_pattern; // EDMAによる送信が完了したので、再びMcBSP0の受信割り込みを許可する EDMA_intClear(EVENT_EDMA_MCBSP0_TX); // McBSPがストールしている可能性が高いので、それを解消する // spru190 11.3.7.1, avoid RFULL error while(MCBSP_rfull(hMcBSP0)){ MCBSP_read(hMcBSP0); } mcbsp0_rfull_count = 0; //IRQ_clear(IRQ_EVT_RINT0); IRQ_enable(IRQ_EVT_RINT0); } //EDMA_intResetAll(); IRQ_clear(IRQ_EVT_EDMAINT); } } void check_McBSP0(){ if(MCBSP_rfull(hMcBSP0)){ Uint32 state(IRQ_globalDisable()); // McBSP RX IRQが有効 // McBSPがストールしているので、それを解消する // spru190 11.3.7.1, avoid RFULL error while(MCBSP_rfull(hMcBSP0)){ MCBSP_read(hMcBSP0); } if(++mcbsp0_rfull_count >= 0x100){ // 何かがおかしい // 強制的にMcBSP RX IRQを有効化 mcbsp0_rfull_count = 0; IRQ_enable(IRQ_EVT_RINT0); } IRQ_globalRestore(state); } } extern "C" { void isr_McBSP0_TX(){ IRQ_clear(MCBSP_getXmtEventId(hMcBSP0)); } } extern "C" { #ifdef DEBUG_HST_USE SEM_Handle sem_RX_done(NULL); #endif void isr_HST_log_in(){ #ifdef DEBUG_HST_USE // get a pointer to the host channel's pipe object PIP_Obj *src = HST_getpipe(&hst_log_in); for(Uns num = PIP_getReaderNumFrames(src); num > 0; num--){ // get a full frame from the host PIP_get(src); Uint32 bytes = std::min( PIP_getReaderSize(src) * sizeof(Uint32), (unsigned int)PAGE_SIZE); Uint32 remain_frames = (bytes + 2) / 3; char *buf_p = (char *)PIP_getReaderAddr(src); mcbsp_frame_t *frame; if(buffer_McBSP0_RX_free.pop(&frame)){ frame->elements = remain_frames; mcbsp_element_t element; Uint32 frame_index = 0; element.bytes.header = 0x80; while(remain_frames){ Uint32 copy_bytes = ((remain_frames > 1) ? 3 : bytes); element.bytes.header |= ((copy_bytes << 5) | (remain_frames--)); bytes -= copy_bytes; char *dist_address = &(element.bytes.payload[2]); while(copy_bytes--){*(dist_address--) = *(buf_p++);} frame->content[frame_index++] = element; element.bytes.header = 0x00; } buffer_McBSP0_RX.push(&frame); } SEM_postBinary(sem_RX_done); PIP_free(src); } #endif } void ready_HST_log_in(){ #ifdef DEBUG_HST_USE SWI_andn(&hst_in_ready, 1); #endif } } #ifdef DEBUG_HST_USE SEM_Handle sem_TX_ready(NULL); void polling_HST_log_out(){ // get a pointer to the host channel's pipe object PIP_Obj *dist = HST_getpipe(&hst_log_out); while(true){ while(std::min(PIP_getWriterNumFrames(dist), buffer_McBSP0_TX.stored()) == 0){ SEM_pendBinary(sem_TX_ready, SYS_FOREVER); } PIP_alloc(dist); mcbsp_frame_t *frame; if(buffer_McBSP0_TX.pop(&frame)){ PIP_setWriterSize(dist, std::min((frame->content->raw), (Uint32)PAGE_SIZE)); memcpy(PIP_getWriterAddr(dist), (&(frame->content[1])), PIP_getWriterSize(dist) * sizeof(Uint32)); buffer_McBSP0_TX_free.push(&frame); } PIP_put(dist); } } #endif extern "C" { void isr_HST_log_out(){ #ifdef DEBUG_HST_USE if(sem_TX_ready){SEM_postBinary(sem_TX_ready);} #endif } } void external_write(char *buf, unsigned bytes){ mcbsp_frame_t *frame; if(buffer_McBSP0_TX_free.pop(&frame)){ frame->encode(buf, bytes); #ifndef DEBUG_HST_USE frame->content->raw = tx_sync_frame_pattern | (frame->content->raw & 0xFF); if(!frame_McBSP0_TX){ cfgEDMA_McBSP0_TX.src = (Uint32)(frame->content); cfgEDMA_McBSP0_TX.cnt = EDMA_FMK(CNT, ELECNT, frame->elements); sync_frame_TX = frame->content->raw; frame_McBSP0_TX = frame; sync_frame_RX = 1 | rx_sync_frame_pattern; }else{ buffer_McBSP0_TX.push(&frame); sync_frame_RX = ((buffer_McBSP0_TX.stored() + 1) & 0xFF) | rx_sync_frame_pattern; } #else buffer_McBSP0_TX.push(&frame); SEM_postBinary(sem_TX_ready); #endif } } Uint16 sylphide_packet_seq_num = 0; struct sylphide_packet_buf_t{ union { unsigned char combined[PAGE_SIZE * 2]; struct { unsigned char header; unsigned char content[PAGE_SIZE * 2 - 1]; } splitted; } data; unsigned char &operator[](unsigned i){ return data.splitted.content[i]; } }; void sylphide_packet_write( const char header, char *c, unsigned bytes){ sylphide_packet_buf_t buf; Uint32 whole_size( min( SylphideProtocol::Encoder::packet_size(bytes), sizeof(buf.data.splitted.content))); sylphide_packet_seq_num++; buf.data.splitted.header = header; unsigned int payload_offset( SylphideProtocol::Encoder::preprocess( buf, bytes)); memcpy( &buf.data.splitted.content[payload_offset], c, min(bytes, sizeof(buf.data.splitted.content) - payload_offset)); SylphideProtocol::Encoder::postprocess( buf, sylphide_packet_seq_num, whole_size); external_write((char *)buf.data.combined, whole_size + 1); } void telemetry_write(char *c, unsigned bytes){ sylphide_packet_write('T', c, bytes); } void debug_write(char *c, unsigned bytes){ sylphide_packet_write('D', c, bytes); } void check_tsk(){ TSK_Stat stat; TSK_stat(&tsk_RT, &stat); LOG_printf(&trace, "RT_0: %d, %d", stat.attrs.priority, stat.mode); LOG_printf(&trace, "RT_1: %d, %d", stat.used, stat.attrs.stacksize); TSK_stat(&tsk_MU, &stat); LOG_printf(&trace, "MU_0: %d, %d", stat.attrs.priority, stat.mode); LOG_printf(&trace, "MU_1: %d, %d", stat.used, stat.attrs.stacksize); } #define CALIBRATOR_NO_INSTANCE_LIST #include "sylph_state.h" ///< 状態量 #include "sylph_nav.h" ///< 航法 #include "sylph_control.h" ///< コントローラ #include "note/070423/calibration.h" #include "note/070423/packet.h" class SylphideCalibrator : public StandardCalibrator{ protected: Vector3 _acc_bias0, _acc_bias1, _acc_sf, _sigma_acc; Vector3 _gyro_bias0, _gyro_bias1, _gyro_sf, _sigma_gyro; Matrix _acc_mis, _gyro_mis; int _acc_bias_t0, _gyro_bias_t0; void set_vector_float(Vector3 &v, const char *top){ for(int i = 0; i < 3; i++){ v[i] = (float_sylph_t)le_char4_2_num(top[i * 4]); } } void set_mat_float( Matrix &mat, const unsigned &row, const char *top){ for(int j = 0; j < 3; j++){ mat(row, j) = (float_sylph_t)le_char4_2_num(top[j * 4]); } } public: SylphideCalibrator() : StandardCalibrator(), _acc_mis(3, 3), _gyro_mis(3, 3) {} ~SylphideCalibrator(){} /** * キャリブレーションパラメータが記載された設定ページ(Sページ)0を解読します * * === S0ページ仕様(32bytes) === * Header (4byte) * 'S' (1byte) * シーケンスNo (1byte) * Sページの種類 = 0 (1byte) * 拡張 (1byte) * * シーケンスNo (1byte) = 0x00 (+27bytes) * [(4byte,float) _acc_bias0(3), _acc_bias1(3)], [(2byte,short)_acc_bias_t0], * [(1byte, uchar)index_base] * シーケンスNo (1byte) = 0x01 (+24bytes) * [(4byte,float) _acc_sf(3), _sigma_acc(3)] * シーケンスNo (1byte) = 0x02 (+24bytes) * [(4byte,float) _acc_mis00, _acc_mis01, _acc_mis02, * _acc_mis10, _acc_mis11,_acc_mis12] * シーケンスNo (1byte) = 0x03 (+12bytes) * [(4byte,float) _acc_mis20, _acc_mis21, _acc_mis22] * * シーケンスNo (1byte) = 0x08 (+26bytes) * [(4byte,float) _gyro_bias0(3), _gyro_bias1(3)], [(2byte,short)_gyro_bias_t0] * シーケンスNo (1byte) = 0x09 (+24bytes) * [(4byte,float) _gyro_sf(3), _sigma_gyro(3)] * シーケンスNo (1byte) = 0x0A (+24bytes) * [(4byte,float) _gyro_mis00, _gyro_mis01, _gyro_mis02, * _gyro_mis10, _gyro_mis11,_gyro_mis12] * シーケンスNo (1byte) = 0x0B (+12bytes) * [(4byte,float) _gyro_mis20, _gyro_mis21, _gyro_mis22] * * @param snapshot 送信対象 * @return デコードが成功した場合true */ bool decode(const char *s_packet, const int packet_size){ if((packet_size == 0) || (s_packet[0] != 'S') || (s_packet[2] != 0) || ((s_packet[1] & 0xF0) >= 0x10)){return false;} bool is_gyro((s_packet[1]) & 0x08); switch((s_packet[1]) & 0x07){ case 0x00: set_vector_float((is_gyro ? _gyro_bias0 : _acc_bias0), &s_packet[4]); set_vector_float((is_gyro ? _gyro_bias1 : _acc_bias1), &s_packet[16]); if(is_gyro){ _gyro_bias_t0 = le_char2_2_num(s_packet[28]); }else{ _acc_bias_t0 = le_char2_2_num(s_packet[28]); StandardCalibrator::index_base = (unsigned char)s_packet[30]; } break; case 0x01: set_vector_float((is_gyro ? _gyro_sf : _acc_sf), &s_packet[4]); set_vector_float((is_gyro ? _sigma_gyro : _sigma_acc), &s_packet[16]); break; case 0x02: set_mat_float((is_gyro ? _gyro_mis : _acc_mis), 0, &s_packet[4]); set_mat_float((is_gyro ? _gyro_mis : _acc_mis), 1, &s_packet[16]); break; case 0x03: set_mat_float((is_gyro ? _gyro_mis : _acc_mis), 2, &s_packet[4]); break; } return true; } Vector3 acc_bias(int temp_ch) const { return _acc_bias0 + _acc_bias1 * (temp_ch - _acc_bias_t0); } Vector3 acc_sf(int temp_ch) const { return _acc_sf; } Matrix acc_mis() const { return _acc_mis; } Vector3 gyro_bias(int temp_ch) const { return _gyro_bias0 + _gyro_bias1 * (temp_ch - _gyro_bias_t0); } Vector3 gyro_sf(int temp_ch) const { return _gyro_sf; } Matrix gyro_mis() const { return _gyro_mis; } Vector3 sigma_accel() const { return _sigma_acc; } Vector3 sigma_gyro() const { return _sigma_gyro; } } calibrator; extern "C" { void polling_MU(){ G_Packet packet; int orig_priority = TSK_getpri(TSK_self()); while(true){ MBX_pend(&mbx_MU, (Ptr)&packet, SYS_FOREVER); //TSK_setpri(TSK_self(), TSK_getpri(&tsk_RT) - 1); // GPS観測量を用いてMeasurement Updateをする nav_status.measurement_update(packet); nav_status.dump(NAVStatus::DUMP_CORRECT, packet.itow); TSK_setpri(TSK_self(), orig_priority); } } } class StreamProcessor : public AbstractSylphideProcessor { public: static const unsigned int buffer_size; unsigned int invoked; protected: typedef AbstractSylphideProcessor super_t; typedef A_Packet_Observer A_Observer_t; typedef G_Packet_Observer G_Observer_t; typedef F_Packet_Observer F_Observer_t; typedef P_Packet_Observer P_Observer_t; typedef M_Packet_Observer M_Observer_t; /** * Aページ(AD変換結果)の処理器 * */ struct AHandler : public A_Observer_t { bool previous_seek; unsigned int invoked; AHandler() : A_Observer_t(buffer_size), invoked(0) { previous_seek = ready(); } ~AHandler(){} /** * Aページが見つかった際、コールバックされる関数 * * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Aページのオブザーバー */ void operator()(const A_Observer_t &observer){ if(!observer.validate()){return;} unsigned int itow_ms(observer.fetch_ITOW_ms()); // 間引く if((itow_ms % NAV_UPDATE_RATIO_ms) != 0){return;} A_Packet packet; packet.itow = 1E-3 * itow_ms; A_Observer_t::values_t values(observer.fetch_values()); for(int i = 0; i < 8; i++){ packet.ch[i] = values.values[i]; } packet.ch[8] = values.temperature; if(++invoked >= 0x100){ invoked = 0; LOG_printf(&trace, "256 a_packet_s recieved!"); LOG_printf(&trace, "GPS_Time: %f", packet.itow); LOG_printf(&trace, "A_Page_Values : "); for(int i = 0; i < 9; i++){ LOG_printf(&trace, "%d, ", packet.ch[i]); } } nav_status.time_update(packet); nav_status.dump(NAVStatus::DUMP_UPDATE, packet.itow); } } a_handler; /** * Gページ(u-bloxのGPS)の処理器 * */ struct GHandler : public G_Observer_t { unsigned int itow_ms_0x0102, itow_ms_0x0112; G_Packet packet; bool previous_seek; unsigned int fix_mode; unsigned int invoked; unsigned int svinfo_updated; GHandler() : G_Observer_t(buffer_size), itow_ms_0x0102(0), itow_ms_0x0112(0), packet(), fix_mode(G_Observer_t::status_t::NO_FIX), invoked(0), svinfo_updated(0) { previous_seek = ready(); } ~GHandler(){} /** * Gページが見つかった際、コールバックされる関数 * * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * {class, id} = {0x01, 0x02}のとき位置情報が得られる * {class, id} = {0x01, 0x03}のとき測位モード(NOFIX/2D/3D) * {class, id} = {0x01, 0x12}のとき速度情報が得られる * * @param obsrever Gページのオブザーバー */ void operator()(const G_Observer_t &observer){ /*if(!observer.validate()){ for(Uint32 i = 0; i < observer.current_packet_size(); i++){ LOG_printf(&trace, "%d, %02x", i, (unsigned char)observer[i]); } return; }*/ if(++invoked >= 0x100){ invoked = 0; LOG_printf(&trace,"256 g_packet_s recieved!\n"); } bool change_itow(false); G_Observer_t::packet_type_t packet_type(observer.packet_type()); switch(packet_type.mclass){ case 0x01: { switch(packet_type.mid){ case 0x02: { if(observer.validate()){ G_Observer_t::position_t position(observer.fetch_position()); G_Observer_t::position_acc_t position_acc(observer.fetch_position_acc()); itow_ms_0x0102 = observer.fetch_ITOW_ms(); change_itow = true; packet.llh[0] = position.latitude; packet.llh[1] = position.longitude; packet.llh[2] = position.altitude; packet.acc_2d = position_acc.horizontal; packet.acc_v = position_acc.vertical; }else{ LOG_printf(&trace, "Corrupt B562 0102."); } break; } case 0x03: { if(observer.validate()){ G_Observer_t::status_t status(observer.fetch_status()); fix_mode = status.fix_type; }else{ LOG_printf(&trace, "Corrupt B562 0103."); } break; } case 0x12: { if(observer.validate()){ G_Observer_t::velocity_t velocity(observer.fetch_velocity()); G_Observer_t::velocity_acc_t velocity_acc(observer.fetch_velocity_acc()); itow_ms_0x0112 = observer.fetch_ITOW_ms(); change_itow = true; packet.vel_ned[0] = velocity.north; packet.vel_ned[1] = velocity.east; packet.vel_ned[2] = velocity.down; packet.acc_vel = velocity_acc.acc; /*LOG_printf(&trace, "Position(lat, long, alt) :"); LOG_printf(&trace, "%f", packet.llh[0]); LOG_printf(&trace, "%f", packet.llh[1]); LOG_printf(&trace, "%f", packet.llh[2]); LOG_printf(&trace,"Position_Acc(h, v) : %f, %f", packet.acc_2d, packet.acc_v); LOG_printf(&trace,"Velocity(N, E, D) :"); LOG_printf(&trace, "%f", packet.vel_ned[0]); LOG_printf(&trace, "%f", packet.vel_ned[1]); LOG_printf(&trace, "%f", packet.vel_ned[2]); LOG_printf(&trace,"Velocity_Acc : %f", packet.acc_vel);*/ }else{ LOG_printf(&trace, "Corrupt B562 0112."); } break; } case 0x30: { // 衛星情報 if(!observer.validate()){break;} if(++svinfo_updated < 0x8){break;} // 8回に1回 svinfo_updated = 0; /*char buf[PAGE_SIZE]; buf[0] = 'G'; for(int i(0); i < observer.current_packet_size(); i += (sizeof(buf) - 1)){ observer.inspect(&buf[1], sizeof(buf) - 1, i); telemetry_write(buf, sizeof(buf)); // パケットサイズ超えても無視している }*/ break; } } break; } case 0x02: { switch(packet_type.mid){ case 0x10: { if(!observer.validate()){break;} unsigned int num_of_sv((unsigned char)observer[6 + 6]); LOG_printf(&trace, "Number of satellites : %d", num_of_sv); /*for(int i = 0; i < num_of_sv; i++){ G_Observer_t::raw_measurement_t raw_data(observer.fetch_raw(i)); LOG_printf(&trace, "RAW(%d, %d) : ", i, raw_data.sv_number); LOG_printf(&trace, "%f, %f", raw_data.carrier_phase, raw_data.psuedo_range); LOG_printf(&trace, "%f, %d", raw_data.doppler, raw_data.quarity); LOG_printf(&trace, "%d, %d", raw_data.signal_strength, raw_data.lock_indicator); }*/ break; } } } default: break; } if(change_itow && (itow_ms_0x0102 == itow_ms_0x0112)){ packet.itow = (float_sylph_t)1E-3 * itow_ms_0x0102; switch(fix_mode){ case G_Observer_t::status_t::FIX_2D : // 垂直方向の推定が行われていないことを偏差を使ってあらわす packet.acc_v = 1E+3; // 現在高度に置き換え packet.llh[2] = nav_status.navigation().height(); case G_Observer_t::status_t::FIX_3D : // GPS観測量を用いてMeasurement Updateをする MBX_post(&mbx_MU, (Ptr)&packet, 0); break; default: } LOG_printf(&trace, "MU done."); } } } g_handler; /** * Fページ(PWM入出力)の処理器 * */ struct FHandler : public F_Observer_t { bool previous_seek; unsigned int invoked; FHandler() : F_Observer_t(buffer_size), invoked(0) { previous_seek = ready(); } ~FHandler(){} /** * Fページが見つかった際、コールバックされる関数 * * Fページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Fページのオブザーバー */ void operator()(const F_Observer_t &observer){ if(!observer.validate()){return;} ++invoked; unsigned int itow_ms(observer.fetch_ITOW_ms()); F_Observer_t::values_t values(observer.fetch_values()); current.update_servo(1E-3 * itow_ms, values); // 10Hzで操舵状況をテレメトリとして流す if(itow_ms % 100 == 0){ char buf[PAGE_SIZE]; buf[0] = 'F'; observer.inspect(&buf[1], min(observer.current_packet_size(), sizeof(buf) - 1)); telemetry_write(buf, sizeof(buf)); } } } f_handler; /** * Pページ(エアデータ)の処理器 * */ struct PHandler : public P_Observer_t { bool previous_seek; unsigned int invoked; PHandler() : P_Observer_t(buffer_size), invoked(0) { previous_seek = ready(); } ~PHandler(){} /** * Pページが見つかった際、コールバックされる関数 * * Pページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Pページのオブザーバー */ void operator()(const P_Observer_t &observer){ if(!observer.validate()){return;} ++invoked; } } p_handler; /** * Mページ(磁気データ)の処理器 * */ struct MHandler : public M_Observer_t { bool previous_seek; unsigned int invoked; MHandler() : M_Observer_t(buffer_size), invoked(0) { previous_seek = ready(); } ~MHandler(){} /** * Mページが見つかった際、コールバックされる関数 * * Mページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Mページのオブザーバー */ void operator()(const M_Observer_t &observer){ if(!observer.validate()){return;} ++invoked; } } m_handler; /** * SylphideProtocl形式で入ってくるC,Dページ用の処理器 * */ class SylphideStreamHandler : public Sylphide_Packet_Observer { protected: typedef Sylphide_Packet_Observer super_t; typedef SylphideStreamHandler self_t; public: bool previous_seek; unsigned int invoked; void (*handle)(const self_t &oberver); SylphideStreamHandler() : super_t(buffer_size), previous_seek(false), invoked(0), handle(NULL) {} ~SylphideStreamHandler() {} /** * パケットが見つかった際に呼び出される関数 * */ void operator()(const self_t &observer){ if(!observer.validate()){return;} if(handle){handle(observer);} } } c_handler, d_handler; public: StreamProcessor() : super_t(), invoked(0), a_handler(), g_handler(), f_handler(), p_handler(), m_handler(), c_handler(), d_handler() { } virtual ~StreamProcessor(){} void process(char *buffer, int read_count){ /*LOG_printf(&trace, "size: %d", bytes); for(int i = 0; i < bytes; i++){ LOG_printf(&trace, "%d, %02x", i, (unsigned char)buffer[i]); }*/ switch(buffer[0]){ case 'S': calibrator.decode(buffer, read_count); break; case 'A': super_t::process_packet( buffer, read_count, a_handler, a_handler.previous_seek, a_handler); break; case 'G': super_t::process_packet( buffer, read_count, g_handler, g_handler.previous_seek, g_handler); break; #if defined(INS_GPS_ONLY) case 'F': // FPGA case 'P': // Pitot case 'M': // Magnetic case 'C': // Command case 'D': // Debug break; #else case 'F': // FPGA super_t::process_packet( buffer, read_count, f_handler, f_handler.previous_seek, f_handler); break; case 'P': // Pitot super_t::process_packet( buffer, read_count, p_handler, p_handler.previous_seek, p_handler); break; case 'M': // Magnetic super_t::process_packet( buffer, read_count, m_handler, m_handler.previous_seek, m_handler); break; case 'C': // Command super_t::process_packet( buffer, read_count, c_handler, c_handler.previous_seek, c_handler); break; case 'D': // Debug super_t::process_packet( buffer, read_count, d_handler, d_handler.previous_seek, d_handler); break; #endif default: LOG_printf(&trace, "Unknown, %c, %d", buffer[0], read_count); } } }; const unsigned int StreamProcessor::buffer_size = OBSERVER_SIZE; extern "C" { void polling_RX(){ #if defined(IMU_IS_E0) || defined(DEBUG_HST_USE) A_Packet::calibrator = new IMU_E0(); #elif defined(IMU_IS_T0) A_Packet::calibrator = new IMU_T0(); #elif defined(IMU_IS_E1) A_Packet::calibrator = new IMU_E1(); #elif defined(IMU_IS_E2) A_Packet::calibrator = new IMU_E2(); #elif defined(IMU_IS_E3) A_Packet::calibrator = new IMU_E3(); #elif defined(MOGURA_SPECIAL) A_Packet::calibrator = new RotatedCalibrator(*(new IMU_E2()), "-y+x+z"); #elif defined(ORCA_SPECIAL) A_Packet::calibrator = new RotatedCalibrator(*(new IMU_E0()), "-y-x-z"); #else A_Packet::calibrator = &calibrator; #if 0 // E1 + 6g(Rotate=-y-x-z)の較正情報を書き込んでおく const char calib_data[8][32] = { {0x53, 0x00, 0x00, 0x00, 0x43, 0x6d, 0xf7, 0x4a, 0x33, 0x72, 0xf8, 0x4a, 0xb8, 0xa5, 0xfc, 0x4a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x01, 0x00, 0x00, 0xd1, 0xf8, 0xe7, 0x47, 0x76, 0x44, 0xe5, 0x47, 0x07, 0x8f, 0xe3, 0xc7, 0xee, 0x96, 0xe2, 0x3c, 0xd4, 0x3a, 0xe6, 0x3c, 0x2f, 0x53, 0xe4, 0xbc, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x02, 0x00, 0x00, 0xb6, 0x89, 0x65, 0xbc, 0x3e, 0xf9, 0x7f, 0xbf, 0x9d, 0x76, 0x4e, 0xbb, 0x35, 0xf9, 0x7f, 0xbf, 0x0a, 0xb5, 0x65, 0x3c, 0x08, 0x0b, 0x57, 0xbb, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x03, 0x00, 0x00, 0x63, 0xea, 0x59, 0x3b, 0xe0, 0x6d, 0x4b, 0x3b, 0x52, 0xff, 0x7f, 0xbf, 0x35, 0xf9, 0x7f, 0xbf, 0x0a, 0xb5, 0x65, 0x3c, 0x08, 0x0b, 0x57, 0xbb, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x08, 0x00, 0x00, 0xb3, 0xaa, 0xfe, 0x4a, 0xa9, 0x93, 0x00, 0x4b, 0x65, 0x92, 0x01, 0x4b, 0xdf, 0x69, 0xb5, 0xc3, 0x77, 0x11, 0xb4, 0x42, 0x32, 0x91, 0x6a, 0xc3, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x09, 0x00, 0x00, 0x54, 0xa2, 0x11, 0x4a, 0xcf, 0x01, 0x11, 0x4a, 0x82, 0x9c, 0x10, 0x4a, 0x49, 0xbb, 0xd8, 0x3a, 0x99, 0x91, 0xd2, 0x3a, 0xb8, 0x57, 0xdf, 0x3a, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x0a, 0x00, 0x00, 0xfe, 0x2a, 0xdf, 0x3b, 0x8b, 0x2c, 0x80, 0xbf, 0x80, 0xfe, 0x24, 0x3d, 0x0b, 0xf7, 0x7f, 0xbf, 0xb8, 0xfc, 0x5c, 0x3b, 0x51, 0x98, 0x24, 0x3c, 0x7c, 0x01, 0x00, 0x00}, {0x53, 0x0b, 0x00, 0x00, 0x87, 0xc5, 0xaf, 0xbc, 0xb2, 0xe8, 0x4e, 0x3c, 0x93, 0x13, 0x80, 0xbf, 0x0b, 0xf7, 0x7f, 0xbf, 0xb8, 0xfc, 0x5c, 0x3b, 0x51, 0x98, 0x24, 0x3c, 0x7c, 0x01, 0x00, 0x00}}; #else // E0の較正情報を書き込んでおく const char calib_data[8][32] = { {0x53, 0x00, 0x00, 0x00, 0x53, 0xa0, 0xff, 0x4a, 0x07, 0xf2, 0x02, 0x4b, 0x93, 0xbf, 0x02, 0x4b, 0x53, 0xfe, 0x33, 0x45, 0x20, 0xcd, 0xf7, 0x43, 0x92, 0x36, 0x08, 0x46, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x01, 0x00, 0x00, 0xbf, 0xdc, 0xa8, 0x48, 0x3b, 0x1a, 0xae, 0x48, 0x22, 0xc0, 0xae, 0xc8, 0x23, 0x15, 0xf1, 0x3c, 0x9f, 0x60, 0xe6, 0x3c, 0x83, 0xd2, 0xdf, 0xbc, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x02, 0x00, 0x00, 0x6e, 0xf0, 0x7f, 0x3f, 0x3d, 0xe3, 0x42, 0xbb, 0x5c, 0xe4, 0xb0, 0xbc, 0xbf, 0x69, 0x43, 0x3b, 0xb4, 0xff, 0x7f, 0x3f, 0x2f, 0xc9, 0xb1, 0x39, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x03, 0x00, 0x00, 0x0a, 0xe2, 0xb0, 0x3c, 0x20, 0x80, 0xd3, 0xb9, 0xb7, 0xf0, 0x7f, 0x3f, 0xbf, 0x69, 0x43, 0x3b, 0xb4, 0xff, 0x7f, 0x3f, 0x2f, 0xc9, 0xb1, 0x39, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x08, 0x00, 0x00, 0xe9, 0x4f, 0xff, 0x4a, 0xd7, 0xa9, 0x00, 0x4b, 0x8b, 0x99, 0xfe, 0x4a, 0x64, 0x27, 0x4a, 0xc3, 0xaf, 0x4e, 0xd3, 0xc3, 0x38, 0xa3, 0x6e, 0xc3, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x09, 0x00, 0x00, 0xee, 0x3f, 0x10, 0x4a, 0x27, 0x4e, 0x10, 0x4a, 0xa7, 0x1d, 0x12, 0x4a, 0x97, 0xb7, 0xdc, 0x3a, 0x7c, 0x51, 0xdb, 0x3a, 0x32, 0x7a, 0xea, 0x3a, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x0A, 0x00, 0x00, 0xbe, 0xb3, 0x7f, 0x3f, 0xa0, 0x50, 0xb7, 0x3c, 0x18, 0xde, 0xd9, 0xbc, 0x66, 0xf9, 0x39, 0xbb, 0x54, 0xfc, 0x7f, 0x3f, 0x0e, 0x0d, 0x06, 0xbc, 0x7c, 0x01, 0x00, 0x78}, {0x53, 0x0B, 0x00, 0x00, 0x38, 0xfa, 0x83, 0x3d, 0x88, 0xef, 0xa2, 0x3b, 0x2c, 0x0b, 0x80, 0x3f, 0x66, 0xf9, 0x39, 0xbb, 0x54, 0xfc, 0x7f, 0x3f, 0x0e, 0x0d, 0x06, 0xbc, 0x7c, 0x01, 0x00, 0x78}}; #endif for(int i(0); i < sizeof(calib_data) / sizeof(calib_data[0]); i++){ calibrator.decode(calib_data[i], sizeof(calib_data[0])); } #endif StreamProcessor processor; static const int max_frame_length = 32; // * 4 = 128 bytes // McBSP0の送受信用バッファを確保 for(int i = 0; i < buffer_McBSP0_RX_free.size(); i++){ #ifndef MCBSP_BUFFER_IN_IRAM mcbsp_frame_t *frame(new mcbsp_frame_t(max_frame_length)); // SDRAM #else mcbsp_frame_t *frame(new mcbsp_frame_t(max_frame_length, (mcbsp_element_t *)BUF_alloc(&McBSP_BUF))); // IRAM #endif buffer_McBSP0_RX_free.push(&frame); } { buffer_McBSP0_RX_free.pop(&frame_McBSP0_RX); cfgEDMA_McBSP0_RX.dst = (Uint32)&(frame_McBSP0_RX->content[1]); } for(int i = 0; i < buffer_McBSP0_TX_free.size(); i++){ #ifndef MCBSP_BUFFER_IN_IRAM mcbsp_frame_t *frame(new mcbsp_frame_t(max_frame_length)); // SDRAM #else mcbsp_frame_t *frame(new mcbsp_frame_t(max_frame_length, (mcbsp_element_t *)BUF_alloc(&McBSP_BUF))); // IRAM #endif buffer_McBSP0_TX_free.push(&frame); } // McBSP0の受信割り込みを有効 IRQ_enable(IRQ_EVT_RINT0); char buffer[PAGE_SIZE]; Uint32 bytes; while(true){ mcbsp_frame_t *frame; if(!buffer_McBSP0_RX.pop(&frame, 0)){ #ifdef DEBUG_HST_USE SEM_pendBinary(sem_RX_done, SYS_FOREVER); #else TSK_sleep(5); check_McBSP0(); #endif continue; } bytes = frame->decode(buffer, sizeof(buffer)); buffer_McBSP0_RX_free.push(&frame); if(!bytes){ LOG_printf(&trace, "Error %d", bytes); continue; } processor.process(buffer, bytes); } } } void canary_bird() { static int invoked(0); invoked++; MEM_Stat statbuf; MEM_stat(MEM->MALLOCSEG, &statbuf); if(statbuf.size <= statbuf.used){ SYS_exit(-1); } } void my_new_handler() { MEM_Stat statbuf; MEM_stat(MEM->MALLOCSEG, &statbuf); static int invoked(0); invoked++; LOG_printf(&trace, "seg %d: O 0x%x", MEM->MALLOCSEG, statbuf.size); LOG_printf(&trace, "\tU 0x%x\tA 0x%x", statbuf.used, statbuf.length); SYS_exit(-1); } int main(){ std::set_new_handler(my_new_handler); LOG_printf(&trace, "Sylphide started."); #ifdef DEBUG_HST_USE sem_RX_done = SEM_create(0, NULL); sem_TX_ready = SEM_create(0, NULL); TSK_create((Fxn)polling_HST_log_out, NULL); #endif state_t::sem_update_notifier = SEM_create(0, NULL); IRQ_globalEnable(); #ifndef DEBUG_HST_USE init_McBSP0_RX_TX_handler(); #endif mcbsp0_rx_head.raw = 0; return 0; }