/** * SampleAndCompute * DESCRIPTION: Sample raw sensor data and compute filtered data * PRE-CONDITIONS: The aircraft is flying, not still. * Call SampleAndComputeStill when aircraft is on the ground. * io_board_fd is open to a valid device driver. * POST-CONDITIONS: good flag is FALSE, or all public angle data is correct. * EXCEPTIONS THROWN: NO_IO_BOARD, BAD_IO_DRIVER * EXCEPTIONS HANDLED: None */ void ahrs::SampleAndCompute (void) { if (data_source == NULL) ThrowException (NO_IO_BOARD); if (data_source->Sample() == FALSE) return; // No new sensor data available, so don't compute anything /****** Compute AHRS from raw sensor data *******/ if (TheGPS->good) { compute_pitch(data_source->dt, TheGPS->delta_v); good = TRUE; } else { compute_pitch(data_source->dt, 0); good = FALSE; } compute_roll_flying (data_source->dt); compute_heading_flying (data_source->dt); compute_yaw(); // static int i = 0; // if ((++i % 100) == 0) // { // printf ("roll = %10f, pitch = %10f, heading = %10f\n", // roll_angle * 180 / M_PI, // pitch_angle * 180 / M_PI, // heading_angle * 180 / M_PI); // } }
//======================================================================================= // PitchAssigner implementation //======================================================================================= void PitchAssigner::assign_pitch(ImoScore* pScore) { StaffObjsCursor cursor(pScore); ImoKeySignature* pKey = NULL; reset_accidentals(pKey); while(!cursor.is_end()) { ImoStaffObj* pSO = cursor.get_staffobj(); if (pSO->is_note()) { ImoNote* pNote = static_cast<ImoNote*>(pSO); compute_pitch(pNote); } else if (pSO->is_barline()) { reset_accidentals(pKey); } else if (pSO->is_key_signature()) { pKey = static_cast<ImoKeySignature*>( pSO ); reset_accidentals(pKey); } cursor.move_next(); } }
/** * SampleAndComputeStill * DESCRIPTION: Sample raw sensor data and compute filtered data assuming the aircraft * is on the ground. * PRE-CONDITIONS: The aircraft is still, not flying. * Call SampleAndCompute when aircraft is flying. * io_board_fd is open to a valid device driver. * POST-CONDITIONS: good flag is FALSE, or all public angle data is correct. * EXCEPTIONS THROWN: NO_IO_BOARD, BAD_IO_DRIVER * EXCEPTIONS HANDLED: None */ void ahrs::SampleAndComputeStill (void) { if (data_source == NULL) ThrowException (NO_IO_BOARD); if (data_source->Sample() == FALSE) return; // No new sensor data available, so don't compute anything /****** Compute AHRS from raw sensor data *******/ compute_pitch(0, 0); compute_roll_still (); compute_heading_still (); compute_yaw(); }
int main(int argc, char **argv){ int i; int aver,val,val2; int16_t buf[BUFSIZE]; double d_buffer[BUFSIZE]; double pitch = 0.0; struct timespec before,after; struct pitch_tracker_params *settings; snd_pcm_uframes_t period_size = PERIOD_SIZE; //ALSA PCM configuration snd_pcm_t *handle; snd_pcm_hw_params_t *params; int dir,rc; snd_pcm_uframes_t frames; /* Open PCM device for capture */ rc = snd_pcm_open( &handle, "default" , SND_PCM_STREAM_CAPTURE , 0); if(rc < 0 ){ fprintf(stderr,"unable to open pcm device: %s\n",snd_strerror(rc)); exit(1); } /* Allocate a hardware parameters object. */ snd_pcm_hw_params_alloca(¶ms); /* Fill it in with default values. */ snd_pcm_hw_params_any(handle, params); /* Set the desired hardware parameters. */ /* Interleaved mode */ snd_pcm_hw_params_set_access(handle, params,SND_PCM_ACCESS_RW_INTERLEAVED); /* unsigned 16-bit format */ snd_pcm_hw_params_set_format(handle, params,SND_PCM_FORMAT_S16); snd_pcm_hw_params_set_channels(handle, params, 1); /* 44100 bits/second sampling rate */ val = 44100; snd_pcm_hw_params_set_rate_near(handle,params, &val, &dir); /* set size time*/ snd_pcm_hw_params_set_period_size_near(handle, params, &period_size , &dir); /* write configuration */ rc = snd_pcm_hw_params(handle,params); /*Display info*/ printf("PCM handle name = '%s'\n", snd_pcm_name(handle)); printf("PCM state = %s\n", snd_pcm_state_name(snd_pcm_state(handle))); snd_pcm_hw_params_get_access(params, (snd_pcm_access_t *) &val); printf("access type = %s\n", snd_pcm_access_name((snd_pcm_access_t)val)); snd_pcm_hw_params_get_format(params, &val); printf("format = '%s' (%s)\n", snd_pcm_format_name((snd_pcm_format_t)val), snd_pcm_format_description( (snd_pcm_format_t)val)); snd_pcm_hw_params_get_subformat(params, (snd_pcm_subformat_t *)&val); printf("subformat = '%s' (%s)\n", snd_pcm_subformat_name((snd_pcm_subformat_t)val), snd_pcm_subformat_description( (snd_pcm_subformat_t)val)); snd_pcm_hw_params_get_channels(params, &val); printf("channels = %d\n", val); snd_pcm_hw_params_get_rate(params, &val, &dir); printf("rate = %d bps\n", val); snd_pcm_hw_params_get_period_time(params, &val, &dir); printf("period time = %d us\n", val); snd_pcm_hw_params_get_period_size(params, &frames, &dir); printf("period size = %d frames\n", (int)frames); snd_pcm_hw_params_get_buffer_time(params, &val, &dir); printf("buffer time = %d us\n", val); snd_pcm_hw_params_get_buffer_size(params, (snd_pcm_uframes_t *) &val); printf("buffer size = %d frames\n", val); snd_pcm_hw_params_get_periods(params, &val, &dir); printf("periods per buffer = %d frames\n", val); snd_pcm_hw_params_get_rate_numden(params, &val, &val2); printf("exact rate = %d/%d bps\n", val, val2); val = snd_pcm_hw_params_get_sbits(params); printf("significant bits = %d\n", val); val = snd_pcm_hw_params_is_batch(params); printf("is batch = %d\n", val); val = snd_pcm_hw_params_is_block_transfer(params); printf("is block transfer = %d\n", val); val = snd_pcm_hw_params_is_double(params); printf("is double = %d\n", val); val = snd_pcm_hw_params_is_half_duplex(params); printf("is half duplex = %d\n", val); val = snd_pcm_hw_params_is_joint_duplex(params); printf("is joint duplex = %d\n", val); val = snd_pcm_hw_params_can_overrange(params); printf("can overrange = %d\n", val); val = snd_pcm_hw_params_can_mmap_sample_resolution(params); printf("can mmap = %d\n", val); val = snd_pcm_hw_params_can_pause(params); printf("can pause = %d\n", val); val = snd_pcm_hw_params_can_resume(params); printf("can resume = %d\n", val); val = snd_pcm_hw_params_can_sync_start(params); printf("can sync start = %d\n", val); settings = open_pitch_tracker(); while(1){ rc = snd_pcm_readi(handle, buf, frames); if (rc == -EPIPE) { /* EPIPE means overrun */ fprintf(stderr, "overrun occurred\n"); snd_pcm_prepare(handle); } else if (rc < 0) { fprintf(stderr, "error from read: %s\n", snd_strerror(rc)); } else if (rc != (int)frames) { fprintf(stderr, "short read, read %d frames\n", rc); } for( i = 0 ; i< BUFSIZE ; i++){ d_buffer[i] = (double) buf[i]; } pitch = compute_pitch(d_buffer, BUFSIZE, S16, settings ,ACCURACY); if( pitch != 0.0 ) printf("frequency -> %f\n",pitch); memset(buf,0,BUFSIZE); } close_pitch_tracker(&settings); snd_pcm_drain(handle); snd_pcm_close(handle); return 0; }