// update non-fusion data
static upm_result_t _update_non_fusion_data(const bno055_context dev)
{
assert(dev != NULL);
// bail (with success code) if we are in config mode...
if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE)
return UPM_SUCCESS;
if (bno055_set_page(dev, 0, false))
return UPM_ERROR_OPERATION_FAILED;
const int nonFusionBytes = 18;
uint8_t buf[nonFusionBytes];
if (bno055_read_regs(dev, BNO055_REG_ACC_DATA_X_LSB, buf, nonFusionBytes))
return UPM_ERROR_OPERATION_FAILED;
dev->accX = INT16_TO_FLOAT(buf[0], buf[1]);
dev->accY = INT16_TO_FLOAT(buf[2], buf[3]);
dev->accZ = INT16_TO_FLOAT(buf[4], buf[5]);
dev->magX = INT16_TO_FLOAT(buf[6], buf[7]);
dev->magY = INT16_TO_FLOAT(buf[8], buf[9]);
dev->magZ = INT16_TO_FLOAT(buf[10], buf[11]);
dev->gyrX = INT16_TO_FLOAT(buf[12], buf[13]);
dev->gyrY = INT16_TO_FLOAT(buf[14], buf[15]);
dev->gyrZ = INT16_TO_FLOAT(buf[16], buf[17]);
return UPM_SUCCESS;
}
upm_result_t
lis3dh_update(const lis3dh_context dev)
{
assert(dev != NULL);
// Max axes data length, 2 bytes per axis * 3 axes
const int bufLen = 6;
// Max temperature data length
const int temperatureBufLen = 2;
// We reuse the same array when reading acceleration and then temperature data
uint8_t buf[bufLen];
if (lis3dh_read_regs(dev, LIS3DH_REG_OUT_X_L, buf, bufLen) != bufLen) {
printf("%s: lis3dh_read_regs() failed to read %d bytes of axes data\n", __FUNCTION__, bufLen);
return UPM_ERROR_OPERATION_FAILED;
}
// X MSB LSB
dev->accX = INT16_TO_FLOAT(buf[1], buf[0]);
// Y
dev->accY = INT16_TO_FLOAT(buf[3], buf[2]);
// Z
dev->accZ = INT16_TO_FLOAT(buf[5], buf[4]);
// Get the temperature
if (lis3dh_read_regs(dev, LIS3DH_REG_OUT_ADC3_L, buf, temperatureBufLen) != temperatureBufLen) {
printf("%s: lis3dh_read_regs() failed to read %d bytes of temperature data\n",
__FUNCTION__,
temperatureBufLen);
return UPM_ERROR_OPERATION_FAILED;
}
dev->temperature = INT16_TO_FLOAT(buf[1], buf[0]);
return UPM_SUCCESS;
}
// load fusion data
static upm_result_t _update_fusion_data(const bno055_context dev)
{
assert(dev != NULL);
// bail (with success code) if we are in config mode, or aren't in
// a fusion mode...
if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE ||
dev->currentMode < BNO055_OPERATION_MODE_IMU)
return UPM_SUCCESS;
if (bno055_set_page(dev, 0, false))
return UPM_ERROR_OPERATION_FAILED;
// FIXME/MAYBE? - abort early if SYS calibration is == 0?
const int fusionBytes = 26;
uint8_t buf[fusionBytes];
if (bno055_read_regs(dev, BNO055_REG_EUL_HEADING_LSB, buf, fusionBytes))
return UPM_ERROR_OPERATION_FAILED;
dev->eulHeading = INT16_TO_FLOAT(buf[0], buf[1]);
dev->eulRoll = INT16_TO_FLOAT(buf[2], buf[3]);
dev->eulPitch = INT16_TO_FLOAT(buf[4], buf[5]);
dev->quaW = INT16_TO_FLOAT(buf[6], buf[7]);
dev->quaX = INT16_TO_FLOAT(buf[8], buf[9]);
dev->quaY = INT16_TO_FLOAT(buf[10], buf[11]);
dev->quaZ = INT16_TO_FLOAT(buf[12], buf[13]);
dev->liaX = INT16_TO_FLOAT(buf[14], buf[15]);
dev->liaY = INT16_TO_FLOAT(buf[16], buf[17]);
dev->liaZ = INT16_TO_FLOAT(buf[18], buf[19]);
dev->grvX = INT16_TO_FLOAT(buf[20], buf[21]);
dev->grvY = INT16_TO_FLOAT(buf[22], buf[23]);
dev->grvZ = INT16_TO_FLOAT(buf[24], buf[25]);
return UPM_SUCCESS;
}
int main(void)
{
MMSample *sampleFileDataStart = WaveTable;
size_t i;
/* Enable signalling routines for errors */
error_sig_init();
/* Enable external SRAM */
FMC_Config();
codecDmaTxPtr = NULL;
codecDmaRxPtr = NULL;
/* The bus the signal chain is reading */
MMBus *inBus = MMBus_new(BUS_BLOCK_SIZE,BUS_NUM_CHANS);
/* The bus the signal chain is writing */
MMBus *outBus = MMBus_new(BUS_BLOCK_SIZE,BUS_NUM_CHANS);
/* a signal chain to put the signal processors into */
MMSigChain sigChain;
MMSigChain_init(&sigChain);
/* A constant that zeros the bus each iteration */
MMSigConst sigConst;
MMSigConst_init(&sigConst,outBus,0,MMSigConst_doSum_FALSE);
/* put sig constant at the top of the sig chain */
MMSigProc_insertAfter(&sigChain.sigProcs,&sigConst);
/* initialize wavetables */
WaveTable_init();
/* Give access to samples of sound as wave table */
MMArray_set_data(&samples, WaveTable);
MMArray_set_length(&samples, WAVTABLE_LENGTH_SAMPLES);
/* Set with this samplerate so it plays at normal speed when midi note 69
* received */
samples.samplerate = 440 * WAVTABLE_LENGTH_SAMPLES;//CODEC_SAMPLE_RATE;
/* Allow MMWavTabRecorder to record into samples */
MMWavTabRecorder_init(&wtr);
wtr.buffer = &samples;
wtr.inputBus = inBus;
wtr.currentIndex = 0;
wtr.state = MMWavTabRecorderState_RECORDING;
/* Put MMWavTabRecorder at the top of the signal chain */
MMSigProc_insertAfter(&sigChain.sigProcs,&wtr);
/* Make poly voice manager */
pvm = MMPolyManager_new(MIDI_NUM_NOTES);
/* Enable MIDI hardware */
MIDI_low_level_setup();
/* Initialize MIDI Message builder */
MIDIMsgBuilder_init(&midiMsgBuilder);
/* set up the MIDI router to trigger samples */
MIDI_Router_Standard_init(&midiRouter);
for (i = 0; i < MIDI_NUM_NOTES; i++) {
/* Initialize sample player */
MMTrapEnvedSamplePlayer_init(&spsps[i], outBus, BUS_BLOCK_SIZE,
1. / (MMSample)CODEC_SAMPLE_RATE);
/* Make new poly voice and add it to the poly voice manager */
MMPolyManager_addVoice(pvm, i, (MMPolyVoice*)MMPvtesp_new(&spsps[i]));
/* insert in signal chain after sig const*/
MMSigProc_insertAfter(&sigConst, &spsps[i]);
}
MIDI_Router_addCB(&midiRouter.router, MIDIMSG_NOTE_ON, 1, MIDI_note_on_do, spsps);
MIDI_Router_addCB(&midiRouter.router, MIDIMSG_NOTE_OFF, 1, MIDI_note_off_do, spsps);
MIDI_CC_CB_Router_addCB(&midiRouter.cbRouters[0],2,MIDI_cc_do,&wtr);
MIDI_CC_CB_Router_addCB(&midiRouter.cbRouters[0],3,MIDI_cc_rate_control,&playbackRate);
MIDI_CC_CB_Router_addCB(&midiRouter.cbRouters[0],4,MIDI_cc_period_control,&eventPeriod);
MIDI_CC_CB_Router_addCB(&midiRouter.cbRouters[0],5,MIDI_cc_length_control,&eventLength);
/* set up note scheduler */
MMSeq *sequence = MMSeq_new();
MMSeq_init(sequence, 0);
/* Enable codec */
i2s_dma_full_duplex_setup(CODEC_SAMPLE_RATE);
while (1) {
while (!(codecDmaTxPtr && codecDmaRxPtr));
if ((MMSeq_getCurrentTime(sequence) % eventPeriod) == 0) {
/* Make event */
NoteOnEvent *noe = NoteOnEvent_new();
MMPvtespParams *params = MMPvtespParams_new();
params->paramType = MMPvtespParamType_NOTEON;
params->note = get_next_free_voice_number();
params->amplitude = 1.0;
params->interpolation = MMInterpMethod_CUBIC;
params->index = 0;
params->attackTime = ATTACK_TIME;
params->releaseTime = RELEASE_TIME;
params->samples = &samples;
params->loop = 1;
params->rate = playbackRate;
params->rateSource = MMPvtespRateSource_RATE;
NoteOnEvent_init(noe,pvm,params,sequence,eventLength);
/* Schedule event to happen now */
MMSeq_scheduleEvent(sequence,(MMEvent*)noe,MMSeq_getCurrentTime(sequence));
}
/* Do scheduled events and tick */
MMSeq_doAllCurrentEvents(sequence);
MMSeq_tick(sequence);
MIDI_process_buffer(); /* process MIDI at most every audio block */
MMSigProc_tick(&sigChain);
size_t i;
for (i = 0; i < CODEC_DMA_BUF_LEN; i += 2) {
/* write out data */
codecDmaTxPtr[i] = FLOAT_TO_INT16(outBus->data[i/2] * 0.1);
codecDmaTxPtr[i+1] = FLOAT_TO_INT16(outBus->data[i/2] * 0.1);
/* read in data */
inBus->data[i/2] = INT16_TO_FLOAT(codecDmaRxPtr[i]);
}
codecDmaTxPtr = NULL;
codecDmaRxPtr = NULL;
}
}