diff --git a/conf/airframes/gorrazoptere_091.xml b/conf/airframes/gorrazoptere_091.xml
index 2d55c6e245..ffc57de764 100644
--- a/conf/airframes/gorrazoptere_091.xml
+++ b/conf/airframes/gorrazoptere_091.xml
@@ -26,14 +26,14 @@
-
-
-
+
+
+
-
-
-
-
+
+
+
+
diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml
new file mode 100644
index 0000000000..6a1cf5fac4
--- /dev/null
+++ b/conf/airframes/microjet5.xml
@@ -0,0 +1,222 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
+
+FLASH_MODE=IAP
+
+ap.CFLAGS += -DFBW -DAP -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
+ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
+
+ap.CFLAGS += -DLED_3_BANK=0 -DLED_3_PIN=17 -DLED_4_BANK=0 -DLED_4_PIN=18 -DLED_5_BANK=0 -DLED_5_PIN=19 -DLED_6_BANK=0 -DLED_6_PIN=20
+
+
+ap.srcs += commands.c
+
+ap.CFLAGS += -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015
+ap.srcs += actuators.c $(SRC_ARCH)/servos_4015_hw.c
+
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
+ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
+
+ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+
+#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
+#ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
+
+
+ap.CFLAGS += -DINTER_MCU
+ap.srcs += inter_mcu.c
+
+ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_5 -DUSE_AD1_7 -DUSE_AD1_3 -DUSE_AD1_5
+#ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_5 -DUSE_AD1_7 -DUSE_AD1_3 -DUSE_AD1_5
+ap.srcs += $(SRC_ARCH)/adc_hw.c
+
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B19200
+
+ap.srcs += gps_ubx.c gps.c
+
+ap.CFLAGS += -DINFRARED
+ap.srcs += infrared.c estimator.c
+
+ap.CFLAGS += -DNAV
+ap.srcs += nav.c pid.c
+
+
+ap.CFLAGS += -DGYRO -DSPARK_FUN -DATTITUDE_RATE_MODE
+ap.srcs += gyro.c
+
+# Harware In The Loop
+
+#ap.CFLAGS += -DHITL
+
+
+# Config for SITL simulation
+include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+
+test.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
+test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
+
+#tunel.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
+tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
+
+
+
+
diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml
index 9f47d7b8e0..d633aa4a97 100644
--- a/conf/flight_plans/generic.xml
+++ b/conf/flight_plans/generic.xml
@@ -21,20 +21,20 @@
+
+
+
+
+
-
-
-
+
@@ -43,9 +43,11 @@
+
diff --git a/conf/flight_plans/grz.xml b/conf/flight_plans/grz.xml
index 5576149be4..11472b9bdc 100644
--- a/conf/flight_plans/grz.xml
+++ b/conf/flight_plans/grz.xml
@@ -4,23 +4,38 @@
-
-
-
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
-
+
@@ -28,6 +43,14 @@
+
+
+
+
+
+
+
+
@@ -38,10 +61,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/grz.xml b/conf/telemetry/grz.xml
index a050a7660b..d6d066aeef 100644
--- a/conf/telemetry/grz.xml
+++ b/conf/telemetry/grz.xml
@@ -3,14 +3,14 @@
-
-
-
+
+
-
+
+
diff --git a/sw/airborne/ahrs_new.c b/sw/airborne/ahrs_new.c
index 466a465800..6552779db7 100644
--- a/sw/airborne/ahrs_new.c
+++ b/sw/airborne/ahrs_new.c
@@ -1,4 +1,5 @@
+#include "std.h"
#include "ahrs_new.h"
#include
@@ -118,7 +119,7 @@ real_t Qdot[4];
*/
//#define Q_gyro 0.0075
/* I measured about 0.009 rad/s noise */
-#define Q_gyro 8e-05
+#define Q_gyro 8e-03
/*
@@ -131,6 +132,7 @@ real_t Qdot[4];
#define R_pitch 1.3 * 1.3
#define R_roll 1.3 * 1.3
#define R_yaw 2.5 * 2.5
+// #define R_yaw 1. * 1.
//#define R_pitch 0.1 * 0.1
//#define R_roll 0.1 * 0.1
//#define R_yaw 0.5 * 0.5
@@ -535,13 +537,17 @@ run_kalman(
bias_p += K[4] * error;
bias_q += K[5] * error;
bias_r += K[6] * error;
+
+/* Bound(bias_p, -0.1, 0.1); */
+/* Bound(bias_q, -0.1, 0.1); */
+/* Bound(bias_r, -0.1, 0.1); */
#endif
/*
* We would normally normalize our quaternion here, but
* instead we will allow our caller to do it
*/
- //norm_quat();
+ // norm_quat();
}
@@ -660,11 +666,27 @@ ahrs_pitch_update(
*/
real_t ahrs_heading_of_mag( const int16_t* mag ) {
const real_t ctheta = cos( ahrs_euler[1] );
- const real_t mn = ctheta * mag[0]
- - (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta;
+
+/* const real_t mn = ctheta * mag[0] */
+/* - (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta; */
- const real_t me =
- (dcm22 * mag[1] - dcm12 * mag[2]) / ctheta;
+/* const real_t me = */
+/* (dcm22 * mag[1] - dcm12 * mag[2]) / ctheta; */
+
+ /***/
+ const real_t stheta = sin( ahrs_euler[AXIS_Y] );
+ const real_t cphi = cos( ahrs_euler[AXIS_X] );
+ const real_t sphi = sin( ahrs_euler[AXIS_X] );
+
+ const real_t mn =
+ ctheta* mag[0]+
+ sphi*stheta* mag[1]+
+ cphi*stheta* mag[2];
+ const real_t me =
+ 0* mag[0]+
+ cphi* mag[1]+
+ -sphi* mag[2];
+ /***/
const real_t heading = -atan2( me, mn );
diff --git a/sw/airborne/control_grz.c b/sw/airborne/control_grz.c
index 24032148ca..c4f94ad031 100644
--- a/sw/airborne/control_grz.c
+++ b/sw/airborne/control_grz.c
@@ -37,24 +37,29 @@
while ( n < lower) { n += (uper - lower);} \
}
+
+float trim_roll = -450;
+float trim_yaw = -250;
+float trim_pitch = 0;
+
float max_roll_dot_sp = (1.5/MAX_PPRZ);
float max_pitch_dot_sp = (1.5/MAX_PPRZ);
float max_yaw_dot_sp = (1.5/MAX_PPRZ);
-float max_roll_sp = (0.8/MAX_PPRZ);
-float max_pitch_sp = (0.8/MAX_PPRZ);
+float max_roll_sp = (0.4/MAX_PPRZ);
+float max_pitch_sp = (0.4/MAX_PPRZ);
/* yaw is incremented acording to stick position */
#define UPDATE_SP_DT 0.016384
float max_yaw_sp = (0.8/MAX_PPRZ*UPDATE_SP_DT);
-float max_v_sp = (3./MAX_PPRZ);
+float max_v_sp = (6./MAX_PPRZ);
static float throttle_setpoint = 0.;
-float max_z_dot_sp = (10./MAX_PPRZ);
-float ctl_grz_z_dot_pgain = -1200;
+float max_z_dot_sp = (5./MAX_PPRZ);
+float ctl_grz_z_dot_pgain = -2000;
float ctl_grz_z_dot_igain = 3.;
-float ctl_grz_z_dot_dgain = 5.;
+float ctl_grz_z_dot_dgain = 8.;
float ctl_grz_z_dot = 0;
float ctl_grz_z_dot_setpoint = 0.;
float ctl_grz_z_dot_sum_err = 0.;
@@ -88,25 +93,24 @@ struct pid ve_pid;
void ctl_grz_init( void ) {
- roll_dot_pid.p_gain = 4000.;
- roll_dot_pid.i_gain = 0.;
- roll_dot_pid.d_gain = 2.5;
+ roll_dot_pid.p_gain = 4700.;
+ roll_dot_pid.i_gain = 1.;
+ roll_dot_pid.d_gain = 1.875;
- pitch_dot_pid.p_gain = -4000.;
- pitch_dot_pid.i_gain = 0.;
- pitch_dot_pid.d_gain = 2.5;
+ pitch_dot_pid.p_gain = -4700.;
+ pitch_dot_pid.i_gain = 1.;
+ pitch_dot_pid.d_gain = 1.875;
- yaw_dot_pid.p_gain = -4500.;
- yaw_dot_pid.i_gain = 0.;
- yaw_dot_pid.d_gain = 0.;
+ yaw_dot_pid.p_gain = -4700.;
+ yaw_dot_pid.i_gain = 1.;
+ yaw_dot_pid.d_gain = 2.;
-
- roll_pid.p_gain = -2.5;
+ roll_pid.p_gain = -5;
roll_pid.i_gain = 0.;
roll_pid.d_gain = 1.;
roll_pid.saturation = 3.;
- pitch_pid.p_gain = -2.5;
+ pitch_pid.p_gain = -5;
pitch_pid.i_gain = 0.;
pitch_pid.d_gain = 1.;
pitch_pid.saturation = 3.;
@@ -115,15 +119,16 @@ void ctl_grz_init( void ) {
yaw_pid.i_gain = 0.;
yaw_pid.d_gain = 0.;
yaw_pid.saturation = 2.;
+ yaw_pid.set_point = 0.; // RadOfDeg(-20);
vn_pid.p_gain = -0.1;
vn_pid.i_gain = 0.;
vn_pid.d_gain = 0.;
vn_pid.saturation = .2;
- ve_pid.p_gain = -0.1;
- ve_pid.i_gain = 0.;
- ve_pid.d_gain = 0.;
+ ve_pid.p_gain = -0.2;
+ ve_pid.i_gain = 1.;
+ ve_pid.d_gain = 1.;
ve_pid.saturation = .2;
}
@@ -149,25 +154,25 @@ void ctl_grz_set_setpoints_auto1( void ) {
}
#endif
// ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
- // throttle_setpoint = rc_values[RADIO_THROTTLE];
- ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.4;
+ throttle_setpoint = rc_values[RADIO_THROTTLE];
+ // ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
}
void ctl_grz_set_setpoints_auto2( void ) {
-/* roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp; */
-/* pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp; */
+ roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
+ pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
- float vright = rc_values[RADIO_ROLL] * max_v_sp;
- float vfront = rc_values[RADIO_PITCH] * max_v_sp;
+/* float vright = - rc_values[RADIO_ROLL] * max_v_sp; */
+/* float vfront = - rc_values[RADIO_PITCH] * max_v_sp; */
- float alpha = M_PI/2. - yaw_pid.measure;
- float cos_alpha = cos(alpha);
- float sin_alpha = sin(alpha);
- ve_pid.set_point = cos_alpha*vright + sin_alpha*vfront;
- vn_pid.set_point = sin_alpha*vright - cos_alpha*vfront;
-
- yaw_dot_pid.set_point = - rc_values[RADIO_YAW] * max_yaw_dot_sp;
- ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.4;
+/* float alpha = M_PI/2. - yaw_pid.measure; */
+/* float cos_alpha = cos(alpha); */
+/* float sin_alpha = sin(alpha); */
+/* ve_pid.set_point = sin_alpha*vright + cos_alpha*vfront; */
+/* vn_pid.set_point = - cos_alpha*vright + sin_alpha*vfront; */
+
+ ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
+ // ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
// throttle_setpoint = rc_values[RADIO_THROTTLE];
}
@@ -189,17 +194,34 @@ void ctl_grz_rate_run ( void ) {
float err = roll_dot_pid.measure - roll_dot_pid.set_point;
float d_err = err - roll_dot_pid.last_err;
roll_dot_pid.last_err = err;
- commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain) * roll_dot_pid.p_gain));
+ if (flying) {
+ roll_dot_pid.sum_err += err/100;
+ Bound(roll_dot_pid.sum_err, -0.2, 0.2);
+ } else
+ roll_dot_pid.sum_err = 0.;
+
+ commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain + roll_dot_pid.sum_err * roll_dot_pid.i_gain) * roll_dot_pid.p_gain));
err = pitch_dot_pid.measure - pitch_dot_pid.set_point;
d_err = err - pitch_dot_pid.last_err;
pitch_dot_pid.last_err = err;
- commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain) * pitch_dot_pid.p_gain));
+ if (flying) {
+ pitch_dot_pid.sum_err += err/100;
+ Bound(pitch_dot_pid.sum_err, -0.2, 0.2);
+ } else
+ pitch_dot_pid.sum_err = 0;
+ commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain + pitch_dot_pid.sum_err * pitch_dot_pid.i_gain) * pitch_dot_pid.p_gain));
+
err = yaw_dot_pid.measure - yaw_dot_pid.set_point;
d_err = err - yaw_dot_pid.last_err;
yaw_dot_pid.last_err = err;
- commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain) * yaw_dot_pid.p_gain));
+ if (flying) {
+ yaw_dot_pid.sum_err += err/100;
+ Bound(yaw_dot_pid.sum_err, -0.2, 0.2);
+ } else
+ yaw_dot_pid.sum_err = 0;
+ commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain + yaw_dot_pid.sum_err * yaw_dot_pid.i_gain) * yaw_dot_pid.p_gain));
commands[COMMAND_THROTTLE] = throttle_setpoint;
}
@@ -207,7 +229,6 @@ void ctl_grz_rate_run ( void ) {
void ctl_grz_attitude_run ( void ) {
-
float err = roll_pid.measure - roll_pid.set_point;
float d_err = err - roll_pid.last_err;
roll_pid.last_err = err;
@@ -243,7 +264,7 @@ void ctl_grz_alt_run( void ) {
ctl_grz_z_dot_setpoint = Chop(((err + d_err * ctl_grz_z_dgain + ctl_grz_z_sum_err * ctl_grz_z_igain) * ctl_grz_z_pgain), MIN_Z_DOT, MAX_Z_DOT);
}
-void ctl_grz_speed_run( void ) {
+void ctl_grz_z_dot_run( void ) {
float err = ctl_grz_z_dot - ctl_grz_z_dot_setpoint;
static float z_dot_last_err;
float d_err = err - z_dot_last_err;
@@ -252,7 +273,8 @@ void ctl_grz_speed_run( void ) {
if (flying) {
ctl_grz_z_dot_sum_err += err/600;
Bound(ctl_grz_z_dot_sum_err, -1., 1.);
- }
+ } else
+ ctl_grz_z_dot_sum_err = 0;
throttle_setpoint = TRIM_PPRZ(ctl_grz_throttle_level + (int16_t)((err + d_err * ctl_grz_z_dot_dgain + ctl_grz_z_dot_sum_err * ctl_grz_z_dot_igain) * ctl_grz_z_dot_pgain));
}
@@ -266,20 +288,33 @@ void ctl_grz_horiz_speed_run ( void ) {
float err = ve_pid.measure - ve_pid.set_point;
float d_err = err - ve_pid.last_err;
ve_pid.last_err = err;
- east_angle_set_point = (err + d_err * ve_pid.d_gain) * ve_pid.p_gain;
- Bound(east_angle_set_point, ve_pid.saturation, -ve_pid.saturation);
+ ve_pid.sum_err += err/100;
+ east_angle_set_point = (err + d_err * ve_pid.d_gain + ve_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain;
+ Bound(east_angle_set_point, -ve_pid.saturation, ve_pid.saturation);
+ Bound(ve_pid.sum_err, -3, 3);
- err = ve_pid.measure - vn_pid.set_point;
+ err = vn_pid.measure - vn_pid.set_point;
d_err = err - vn_pid.last_err;
vn_pid.last_err = err;
- north_angle_set_point = (err + d_err * ve_pid.d_gain) * ve_pid.p_gain;
- Bound(north_angle_set_point, vn_pid.saturation, -vn_pid.saturation);
+ vn_pid.sum_err += err/100;
+ north_angle_set_point = - ((err + d_err * ve_pid.d_gain + vn_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain);
+ Bound(north_angle_set_point, -vn_pid.saturation, vn_pid.saturation);
+ Bound(vn_pid.sum_err, -3, 3);
- float alpha = - (M_PI/2. - yaw_pid.measure);
+ float alpha = (M_PI/2. - yaw_pid.measure);
float cos_alpha = cos(alpha);
float sin_alpha = sin(alpha);
- roll_pid.set_point = cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
- pitch_pid.set_point = sin_alpha*east_angle_set_point - cos_alpha*north_angle_set_point;
-
+ roll_pid.set_point = sin_alpha*east_angle_set_point + cos_alpha*north_angle_set_point;
+ pitch_pid.set_point = - cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
}
+
+void ctl_grz_reset( void ) {
+ ctl_grz_z_dot_sum_err = 0;
+ ve_pid.sum_err = 0;
+ vn_pid.sum_err = 0;
+ roll_dot_pid.sum_err = 0;
+ pitch_dot_pid.sum_err = 0;
+ yaw_dot_pid.sum_err = 0;
+}
+
diff --git a/sw/airborne/control_grz.h b/sw/airborne/control_grz.h
index 632c8858d2..ee8fa17a92 100644
--- a/sw/airborne/control_grz.h
+++ b/sw/airborne/control_grz.h
@@ -74,7 +74,7 @@ extern void ctl_grz_set_measures( void );
extern void ctl_grz_rate_run ( void );
extern void ctl_grz_attitude_run ( void );
-extern void ctl_grz_speed_run ( void );
+extern void ctl_grz_z_dot_run ( void );
extern void ctl_grz_alt_run ( void );
extern void ctl_grz_horiz_speed_run ( void );
@@ -85,5 +85,9 @@ extern struct pid ve_pid;
extern bool_t flying;
+void ctl_grz_reset( void );
+
+extern float trim_roll, trim_yaw, trim_pitch;
+
#endif // CONTROL_GRZ_H
diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h
index f0b92f5f9c..b18753ee41 100644
--- a/sw/airborne/imu_v3.h
+++ b/sw/airborne/imu_v3.h
@@ -52,7 +52,7 @@ extern struct adc_buf buf_bat;
#define IMU_GYRO_X_GAIN -0.000220177991888642784
#define IMU_GYRO_Y_GAIN -0.000214915108532129801
-#define IMU_GYRO_Z_GAIN 0.0002
+#define IMU_GYRO_Z_GAIN 0.0002104
#define ImuUpdateGyros() { \
\
diff --git a/sw/airborne/main_grz.c b/sw/airborne/main_grz.c
index ac879c4965..b59496726c 100644
--- a/sw/airborne/main_grz.c
+++ b/sw/airborne/main_grz.c
@@ -37,6 +37,10 @@ uint16_t rangemeter; /* cm */
#define SWITCH_PEDIOD_MIN 30 /* 60Hz */
+
+bool_t sum_err_reset;
+
+
/** 60Hz */
void periodic_task_ap( void ) {
@@ -46,8 +50,13 @@ void periodic_task_ap( void ) {
_10Hz++;
if (_10Hz>=6) _10Hz = 0;
- if (!_10Hz)
+ if (!_10Hz) {
PeriodicSendAp();
+ if (sum_err_reset) {
+ sum_err_reset = FALSE;
+ ctl_grz_reset();
+ }
+ }
static uint16_t last_rangemeters[NB_RANGES];
static uint8_t last_idx;
@@ -55,13 +64,13 @@ void periodic_task_ap( void ) {
// ClearBit(ADCSRA, ADIE);
rangemeter = RangeCmOfAdc(rangemeter_adc_buf.sum/rangemeter_adc_buf.av_nb_sample);
// SetBit(ADCSRA, ADIE);
- ctl_grz_z = rangemeter / 100.; /* cm -> m */
last_idx++;
if (last_idx >= NB_RANGES) last_idx = 0;
float last_avg = (float)sum_rangemeters / NB_RANGES;
sum_rangemeters += rangemeter - last_rangemeters[last_idx];
last_rangemeters[last_idx] = rangemeter;
float avg = (float)sum_rangemeters / NB_RANGES;
+ ctl_grz_z = avg / 100.; /* cm -> m */
ctl_grz_z_dot = ((avg - last_avg) * 61. / 100.);
@@ -90,19 +99,19 @@ void periodic_task_ap( void ) {
if (pprz_mode == PPRZ_MODE_AUTO2) {
ctl_grz_set_setpoints_auto2();
- }
- else if (pprz_mode == PPRZ_MODE_AUTO1) {
- ctl_grz_alt_run();
+ } else if (pprz_mode == PPRZ_MODE_AUTO1) {
ctl_grz_set_setpoints_auto1();
}
if (pprz_mode >= PPRZ_MODE_AUTO2) {
- if (!_10Hz)
- ctl_grz_horiz_speed_run();
+ ctl_grz_z_dot_run();
+/* if (!_10Hz) */
+/* ctl_grz_horiz_speed_run(); */
}
if (pprz_mode >= PPRZ_MODE_AUTO1) {
- ctl_grz_speed_run();
+ // ctl_grz_alt_run();
+ // ctl_grz_z_dot_run();
ctl_grz_attitude_run();
}
diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c
index 77bcd12e7c..ee3e211a32 100644
--- a/sw/airborne/main_imu.c
+++ b/sw/airborne/main_imu.c
@@ -79,7 +79,7 @@ static inline void main_event_task( void ) {
micromag_data_available = FALSE;
spi_cur_slave = SPI_SLAVE_NONE;
ImuUpdateMag();
- // DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
+ DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
spi_cur_slave = SPI_SLAVE_MAX;
max1167_read();
}
@@ -146,7 +146,8 @@ static inline void ahrs_task( void ) {
ahrs_state_update();
ahrs_compass_update(ahrs_heading_of_mag(imu_mag));
DOWNLINK_SEND_AHRS(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
- //DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]);
+ // DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]);
+ // DOWNLINK_SEND_AHRS2(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]);
break;
}
ahrs_step++;