// return indication of whether archive is currently being built; this is
// used to prevent reloading during that time (see call site).
static bool ProgressiveBuildArchive()
{
ONCE(g_GUI->SendEventToAll("archivebuildercomplete"));
return false;
#if 0
int ret = vfs_opt_auto_build("../logs/trace.txt", "mods/official/official%02d.zip", "mods/official/mini%02d.zip");
if(ret == INFO::ALL_COMPLETE)
{
// nothing to do; will return false below
}
else if(ret < 0)
DEBUG_DISPLAY_ERROR(L"Archive build failed");
else if(ret == INFO::OK)
g_GUI.SendEventToAll("archivebuildercomplete");
// in progress
else
{
int percent = (int)ret;
g_ScriptingHost.SetGlobal("g_ArchiveBuilderProgress", INT_TO_JSVAL(percent));
g_GUI.SendEventToAll("archivebuilderprogress");
return true;
}
return false;
#endif
}
void DemoMainloop()
{
DemoUpdateStart();
if (speechtick < DemoTick())
{
int h = gSpeechbus.play(gSpeech[speechcount % 10], (rand() % 200) / 50.0f + 2, (rand() % 20) / 10.0f - 1);
speechcount++;
gSoloud.setRelativePlaySpeed(h, (rand() % 100) / 200.0f + 0.75f);
gSoloud.fadePan(h, (rand() % 20) / 10.0f - 1, 2);
speechtick = DemoTick() + 4000;
}
float *buf = gSoloud.getWave();
float *fft = gSoloud.calcFFT();
ONCE(ImGui::SetNextWindowPos(ImVec2(500, 20)));
ImGui::Begin("Output");
ImGui::PlotLines("##Wave", buf, 256, 0, "Wave", -1, 1, ImVec2(264, 80));
ImGui::PlotHistogram("##FFT", fft, 256 / 2, 0, "FFT", 0, 10, ImVec2(264, 80), 8);
ImGui::Text("Speech bus volume : %d%%", (int)floor(gSoloud.getVolume(gSpeechbusHandle) * 100));
ImGui::Text("Music bus volume : %d%%", (int)floor(gSoloud.getVolume(gMusicbusHandle) * 100));
ImGui::Text("Sfx bus volume : %d%%", (int)floor(gSoloud.getVolume(gSfxbusHandle) * 100));
ImGui::Text("Active voices : %d", gSoloud.getActiveVoiceCount());
ImGui::End();
ONCE(ImGui::SetNextWindowPos(ImVec2(20, 20)));
ImGui::Begin("Control");
if (ImGui::SliderFloat("Speech bus volume", &gSpeechvol, 0, 2))
{
gSoloud.setVolume(gSpeechbusHandle, gSpeechvol);
}
if (ImGui::SliderFloat("Music bus volume", &gMusicvol, 0, 2))
{
gSoloud.setVolume(gMusicbusHandle, gMusicvol);
}
if (ImGui::SliderFloat("Sfx bus volume", &gSfxvol, 0, 2))
{
gSoloud.setVolume(gSfxbusHandle, gSfxvol);
}
ImGui::End();
DemoUpdateEnd();
}
int String::Convert(const StringPieceT<X> &in, basic_string<Y> *out, const char *from, const char *to) {
if (!strcmp(from, to)) { String::Copy(in, out); return in.len; }
#ifdef WIN32
if (!strcmp(from, "UTF-16LE") && !strcmp(to, "UTF-8")) {
out->resize(WideCharToMultiByte(CP_UTF8, 0, (wchar_t*)in.data(), in.size(), NULL, 0, NULL, NULL));
WideCharToMultiByte(CP_UTF8, 0, (wchar_t*)in.data(), in.size(), (char*)&(*out)[0], out->size(), NULL, NULL);
return in.len;
}
#endif
ONCE(ERROR("conversion from ", from, " to ", to, " not supported. copying. #define LFL_ICONV"));
String::Copy(in, out);
return in.len;
}
JSBool StartJsTimer(JSContext* cx, uintN argc, jsval* vp)
{
ONCE(InitJsTimers());
JSU_REQUIRE_PARAMS(1);
size_t slot = ToPrimitive<size_t>(JS_ARGV(cx, vp)[0]);
if (slot >= MAX_JS_TIMERS)
return JS_FALSE;
js_start_times[slot].SetFromTimer();
JS_SET_RVAL(cx, vp, JSVAL_VOID);
return JS_TRUE;
}
void DemoMainloop()
{
int i, j;
DemoUpdateStart();
float tick = DemoTick() / 1000.0f;
gSoloud.set3dListenerParameters(
(float)gMouseX, 0, (float)gMouseY,
0, 0, 0,
0, 1, 0);
gSoloud.update3dAudio();
for (i = 0; i < VOICEGRID; i++)
{
for (j = 0; j < VOICEGRID; j++)
{
float v = gSoloud.getOverallVolume(gSndHandle[i * VOICEGRID + j]);
DemoTriangle(
i * 15 + 20.0f, j * 15 + 20.0f,
i * 15 + 20.0f - 5, j * 15 + 20.0f + 5,
i * 15 + 20.0f + 5, j * 15 + 20.0f + 5,
0xff000000 | (int)(v * 0xff) * 0x010101);
DemoTriangle(
i * 15 + 20.0f, j * 15 + 20.0f + 10,
i * 15 + 20.0f - 5, j * 15 + 20.0f + 5,
i * 15 + 20.0f + 5, j * 15 + 20.0f + 5,
0xff000000 | (int)(v * 0xff) * 0x010101);
}
}
float *buf = gSoloud.getWave();
float *fft = gSoloud.calcFFT();
ONCE(ImGui::SetNextWindowPos(ImVec2(500, 20)));
ImGui::Begin("Output");
ImGui::PlotLines("##Wave", buf, 256, 0, "Wave", -1, 1, ImVec2(264, 80));
ImGui::PlotHistogram("##FFT", fft, 256 / 2, 0, "FFT", 0, 10, ImVec2(264, 80), 8);
ImGui::Text("Active voices : %d", gSoloud.getActiveVoiceCount());
ImGui::Text("Total voices : %d", gSoloud.getVoiceCount());
ImGui::Text("Maximum voices : %d", VOICE_COUNT);
ImGui::End();
DemoUpdateEnd();
}
void CProfileManager::Frame()
{
ONCE(alloc_hook_initialize());
root->time_frame_current += (timer_Time() - root->start);
root->mallocs_frame_current += (get_memory_alloc_count() - root->start_mallocs);
root->Frame();
if (needs_structural_reset)
{
PerformStructuralReset();
needs_structural_reset = false;
}
root->start = timer_Time();
root->start_mallocs = get_memory_alloc_count();
}
//-----------------------------------------------------------------------------
// Name: Render()
// Desc: Draws the scene
//-----------------------------------------------------------------------------
VOID Render()
{
PROFILE_FUNCTION();
ONCE( LOG_PRINT( " Entered" ) );
LPDIRECT3DDEVICE9 pd3dDevice = DIRECT3D9.GetDevice();
D3DXMATRIX matWorld;
D3DXMatrixIdentity( &matWorld );
pd3dDevice->SetTransform( D3DTS_WORLD, &matWorld );
g_Camera.SetPose( g_CameraController.GetPose() );
ShaderManagerHub.PushViewAndProjectionMatrices( g_Camera );
D3DXMATRIX mat;
g_Camera.GetCameraMatrix( mat );
// g_pTest->UpdateViewTransform( mat );
g_Camera.GetProjectionMatrix( mat );
// g_pTest->UpdateProjectionTransform( mat );
// clear the backbuffer to a blue color
pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET|D3DCLEAR_ZBUFFER, 0xFF303030, 1.0f, 0 );
// begin the scene
pd3dDevice->BeginScene();
if( g_pTest )
g_pTest->Render( g_Camera );
// rendering
char acStr[256];
sprintf( acStr, "%f", GlobalTimer().GetFPS() );
g_pFont->DrawText( acStr, Vector2(20,20), 0xFFFFFFFF );
// g_pFont->DrawText( to_string(GlobalTimer().GetFPS()), D3DXVECTOR2(20,20), 0xFFFFFFFF );
int i=0;
const vector<string>& vecProfileResults = GetProfileText();
BOOST_FOREACH( const string& text, vecProfileResults )
{
g_pFont->DrawText( text.c_str(), Vector2( 20, 40 + i*16 ), 0xF0F0F0FF );
i++;
}
void c(int x){ONCE(); if(x != sizeof(int)) abort(); }
void b(int x){ONCE(); if(x != sizeof(int) * 3) abort(); }
void Movie_audio::decodeFrontFrame(Chunk& sfBuffer)
{
unsigned audioPacketOffset = 0;
int res = 1;
sfBuffer.samples = NULL;
sfBuffer.sampleCount = 0;
// This buffer is used by sf::SoundStream and should therefore
// not be destroyed before we're done with the previous chunk
//if (m_buffer) av_free(m_buffer);
//m_buffer = (sf::Int16 *)av_malloc(AUDIO_BUFSIZ);
while (audioPacketOffset < m_sampleRate && res)
{
int frame_size = AUDIO_BUFSIZ;
AVPacket *audioPacket = NULL;
// Stop here if there is no frame to decode
if (!hasPendingDecodableData())
{
if (!readChunk())
{
if (Movie::usesDebugMessages())
std::cerr << "Movie_audio::DecodeFrontFrame() - no frame currently available for decoding. Aborting decoding sequence." << std::endl;
return;
}
}
// Get the front audio packet
audioPacket = frontFrame();
// Decode it
res = avcodec_decode_audio3(m_codecCtx,
(sf::Int16 *)((char *)m_buffer + audioPacketOffset),
&frame_size, audioPacket);
if (res < 0)
{
std::cerr << "Movie_audio::DecodeFrontFrame() - an error occured while decoding the audio frame" << std::endl;
}
else
{
audioPacketOffset += frame_size;
if (m_codecCtx->sample_fmt != SAMPLE_FMT_S16)
{
// Never happened to me for now, which is fine
if (Movie::usesDebugMessages())
{
ONCE(std::cerr << "Movie_audio::DecodeFrontFrame() - audio format for the current movie is not signed 16 bits and sfe::Movie does not support audio resampling yet" << std::endl);
}
}
sfBuffer.samples = m_buffer;
sfBuffer.sampleCount = audioPacketOffset / sizeof(sf::Int16);
}
popFrame();
}
}
void DemoMainloop_megademo()
{
DemoUpdateStart();
ONCE(ImGui::SetNextWindowPos(ImVec2(20, 0)));
ImGui::Begin("Select sub-demo to run");
if (ImGui::Button("Multimusic"))
{
DemoEntry_multimusic(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_multimusic;
}
ImGui::Text("Multimusic demo plays multiple music tracks\n"
"with interactive options to fade between them.");
ImGui::Separator();
if (ImGui::Button("Monotone"))
{
DemoEntry_monotone(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_monotone;
}
ImGui::Text("Monotone demo plays a \"monotone\" tracker song\n"
"with various interative options and filters.");
ImGui::Separator();
if (ImGui::Button("Mixbusses"))
{
DemoEntry_mixbusses(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_mixbusses;
}
ImGui::Text("Mixbusses creates three different mix busses,\n"
"plays different sounds in each and lets user\n"
"adjust the volume of the bus interactively.");
ImGui::Separator();
if (ImGui::Button("3d test"))
{
DemoEntry_test3d(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_test3d;
}
ImGui::Text("3d test demo is a test of the 3d sound.");
ImGui::Separator();
if (ImGui::Button("pewpew"))
{
DemoEntry_pewpew(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_pewpew;
}
ImGui::Text("pewpew demo shows the use of play_clocked\n"
"and how delaying sound makes it feel like\n"
"it plays earlier.");
ImGui::Separator();
if (ImGui::Button("radiogaga"))
{
DemoEntry_radiogaga(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_radiogaga;
}
ImGui::Text("radiogaga demo demonstrates audio queues\n"
"to generate endless music by chaining random\n"
"clips of music.\n");
ImGui::Separator();
if (ImGui::Button("space"))
{
DemoEntry_space(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_space;
}
ImGui::Text("space demo is a mockup of a conversation\n"
"in a space game, and shows use of the\n"
"visualization interfaces.");
ImGui::Separator();
if (ImGui::Button("speechfilter"))
{
DemoEntry_speechfilter(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_speechfilter;
}
ImGui::Text("speechfilter demonstrates various\n"
"DSP effects on speech synthesis.");
ImGui::Separator();
if (ImGui::Button("tedsid"))
{
DemoEntry_tedsid(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_tedsid;
}
ImGui::Text("tedsid demonstrates the MOS TED and SID\n"
"synthesis engines.");
ImGui::Separator();
if (ImGui::Button("virtualvoices"))
{
DemoEntry_virtualvoices(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_virtualvoices;
}
ImGui::Text("virtualvoices demonstrates playing way\n"
"more sounds than is actually possible,\n"
"and having the ones active that matter.");
ImGui::Separator();
if (ImGui::Button("wavformats"))
{
DemoEntry_wavformats(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_wavformats;
}
ImGui::Text("wavformats test plays files with\n"
"all sorts of wave file formats.");
ImGui::Separator();
if (ImGui::Button("speakers"))
{
DemoEntry_speakers(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_speakers;
}
ImGui::Text("speakers test plays single sounds\n"
"through surround speakers.\n");
ImGui::Separator();
if (ImGui::Button("thebutton"))
{
DemoEntry_thebutton(gArgc, gArgv);
DemoMainloopPtr = DemoMainloop_thebutton;
}
ImGui::Text("thebutton test shows one way of\n"
"avoiding actor speaking on top\n"
"of themselves.\n");
ImGui::End();
DemoUpdateEnd();
}
void a(int x){ONCE(); if(x != sizeof(int) * 2 * 3) abort(); }
// Entry point
int main(int argc, char *argv[])
{
DemoInit();
gMusic.load("audio/Jakim - Aboriginal Derivatives.mon");
gMusic.setParams(10);
gEcho.setParams(0.2f, 0.5f, 0.05f);
gBiquad.setParams(SoLoud::BiquadResonantFilter::LOWPASS, 44100, 4000, 2);
gMusic.setLooping(1);
gMusic.setFilter(0, &gBiquad);
gMusic.setFilter(1, &gLofi);
gMusic.setFilter(2, &gEcho);
gMusic.setFilter(3, &gDCRemoval);
gSoloud.init(SoLoud::Soloud::CLIP_ROUNDOFF | SoLoud::Soloud::ENABLE_VISUALIZATION);
gMusichandle = gSoloud.play(gMusic);
float filter_param0[4] = { 0, 0, 0, 0 };
float filter_param1[4] = { 1000, 8000, 0, 0 };
float filter_param2[4] = { 2, 3, 0, 0 };
int hwchannels = 4;
int waveform = 0;
// Main loop: loop forever.
while (1)
{
gSoloud.setFilterParameter(gMusichandle, 0, 0, filter_param0[0]);
gSoloud.setFilterParameter(gMusichandle, 1, 0, filter_param0[1]);
gSoloud.setFilterParameter(gMusichandle, 2, 0, filter_param0[2]);
gSoloud.setFilterParameter(gMusichandle, 3, 0, filter_param0[3]);
gSoloud.setFilterParameter(gMusichandle, 0, 2, filter_param1[0]);
gSoloud.setFilterParameter(gMusichandle, 0, 3, filter_param2[0]);
gSoloud.setFilterParameter(gMusichandle, 1, 1, filter_param1[1]);
gSoloud.setFilterParameter(gMusichandle, 1, 2, filter_param2[1]);
DemoUpdateStart();
float *buf = gSoloud.getWave();
float *fft = gSoloud.calcFFT();
ONCE(ImGui::SetNextWindowPos(ImVec2(500, 20)));
ImGui::Begin("Output");
ImGui::PlotLines("##Wave", buf, 256, 0, "Wave", -1, 1, ImVec2(264, 80));
ImGui::PlotHistogram("##FFT", fft, 256/2, 0, "FFT", 0, 10, ImVec2(264,80),8);
ImGui::Text("Music volume : %d%%", (int)floor(gSoloud.getVolume(gMusichandle) * 100));
ImGui::Text("Active voices : %d", gSoloud.getActiveVoiceCount());
ImGui::End();
ONCE(ImGui::SetNextWindowPos(ImVec2(20, 20)));
ImGui::Begin("Control");
if (ImGui::SliderInt("Channels", &hwchannels, 1, 4))
{
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::CollapsingHeader("Waveform", (const char*)0, true, false))
{
if (ImGui::RadioButton("Square", waveform == SoLoud::Monotone::SQUARE))
{
waveform = SoLoud::Monotone::SQUARE;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("Saw", waveform == SoLoud::Monotone::SAW))
{
waveform = SoLoud::Monotone::SAW;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("Sin", waveform == SoLoud::Monotone::SIN))
{
waveform = SoLoud::Monotone::SIN;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("SawSin", waveform == SoLoud::Monotone::SAWSIN))
{
waveform = SoLoud::Monotone::SAWSIN;
gMusic.setParams(hwchannels, waveform);
}
}
ImGui::Separator();
ImGui::Text("Biquad filter (lowpass)");
ImGui::SliderFloat("Wet##4", &filter_param0[0], 0, 1);
ImGui::SliderFloat("Frequency##4", &filter_param1[0], 0, 8000);
ImGui::SliderFloat("Resonance##4", &filter_param2[0], 1, 20);
ImGui::Separator();
ImGui::Text("Lofi filter");
ImGui::SliderFloat("Wet##2", &filter_param0[1], 0, 1);
ImGui::SliderFloat("Rate##2", &filter_param1[1], 1000, 8000);
ImGui::SliderFloat("Bit depth##2", &filter_param2[1], 0, 8);
ImGui::Separator();
ImGui::Text("Echo filter");
ImGui::SliderFloat("Wet##3", &filter_param0[2], 0, 1);
ImGui::Separator();
ImGui::Text("DC removal filter");
ImGui::SliderFloat("Wet##1", &filter_param0[3], 0, 1);
ImGui::End();
DemoUpdateEnd();
}
return 0;
}
void main_step(const float dt,
const marg_data_t *marg_data,
const gps_data_t *gps_data,
const float ultra,
const float baro,
const float voltage,
const float current,
const float channels[MAX_CHANNELS],
const uint16_t sensor_status,
const bool override_hw)
{
vec2_t ne_pos_err, ne_speed_sp, ne_spd_err;
vec2_init(&ne_pos_err);
vec2_init(&ne_speed_sp);
vec2_init(&ne_spd_err);
vec3_t mag_normal;
vec3_init(&mag_normal);
float u_pos_err = 0.0f;
float u_spd_err = 0.0f;
int bb_rate;
if (calibrate)
{
ONCE(LOG(LL_INFO, "publishing calibration data; actuators disabled"));
bb_rate = 20;
goto out;
}
else
bb_rate = 1;
pos_in.dt = dt;
pos_in.ultra_u = ultra;
pos_in.baro_u = baro;
if (!(sensor_status & MARG_VALID))
{
marg_err += 1;
if (marg_err > 500)
{
/* we are in serious trouble */
memset(setpoints, 0, sizeof(float) * platform.n_motors);
platform_write_motors(setpoints);
die();
}
goto out;
}
marg_err = 0;
float cal_channels[MAX_CHANNELS];
memcpy(cal_channels, channels, sizeof(cal_channels));
if (sensor_status & RC_VALID)
{
/* apply calibration if remote control input is valid: */
float cal_data[3] = {channels[CH_PITCH], channels[CH_ROLL], channels[CH_YAW]};
cal_sample_apply(&rc_cal, cal_data);
cal_channels[CH_PITCH] = cal_data[0];
cal_channels[CH_ROLL] = cal_data[1];
cal_channels[CH_YAW] = cal_data[2];
}
/* perform gyro calibration: */
marg_data_t cal_marg_data;
marg_data_init(&cal_marg_data);
marg_data_copy(&cal_marg_data, marg_data);
if (cal_sample_apply(&gyro_cal, cal_marg_data.gyro.ve) == 0 && gyro_moved(&marg_data->gyro))
{
if (interval_measure(&gyro_move_interval) > 1.0)
LOG(LL_ERROR, "gyro moved during calibration, retrying");
cal_reset(&gyro_cal);
}
/* update relative gps position, if we have a fix: */
float mag_decl;
if (sensor_status & GPS_VALID)
{
gps_util_update(&gps_rel_data, gps_data);
pos_in.pos_n = gps_rel_data.dn;
pos_in.pos_e = gps_rel_data.de;
pos_in.speed_n = gps_rel_data.speed_n;
pos_in.speed_e = gps_rel_data.speed_e;
ONCE(gps_start_set(gps_data));
mag_decl = mag_decl_get();
}
else
{
pos_in.pos_n = 0.0f;
pos_in.pos_e = 0.0f;
pos_in.speed_n = 0.0f;
pos_in.speed_e = 0.0f;
mag_decl = 0.0f;
}
/* apply acc/mag calibration: */
acc_mag_cal_apply(&cal_marg_data.acc, &cal_marg_data.mag);
vec_copy(&mag_normal, &cal_marg_data.mag);
/* apply current magnetometer compensation: */
cmc_apply(&cal_marg_data.mag, current);
//printf("%f %f %f %f\n", current, cal_marg_data.mag.x, cal_marg_data.mag.y, cal_marg_data.mag.z);
/* determine flight state: */
bool flying = flight_state_update(&cal_marg_data.acc.ve[0]);
if (!flying && pos_in.ultra_u == 7.0)
pos_in.ultra_u = 0.2;
/* compute orientation estimate: */
euler_t euler;
int ahrs_state = cal_ahrs_update(&euler, &cal_marg_data, mag_decl, dt);
if (ahrs_state < 0 || !cal_complete(&gyro_cal))
goto out;
ONCE(init = 1; LOG(LL_DEBUG, "system initialized; orientation = yaw: %f pitch: %f roll: %f", euler.yaw, euler.pitch, euler.roll));
/* rotate local ACC measurements into global NEU reference frame: */
vec3_t world_acc;
vec3_init(&world_acc);
body_to_neu(&world_acc, &euler, &cal_marg_data.acc);
/* center global ACC readings: */
filter1_run(&lp_filter, &world_acc.ve[0], &acc_vec[0]);
FOR_N(i, 3)
pos_in.acc.ve[i] = world_acc.ve[i] - acc_vec[i];
/* compute next 3d position estimate using Kalman filters: */
pos_t pos_est;
vec2_init(&pos_est.ne_pos);
vec2_init(&pos_est.ne_speed);
pos_update(&pos_est, &pos_in);
/* execute flight logic (sets cm_x parameters used below): */
bool hard_off = false;
bool motors_enabled = flight_logic_run(&hard_off, sensor_status, flying, cal_channels, euler.yaw, &pos_est.ne_pos, pos_est.baro_u.pos, pos_est.ultra_u.pos, platform.max_thrust_n, platform.mass_kg, dt);
/* execute up position/speed controller(s): */
float a_u = 0.0f;
if (cm_u_is_pos())
{
if (cm_u_is_baro_pos())
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.baro_u.pos, pos_est.baro_u.speed, dt);
else /* ultra pos */
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.ultra_u.pos, pos_est.ultra_u.speed, dt);
}
else if (cm_u_is_spd())
a_u = u_speed_step(&u_spd_err, cm_u_sp(), pos_est.baro_u.speed, dt);
else
a_u = cm_u_sp();
/* execute north/east navigation and/or read speed vector input: */
if (cm_att_is_gps_pos())
{
vec2_t pos_sp;
vec2_init(&pos_sp);
cm_att_sp(&pos_sp);
navi_run(&ne_speed_sp, &ne_pos_err, &pos_sp, &pos_est.ne_pos, dt);
}
else if (cm_att_is_gps_spd())
cm_att_sp(&ne_speed_sp);
/* execute north/east speed controller: */
vec2_t a_ne;
vec2_init(&a_ne);
ne_speed_ctrl_run(&a_ne, &ne_spd_err, &ne_speed_sp, dt, &pos_est.ne_speed);
vec3_t a_neu;
vec3_set(&a_neu, a_ne.x, a_ne.y, a_u);
vec3_t f_neu;
vec3_init(&f_neu);
vec_scalar_mul(&f_neu, &a_neu, platform.mass_kg); /* f[i] = a[i] * m, makes ctrl device-independent */
float hover_force = platform.mass_kg * 9.81f;
f_neu.z += hover_force;
/* execute NEU forces optimizer: */
float thrust;
vec2_t pr_pos_sp;
vec2_init(&pr_pos_sp);
att_thrust_calc(&pr_pos_sp, &thrust, &f_neu, euler.yaw, platform.max_thrust_n, 0);
/* execute direct attitude angle control, if requested: */
if (cm_att_is_angles())
cm_att_sp(&pr_pos_sp);
/* execute attitude angles controller: */
vec2_t att_err;
vec2_init(&att_err);
vec2_t pr_spd;
vec2_set(&pr_spd, -cal_marg_data.gyro.y, cal_marg_data.gyro.x);
vec2_t pr_pos, pr_pos_ctrl;
vec2_set(&pr_pos, -euler.pitch, euler.roll);
vec2_init(&pr_pos_ctrl);
att_ctrl_step(&pr_pos_ctrl, &att_err, dt, &pr_pos, &pr_spd, &pr_pos_sp);
float piid_sp[3] = {0.0f, 0.0f, 0.0f};
piid_sp[PIID_PITCH] = pr_pos_ctrl.ve[0];
piid_sp[PIID_ROLL] = pr_pos_ctrl.ve[1];
/* execute direct attitude rate control, if requested:*/
if (cm_att_is_rates())
{
vec2_t rates_sp;
vec2_init(&rates_sp);
cm_att_sp(&rates_sp);
piid_sp[PIID_PITCH] = rates_sp.ve[0];
piid_sp[PIID_ROLL] = rates_sp.ve[1];
}
/* execute yaw position controller: */
float yaw_speed_sp, yaw_err;
if (cm_yaw_is_pos())
yaw_speed_sp = yaw_ctrl_step(&yaw_err, cm_yaw_sp(), euler.yaw, cal_marg_data.gyro.z, dt);
else
yaw_speed_sp = cm_yaw_sp(); /* direct yaw speed control */
//yaw_speed_sp = yaw_ctrl_step(&yaw_err, 0.0, euler.yaw, cal_marg_data.gyro.z, dt);
piid_sp[PIID_YAW] = yaw_speed_sp;
/* execute stabilizing PIID controller: */
f_local_t f_local = {{thrust, 0.0f, 0.0f, 0.0f}};
float piid_gyros[3] = {cal_marg_data.gyro.x, -cal_marg_data.gyro.y, cal_marg_data.gyro.z};
piid_run(&f_local.ve[1], piid_gyros, piid_sp, dt);
/* computate rpm ^ 2 out of the desired forces: */
inv_coupling_calc(rpm_square, f_local.ve);
/* compute motor setpoints out of rpm ^ 2: */
piid_int_enable(platform_ac_calc(setpoints, motors_spinning(), voltage, rpm_square));
/* enables motors, if flight logic requests it: */
motors_state_update(flying, dt, motors_enabled);
/* reset controllers, if motors are not controllable: */
if (!motors_controllable())
{
navi_reset();
ne_speed_ctrl_reset();
u_ctrl_reset();
att_ctrl_reset();
piid_reset();
}
/* handle special cases for motor setpoints: */
if (motors_starting())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.min;
if (hard_off || motors_stopping())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.off_val;
printf("%f %f %f\n", rad2deg(euler.pitch), rad2deg(euler.roll), rad2deg(euler.yaw));
/* write motors: */
if (!override_hw)
{
//platform_write_motors(setpoints);
}
/* set monitoring data: */
mon_data_set(ne_pos_err.x, ne_pos_err.y, u_pos_err, yaw_err);
out:
EVERY_N_TIMES(bb_rate, blackbox_record(dt, marg_data, gps_data, ultra, baro, voltage, current, channels, sensor_status, /* sensor inputs */
&ne_pos_err, u_pos_err, /* position errors */
&ne_spd_err, u_spd_err /* speed errors */,
&mag_normal));
}
int x(){ONCE();return 2;}
int y(){ONCE();return 3;}
void DemoMainloop()
{
gSoloud.setFilterParameter(gMusichandle, 0, 0, filter_param0[0]);
gSoloud.setFilterParameter(gMusichandle, 1, 0, filter_param0[1]);
gSoloud.setFilterParameter(gMusichandle, 2, 0, filter_param0[2]);
gSoloud.setFilterParameter(gMusichandle, 3, 0, filter_param0[3]);
gSoloud.setFilterParameter(gMusichandle, 0, 2, filter_param1[0]);
gSoloud.setFilterParameter(gMusichandle, 0, 3, filter_param2[0]);
gSoloud.setFilterParameter(gMusichandle, 1, 1, filter_param1[1]);
gSoloud.setFilterParameter(gMusichandle, 1, 2, filter_param2[1]);
DemoUpdateStart();
float *buf = gSoloud.getWave();
float *fft = gSoloud.calcFFT();
ONCE(ImGui::SetNextWindowPos(ImVec2(500, 20)));
ImGui::Begin("Output");
ImGui::PlotLines("##Wave", buf, 256, 0, "Wave", -1, 1, ImVec2(264, 80));
ImGui::PlotHistogram("##FFT", fft, 256/2, 0, "FFT", 0, 10, ImVec2(264,80),8);
ImGui::Text("Music volume : %d%%", (int)floor(gSoloud.getVolume(gMusichandle) * 100));
ImGui::Text("Active voices : %d", gSoloud.getActiveVoiceCount());
ImGui::End();
ONCE(ImGui::SetNextWindowPos(ImVec2(20, 20)));
ImGui::Begin("Control");
if (ImGui::SliderInt("Channels", &hwchannels, 1, 4))
{
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::CollapsingHeader("Waveform", (const char*)0, true, false))
{
if (ImGui::RadioButton("Square", waveform == SoLoud::Monotone::SQUARE))
{
waveform = SoLoud::Monotone::SQUARE;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("Saw", waveform == SoLoud::Monotone::SAW))
{
waveform = SoLoud::Monotone::SAW;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("Sin", waveform == SoLoud::Monotone::SIN))
{
waveform = SoLoud::Monotone::SIN;
gMusic.setParams(hwchannels, waveform);
}
if (ImGui::RadioButton("SawSin", waveform == SoLoud::Monotone::SAWSIN))
{
waveform = SoLoud::Monotone::SAWSIN;
gMusic.setParams(hwchannels, waveform);
}
}
ImGui::Separator();
ImGui::Text("Biquad filter (lowpass)");
ImGui::SliderFloat("Wet##4", &filter_param0[0], 0, 1);
ImGui::SliderFloat("Frequency##4", &filter_param1[0], 0, 8000);
ImGui::SliderFloat("Resonance##4", &filter_param2[0], 1, 20);
ImGui::Separator();
ImGui::Text("Lofi filter");
ImGui::SliderFloat("Wet##2", &filter_param0[1], 0, 1);
ImGui::SliderFloat("Rate##2", &filter_param1[1], 1000, 8000);
ImGui::SliderFloat("Bit depth##2", &filter_param2[1], 0, 8);
ImGui::Separator();
ImGui::Text("Echo filter");
ImGui::SliderFloat("Wet##3", &filter_param0[2], 0, 1);
ImGui::Separator();
ImGui::Text("DC removal filter");
ImGui::SliderFloat("Wet##1", &filter_param0[3], 0, 1);
ImGui::End();
DemoUpdateEnd();
}
/**
Update all the entities cerrently existing in the stage.
This function must be called once per frame.
- Basic steps
- 1. Call BaseEntity::UpdateBaseEntity( dt ) for each base entity
- 2. Save positions of copy entities
- 3. Run physics simulator
- 4. Remove terminated entities from the active list
- 5. Call CCopyEntity::Act() for each copy entity except for child entities
- 6. Update link to the entity tree node if an entity has changed its position in Act()
TODO: Do 5 & 6 in a single loop to update link for each entity right after is Act().
Current code does this in separate loops.
*/
void EntityManager::UpdateAllEntities( float dt )
{
CCopyEntity *pEntity = NULL;
CCopyEntity *pPrevEntity = NULL;
CCopyEntity *pTouchedEnt = NULL;
ONCE( LOG_PRINT( " - updating base entities" ) );
size_t i, num_base_entities = m_vecpBaseEntity.size();
for( i=0; i<num_base_entities; i++ )
{
m_vecpBaseEntity[i]->UpdateBaseEntity( dt );
}
// save the current entity positions
for( pEntity = m_pEntityInUse.get();
pEntity != NULL;
pEntity = pEntity->m_pNextRawPtr )
{
pEntity->PrevPosition() = pEntity->GetWorldPosition();
}
// run physics simulator
// entity position may be modified in this call
UpdatePhysics( dt );
ONCE( LOG_PRINT( " - updated physics" ) );
// remove terminated entities from the active entity list
ReleaseTerminatedEntities();
ONCE( LOG_PRINT( " - removed terminated entities from the active entity list" ) );
// update active entities
for( pEntity = this->m_pEntityInUse.get(), pPrevEntity = NULL;
pEntity != NULL;
pPrevEntity = pEntity, pEntity = pEntity->m_pNextRawPtr )
{
// before updating pEntity, check if it has been terminated.
if( !pEntity->inuse )
continue;
// set the results of physics simulation to
// pose, velocity and angular velocity of the entity
// if( pEntity->pPhysicsActor && pEntity->GetEntityFlags() & BETYPE_USE_PHYSSIM_RESULTS )
if( 0 < pEntity->m_vecpPhysicsActor.size() && pEntity->GetEntityFlags() & BETYPE_USE_PHYSSIM_RESULTS )
pEntity->UpdatePhysics();
if( pEntity->sState & CESTATE_ATREST )
continue;
if( !pEntity->m_pParent || !pEntity->m_pParent->inuse )
{
// 'pEntity' has no parent or its parent is already terminated
pEntity->pBaseEntity->Act( pEntity );
}
if( !pEntity->inuse )
continue; // terminated in its own update routine
UpdateEntityAfterMoving( pEntity );
// deal with entities touched during this frame
for(int i=0; i<pEntity->vecpTouchedEntity.size(); i++)
{
pTouchedEnt = pEntity->vecpTouchedEntity[i];
pEntity->pBaseEntity->Touch( pEntity, pTouchedEnt );
if( pTouchedEnt )
pTouchedEnt->pBaseEntity->Touch( pTouchedEnt, pEntity );
}
// clear touched entities for the next frame
pEntity->vecpTouchedEntity.clear();
}
ONCE( LOG_PRINT( " - updated active entities" ) );
// unlink and link the entity in the entity tree if it changed its position
/*
for( pEntity = m_pEntityInUse.get();
pEntity != NULL;
pPrevEntity = pEntity, pEntity = pEntity->m_pNextRawPtr )
{
if( !pEntity->inuse )
continue;
UpdateEntityAfterMoving();
}
*/
}
static void loadFFmpeg()
{
ONCE(av_register_all());
ONCE(avcodec_register_all());
ONCE(Log::initialize());
}