Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Update kitewing/csc_ap and mercury to use csc_board define, servos_csc.c

Browse Source 
no longer needs to send actuators, as all users are now converted to
use SendCscFromActuators() instead
This commit is contained in:
Allen Ibara
2009-07-30 19:44:20 +00:00
parent 623b143317
commit 2a079e1a95
11 changed files with 86 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/csc_ap.xml
+21
View File
@@ -7,6 +7,27 @@
<servo name="RUDDER" no="2" min="1000" neutral="1500" max="2000"/>
</servos>
<csc_boards>
<board id="0">
<msg id="SERVO_CMD_ID" type="ServoCmd">
<field_map field="servos[0]" servo_id="SERVO_AILERON"/>
<field_map field="servos[1]" servo_id="SERVO_ELEVATOR"/>
<field_map field="servos[2]" servo_id="SERVO_RUDDER"/>
</msg>
</board>
<!--
<board id="1">
<msg id="SERVO_CMD_ID" type="ServoCmd">
<field_map field="servos[0]" servo_id="DRAG1"/>
<field_map field="servos[1]" servo_id="DRAG2"/>
<field_map field="servos[2]" servo_id="DRAG3""/>
<field_map field="servos[3]" servo_id="DRAG4"/>
</msg>
</board>
-->
</csc_boards>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
conf/airframes/mercury.xml
+42 -8
View File
@@ -7,6 +7,15 @@
<servo name="S2" no="1" min="11287" neutral="32768" max="54249"/>
</servos>
<csc_boards>
<board id="0">
<msg id="SERVO_CMD_ID" type="ServoCmd">
<field_map field="servos[0]" servo_id="SERVO_S1"/>
<field_map field="servos[1]" servo_id="SERVO_S2"/>
</msg>
</board>
</csc_boards>
<section name="PROP_MISC">
<define name="PROPS_NB" value="4" />
</section>
@@ -149,21 +158,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="6000"/>
<define name="SP_MAX_R" value="1000"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(90.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-100"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
conf/autopilot/mercury.makefile
+9 -1
View File
@@ -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
sw/airborne/arm7/servos_csc.c
-2
View File
@@ -8,8 +8,6 @@
#define SERVOS_PERIOD SERVOS_TICS_OF_USEC(25000)
#endif
uint16_t csc_servo_values[SERVO_COUNT];
void actuators_init ( void )
{
sw/airborne/arm7/servos_csc.h
+1 -3
View File
@@ -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 */
sw/airborne/csc/csc_ap_link.c
+8 -2
View File
@@ -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;
sw/airborne/csc/csc_ap_link.h
+1
View File
@@ -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));
sw/airborne/csc/csc_ap_main.c
+2
View File
@@ -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();
}
sw/airborne/csc/csc_can.c
-24
View File
@@ -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 */
}
sw/airborne/csc/csc_main.c
+1 -1
View File
@@ -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;
sw/airborne/csc/mercury_ap.c
+1
View File
@@ -187,4 +187,5 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
(!booz2_autopilot_in_flight));
ActuatorsCommit();
SendCscFromActuators();
}