[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4196] CSC updates
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [4196] CSC updates |
|
Date: |
Mon, 21 Sep 2009 22:06:13 +0000 |
Revision: 4196
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4196
Author: aibara
Date: 2009-09-21 22:06:12 +0000 (Mon, 21 Sep 2009)
Log Message:
-----------
CSC updates
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/mercury_csc.xml
paparazzi3/trunk/conf/settings/csc_ap.xml
paparazzi3/trunk/conf/settings/mercury_csc.xml
paparazzi3/trunk/conf/telemetry/csc_ap.xml
paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
paparazzi3/trunk/sw/airborne/csc/csc_ap_link.c
paparazzi3/trunk/sw/airborne/csc/csc_ap_link.h
paparazzi3/trunk/sw/airborne/csc/csc_ap_main.c
paparazzi3/trunk/sw/airborne/csc/csc_autopilot.c
paparazzi3/trunk/sw/airborne/csc/csc_autopilot.h
paparazzi3/trunk/sw/airborne/csc/csc_can.h
paparazzi3/trunk/sw/airborne/csc/csc_datalink.c
paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h
paparazzi3/trunk/sw/airborne/csc/csc_telemetry.c
paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h
paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c
Modified: paparazzi3/trunk/conf/airframes/mercury_csc.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/mercury_csc.xml 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/conf/airframes/mercury_csc.xml 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -2,10 +2,11 @@
<!-- Four Motors -->
<section name="BUSS_TWI" prefix="BUSS_TWI_BLMC_">
- <define name="NB" value="4" />
+ <define name="NB" value="8" />
+ <define name="NB_SEND" value="6" />
</section>
<section name="BUSS_BLMC" prefix="BUSS_BLMC_">
- <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+ <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58, 0x5A, 0x5E, 0x5C,
0x60 }"/>
</section>
<!-- Up to Four Servos -->
@@ -32,12 +33,16 @@
ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200
-DUART1_VIC_SLOT=6
ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
-ap.CFLAGS += -DCAN1_BTR=CANBitrate125k_2MHz
+#ap.CFLAGS += -DCAN1_BTR=CANBitrate125k_2MHz
+#ap.CFLAGS += -DCAN1_BTR=CANBitrate312k5_5MHz
+ap.CFLAGS += -DCAN1_BTR=CANBitrate187k5_3MHz
+#ap.CFLAGS += -DCAN1_BTR=CANBitrate156k25_2_5MHz
ap.CFLAGS += -DMOTORS=\"buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC_MOTOR
ap.srcs += $(SRC_CSC_ARCH)/buss_twi_blmc_hw.c
# on I2C0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
+ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=125 -DI2C0_SCLH=125 -DI2C0_VIC_SLOT=10
+ap.CFLAGS += -DI2C0_STOP_HANDLER_HEADER=\"csc/arm7/buss_twi_blmc_hw.h\"
-DI2C0_STOP_HANDLER="motors_commit_next"
ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
include $(PAPARAZZI_SRC)/conf/autopilot/mercury_csc.makefile
Modified: paparazzi3/trunk/conf/settings/csc_ap.xml
===================================================================
--- paparazzi3/trunk/conf/settings/csc_ap.xml 2009-09-21 19:22:28 UTC (rev
4195)
+++ paparazzi3/trunk/conf/settings/csc_ap.xml 2009-09-21 22:06:12 UTC (rev
4196)
@@ -5,7 +5,7 @@
<settings>
<dl_settings>
<dl_settings NAME="Misc" >
- <dl_setting VAR="telemetry_mode_Ap" MIN="0" STEP="1" MAX="1"
module="csc_telemetry" shortname="telemetry"
+ <dl_setting VAR="telemetry_mode_Ap_DefaultChannel" MIN="0" STEP="1"
MAX="1" module="csc_telemetry" shortname="telemetry"
values="Default|Debug"/>
</dl_settings>
<dl_settings NAME="imu params">
@@ -54,6 +54,11 @@
<dl_setting VAR="csc_trims.aileron" MIN="-6000" STEP="10" MAX="6000"
module="csc_autopilot" shortname="aileron trim"/>
<dl_setting VAR="csc_trims.rudder" MIN="-6000" STEP="10" MAX="6000"
module="csc_autopilot" shortname="rudder trim"/>
</dl_settings>
-
+ <dl_settings NAME="GPS">
+ <dl_setting VAR="csc_gps_setzero" MIN="0" STEP="1" MAX="1"
module="csc_autopilot" shortname="zero GPS"/>
+ <dl_setting VAR="csc_gps_weight" MIN="-2" STEP="0.01" MAX="2"
module="csc_autopilot" shortname="GPS weight"/>
+ <dl_setting VAR="csc_gps_gain" MIN="-1" STEP="0.0001" MAX="1"
module="csc_autopilot" shortname="GPS gain"/>
+ <dl_setting VAR="csc_gps_filter_weight" MIN="0" STEP="0.0001" MAX="1"
module="csc_autopilot" shortname="GPS low-pass coeff"/>
+ </dl_settings>
</dl_settings>
</settings>
Modified: paparazzi3/trunk/conf/settings/mercury_csc.xml
===================================================================
--- paparazzi3/trunk/conf/settings/mercury_csc.xml 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/conf/settings/mercury_csc.xml 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -5,7 +5,7 @@
<settings>
<dl_settings>
<dl_settings NAME="Misc" >
- <dl_setting VAR="telemetry_mode_Ap" MIN="0" STEP="1" MAX="1"
module="csc/csc_telemetry" shortname="telemetry"
+ <dl_setting VAR="telemetry_mode_Ap_DefaultChannel" MIN="0" STEP="1"
MAX="1" module="csc/csc_telemetry" shortname="telemetry"
values="Default|Debug"/>
</dl_settings>
<!-- <dl_settings NAME="imu params">
Modified: paparazzi3/trunk/conf/telemetry/csc_ap.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/csc_ap.xml 2009-09-21 19:22:28 UTC (rev
4195)
+++ paparazzi3/trunk/conf/telemetry/csc_ap.xml 2009-09-21 22:06:12 UTC (rev
4196)
@@ -6,13 +6,14 @@
<message name="ALIVE" period="5"/>
<message name="ATTITUDE" period="0.1"/>
<message name="BOOZ2_INS3" period="0.25"/>
- <message name="GPS" period="1.5"/>
<message name="GPS_SOL" period="1.5"/>
- <message name="VANE_SENSOR" period="0.1"/>
- <message name="SIMPLE_COMMANDS" period="0.4"/>
+ <message name="VANE_SENSOR" period="0.2"/>
+ <message name="SIMPLE_COMMANDS" period="0.2"/>
<message name="QUAD_STATUS" period="1.5"/>
<message name="DL_VALUE" period="1.5"/>
<message name="CONTROLLER_GAINS" period="1"/>
+ <message name="GPS_ERROR" period="0.25"/>
+ <message name="GPS" period="1.5"/>
</mode>
<mode name="debug">
<message name="ALIVE" period="5"/>
@@ -26,6 +27,8 @@
<message name="IMU_MAG" period="0.4"/>
<message name="DL_VALUE" period="0.5"/>
<message name="RMAT_DEBUG" period="1"/>
+ <message name="GPS_SOL" period="1.5"/>
+ <message name="BOOZ2_INS3" period="0.25"/>
</mode>
</process>
</telemetry>
Modified: paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c 2009-09-21
19:22:28 UTC (rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c 2009-09-21
22:06:12 UTC (rev 4196)
@@ -37,18 +37,20 @@
}
-void motors_commit()
+void motors_commit(int force)
{
- buss_twi_blmc_idx = 0;
- buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;
- buss_twi_blmc_send_next();
+ if (force || buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) {
+ buss_twi_blmc_idx = 0;
+ buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;
+ buss_twi_blmc_send_next();
+ }
}
void motors_commit_next()
{
buss_twi_blmc_idx++;
- if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB)
+ if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB_SEND)
buss_twi_blmc_send_next();
else
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
Modified: paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h 2009-09-21
19:22:28 UTC (rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h 2009-09-21
22:06:12 UTC (rev 4196)
@@ -14,11 +14,7 @@
void motors_init ( void );
void motors_set_motor(uint8_t id, int16_t value);
-void motors_commit();
+void motors_commit(int force);
void motors_commit_next();
-
-#define BussTwiBlmcNext() motors_commit_next();
-
-
#endif /* BUSS_TWI_BLMC_HW_H */
Modified: paparazzi3/trunk/sw/airborne/csc/csc_ap_link.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_ap_link.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_ap_link.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -3,12 +3,13 @@
#include "csc_can.h"
#include "led.h"
+
int32_t csc_ap_link_error_cnt;
static void (* servo_msg_cb)(struct CscServoCmd *);
static void (* motor_msg_cb)(struct CscMotorMsg *);
static void (* rc_msg_cb)(struct CscRCMsg *);
-static void (* prop_msg_cb)(struct CscPropCmd *);
+static void (* prop_msg_cb)(struct CscPropCmd *, int idx);
static void (* gpsfix_cb)(struct CscGPSFixMsg *);
static void (* gpspos_cb)(struct CscGPSPosMsg *);
static void (* gpsacc_cb)(struct CscGPSAccMsg *);
@@ -67,7 +68,7 @@
motor_msg_cb = cb;
}
-void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd))
+void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd, int idx))
{
prop_msg_cb = cb;
}
@@ -130,7 +131,7 @@
LED_ON(ERROR_LED);
csc_ap_link_error_cnt++;
} else
- prop_msg_cb((struct CscPropCmd *) &msg->dat_a);
+ prop_msg_cb((struct CscPropCmd *) &msg->dat_a, 0);
break;
case CSC_GPS_FIX_ID:
if (len != sizeof(struct CscGPSFixMsg)){
@@ -153,6 +154,13 @@
} else
gpsacc_cb((struct CscGPSAccMsg *) &msg->dat_a);
break;
+ case CSC_PROP2_CMD_ID:
+ if (len != sizeof(struct CscPropCmd)){
+ LED_ON(ERROR_LED);
+ csc_ap_link_error_cnt++;
+ } else
+ prop_msg_cb((struct CscPropCmd *) &msg->dat_a, 1);
+ break;
}
}
Modified: paparazzi3/trunk/sw/airborne/csc/csc_ap_link.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_ap_link.h 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_ap_link.h 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -16,7 +16,7 @@
void csc_ap_link_send_adc(float adc1, float adc2);
void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd));
void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg));
-void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd));
+void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd, int idx));
void csc_ap_link_set_rc_cmd_cb(void (* cb)(struct CscRCMsg *msg));
void csc_ap_link_set_gpsfix_cb(void (* cb)(struct CscGPSFixMsg *msg));
void csc_ap_link_set_gpspos_cb(void (* cb)(struct CscGPSPosMsg *msg));
Modified: paparazzi3/trunk/sw/airborne/csc/csc_ap_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_ap_main.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_ap_main.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -185,7 +185,7 @@
}
xsens_periodic_task();
if (pprz_mode == PPRZ_MODE_AUTO1)
- csc_ap_periodic(cpu_time);
+ csc_ap_periodic(cpu_time, &booz_ins_gps_pos_cm_ned,
&booz_ins_gps_speed_cm_s_ned);
if (csc_trims_set) {
csc_trims_set = 0;
Modified: paparazzi3/trunk/sw/airborne/csc/csc_autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_autopilot.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_autopilot.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -37,6 +37,7 @@
#include "pwm_input.h"
#include "LPC21xx.h"
#include "print.h"
+#include "booz/booz2_gps.h"
struct control_gains csc_gains;
struct control_reference csc_reference;
@@ -58,7 +59,19 @@
float csc_drag_pitch = 0;
float csc_drag_yaw = 0;
+static float csc_gps_zero_x = 0;
+static float csc_gps_zero_y = 0;
+struct gps_reference csc_gps_errors;
+int csc_gps_setzero = 0;
+float csc_gps_weight = 0;
+float csc_gps_gain = 0;
+float csc_gps_filter_weight = 0;
+
#define PWM_INPUT_COUNTS_PER_REV 61358.
+
+#define BOOZ2_NED_METERS 0.00380625
+#define BOOZ2_NED_METERS_SEC 0.0000019073
+
static void update_vane_angle( void )
{
csc_vane_angle = (csc_vane_filter_constant * csc_vane_angle) +
(1-csc_vane_filter_constant)*RadOfDeg( 360. * pwm_input_duration /
PWM_INPUT_COUNTS_PER_REV) - RadOfDeg(csc_vane_angle_offset);
@@ -84,6 +97,10 @@
csc_trims.aileron = -80;
csc_trims.rudder = 80;
+ csc_gps_weight = 0;
+ csc_gps_gain = 1;
+ csc_gps_filter_weight = 0;
+
csc_gamma.roll_kp = 0;
csc_gamma.roll_kd = 0;
csc_gamma.roll_ki = 0;
@@ -93,7 +110,7 @@
csc_gamma.yaw_kp = 0;
csc_gamma.yaw_kd = 0;
csc_gamma.yaw_ki = 0;
-
+
memset(&csc_reference, 0, sizeof(struct control_reference));
}
@@ -119,6 +136,11 @@
}
}
+static float compute_ref_trajectory()
+{
+
+}
+
void csc_autopilot_set_roll_ki(float ki)
{
update_ki(ki, &csc_gains.roll_ki, &csc_errors.eulers_i.phi);
@@ -134,6 +156,21 @@
update_ki(ki, &csc_gains.yaw_ki, &csc_errors.eulers_i.psi);
}
+static void csc_zero_gps(struct NedCoor_i *ned_pos, struct NedCoor_i *ned_vel)
+{
+ csc_gps_zero_x = ned_pos->x*BOOZ2_NED_METERS;
+ csc_gps_zero_y = ned_pos->y*BOOZ2_NED_METERS;
+ csc_gps_setzero = 0;
+}
+
+static void calculate_gps_errors(struct gps_reference *error, struct NedCoor_i
*ned_pos, struct NedCoor_i *ned_vel)
+{
+ error->pos.x = (ned_pos->x*BOOZ2_NED_METERS -
csc_gps_zero_x)*csc_gps_gain*(1 - csc_gps_filter_weight) +
error->pos.x*csc_gps_filter_weight;
+ error->pos.y = (ned_pos->y*BOOZ2_NED_METERS -
csc_gps_zero_y)*csc_gps_gain*(1 - csc_gps_filter_weight) +
error->pos.x*csc_gps_filter_weight;
+ error->rate.x = -ned_vel->x*BOOZ2_NED_METERS_SEC*csc_gps_gain*(1 -
csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
+ error->rate.y = -ned_vel->y*BOOZ2_NED_METERS_SEC*csc_gps_gain*(1 -
csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
+}
+
static void calculate_errors(struct control_reference *errors)
{
struct FloatEulers xsens_eulers;
@@ -149,10 +186,13 @@
errors->eulers.phi = xsens_eulers.phi - csc_reference.eulers.phi;
errors->eulers.theta = xsens_eulers.theta - csc_reference.eulers.theta;
- //errors->eulers.psi = xsens_eulers.psi - csc_reference.eulers.psi;
- //errors->eulers.psi = csc_vane_angle - csc_reference.eulers.psi;
+ // The following mess mixes in the wind vane and the GPS errors in
+ // an arbitrary fashion.
errors->eulers.psi = (csc_vane_angle*csc_vane_weight)
- + (xsens_eulers.psi - csc_reference.eulers.psi)*(1 - csc_vane_weight);
+ + (xsens_eulers.psi - csc_reference.eulers.psi)*(1 - csc_vane_weight);
+ errors->eulers.psi = (1 - csc_gps_weight)*errors->eulers.psi
+ + csc_gps_weight*(cos(xsens_eulers.psi)*csc_gps_errors.pos.x
+ + sin(xsens_eulers.psi)*csc_gps_errors.pos.y);
errors->rates.p = xsens_rates.p - csc_reference.rates.p;
errors->rates.q = xsens_rates.q - csc_reference.rates.q;
@@ -190,11 +230,14 @@
reference->eulers.psi /= MAX_PPRZ;
}
-void csc_ap_periodic(int time)
+void csc_ap_periodic(int time, struct NedCoor_i *ned_pos, struct NedCoor_i
*ned_vel)
{
//static int counter = 0;
update_vane_angle();
calculate_reference(&csc_reference, time);
+ if (csc_gps_setzero)
+ csc_zero_gps(ned_pos, ned_vel);
+ calculate_gps_errors(&csc_gps_errors, ned_pos, ned_vel);
calculate_errors(&csc_errors);
commands[COMMAND_ROLL] = -csc_gains.roll_kp * (csc_errors.eulers.phi)
@@ -247,3 +290,4 @@
gains->yaw_kd += -csc_gamma.yaw_kd*errors->eulers.psi*xsens_gyro_z[xsens_id];
gains->yaw_ki += -csc_gamma.yaw_ki*errors->eulers.psi*errors->eulers_i.psi;
}
+
Modified: paparazzi3/trunk/sw/airborne/csc/csc_autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_autopilot.h 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_autopilot.h 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -28,6 +28,7 @@
#include "types.h"
#include "std.h"
#include "math/pprz_algebra_float.h"
+#include "math/pprz_geodetic_int.h"
struct control_gains {
float pitch_kp;
@@ -47,6 +48,11 @@
struct FloatEulers eulers_i;
};
+struct gps_reference {
+ struct FloatVect3 pos;
+ struct FloatVect3 rate;
+};
+
struct control_trims {
int elevator;
int aileron;
@@ -69,13 +75,18 @@
extern float csc_drag_pitch;
extern float csc_drag_yaw;
+extern struct gps_reference csc_gps_errors;
+extern int csc_gps_setzero;
+extern float csc_gps_weight;
+extern float csc_gps_gain;
+extern float csc_gps_filter_weight;
void csc_autopilot_set_roll_ki(float ki);
void csc_autopilot_set_pitch_ki(float ki);
void csc_autopilot_set_yaw_ki(float ki);
void csc_ap_init( void );
-void csc_ap_periodic (int time);
+void csc_ap_periodic (int time, struct NedCoor_i *ned_pos, struct NedCoor_i
*ned_vel);
void csc_ap_set_trims (void );
void csc_ap_clear_ierrors (void );
void csc_ap_update_gains(struct control_reference *errors, struct
control_gains *gains);
Modified: paparazzi3/trunk/sw/airborne/csc/csc_can.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_can.h 2009-09-21 19:22:28 UTC (rev
4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_can.h 2009-09-21 22:06:12 UTC (rev
4196)
@@ -12,7 +12,9 @@
// Common CAN bit rates
#define CANBitrate62k5_1MHz 0x001C001D
#define CANBitrate125k_2MHz 0x001C000E
-#define CANBitrate125k_3MHz 0x001C0009
+#define CANBitrate156k25_2_5MHz 0x001C000B
+#define CANBitrate187k5_3MHz 0x001C0009
+#define CANBitrate312k5_5MHz 0x001C0005
#define CSC_BOARD_MASK 0x0F
#define CSC_MSGID_MASK 0x7F
Modified: paparazzi3/trunk/sw/airborne/csc/csc_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_datalink.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_datalink.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -15,7 +15,7 @@
case DL_PING:
{
- DOWNLINK_SEND_PONG();
+ DOWNLINK_SEND_PONG(DefaultChannel);
}
break;
@@ -24,7 +24,7 @@
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
- DOWNLINK_SEND_DL_VALUE(&i, &var);
+ DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &var);
}
break;
Modified: paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -17,6 +17,7 @@
#define CSC_GPS_FIX_ID 7
#define CSC_GPS_POS_ID 8
#define CSC_GPS_ACC_ID 9
+#define CSC_PROP2_CMD_ID 10
/* Received from the autopilot */
Modified: paparazzi3/trunk/sw/airborne/csc/csc_telemetry.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_telemetry.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_telemetry.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -25,4 +25,4 @@
#include "csc_telemetry.h"
-uint8_t telemetry_mode_Ap;
+uint8_t telemetry_mode_Ap_DefaultChannel;
Modified: paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -27,60 +27,62 @@
#include "actuators.h"
+extern uint8_t telemetry_mode_Ap_DefaultChannel;
+
#include "settings.h"
#include "booz/booz2_gps.h"
-#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
+#define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan)
-#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM)
+#define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM)
-#define PERIODIC_SEND_DOWNLINK() { \
+#define PERIODIC_SEND_DOWNLINK(_chan) { \
static uint16_t last; \
uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_0; \
last = downlink_nb_bytes; \
- DOWNLINK_SEND_DOWNLINK(&downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
+ DOWNLINK_SEND_DOWNLINK(_chan, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
}
#ifdef PROPS_NB
#include "mercury_ap.h"
-#define PERIODIC_SEND_MERCURY_PROPS()
DOWNLINK_SEND_MERCURY_PROPS(&(mixed_commands[0]),&(mixed_commands[1]),&(mixed_commands[2]),&(mixed_commands[3]))
+#define PERIODIC_SEND_MERCURY_PROPS(_chan) DOWNLINK_SEND_MERCURY_PROPS(_chan,
&(mixed_commands[0]),&(mixed_commands[1]),&(mixed_commands[2]),&(mixed_commands[3]))
#endif /* PROPS_NB */
#ifdef BUSS_TWI_BLMC_NB
-#define PERIODIC_SEND_MERCURY_PROPS()
DOWNLINK_SEND_MERCURY_PROPS(&(motor_power[0]),&(motor_power[1]),&(motor_power[2]),&(motor_power[3]))
+#define PERIODIC_SEND_MERCURY_PROPS(_chan) DOWNLINK_SEND_MERCURY_PROPS(_chan,
&(motor_power[0]),&(motor_power[1]),&(motor_power[2]),&(motor_power[3]))
#endif /* BUSS_TWI_BLMC_NB */
#ifdef COMMANDS_NB
#include "commands.h"
-#define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
+#define PERIODIC_SEND_COMMANDS(_chan) DOWNLINK_SEND_COMMANDS(_chan,
COMMANDS_NB, commands)
#endif /* COMMANDS_NB */
-#define PERIODIC_SEND_SIMPLE_COMMANDS()
DOWNLINK_SEND_SIMPLE_COMMANDS(&commands[0], &commands[1], &commands[2])
-#define PERIODIC_SEND_CONTROLLER_GAINS()
DOWNLINK_SEND_CONTROLLER_GAINS(&csc_gains.roll_kp, &csc_gains.roll_kd,
&csc_gains.roll_ki, &csc_gains.pitch_kp, &csc_gains.pitch_kd,
&csc_gains.pitch_ki, &csc_gains.yaw_kp, &csc_gains.yaw_kd, &csc_gains.yaw_ki)
+#define PERIODIC_SEND_SIMPLE_COMMANDS(_chan)
DOWNLINK_SEND_SIMPLE_COMMANDS(_chan, &commands[0], &commands[1], &commands[2])
+#define PERIODIC_SEND_CONTROLLER_GAINS(_chan)
DOWNLINK_SEND_CONTROLLER_GAINS(_chan, &csc_gains.roll_kp, &csc_gains.roll_kd,
&csc_gains.roll_ki, &csc_gains.pitch_kp, &csc_gains.pitch_kd,
&csc_gains.pitch_ki, &csc_gains.yaw_kp, &csc_gains.yaw_kd, &csc_gains.yaw_ki)
#ifdef SERVOS_NB
-#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
+#define PERIODIC_SEND_ACTUATORS(_chan) DOWNLINK_SEND_ACTUATORS(_chan,
SERVOS_NB, actuators)
#endif
#ifdef USE_RADIO_CONTROL
#include "booz_radio_control.h"
-#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL,
radio_control.values)
-#define PERIODIC_SEND_QUAD_STATUS()
DOWNLINK_SEND_QUAD_STATUS(&radio_control.status, &pprz_mode, &vsupply,
&cpu_time)
+#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan,
RADIO_CONTROL_NB_CHANNEL, radio_control.values)
+#define PERIODIC_SEND_QUAD_STATUS(_chan) DOWNLINK_SEND_QUAD_STATUS(_chan,
&radio_control.status, &pprz_mode, &vsupply, &cpu_time)
#else
#ifdef RADIO_CONTROL
-#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
-#define PERIODIC_SEND_QUAD_STATUS() DOWNLINK_SEND_QUAD_STATUS(&rc_status,
&pprz_mode, &vsupply, &cpu_time)
+#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, PPM_NB_PULSES,
rc_values)
+#define PERIODIC_SEND_QUAD_STATUS(_chan) DOWNLINK_SEND_QUAD_STATUS(_chan,
&rc_status, &pprz_mode, &vsupply, &cpu_time)
#endif
#endif /* USE_RADIO_CONTROL */
#ifdef USE_GPS
-#define PERIODIC_SEND_BOOZ2_GPS() { \
- DOWNLINK_SEND_BOOZ2_GPS( &booz_gps_state.ecef_pos.x, \
- &booz_gps_state.ecef_pos.y, \
- &booz_gps_state.ecef_pos.z, \
+#define PERIODIC_SEND_BOOZ2_GPS(_chan) { \
+ DOWNLINK_SEND_BOOZ2_GPS( &booz_ins_gps_pos_cm_ned.x, \
+ &booz_ins_gps_pos_cm_ned.y, \
+ &booz_ins_gps_pos_cm_ned.z, \
&booz_gps_state.ecef_speed.x, \
&booz_gps_state.ecef_speed.y, \
&booz_gps_state.ecef_speed.z, \
@@ -91,8 +93,17 @@
&booz_gps_state.fix); \
}
#endif
+
+#define PERIODIC_SEND_GPS_ERROR() { \
+ DOWNLINK_SEND_GPS_ERROR( &csc_gps_errors.pos.x, \
+ &csc_gps_errors.pos.y, \
+ &csc_gps_errors.pos.z, \
+ &csc_gps_errors.rate.x, \
+ &csc_gps_errors.rate.y, \
+ &csc_gps_errors.rate.z) \
+ }
-#define PERIODIC_SEND_BOOZ2_INS3() { \
+#define PERIODIC_SEND_BOOZ2_INS3(_chan) { \
DOWNLINK_SEND_BOOZ2_INS3(&booz_ins_gps_pos_cm_ned.x, \
&booz_ins_gps_pos_cm_ned.y, \
&booz_ins_gps_pos_cm_ned.z, \
@@ -101,10 +112,10 @@
&booz_ins_gps_speed_cm_s_ned.z \
) }
-#define PERIODIC_SEND_GPS_SOL() DOWNLINK_SEND_GPS_SOL(&booz_gps_state.pacc, \
+#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan,
&booz_gps_state.pacc, \
&booz_gps_state.sacc, &booz_gps_state.pdop, &booz_gps_state.num_sv)
-#define PERIODIC_SEND_GPS() { uint32_t zero = 0;
DOWNLINK_SEND_GPS(&booz_gps_state.fix, \
+#define PERIODIC_SEND_GPS(_chan) { uint32_t zero = 0; DOWNLINK_SEND_GPS(_chan,
&booz_gps_state.fix, \
&booz_ins_gps_pos_cm_ned.y, \
&booz_ins_gps_pos_cm_ned.x, \
&zero, \
@@ -115,6 +126,5 @@
&zero, \
&zero, &zero) }
-extern uint8_t telemetry_mode_Ap;
#endif /* CSC_TELEMETRY_H */
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2009-09-21 19:22:28 UTC
(rev 4195)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2009-09-21 22:06:12 UTC
(rev 4196)
@@ -55,7 +55,7 @@
#include "csc_ap_link.h"
static inline void on_servo_cmd(struct CscServoCmd *cmd);
static inline void on_motor_cmd(struct CscMotorMsg *msg);
-static inline void on_prop_cmd(struct CscPropCmd *msg);
+static inline void on_prop_cmd(struct CscPropCmd *msg, int idx);
#define SERVO_TIMEOUT (SYS_TICS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
@@ -112,7 +112,7 @@
static uint32_t csc_loops = 0;
#ifdef DOWNLINK
- PeriodicSendAp();
+ PeriodicSendAp_DefaultChannel();
#endif
if (servo_cmd_timeout > SERVO_TIMEOUT) {
@@ -143,12 +143,12 @@
#define MAX_SERVO SYS_TICS_OF_USEC(2000)
#ifdef USE_BUSS_TWI_BLMC_MOTOR
-static void on_prop_cmd(struct CscPropCmd *cmd)
+static void on_prop_cmd(struct CscPropCmd *cmd, int idx)
{
- for(uint8_t i = 0; i < BUSS_TWI_BLMC_NB; i++)
- motors_set_motor(i,cmd->speeds[i]);
+ for(uint8_t i = 0; i < 4; i++)
+ motors_set_motor(i + idx * 4, cmd->speeds[i]);
- motors_commit();
+ motors_commit(idx == 1);
++can_msg_count;
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4196] CSC updates,
Allen Ibara <=