diff --git a/conf/autopilot/conf_booz.makefile b/conf/autopilot/conf_booz.makefile
index 1bbf5ba8da..e266f4cf4f 100644
--- a/conf/autopilot/conf_booz.makefile
+++ b/conf/autopilot/conf_booz.makefile
@@ -28,9 +28,8 @@ flt.srcs += $(SRC_ARCH)/adc_hw.c
flt.srcs += max1167.c $(SRC_ARCH)/max1167_hw.c
-flt.CFLAGS += -DDISABLE_MAGNETOMETER
-flt.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
-flt.CFLAGS += -I2C_BUF_LEN=32 -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
+#flt.CFLAGS += -DDISABLE_MAGNETOMETER
+flt.CFLAGS += -DUSE_AMI601 -I2C_BUF_LEN=32 -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
flt.srcs += AMI601.c i2c.c $(SRC_ARCH)/i2c_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
@@ -90,7 +89,7 @@ ctl.srcs += booz_link_mcu.c $(SRC_ARCH)/booz_link_mcu_hw.c
ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c
ctl.srcs += commands.c
-ctl.CFLAGS += -DDISABLE_NAV
+ctl.CFLAGS += -DDISABLE_NAV -DDISABLE_PSI_CONTROL
ctl.srcs += booz_estimator.c \
booz_control.c \
booz_nav.c \
diff --git a/conf/messages.xml b/conf/messages.xml
index aa16e683af..f65e718784 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -291,9 +291,9 @@
-
-
-
+
+
+
@@ -535,6 +535,10 @@
+
+
+
+
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
index 50a5063249..e771e73cf7 100644
--- a/conf/telemetry/booz.xml
+++ b/conf/telemetry/booz.xml
@@ -28,12 +28,14 @@
-
+
-
-
-
-
+
+
+
+
+
+
diff --git a/sw/airborne/AMI601.c b/sw/airborne/AMI601.c
index 11d0980c86..0597016212 100644
--- a/sw/airborne/AMI601.c
+++ b/sw/airborne/AMI601.c
@@ -1,33 +1,20 @@
#include "AMI601.h"
-#include "i2c.h"
+
#include "led.h"
#include
-#define FOO_DELAY() { \
- uint16_t foo = 1; \
- while (foo) foo++; \
- }
-
-
-#define AMI601_SLAVE_ADDR 0x60
-
-float ami601_ax;
-float ami601_ay;
-float ami601_az;
-
-float ami601_mx;
-float ami601_my;
-float ami601_mz;
uint8_t ami601_foo1;
uint8_t ami601_foo2;
uint8_t ami601_foo3;
-
uint16_t ami601_val[AMI601_NB_CHAN];
-static volatile bool_t ami601_i2c_done;
+
+volatile uint8_t ami601_status;
+volatile bool_t ami601_i2c_done;
+volatile uint32_t ami601_nb_err;
/* TRG ( trigger) P0.28 */
#define AMI601_TRG_PIN 28
@@ -62,83 +49,24 @@ void ami601_init( void ) {
ami601_val[i] = 0;
}
ami601_i2c_done = TRUE;
+ ami601_nb_err = 0;
+ ami601_status = AMI601_IDLE;
-
- // FOO_DELAY();
/* assert reset */
// SetBit(AMI601_RST_IOSET , AMI601_RST_PIN);
}
-void ami601_periodic( void ) {
- /* pulse TRG */
- // SetBit(AMI601_TRG_IOCLR , AMI601_TRG_PIN);
- // FOO_DELAY();
- // SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
-
- if (ami601_i2c_done) {
- // LED_ON(2);
+void ami601_read( void ) {
+ if (ami601_status != AMI601_IDLE) {
+ ami601_nb_err++;
+ }
+ else {
+ ami601_i2c_done = FALSE;
+ ami601_status = AMI601_SENDING_REQ;
const uint8_t read_cmd[] = { 0x55, 0xAA, 0X14};
memcpy((void*)i2c_buf, read_cmd, sizeof(read_cmd));
i2c_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done);
- ami601_i2c_done = FALSE;
-
- while (!ami601_i2c_done);
-
- FOO_DELAY();
-
- i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done);
- while (!ami601_i2c_done);
- // LED_OFF(2);
-
- ami601_foo1 = i2c_buf[0]; // AA ?
- ami601_foo2 = i2c_buf[1]; // 55 ?
- ami601_foo3 = i2c_buf[2]; // ERR ?
-
- uint8_t i;
- for (i=0; i< AMI601_NB_CHAN; i++) {
- ami601_val[i] = i2c_buf[3 + 2 * i];
- ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256;
- }
-
-
-
}
-
-}
-
-
-
-void ami601_send_sleep( void ) {
-
-
-}
-
-#define AX_NEUTRAL 2323
-#define AY_NEUTRAL 2323
-#define AZ_NEUTRAL 2323
-
-#define AX_GAIN 0.0283936
-#define AY_GAIN -0.0283936
-#define AZ_GAIN 0.0283936
-
-#define MX_NEUTRAL 2048
-#define MY_NEUTRAL 2048
-#define MZ_NEUTRAL 2048
-
-#define MX_GAIN -1.
-#define MY_GAIN 1.
-#define MZ_GAIN 1.
-
-void ami601_scale_measures(void) {
-
- ami601_ax = (ami601_val[3] - AX_NEUTRAL) * AX_GAIN;
- ami601_ay = (ami601_val[5] - AY_NEUTRAL) * AY_GAIN;
- ami601_az = (ami601_val[1] - AZ_NEUTRAL) * AZ_GAIN;
-
- ami601_mx = (ami601_val[0] - MX_NEUTRAL) * MX_GAIN;
- ami601_my = (ami601_val[4] - MY_NEUTRAL) * MY_GAIN;
- ami601_mz = (ami601_val[2] - MZ_NEUTRAL) * MZ_GAIN;
-
}
diff --git a/sw/airborne/AMI601.h b/sw/airborne/AMI601.h
index ab4c844d85..f0988ef8de 100644
--- a/sw/airborne/AMI601.h
+++ b/sw/airborne/AMI601.h
@@ -2,9 +2,11 @@
#define AMI601_H
#include "std.h"
+#include "i2c.h"
extern void ami601_init( void );
+extern void ami601_read( void );
extern void ami601_periodic( void );
extern void ami601_scale_measures(void);
@@ -22,5 +24,53 @@ extern float ami601_mx;
extern float ami601_my;
extern float ami601_mz;
+#define AMI601_IDLE 0
+#define AMI601_SENDING_REQ 1
+#define AMI601_WAITING_MEASURE 2
+#define AMI601_READING_MEASURE 3
+#define AMI601_DATA_AVAILABLE 4
+extern volatile uint8_t ami601_status;
+extern volatile bool_t ami601_i2c_done;
+extern volatile uint32_t ami601_nb_err;
+
+#define AMI601_SLAVE_ADDR 0x60
+
+
+#define AMI601EventCheckAndHandle() { \
+ switch (ami601_status) { \
+ case AMI601_SENDING_REQ : \
+ if ( ami601_i2c_done ) { \
+ /* trigger delay for measurement */ \
+ T0MR1 = T0TC + SYS_TICS_OF_USEC(4096); \
+ /* clear match 1 interrupt */ \
+ T0IR = TIR_MR1I; \
+ /* enable match 1 interrupt */ \
+ T0MCR |= TMCR_MR1_I; \
+ ami601_status = AMI601_WAITING_MEASURE; \
+ } \
+ break; \
+ case AMI601_READING_MEASURE : \
+ if ( ami601_i2c_done ) { \
+ ami601_foo1 = i2c_buf[0]; /* AA ? */ \
+ ami601_foo2 = i2c_buf[1]; /* 55 ? */ \
+ ami601_foo3 = i2c_buf[2]; /* ERR ? */ \
+ uint8_t i; \
+ for (i=0; i< AMI601_NB_CHAN; i++) { \
+ ami601_val[i] = i2c_buf[3 + 2 * i]; \
+ ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256; \
+ } \
+ ami601_status = AMI601_DATA_AVAILABLE; \
+ } \
+ break; \
+ } \
+ }
+
+#define AMI601ReadMeasure() { \
+ /* disable match 1 interrupt */ \
+ /* T0MCR |= TMCR_MR1_I; */ \
+ ami601_i2c_done = FALSE; \
+ ami601_status = AMI601_READING_MEASURE; \
+ i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done); \
+ }
#endif /* AMI601_H */
diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c
index 599f0e0125..b74acf6cb1 100644
--- a/sw/airborne/arm7/sys_time_hw.c
+++ b/sw/airborne/arm7/sys_time_hw.c
@@ -22,6 +22,10 @@ uint32_t last_periodic_event;
#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I)
#endif /* MB_TACHO */
+#ifdef USE_AMI601
+#include "AMI601.h"
+#endif
+
void TIMER0_ISR ( void ) {
ISR_ENTRY();
@@ -62,6 +66,12 @@ void TIMER0_ISR ( void ) {
T0IR = TIR_CR0I;
}
#endif /* MB_TACHO */
+#ifdef USE_AMI601
+ if (T0IR&TIR_MR1I) {
+ AMI601ReadMeasure();
+ T0IR = TIR_MR1I;
+ }
+#endif /* USE_AMI601 */
}
VICVectAddr = 0x00000000;
diff --git a/sw/airborne/booz_ahrs.h b/sw/airborne/booz_ahrs.h
index 91e52dcaf3..7045eacab2 100644
--- a/sw/airborne/booz_ahrs.h
+++ b/sw/airborne/booz_ahrs.h
@@ -26,8 +26,8 @@ extern FLOAT_T booz_ahrs_measure_theta;
extern FLOAT_T booz_ahrs_measure_psi;
extern void booz_ahrs_init(void);
-extern void booz_ahrs_start( const float* accel, const float* gyro, const int16_t* mag);
-extern void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag);
+extern void booz_ahrs_start( const float* accel, const float* gyro, const float* mag);
+extern void booz_ahrs_run(const float* accel, const float* gyro, const float* mag);
/*
extern void booz_ahrs_predict( const float* gyro );
extern void booz_ahrs_update( const float* accel, const int16_t* mag );
diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c
index fbf96d883a..1745483af1 100644
--- a/sw/airborne/booz_control.c
+++ b/sw/airborne/booz_control.c
@@ -116,12 +116,12 @@ void booz_control_attitude_read_setpoints_from_rc(void) {
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
-#ifndef DISABLE_MAGNETOMETER
+#ifndef DISABLE_PSI_CONTROL
booz_control_attitude_psi_sp = rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
#else
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
-#endif /* DISABLE_MAGNETOMETER */
+#endif /* DISABLE_PSI_CONTROL */
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
@@ -136,7 +136,7 @@ void booz_control_attitude_run(void) {
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
-#ifndef DISABLE_MAGNETOMETER
+#ifndef DISABLE_PSI_CONTROL
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
const float cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
@@ -146,7 +146,7 @@ void booz_control_attitude_run(void) {
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
-#endif /* DISABLE_MAGNETOMETER */
+#endif /* DISABLE_PSI_CONTROL */
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c
index 590217710d..7fdf36f52b 100644
--- a/sw/airborne/booz_filter_main.c
+++ b/sw/airborne/booz_filter_main.c
@@ -74,6 +74,8 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
case 0:
booz_filter_telemetry_periodic_task();
break;
+<<<<<<< booz_filter_main.c
+=======
#ifndef SITL
// case 1:
/* triger measurements */
@@ -81,6 +83,7 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
// Z Y X
// DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
#endif
+>>>>>>> 1.11
}
}
diff --git a/sw/airborne/imu_v3.c b/sw/airborne/imu_v3.c
index cd4b567bd8..a9cd3e7d03 100644
--- a/sw/airborne/imu_v3.c
+++ b/sw/airborne/imu_v3.c
@@ -5,13 +5,9 @@
#include CONFIG
#include "adc.h"
-/* accelerometers in arbitrary unit */
float imu_accel[AXIS_NB];
-/* gyros in rad/s */
-float imu_gyro[AXIS_NB];
-/* magnetometer in arbitrary unit */
-int16_t imu_mag[AXIS_NB];
-/* battery in volts */
+float imu_gyro[AXIS_NB];
+float imu_mag[AXIS_NB];
float imu_bat;
float imu_gyro_prev[AXIS_NB];
diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h
index a26541dd3a..a7f8fee3ce 100644
--- a/sw/airborne/imu_v3.h
+++ b/sw/airborne/imu_v3.h
@@ -11,7 +11,7 @@
/* calibrated sensor readings */
extern float imu_accel[AXIS_NB]; /* accelerometers in arbitrary unit */
extern float imu_gyro[AXIS_NB]; /* gyros in rad/s */
-extern int16_t imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
+extern float imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
extern float imu_bat; /* battery in volts */
extern float imu_gyro_prev[AXIS_NB]; /* previous gyros in rad/s */
@@ -92,6 +92,7 @@ extern struct adc_buf buf_bat;
( 1. - IMU_GYRO_LP_ALPHA) * imu_gyro_lp[AXIS_R]; \
}
+#ifdef USE_MICROMAG
#define ImuUpdateMag() { \
imu_mag_raw[AXIS_X] = micromag_values[0]; \
imu_mag_raw[AXIS_Y] = micromag_values[1]; \
@@ -100,7 +101,31 @@ extern struct adc_buf buf_bat;
imu_mag[AXIS_Y] = -micromag_values[1]; \
imu_mag[AXIS_Z] = micromag_values[2]; \
}
+#endif /* USE_MICROMAG */
+#ifdef USE_AMI601
+
+#define AMI_MCX 2
+#define AMI_MCY 4
+#define AMI_MCZ 0
+#define AMI_MNX 1977.5055
+#define AMI_MNY 2145.2051
+#define AMI_MNZ 1931.1511
+#define AMI_MGX 447.26642
+#define AMI_MGY 450.52012
+#define AMI_MGZ 467.83817
+
+
+#define ImuUpdateMag() { \
+ imu_mag_raw[AXIS_X] = ami601_val[AMI_MCX]; \
+ imu_mag_raw[AXIS_Y] = ami601_val[AMI_MCY]; \
+ imu_mag_raw[AXIS_Z] = ami601_val[AMI_MCZ]; \
+ imu_mag[AXIS_X] = AMI_MGX * ((float)imu_mag_raw[AXIS_X] - AMI_MNX); \
+ imu_mag[AXIS_Y] = AMI_MGY * ((float)imu_mag_raw[AXIS_Y] - AMI_MNY); \
+ imu_mag[AXIS_Z] = AMI_MGZ * ((float)imu_mag_raw[AXIS_Z] - AMI_MNZ); \
+ }
+
+#endif /* USE_AMI601 */
#define IMU_BAT_NEUTRAL 2
#define IMU_BAT_GAIN .010101
@@ -134,24 +159,28 @@ extern struct adc_buf buf_bat;
}
-#define ImuEventCheckAndHandle(user_handler) { \
- if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
- max1167_status = STA_MAX1167_IDLE; \
- ImuUpdateGyros(); \
- ImuUpdateAccels(); \
- ImuUpdateMag(); \
- user_handler(); \
- } \
+#define ImuEventCheckAndHandle(user_handler) { \
+ if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
+ max1167_status = STA_MAX1167_IDLE; \
+ ImuUpdateGyros(); \
+ ImuUpdateAccels(); \
+ user_handler(); \
+ } \
+ AMI601EventCheckAndHandle(); \
+ if (ami601_status == AMI601_DATA_AVAILABLE) { \
+ ImuUpdateMag(); \
+ ami601_status = AMI601_IDLE; \
+ } \
}
-#define ImuPeriodic() { \
- /* if (max1167_status != STA_MAX1167_IDLE) { \
- DOWNLINK_SEND_AHRS_OVERRUN(); \
- } \
- else { */ \
- max1167_read(); \
- micromag_read(); \
- /* } */ \
+#define ImuPeriodic() { \
+ max1167_read(); \
+ static uint8_t imu_pre; \
+ imu_pre++; \
+ if (imu_pre > 4) { \
+ imu_pre=0; \
+ ami601_read(); \
+ } \
}
diff --git a/sw/airborne/multitilt.c b/sw/airborne/multitilt.c
index 35a5d225a1..4ba4f6c0a7 100644
--- a/sw/airborne/multitilt.c
+++ b/sw/airborne/multitilt.c
@@ -20,7 +20,7 @@ static const float Q_bias = 0.0005;
//static const float R_accel = 0.3;
static const float R_accel = 0.5;
-static inline void multitilt_update( const float* accel, const int16_t* mag );
+static inline void multitilt_update( const float* accel, const float* mag );
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
static inline void multitilt_predict( const float* gyro );
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]);
@@ -40,7 +40,7 @@ static inline float theta_of_accel( const float* accel) {
return asin( accel[AXIS_X] / sqrt( g2 ) );
}
-static inline float psi_of_mag( const int16_t* mag) {
+static inline float psi_of_mag( const float* mag) {
/* untilt magnetometer */
const float ctheta = cos( booz_ahrs_theta );
const float stheta = sin( booz_ahrs_theta );
@@ -73,7 +73,7 @@ void booz_ahrs_init(void) {
booz_ahrs_br = 0.;
}
-void booz_ahrs_start(const float* accel, const float* gyro, const int16_t* mag) {
+void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
/* reset covariance matrices */
const float cov_init[2][2] = {{1., 0.},
{0., 1.}};
@@ -104,7 +104,7 @@ void booz_ahrs_start(const float* accel, const float* gyro, const int16_t* mag)
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
}
-void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag) {
+void booz_ahrs_run(const float* accel, const float* gyro, const float* mag) {
multitilt_predict(gyro);
multitilt_update(accel, mag);
}
@@ -175,7 +175,7 @@ static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, flo
}
-static inline void multitilt_update( const float* accel, const int16_t* mag ) {
+static inline void multitilt_update( const float* accel, const float* mag ) {
booz_ahrs_measure_phi = phi_of_accel(accel);
float err_phi = booz_ahrs_measure_phi - booz_ahrs_phi;
diff --git a/sw/in_progress/inertial/scilab/calib_rotation.sce b/sw/in_progress/inertial/scilab/calib_rotation.sce
index fe3d0445af..fb1eb99938 100644
--- a/sw/in_progress/inertial/scilab/calib_rotation.sce
+++ b/sw/in_progress/inertial/scilab/calib_rotation.sce
@@ -3,7 +3,7 @@ clf();
//##########
//##########
-function [rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log(filename)
+function [rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log(filename)
rm_x=[];
rm_y=[];
@@ -20,9 +20,9 @@ function [rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log(filename)
if strindex(line, '#') ~= 1 & length(line) ~= 0,
[nb_scan, _mz, _my, _mx] = msscanf(1, line, '148 IMU_MAG_RAW %f %f %f');
if nb_scan == 3,
- rm_z = [rm_z _mz];
- rm_y = [rm_y _my];
rm_x = [rm_x _mx];
+ rm_y = [rm_y _my];
+ rm_z = [rm_z _mz];
else
[nb_scan, _ax, _ay, _az] = msscanf(1, line, '148 IMU_ACCEL_RAW %f %f %f');
if nb_scan == 3,
@@ -57,7 +57,7 @@ function [nx, ny, nz, gx, gy, gz] = min_max_calib(dx, dy, dz)
gz = (max_z - min_z) / 2.;
endfunction
-[rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
+[rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
[m_mm_nx, m_mm_ny, m_mm_nz, m_mm_gx, m_mm_gy, m_mm_gz] = ...
min_max_calib(rm_x, rm_y, rm_z);
@@ -77,21 +77,24 @@ m_mm_norm = sqrt(m_mm_x^2 + m_mm_y^2 + m_mm_z^2);
idx_m = 1:length(rm_z);
-a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
-a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
-a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
+//a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
+//a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
+//a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
-a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
-idx_a = 1:length(ra_z);
+//a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
+//idx_a = 1:length(ra_z);
subplot(3,2,1);
param3d(rm_x, rm_y, rm_z);
subplot(3,2,2);
-param3d(ra_x, ra_y, ra_z);
+plot(idx_m, rm_x);
+
+//subplot(3,2,2);
+//param3d(ra_x, ra_y, ra_z);
subplot(3,2,3);
plot(idx_m, m_mm_norm, idx_m, ones(1, length(rm_z)));
-subplot(3,2,4);
-plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
+//subplot(3,2,4);
+//plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
subplot(3,2,5);
alpha = 0:0.1:2*%pi;
@@ -99,10 +102,10 @@ c_x = m_mm_nx+ m_mm_gx * cos(alpha);
c_y = m_mm_ny+ m_mm_gy * sin(alpha);
plot(rm_x, rm_y, c_x, c_y)
-subplot(3,2,6);
-c_x = a_mm_nx+ a_mm_gx * cos(alpha);
-c_y = a_mm_ny+ a_mm_gy * sin(alpha);
-plot(ra_x, ra_y, c_x, c_y)
+//subplot(3,2,6);
+//c_x = a_mm_nx+ a_mm_gx * cos(alpha);
+//c_y = a_mm_ny+ a_mm_gy * sin(alpha);
+//plot(ra_x, ra_y, c_x, c_y)
//