Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-08 02:15:53 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-04-07 04:57:13 +00:00
parent 2a85c909b8
commit ea0fa3d288
2 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/vor/i86_vor_test_float_demod.c
+3 -2
View File
@@ -12,11 +12,12 @@ int main(int argc, char** argv) {
for (i=0; i<nb_samples; i++) {
vor_float_demod_run (float_buf[i]);
#if 0
printf("%f %f %f %f %f %f %f %f\n",
i * vfd_te, float_buf[i],
0., vfd_ref_err, vfd_ref_phi, vfd_var_phi, vfd_fm_phi, vfd_qdr );
#endif
}
sw/airborne/vor/vor_int_demod.c
+16 -2
View File
@@ -28,12 +28,16 @@
#include "vor_int_filters.h"
#define VID_ANGLE_RES 16
#define VID_ANGLE_FACT (1 << VID_ANGLE_RES)
#define VID_PANGLE(a) (a * VID_ANGLE_FACT + 0.5)
#define VID_NANGLE(a) (a * VID_ANGLE_FACT - 0.5)
#define VID_PANGLE(a) ((a) * (float)VID_ANGLE_FACT + 0.5)
#define VID_NANGLE(a) ((a) * (float)VID_ANGLE_FACT - 0.5)
//#define VID_DANGLE VID_PANGLE(2. * M_PI * 502. / 15000000.)
const int32_t vid_ref_freq;
int32_t vid_ref_angle;
int32_t vid_ref_phi;
int32_t vid_ref_err;
const int32_t vid_ref_alpha = VID_NANGLE(-1.2);
@@ -41,6 +45,7 @@ const int32_t vid_ref_alpha = VID_NANGLE(-1.2);
void vor_int_demod_init( void) {
vid_ref_angle = 0;
vid_ref_phi = VID_PANGLE(M_PI);
vid_ref_err = 0;
@@ -49,6 +54,15 @@ void vor_int_demod_init( void) {
void vor_int_demod_run ( uint16_t sample) {
// phase error re-injection
vid_ref_phi -= vid_ref_alpha * vid_ref_err;
// local oscillator phase
// ti = i * 502/15000000
// angle_i = 2*pi*9960.*ti
// angle += 2*pi*9960*502/15000000
// int32_t angle =
// const float vfd_ref_phase = 2. * M_PI * vfd_ref_freq * ti + vfd_ref_phi;
}