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 25 Packages Projects Releases Wiki Activity

Updates to CSC/MERCURY for new booz directory structure

Browse Source
This commit is contained in:
Allen Ibara
2009-07-22 22:34:50 +00:00
parent c6f01558a7
commit e60bb6b470
13 changed files with 155 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/airframe.dtd
+17 -1
View File
@@ -1,8 +1,12 @@
<!-- Paparazzi airframe DTD -->
<!ELEMENT airframe (servos|commands|rc_commands|ap_only_commands|command_laws|section|makefile|modules)*>
<!ELEMENT airframe (servos|commands|csc_boards|rc_commands|ap_only_commands|command_laws|section|makefile|modules)*>
<!ELEMENT servos (servo)*>
<!ELEMENT commands (axis)*>
<!ELEMENT csc_boards (board)*>
<!ELEMENT board (msg)*>
<!ELEMENT msg (field_map)*>
<!ELEMENT field_map EMPTY>
<!ELEMENT rc_commands (set)*>
<!ELEMENT ap_only_commands (copy)*>
<!ELEMENT command_laws (let|set)*>
@@ -27,6 +31,7 @@ name CDATA #IMPLIED>
<!ATTLIST rc_commands>
<!ATTLIST ap_only_commands>
<!ATTLIST command_laws>
<!ATTLIST csc_boards>
<!ATTLIST section
name CDATA #IMPLIED
@@ -39,6 +44,17 @@ min CDATA #REQUIRED
neutral CDATA #REQUIRED
max CDATA #REQUIRED>
<!ATTLIST board
id CDATA #REQUIRED>
<!ATTLIST msg
id CDATA #REQUIRED
type CDATA #REQUIRED>
<!ATTLIST field_map
field CDATA #REQUIRED
servo_id CDATA #REQUIRED>
<!ATTLIST axis
name CDATA #REQUIRED
failsafe_value CDATA #REQUIRED>
conf/airframes/mercury.xml
+29 -13
View File
@@ -31,10 +31,10 @@
</section>
<section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
<define name="MIN_ERR_Z" value="BOOZ_POS_I_OF_F(-10.)"/>
<define name="MAX_ERR_Z" value="BOOZ_POS_I_OF_F( 10.)"/>
<define name="MIN_ERR_ZD" value="BOOZ_SPEED_I_OF_F(-10.)"/>
<define name="MAX_ERR_ZD" value="BOOZ_SPEED_I_OF_F( 10.)"/>
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
@@ -74,9 +74,10 @@
<define name="GYRO_Q_NEUTRAL" value="32745"/>
<define name="GYRO_R_NEUTRAL" value="32447"/>
<define name="GYRO_P_SENS_NUM" value="(1<<IRATE_RES)" />
<define name="GYRO_Q_SENS_NUM" value="(1<<IRATE_RES)" />
<define name="GYRO_R_SENS_NUM" value="(1<<IRATE_RES)" />
<!-- FIXME: this wants GYRO_[P|Q|R]_SENS, defines -->
<define name="GYRO_P_SENS_NUM" value="(1<<INT32_RATE_FRAC)" />
<define name="GYRO_Q_SENS_NUM" value="(1<<INT32_RATE_FRAC)" />
<define name="GYRO_R_SENS_NUM" value="(1<<INT32_RATE_FRAC)" />
<define name="GYRO_P_SENS_DEN" value="-1174" />
<define name="GYRO_Q_SENS_DEN" value="1165" />
@@ -95,9 +96,10 @@
<define name="ACCEL_Y_NEUTRAL" value="33240"/>
<define name="ACCEL_Z_NEUTRAL" value="33029"/>
<define name="ACCEL_X_SENS_NUM" value="(1<<IACCEL_RES)" />
<define name="ACCEL_Y_SENS_NUM" value="(1<<IACCEL_RES)" />
<define name="ACCEL_Z_SENS_NUM" value="(1<<IACCEL_RES)" />
<!-- FIXME: this wants ACCEL_[X|Y|Z]_SENS, defines -->
<define name="ACCEL_X_SENS_NUM" value="(1<<INT32_ACCEL_FRAC)" />
<define name="ACCEL_Y_SENS_NUM" value="(1<<INT32_ACCEL_FRAC)" />
<define name="ACCEL_Z_SENS_NUM" value="(1<<INT32_ACCEL_FRAC)" />
<define name="ACCEL_X_SENS_DEN" value="-414" />
<define name="ACCEL_Y_SENS_DEN" value="411" />
@@ -111,9 +113,10 @@
<define name="MAG_Z_NEUTRAL" value="32802"/>
<define name="MAG_X_SENS_NUM" value="(1<<IMAG_RES)" />
<define name="MAG_Y_SENS_NUM" value="(1<<IMAG_RES)" />
<define name="MAG_Z_SENS_NUM" value="(1<<IMAG_RES)" />
<!-- FIXME: this wants MAG_[X|Y|Z]_SENS, define -->
<define name="MAG_X_SENS_NUM" value="(1<<11)" />
<define name="MAG_Y_SENS_NUM" value="(1<<11)" />
<define name="MAG_Z_SENS_NUM" value="(1<<11)" />
<define name="MAG_X_SENS_DEN" value="-7730" />
<define name="MAG_Y_SENS_DEN" value="7576" />
@@ -132,6 +135,18 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
@@ -172,6 +187,7 @@
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/mercury.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/csc_baro.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/subsystems/booz2_radio_control_dummy.makefile
</makefile>
</airframe>
conf/autopilot/mercury.makefile
+3 -3
View File
@@ -74,12 +74,12 @@ ap.srcs += $(SRC_CSC)/csc_datalink.c
ap.CFLAGS += -DPPRZ_TRIG_CONST=const
ap.srcs += $(SRC_CSC)/mercury_xsens.c $(SRC_BOOZ)/booz_imu.c math/pprz_trig_int.c
ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ2_IMU_TYPE=\"mercury_xsens.h\"
ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ_IMU_TYPE_H=\"mercury_xsens.h\"
ap.srcs += $(SRC_BOOZ)/booz2_filter_attitude_cmpl_euler.c $(SRC_BOOZ)/booz_ahrs_aligner.c
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)/booz2_stabilization.c $(SRC_BOOZ)/booz2_stabilization_attitude.c
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c $(SRC_BOOZ)/stabilization/booz_stabilization_attitude.c $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
# AHI copied from booz w/ light modifications for vertical control
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
conf/settings/mercury.xml
+23 -16
View File
@@ -5,8 +5,8 @@
<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"
values="Default|Debug"/>
<dl_setting VAR="telemetry_mode_Ap" MIN="0" STEP="1" MAX="2" module="csc/csc_telemetry" shortname="telemetry"
values="Default|Debug|Props"/>
</dl_settings>
<dl_settings NAME="imu params">
<dl_setting VAR="xsens_setzero" MIN="0" STEP="1" MAX="1" module="csc/mercury_xsens" shortname="zero orientation"/>
@@ -15,22 +15,22 @@
<dl_settings NAME="Att Loop">
<dl_setting VAR="booz_stabilization_pgain.x" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="pgain phi"/>
<dl_setting VAR="booz_stabilization_pgain.y" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="pgain tetha"/>
<dl_setting VAR="booz_stabilization_pgain.z" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="pgain psi"/>
<dl_setting VAR="booz_stabilization_dgain.x" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="dgain p"/>
<dl_setting VAR="booz_stabilization_dgain.y" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="dgain q"/>
<dl_setting VAR="booz_stabilization_dgain.z" MIN="-4000" STEP="1" MAX="-1" module="booz2_stabilization_attitude" shortname="dgain r"/>
<dl_setting VAR="booz_stabilization_igain.x" MIN="-300" STEP="1" MAX="0" module="booz2_stabilization_attitude" shortname="igain phi"/>
<dl_setting VAR="booz_stabilization_igain.y" MIN="-300" STEP="1" MAX="0" module="booz2_stabilization_attitude" shortname="igain theta"/>
<dl_setting VAR="booz_stabilization_igain.z" MIN="-300" STEP="1" MAX="0" module="booz2_stabilization_attitude" shortname="igain psi"/>
<dl_setting VAR="booz_stabilization_ddgain.x" MIN="0" STEP="1" MAX="500" module="booz2_stabilization_attitude" shortname="ddgain p"/>
<dl_setting VAR="booz_stabilization_ddgain.y" MIN="0" STEP="1" MAX="500" module="booz2_stabilization_attitude" shortname="ddgain q"/>
<dl_setting VAR="booz_stabilization_ddgain.z" MIN="0" STEP="1" MAX="500" module="booz2_stabilization_attitude" shortname="ddgain r"/>
<dl_setting VAR="booz_stabilization_pgain.x" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi"/>
<dl_setting VAR="booz_stabilization_pgain.y" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain tetha"/>
<dl_setting VAR="booz_stabilization_pgain.z" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain psi"/>
<dl_setting VAR="booz_stabilization_dgain.x" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain p"/>
<dl_setting VAR="booz_stabilization_dgain.y" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain q"/>
<dl_setting VAR="booz_stabilization_dgain.z" MIN="-4000" STEP="1" MAX="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain r"/>
<dl_setting VAR="booz_stabilization_igain.x" MIN="-300" STEP="1" MAX="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi"/>
<dl_setting VAR="booz_stabilization_igain.y" MIN="-300" STEP="1" MAX="0" module="stabilization/booz_stabilization_attitude" shortname="igain theta"/>
<dl_setting VAR="booz_stabilization_igain.z" MIN="-300" STEP="1" MAX="0" module="stabilization/booz_stabilization_attitude" shortname="igain psi"/>
<dl_setting VAR="booz_stabilization_ddgain.x" MIN="0" STEP="1" MAX="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain p"/>
<dl_setting VAR="booz_stabilization_ddgain.y" MIN="0" STEP="1" MAX="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain q"/>
<dl_setting VAR="booz_stabilization_ddgain.z" MIN="0" STEP="1" MAX="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain r"/>
</dl_settings>
<dl_settings NAME="Filter">
<dl_setting VAR="booz2_face_reinj_1" MIN="512" STEP="512" MAX="262144" module="booz2_filter_attitude_cmpl_euler" shortname="reinj_1"/>
<dl_setting VAR="booz2_face_reinj_1" MIN="512" STEP="512" MAX="262144" module="ahrs/booz2_filter_attitude_cmpl_euler" shortname="reinj_1"/>
</dl_settings>
<dl_settings NAME="Servos">
@@ -39,7 +39,14 @@
<dl_setting VAR="mercury_supervision_yaw_comp_slope" MIN="0" STEP="1" MAX="40" module="mercury_supervision" shortname="comp slope"/>
<dl_setting VAR="mercury_supervision_yaw_comp_offset" MIN="1700" STEP="1" MAX="3700" module="mercury_supervision" shortname="comp offset"/>
</dl_settings>
<dl_settings NAME="Props" >
<dl_setting VAR="props_throttle_pass" MIN="0" STEP="1" MAX="1" module="mercury_ap" shortname="throttle pass" values="FALSE|TRUE"/>
<dl_setting VAR="props_enable[PROP_UPPER_LEFT]" MIN="0" STEP="1" MAX="1" module="mercury_ap" shortname="upper left" values="FALSE|TRUE"/>
<dl_setting VAR="props_enable[PROP_UPPER_RIGHT]" MIN="0" STEP="1" MAX="1" module="mercury_ap" shortname="upper right" values="FALSE|TRUE"/>
<dl_setting VAR="props_enable[PROP_LOWER_LEFT]" MIN="0" STEP="1" MAX="1" module="mercury_ap" shortname="lower left" values="FALSE|TRUE"/>
<dl_setting VAR="props_enable[PROP_LOWER_RIGHT]" MIN="0" STEP="1" MAX="1" module="mercury_ap" shortname="lower right" values="FALSE|TRUE"/>
</dl_settings>
sw/airborne/csc/csc_autopilot.h
+1 -1
View File
@@ -27,7 +27,7 @@
#include "types.h"
#include "std.h"
#include "pprz_algebra_float.h"
#include "math/pprz_algebra_float.h"
struct control_gains {
float pitch_kp;
sw/airborne/csc/csc_telemetry.h
+5 -6
View File
@@ -63,12 +63,11 @@
#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
#endif
#ifdef RADIO_CONTROL
#include "radio_control.h"
#define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(&last_ppm_cpt, PPM_NB_PULSES, ppm_pulses)
#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)
#endif /* RADIO_CONTROL */
#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)
#endif /* USE_RADIO_CONTROL */
extern uint8_t telemetry_mode_Ap;
sw/airborne/csc/csc_xsens.c
+2 -1
View File
@@ -36,7 +36,7 @@
#include "messages.h"
#include "uart.h"
//#include "com_stats.h"
#include "pprz_algebra_float.h"
#include "math/pprz_algebra_float.h"
#include "string.h"
void parse_xsens_msg(uint8_t xsens_id, uint8_t c );
@@ -211,6 +211,7 @@ static uint8_t xsens_msg_idx[XSENS_COUNT];
static uint8_t ck[XSENS_COUNT];
static uint8_t send_ck[XSENS_COUNT];
void xsens_init( void )
{
for (int i = 0; i < XSENS_COUNT; i++) {
sw/airborne/csc/csc_xsens.h
+1 -1
View File
@@ -26,7 +26,7 @@
#define __CSC_XSENS_H__
#include "types.h"
#include "pprz_algebra_float.h"
#include "math/pprz_algebra_float.h"
#define XSENS_COUNT 1
sw/airborne/csc/mercury_ap.c
+38 -18
View File
@@ -27,12 +27,12 @@
#include "commands.h"
#include "mercury_xsens.h"
#include "booz2_autopilot.h"
#include "booz2_stabilization.h"
#include "booz2_stabilization_attitude.h"
#include "booz_stabilization.h"
#include "stabilization/booz_stabilization_attitude.h"
#include "led.h"
#include "pprz_algebra_float.h"
#include "math/pprz_algebra_float.h"
#include "string.h"
#include "radio_control.h"
#include "booz_radio_control.h"
#include "mercury_supervision.h"
#include "actuators.h"
#include "props_csc.h"
@@ -54,6 +54,9 @@ uint32_t booz2_autopilot_in_flight_counter;
uint16_t mercury_yaw_servo_gain;
uint8_t props_enable[PROPS_NB];
uint8_t props_throttle_pass;
#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME (512/4)
#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
@@ -69,6 +72,11 @@ void csc_ap_init(void) {
booz2_autopilot_in_flight_counter = 0;
booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
booz2_autopilot_tol = 0;
props_throttle_pass = 0;
for(uint8_t i = 0; i < PROPS_NB; i++){
props_enable[i] = 1;
}
}
@@ -76,7 +84,7 @@ void csc_ap_init(void) {
#define BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT() { \
if (booz2_autopilot_in_flight) { \
if (booz2_autopilot_in_flight_counter > 0) { \
if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
if (radio_control.values[RADIO_CONTROL_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
booz2_autopilot_in_flight_counter--; \
if (booz2_autopilot_in_flight_counter == 0) { \
booz2_autopilot_in_flight = FALSE; \
@@ -90,7 +98,7 @@ void csc_ap_init(void) {
else { /* not in flight */ \
if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME && \
booz2_autopilot_motors_on) { \
if (rc_values[RADIO_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
if (radio_control.values[RADIO_CONTROL_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
booz2_autopilot_in_flight_counter++; \
if (booz2_autopilot_in_flight_counter == BOOZ2_AUTOPILOT_IN_FLIGHT_TIME) \
booz2_autopilot_in_flight = TRUE; \
@@ -104,9 +112,9 @@ void csc_ap_init(void) {
#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() { \
if(!booz2_autopilot_motors_on){ \
if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \
(rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \
rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \
if (radio_control.values[RADIO_CONTROL_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \
if ( booz2_autopilot_motors_on_counter < BOOZ2_AUTOPILOT_MOTOR_ON_TIME) { \
booz2_autopilot_motors_on_counter++; \
if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME){ \
@@ -142,27 +150,39 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
if(kill) booz2_autopilot_motors_on = FALSE;
booz2_autopilot_in_flight = _in_flight;
booz2_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
booz2_stabilization_attitude_run(booz2_autopilot_in_flight);
booz_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
booz_stabilization_attitude_run(booz2_autopilot_in_flight);
booz2_guidance_v_run(booz2_autopilot_in_flight);
booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 105 / 7200 + 95;
booz_stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 105 / 7200 + 95;
CscSetCommands(booz2_stabilization_cmd,
CscSetCommands(booz_stabilization_cmd,
booz2_autopilot_in_flight,booz2_autopilot_motors_on);
BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on);
props_set(PROP_UPPER_LEFT, mixed_commands[PROP_UPPER_LEFT]);
props_set(PROP_LOWER_RIGHT,mixed_commands[PROP_LOWER_RIGHT]);
props_set(PROP_LOWER_LEFT, mixed_commands[PROP_LOWER_LEFT]);
props_set(PROP_UPPER_RIGHT,mixed_commands[PROP_UPPER_RIGHT]);
if(booz2_autopilot_motors_on && props_throttle_pass){
Bound(booz2_stabilization_cmd[COMMAND_THRUST],0,255);
for(uint8_t i = 0; i < PROPS_NB; i++)
mixed_commands[i] = booz2_stabilization_cmd[COMMAND_THRUST];
}
for(uint8_t i = 0; i < PROPS_NB; i++){
if(props_enable[i])
props_set(i,mixed_commands[i]);
else
props_set(i,0);
}
props_commit();
MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
booz2_stabilization_cmd,
booz_stabilization_cmd,
mixed_commands,
(!booz2_autopilot_in_flight));
ActuatorsCommit();
sw/airborne/csc/mercury_ap.h
+2
View File
@@ -28,6 +28,8 @@
#include "airframe.h"
extern pprz_t mixed_commands[PROPS_NB];
extern uint8_t props_enable[PROPS_NB];
extern uint8_t props_throttle_pass;
void csc_ap_init( void );
void csc_ap_periodic (uint8_t _in_flight, uint8_t kill);
sw/airborne/csc/mercury_main.c
+19 -19
View File
@@ -40,19 +40,20 @@
#include "csc_msg_def.h"
#include ACTUATORS
#include "booz2_imu.h"
#include "booz_ahrs_aligner.h"
#include "booz_ahrs.h"
#include "booz/booz_imu.h"
#include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/booz_ahrs.h"
#include "mercury_xsens.h"
#include "csc_telemetry.h"
#include "csc_adc.h"
#include "mercury_ap.h"
#include "booz2_autopilot.h"
#include "booz2_guidance_v.h"
#include "booz/booz2_autopilot.h"
#include "booz/guidance/booz2_guidance_v.h"
#include "csc_can.h"
#include "csc_baro.h"
#include "booz_radio_control.h"
#include "booz2_stabilization_attitude.h"
#include "booz/stabilization/booz_stabilization_attitude.h"
extern uint8_t vsupply;
@@ -89,17 +90,17 @@ static void on_rc_cmd(struct CscRCMsg *msg)
{
uint16_t aux2_flag;
rc_values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
rc_values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
rc_values[RADIO_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
radio_control.values[RADIO_CONTROL_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
radio_control.values[RADIO_CONTROL_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
radio_control.values[RADIO_CONTROL_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
pprz_mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1;
rc_values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
rc_values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000);
rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
radio_control.values[RADIO_CONTROL_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
radio_control.values[RADIO_CONTROL_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000);
radio_control.values[RADIO_CONTROL_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
time_since_last_ppm = 0;
rc_status = RC_OK;
radio_control.time_since_last_frame = 0;
radio_control.status = RADIO_CONTROL_OK;
}
static inline void csc_main_init( void ) {
@@ -111,19 +112,18 @@ static inline void csc_main_init( void ) {
Uart0Init();
Uart1Init();
booz2_imu_init();
booz_imu_init();
booz_ahrs_aligner_init();
booz_ahrs_init();
xsens_init();
booz2_stabilization_attitude_init();
booz_stabilization_attitude_init();
booz2_guidance_v_init();
booz_ins_init();
csc_adc_init();
ppm_init();
baro_scp_init();
@@ -138,7 +138,7 @@ static inline void csc_main_init( void ) {
csc_ap_init();
int_enable();
booz2_stabilization_attitude_enter();
booz_stabilization_attitude_enter();
}
@@ -147,7 +147,7 @@ static inline void csc_main_periodic( void )
static uint32_t csc_loops = 0;
PeriodicSendAp();
radio_control_periodic_task();
radio_control_periodic();
cpu_time++;
sw/airborne/csc/mercury_supervision.h
+2 -2
View File
@@ -72,8 +72,8 @@ extern pprz_t mercury_supervision_yaw_comp_offset;
#define MERCURY_SURFACES_SUPERVISION_RUN(_actuators,_cmds,_props,_surfaces_manual) { \
if (_surfaces_manual) { \
_actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*rc_values[RADIO_YAW])/2/7200; \
_actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*rc_values[RADIO_YAW])/2/7200; \
_actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*radio_control.values[RADIO_CONTROL_YAW])/2/7200; \
_actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*radio_control.values[RADIO_CONTROL_YAW])/2/7200; \
} else { \
int32_t bndcmd = (mercury_supervision_yaw_servo_gain*_cmds[COMMAND_YAW]*mercury_supervision_yaw_comp_offset)/(mercury_supervision_yaw_comp_slope*((_props[PROP_UPPER_RIGHT]+_props[PROP_UPPER_LEFT])/2 - 105) + mercury_supervision_yaw_comp_offset); \
Bound(bndcmd,-400,400); \
sw/airborne/csc/mercury_xsens.c
+13 -8
View File
@@ -27,9 +27,10 @@
*/
#include "mercury_xsens.h"
#include "booz2_imu.h"
#include "booz_ahrs.h"
#include "booz_ahrs_aligner.h"
#include "booz/booz_imu.h"
#include "booz/booz_ahrs.h"
#include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/booz_imu.h"
#include "csc_booz2_ins.h"
#include <inttypes.h>
@@ -40,7 +41,7 @@
#include "messages.h"
#include "uart.h"
//#include "com_stats.h"
#include "pprz_algebra_float.h"
#include "math/pprz_algebra_float.h"
#include "string.h"
void parse_xsens_msg(uint8_t xsens_id, uint8_t c );
@@ -201,7 +202,11 @@ static uint8_t xsens_msg_idx[XSENS_COUNT];
static uint8_t ck[XSENS_COUNT];
static uint8_t send_ck[XSENS_COUNT];
void booz_imu_impl_init( void )
{
}
void xsens_init( void )
{
for (int i = 0; i < XSENS_COUNT; i++) {
@@ -305,9 +310,9 @@ void xsens_parse_msg( uint8_t xsens_id ) {
booz_imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.y = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
Booz2ImuScaleGyro();
Booz2ImuScaleAccel();
Booz2ImuScaleMag();
BoozImuScaleGyro();
BoozImuScaleAccel();
BoozImuScaleMag();
// Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7
// mmt 6/15/09