diff --git a/conf/airframes/csc_ap.xml b/conf/airframes/csc_ap.xml
index 72ab3b9be3..6552873ecf 100644
--- a/conf/airframes/csc_ap.xml
+++ b/conf/airframes/csc_ap.xml
@@ -7,6 +7,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/mercury.xml b/conf/airframes/mercury.xml
index 23bc06e4ab..52d4580a58 100644
--- a/conf/airframes/mercury.xml
+++ b/conf/airframes/mercury.xml
@@ -7,6 +7,15 @@
+
+
+
+
+
+
+
+
+
@@ -149,21 +158,46 @@
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
diff --git a/conf/autopilot/mercury.makefile b/conf/autopilot/mercury.makefile
index d7d746d683..9123982155 100644
--- a/conf/autopilot/mercury.makefile
+++ b/conf/autopilot/mercury.makefile
@@ -79,7 +79,15 @@ ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ_IMU_TYPE_H=\"mercury_xsens.h\"
ap.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.CFLAGS += -DFLOAT_T=float
-ap.srcs += $(SRC_BOOZ)/booz_stabilization.c $(SRC_BOOZ)/stabilization/booz_stabilization_attitude.c $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
+ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
+ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
+
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
+ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
+ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
+
# AHI copied from booz w/ light modifications for vertical control
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
diff --git a/sw/airborne/arm7/servos_csc.c b/sw/airborne/arm7/servos_csc.c
index 67af84bf03..091533d25d 100644
--- a/sw/airborne/arm7/servos_csc.c
+++ b/sw/airborne/arm7/servos_csc.c
@@ -8,8 +8,6 @@
#define SERVOS_PERIOD SERVOS_TICS_OF_USEC(25000)
#endif
-uint16_t csc_servo_values[SERVO_COUNT];
-
void actuators_init ( void )
{
diff --git a/sw/airborne/arm7/servos_csc.h b/sw/airborne/arm7/servos_csc.h
index da21438743..166ee9333b 100644
--- a/sw/airborne/arm7/servos_csc.h
+++ b/sw/airborne/arm7/servos_csc.h
@@ -5,8 +5,6 @@
#include "airframe.h"
#include "actuators.h"
#include "sys_time.h"
-#include "csc_ap_link.h"
-#include "csc_msg_def.h"
#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
#define ChopServo(x,a,b) Chop(x, a, b)
@@ -16,7 +14,7 @@
static inline void ActuatorsCommit(void)
{
- csc_ap_send_msg(CSC_SERVO_CMD_ID, (uint8_t *)&actuators, sizeof(struct CscServoCmd));
+
}
#endif /* SERVOS_CSC_H */
diff --git a/sw/airborne/csc/csc_ap_link.c b/sw/airborne/csc/csc_ap_link.c
index 2fd796b7e6..626da69aa3 100644
--- a/sw/airborne/csc/csc_ap_link.c
+++ b/sw/airborne/csc/csc_ap_link.c
@@ -35,7 +35,8 @@ void csc_ap_link_send_status(uint32_t loops, uint32_t msgs)
}
-void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len)
+// Generic function for sending can messages
+void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t len)
{
struct CscCanMsg out_msg;
@@ -44,13 +45,18 @@ void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len)
// set frame length
out_msg.frame = (len << 16);
// build msg id using our board ID and requested msg id
- out_msg.id = ((CSC_BOARD_ID & CSC_BOARD_MASK) << 7) | (msg_id & CSC_MSGID_MASK);
+ out_msg.id = ((board_id & CSC_BOARD_MASK) << 7) | (msg_id & CSC_MSGID_MASK);
// copy msg payload in host order
memcpy((char *)&out_msg.dat_a, buf, len);
// send via CAN
csc_can1_send(&out_msg);
}
+void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len)
+{
+ can_write_csc(CSC_BOARD_ID, msg_id, buf, len);
+}
+
void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd))
{
servo_msg_cb = cb;
diff --git a/sw/airborne/csc/csc_ap_link.h b/sw/airborne/csc/csc_ap_link.h
index eef0b911d8..12fb674e78 100644
--- a/sw/airborne/csc/csc_ap_link.h
+++ b/sw/airborne/csc/csc_ap_link.h
@@ -11,6 +11,7 @@ extern int32_t csc_ap_link_error_cnt;
void csc_ap_link_init(void);
void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len);
+void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t len);
void csc_ap_link_send_status(uint32_t loops, uint32_t msgs);
void csc_ap_link_send_adc(float adc1, float adc2);
void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd));
diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c
index 9b3e37f5aa..850be760e8 100644
--- a/sw/airborne/csc/csc_ap_main.c
+++ b/sw/airborne/csc/csc_ap_main.c
@@ -47,6 +47,7 @@
#include "csc_autopilot.h"
#include "csc_can.h"
#include "pwm_input.h"
+#include "csc_ap_link.h"
#include "led.h"
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
@@ -194,6 +195,7 @@ static void csc_main_periodic( void )
#ifdef ACTUATORS
SetActuatorsFromCommands(commands);
#endif
+ SendCscFromActuators();
}
diff --git a/sw/airborne/csc/csc_can.c b/sw/airborne/csc/csc_can.c
index c629c79034..ba029728e7 100644
--- a/sw/airborne/csc/csc_can.c
+++ b/sw/airborne/csc/csc_can.c
@@ -25,9 +25,6 @@ bool_t can1_msg_received;
struct CscCanMsg can1_rx_msg;
static void (* can1_callback)(struct CscCanMsg *);
-static bool_t can1_msg_transmit_queued = 0;
-static struct CscCanMsg can1_tx_queued_msg;
-
static void CAN1_Rx_ISR ( void ) __attribute__((naked));
/*static void CAN1_Tx_ISR ( void ) __attribute__((naked));*/
static void CAN1_Err_ISR ( void ) __attribute__((naked));
@@ -143,16 +140,9 @@ void csc_can1_send(struct CscCanMsg* msg) {
}
if (!csc_can1_tx_available()) { /* transmit channel not available */
-#if 0
- if(!can1_msg_transmit_queued){
- can1_msg_transmit_queued = 1;
- memcpy(&can1_tx_queued_msg,msg,sizeof(struct CscCanMsg));
- }
-#else
if (! msg_queue_full(&can1_tx_queue)) {
msg_enqueue(&can1_tx_queue, msg);
}
-#endif
// LED_ON(2);
return;
@@ -210,7 +200,6 @@ void csc_can_event(void)
{
#ifdef USE_CAN1
-#if 1
// drain the RX Queue
while(!msg_queue_empty(&can1_rx_queue)) {
struct CscCanMsg rx_msg;
@@ -219,25 +208,12 @@ void csc_can_event(void)
can1_callback(&rx_msg);
//restoreIRQ(cpsr); // restore global interrupts
}
-#else
- if (can1_msg_received) {
- can1_callback(&can1_rx_msg);
- can1_msg_received = FALSE;
- }
-#endif
-#if 1
if(!msg_queue_empty(&can1_tx_queue) && csc_can1_tx_available()) {
struct CscCanMsg msg;
msg_dequeue(&can1_tx_queue, &msg);
csc_can1_send(&msg);
}
-#else
- if(can1_msg_transmit_queued && csc_can1_tx_available()){
- csc_can1_send(&can1_tx_queued_msg);
- can1_msg_transmit_queued = 0;
- }
-#endif
#endif /* USE_CAN1 */
}
diff --git a/sw/airborne/csc/csc_main.c b/sw/airborne/csc/csc_main.c
index 54dceccbe3..a84393415c 100644
--- a/sw/airborne/csc/csc_main.c
+++ b/sw/airborne/csc/csc_main.c
@@ -137,7 +137,7 @@ static inline void on_gps_event(void) {
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
- ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_speed);
+ ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
}
struct CscGPSFixMsg fix_msg;
diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c
index 59082525f5..630df4ef5f 100644
--- a/sw/airborne/csc/mercury_ap.c
+++ b/sw/airborne/csc/mercury_ap.c
@@ -187,4 +187,5 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
(!booz2_autopilot_in_flight));
ActuatorsCommit();
+ SendCscFromActuators();
}