Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 09:36:19 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

CSC updates for mercury and kitewing/csc_ap

Browse Source
This commit is contained in:
Allen Ibara
2009-07-10 01:03:37 +00:00
parent 97e29b7de2
commit b07f024812
19 changed files with 210 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/csc_ap.xml
+1
View File
@@ -26,6 +26,7 @@
</command_laws>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/pwm_input.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/csc_ap.makefile
</makefile>
conf/airframes/mercury.xml
+14 -7
View File
@@ -8,8 +8,8 @@
<servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="2" min="1000" neutral="1500" max="2000"/>
-->
<servo name="S1" no="0" min="21482" neutral="32768" max="44054"/>
<servo name="S2" no="1" min="21482" neutral="32768" max="44054"/>
<servo name="S1" no="0" min="11287" neutral="32768" max="54249"/>
<servo name="S2" no="1" min="11287" neutral="32768" max="54249"/>
</servos>
<section name="PROP_MISC">
@@ -24,6 +24,13 @@
<define name="UPPER_RIGHT" value="3" />
</section>
<section name="MERCURY_SUPERVISION" PREFIX="MERCURY_SUPERVISION_">
<define name="YAW_SERVO_GAIN" value="20" />
<define name="YAW_MOTOR_GAIN" value="10" />
<define name="YAW_COMP_SLOPE" value="0" />
<define name="YAW_COMP_OFFSET" value="3000" />
</section>
<section name="CSC_ACTUATORS" PREFIX="CSC_ACTUATORS_">
<define name="REMOTE_IDS" value="{1,2,5,6,7,8}" />
</section>
@@ -113,12 +120,12 @@
<define name="MAG_Y_SENS_NUM" value="(1<<IMAG_RES)" />
<define name="MAG_Z_SENS_NUM" value="(1<<IMAG_RES)" />
<define name="MAG_X_SENS_DEN" value="7730" />
<define name="MAG_X_SENS_DEN" value="-7730" />
<define name="MAG_Y_SENS_DEN" value="7576" />
<define name="MAG_Z_SENS_DEN" value="7757" />
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(-1.8))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(-2.2))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
</section>
@@ -133,8 +140,8 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<define name="SP_MAX_THETA" value="6000"/>
<define name="SP_MAX_R" value="1000"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
conf/airframes/mercury_csc.xml
+5
View File
@@ -28,8 +28,13 @@ CSC_ID=0
SRC_BOOZ=booz
SRC_BOOZ_ARCH=booz/arm7
ap.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200 -DUART1_VIC_SLOT=6
ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
include $(PAPARAZZI_SRC)/conf/autopilot/mercury_csc.makefile
</makefile>
</airframe>
conf/autopilot/mercury.makefile
+3 -1
View File
@@ -87,7 +87,7 @@ ap.srcs += $(SRC_CSC)/csc_booz2_ins.c $(SRC_CSC)/csc_booz2_hf_float.c $(SRC_CSC)
ap.srcs += $(SRC_CSC)/csc_booz2_guidance_v.c
ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
ap.CFLAGS += -DAP_LINK_CAN # -DCAN_LED=2
ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_3MHz
ap.CFLAGS += -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
ap.srcs += $(SRC_CSC)/csc_can.c
@@ -109,6 +109,8 @@ ap.CFLAGS += -DRADIO_CONTROL
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ap.srcs += $(SRC_CSC)/mercury_ap.c
ap.srcs += $(SRC_CSC)/mercury_supervision.c
ap.CFLAGS += -DERROR_LED=4
conf/autopilot/mercury_csc.makefile
-7
View File
@@ -95,15 +95,8 @@ ap.srcs += $(SRC_CSC_ARCH)/buss_twi_blmc_hw.c
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
ap.srcs += $(SRC_CSC)/csc_adc.c
ap.CFLAGS += -DTHROTTLE_LINK=Uart0 -DTHROTTLE_LED=3
ap.srcs += $(SRC_CSC)/csc_throttle.c
ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200 -DUART1_VIC_SLOT=6
ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
ap.CFLAGS += -DERROR_LED=4
#
conf/settings/csc_ap.xml
+12 -4
View File
@@ -11,18 +11,26 @@
<dl_settings NAME="imu params">
<dl_setting VAR="xsens_setzero" MIN="0" STEP="1" MAX="1" module="csc_xsens" shortname="zero orientation"/>
<dl_setting VAR="xsens_psi_offset[0]" MIN="-180" STEP="1" MAX="180" module="csc_xsens" shortname="psi offset"/>
<dl_setting VAR="csc_yaw_setpoint_rate" MIN="0" STEP="0.01" MAX="3" module="csc_autopilot" shortname="setpoint rate"/>
<dl_setting VAR="csc_yaw_setpoint_range" MIN="0" STEP="0.01" MAX="100" module="csc_autopilot" shortname="setpoint range"/>
</dl_settings>
<dl_settings NAME="vane sensor">
<dl_setting VAR="csc_vane_angle_offset" MIN="0" STEP="1" MAX="360" module="csc_autopilot" shortname="vane offset"/>
</dl_settings>
<dl_settings NAME="flight params">
<dl_setting VAR="csc_gains.pitch_kp" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="pitch kp"/>
<dl_setting VAR="csc_gains.pitch_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="pitch kd"/>
<dl_setting VAR="csc_gains.pitch_ki" MIN="-20" STEP=".1" MAX="20" module="csc_autopilot" shortname="pitch ki" handler="set_pitch_ki"/>
<dl_setting VAR="csc_gains.pitch_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="pitch ki" handler="set_pitch_ki"/>
<dl_setting VAR="csc_gains.roll_kp" MIN="-8000" STEP="10" MAX="8000" module="csc_autopilot" shortname="roll kp"/>
<dl_setting VAR="csc_gains.roll_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="roll kd"/>
<dl_setting VAR="csc_gains.roll_ki" MIN="-20" STEP=".1" MAX="20" module="csc_autopilot" shortname="roll ki" handler="set_roll_ki"/>
<dl_setting VAR="csc_gains.roll_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="roll ki" handler="set_roll_ki"/>
<dl_setting VAR="csc_gains.yaw_kp" MIN="-10000" STEP="10" MAX="10000" module="csc_autopilot" shortname="yaw kp"/>
<dl_setting VAR="csc_gains.yaw_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="yaw kd"/>
<dl_setting VAR="csc_gains.yaw_ki" MIN="-20" STEP=".1" MAX="20" module="csc_autopilot" shortname="yaw ki" handler="set_yaw_ki"/>
<dl_setting VAR="csc_yaw_weight" MIN="-2" STEP=".01" MAX="2" module="csc_autopilot" shortname="yaw weight"/>
<dl_setting VAR="csc_gains.yaw_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="yaw ki" handler="set_yaw_ki"/>
<dl_setting VAR="csc_yaw_weight" MIN="-2" STEP="0.01" MAX="2" module="csc_autopilot" shortname="yaw weight"/>
<dl_setting VAR="csc_yaw_rudder" MIN="-5" STEP="0.01" MAX="5" module="csc_autopilot" shortname="yaw rudder"/>
<dl_setting VAR="csc_yaw_aileron" MIN="-5" STEP="0.01" MAX="5" module="csc_autopilot" shortname="yaw aileron"/>
<dl_setting VAR="csc_yaw_deadband" MIN="-30" STEP="0.01" MAX="30" module="csc_autopilot" shortname="yaw deadband"/>
</dl_settings>
<dl_settings NAME="trims">
<dl_setting VAR="csc_trims_set" MIN="0" STEP="1" MAX="1" module="csc_ap_main" shortname="set trims"/>
conf/settings/mercury.xml
+7
View File
@@ -33,6 +33,13 @@
<dl_setting VAR="booz2_face_reinj_1" MIN="512" STEP="512" MAX="262144" module="booz2_filter_attitude_cmpl_euler" shortname="reinj_1"/>
</dl_settings>
<dl_settings NAME="Servos">
<dl_setting VAR="mercury_supervision_yaw_servo_gain" MIN="0" STEP="1" MAX="20" module="mercury_supervision" shortname="servo gain"/>
<dl_setting VAR="mercury_supervision_yaw_motor_gain" MIN="0" STEP="1" MAX="20" module="mercury_supervision" shortname="motor gain"/>
<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>
conf/telemetry/csc_ap.xml
+4 -2
View File
@@ -4,8 +4,10 @@
<process name="Ap">
<mode name="default">
<message name="ALIVE" period="5"/>
<message name="ATTITUDE" period="0.04"/>
<message name="COMMANDS" period="0.08"/>
<message name="ATTITUDE" period="0.1"/>
<message name="VANE_SENSOR" period="0.1"/>
<message name="COMMANDS" period="1"/>
<message name="SIMPLE_COMMANDS" period="0.1"/>
<message name="QUAD_STATUS" period="1.1"/>
<message name="DL_VALUE" period="1.5"/>
</mode>
conf/telemetry/mercury.xml
+10 -4
View File
@@ -5,15 +5,21 @@
<mode name="default">
<message name="ALIVE" period="5"/>
<!-- <message name="ATTITUDE" period="0.04"/> -->
<message name="COMMANDS" period="0.08"/>
<message name="QUAD_STATUS" period="1.1"/>
<message name="DL_VALUE" period="1.5"/>
<!-- <message name="BARO_MS5534A" period="0.1"/> -->
<message name="QUAD_STATUS" period="1.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="COMMANDS" period="0.015"/>
<message name="PPM" period="0.015"/>
<message name="BOOZ2_AHRS_EULER" period="0.015"/>
<message name="RC" period="0.15"/>
<message name="ACTUATORS" period="0.15"/>
<message name="DOWNLINK" period="1.1"/>
</mode>
<mode name="debug">
<message name="ALIVE" period="5"/>
<!-- <message name="ATTITUDE" period="0.04"/> -->
<message name="COMMANDS" period="0.5"/>
<!-- <message name="ACTUATORS" period="0.5"/> -->
<message name="ACTUATORS" period="0.5"/>
<message name="QUAD_STATUS" period="1.1"/>
<message name="RC" period="0.5"/>
<message name="PPM" period="0.5"/>
sw/airborne/csc/csc_ap_main.c
+13 -2
View File
@@ -44,6 +44,7 @@
#include "csc_xsens.h"
#include "csc_autopilot.h"
#include "csc_can.h"
#include "pwm_input.h"
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
@@ -53,6 +54,7 @@
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define PPRZ_MODE_OF_RC(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? PPRZ_MODE_MANUAL : PPRZ_MODE_AUTO1)
extern uint8_t vsupply;
uint8_t pprz_mode = PPRZ_MODE_AUTO1;
static uint16_t cpu_time = 0;
@@ -71,11 +73,16 @@ static void nop(struct CscCanMsg *msg)
static void on_rc_cmd(struct CscRCMsg *msg)
{
int aux2_flag;
static int last_aux2_flag = -1;
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 - CSC_RC_OFFSET);
rc_values[RADIO_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
uint8_t mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
rc_values[RADIO_MODE] = mode ? -7000 : ( (mode == 1) ? 0 : 7000);
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_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
time_since_last_ppm = 0;
@@ -101,6 +108,10 @@ static void csc_main_init( void ) {
csc_adc_init();
ppm_init();
#ifdef USE_PWM_INPUT
pwm_input_init();
#endif
csc_ap_link_init();
csc_ap_link_set_servo_cmd_cb(&nop);
csc_ap_link_set_motor_cmd_cb(&nop);
@@ -130,7 +141,7 @@ static void csc_main_periodic( void )
}
xsens_periodic_task();
if (pprz_mode == PPRZ_MODE_AUTO1)
csc_ap_periodic();
csc_ap_periodic(cpu_time);
if (csc_trims_set) {
csc_trims_set = 0;
sw/airborne/csc/csc_autopilot.c
+47 -12
View File
@@ -32,26 +32,44 @@
#include "pprz_algebra_float.h"
#include "string.h"
#include "radio_control.h"
#include "pwm_input.h"
struct control_gains csc_gains;
struct control_reference csc_reference;
struct control_reference csc_errors;
struct control_trims csc_trims;
float csc_vane_angle;
float csc_vane_angle_offset = 180;
static const int xsens_id = 0;
float csc_yaw_weight;
float csc_yaw_rudder;
float csc_yaw_aileron;
float csc_yaw_deadband;
float csc_yaw_setpoint_rate;
float csc_yaw_setpoint_range;
#define PWM_INPUT_COUNTS_PER_REV 61358.
static void update_vane_angle( void )
{
csc_vane_angle = RadOfDeg( 360. * pwm_input_duration / PWM_INPUT_COUNTS_PER_REV) - RadOfDeg(csc_vane_angle_offset);
}
void csc_ap_init( void )
{
csc_gains.roll_kp = 2700;
csc_gains.roll_kd = 2500;
csc_gains.roll_ki = 0;
csc_gains.roll_kp = 4218;
csc_gains.roll_kd = 1000;
csc_gains.roll_ki = 50;
csc_gains.pitch_kp = 0;
csc_gains.pitch_kd = 0;
csc_gains.pitch_ki = 0;
csc_gains.yaw_kp = 8700;
csc_gains.yaw_kd = 900;
csc_gains.yaw_ki = 30;
csc_gains.yaw_kp = 5000;
csc_gains.yaw_kd = 800;
csc_gains.yaw_ki = 10;
csc_yaw_weight = 0;
csc_yaw_rudder = 0.33;
csc_yaw_aileron = 0;
csc_yaw_deadband = 1.00;
csc_trims.elevator = 1800;
csc_trims.aileron = -60;
@@ -121,15 +139,26 @@ static void calculate_errors(struct control_reference *errors)
errors->eulers_i.phi += xsens_eulers.phi;
errors->eulers_i.theta += xsens_eulers.theta;
errors->eulers_i.psi += xsens_eulers.psi;
float yaw_deadband = RadOfDeg(csc_yaw_deadband);
if (errors->eulers.psi <= yaw_deadband && errors->eulers.psi >= -yaw_deadband) {
errors->eulers.psi = 0;
} else if (errors->eulers.psi <= yaw_deadband*2 && errors->eulers.psi >= -yaw_deadband*2) {
if (errors->eulers.psi < 0) {
errors->eulers.psi = (errors->eulers.psi + yaw_deadband)*2;
} else
errors->eulers.psi = (errors->eulers.psi - yaw_deadband)*2;
}
bound_ierror(csc_gains.roll_ki, &errors->eulers_i.phi);
bound_ierror(csc_gains.pitch_ki, &errors->eulers_i.theta);
bound_ierror(csc_gains.yaw_ki, &errors->eulers_i.psi);
}
static void calculate_reference(struct control_reference *reference)
static void calculate_reference(struct control_reference *reference, int time)
{
reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW];
reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW] + 100*csc_yaw_setpoint_range*sin(0.01*time*csc_yaw_setpoint_rate);
reference->eulers.theta = M_PI / 6.0 * rc_values[RADIO_PITCH];
// Mix reference command with yaw reference command to prevent
// fighting ourselves
@@ -140,11 +169,12 @@ static void calculate_reference(struct control_reference *reference)
reference->eulers.psi /= MAX_PPRZ;
}
void csc_ap_periodic( void )
void csc_ap_periodic(int time)
{
static int counter = 0;
calculate_reference(&csc_reference);
calculate_reference(&csc_reference, time);
calculate_errors(&csc_errors);
update_vane_angle();
commands[COMMAND_ROLL] = -csc_gains.roll_kp * (csc_errors.eulers.phi + csc_errors.eulers.psi * csc_yaw_weight)
+ csc_gains.roll_kd * (csc_errors.rates.p + csc_errors.rates.r * csc_yaw_weight)
@@ -156,10 +186,15 @@ void csc_ap_periodic( void )
- csc_gains.pitch_ki * csc_errors.eulers_i.theta;
commands[COMMAND_PITCH] += csc_trims.elevator;
commands[COMMAND_ROLL] += -csc_gains.yaw_kp * csc_errors.eulers.psi
commands[COMMAND_ROLL] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
- csc_gains.yaw_kd * csc_errors.rates.r
- csc_gains.yaw_ki * csc_errors.eulers_i.psi;
- csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_aileron;
commands[COMMAND_YAW] = csc_trims.rudder;
commands[COMMAND_YAW] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
- csc_gains.yaw_kd * csc_errors.rates.r
- csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_rudder;
}
void csc_ap_clear_ierrors( void )
sw/airborne/csc/csc_autopilot.h
+9 -1
View File
@@ -56,16 +56,24 @@ struct control_trims {
extern struct control_gains csc_gains;
extern struct control_reference csc_reference;
extern struct control_trims csc_trims;
extern float csc_vane_angle;
extern float csc_vane_angle_offset;
extern float csc_yaw_weight;
extern float csc_yaw_rudder;
extern float csc_yaw_aileron;
extern float csc_yaw_deadband;
extern float csc_yaw_setpoint_rate;
extern float csc_yaw_setpoint_range;
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 (void );
void csc_ap_periodic (int time);
void csc_ap_set_trims (void );
void csc_ap_clear_ierrors (void );
#define PERIODIC_SEND_VANE_SENSOR() DOWNLINK_SEND_VANE_SENSOR(&csc_vane_angle)
#endif
sw/airborne/csc/csc_can.c
-1
View File
@@ -153,7 +153,6 @@ void csc_can_event(void)
{
#ifdef USE_CAN1
if (can1_msg_received) {
LED_ON(CAN_LED);
can1_callback(&can1_rx_msg);
can1_msg_received = FALSE;
}
sw/airborne/csc/csc_telemetry.h
+24 -14
View File
@@ -34,29 +34,39 @@
#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM)
#define PERIODIC_SEND_DOWNLINK() { \
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); \
}
#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]))
#endif /* PROPS_NB */
#ifdef COMMANDS_NB
#include "commands.h"
#define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
#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 */
#endif /* COMMANDS_NB */
#define PERIODIC_SEND_SIMPLE_COMMANDS() DOWNLINK_SEND_SIMPLE_COMMANDS(&commands[0], &commands[1], &commands[2])
#ifdef SERVOS_NB
#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 */
extern uint8_t telemetry_mode_Ap;
#endif /* CSC_TELEMETRY_H */
sw/airborne/csc/mercury_ap.c
+37 -52
View File
@@ -52,11 +52,13 @@ uint8_t booz2_autopilot_tol;
uint32_t booz2_autopilot_motors_on_counter;
uint32_t booz2_autopilot_in_flight_counter;
uint16_t mercury_yaw_servo_gain;
#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40
#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME (512/4)
#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ * -14 / 20)
#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 14 / 20)
void csc_ap_init(void) {
@@ -101,31 +103,18 @@ void csc_ap_init(void) {
}
#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() { \
if (booz2_autopilot_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 ( booz2_autopilot_motors_on_counter > 0) { \
booz2_autopilot_motors_on_counter--; \
if (booz2_autopilot_motors_on_counter == 0) \
booz2_autopilot_motors_on = FALSE; \
} \
} \
else { /* sticks not in the corner */ \
booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
} \
} \
else { /* motors off */ \
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 ( 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) \
booz2_autopilot_motors_on = TRUE; \
} \
} \
else { \
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){ \
booz2_autopilot_motors_on = TRUE; \
booz2_autopilot_in_flight_counter += BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
} \
} \
} else { \
booz2_autopilot_motors_on_counter = 0; \
} \
} \
@@ -147,39 +136,35 @@ const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
pprz_t mixed_commands[PROPS_NB];
void csc_ap_periodic(int8_t _in_flight, int8_t _motors_on) {
void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
// RunOnceEvery(50, nav_periodic_task_10Hz())
booz2_autopilot_motors_on = _motors_on;
BOOZ2_AUTOPILOT_CHECK_MOTORS_ON();
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);
booz2_guidance_v_run(booz2_autopilot_in_flight);
/* if ( !booz2_autopilot_motors_on ){ */
/* if( booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || */
/* booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) { */
/* CscSetCommands(csc_ap_commands_failsafe, */
/* booz2_autopilot_in_flight, booz2_autopilot_motors_on); */
booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 105 / 7200 + 95;
CscSetCommands(booz2_stabilization_cmd,
booz2_autopilot_in_flight,booz2_autopilot_motors_on);
/* } else { */
booz2_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ + 118;
CscSetCommands(booz2_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]);
props_commit();
Actuator(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*rc_values[RADIO_YAW])/2/7200;
Actuator(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*rc_values[RADIO_YAW])/2/7200;
ActuatorsCommit();
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]);
props_commit();
MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
booz2_stabilization_cmd,
mixed_commands,
(!booz2_autopilot_in_flight));
ActuatorsCommit();
}
sw/airborne/csc/mercury_ap.h
+4 -1
View File
@@ -25,9 +25,12 @@
#ifndef __MERCURY_AP_H__
#define __MERCURY_AP_H__
#include "airframe.h"
extern pprz_t mixed_commands[PROPS_NB];
void csc_ap_init( void );
void csc_ap_periodic (int8_t flight, int8_t motors );
void csc_ap_periodic (uint8_t _in_flight, uint8_t kill);
#endif
sw/airborne/csc/mercury_csc_main.c
-6
View File
@@ -44,7 +44,6 @@
#include "downlink.h"
#include "csc_throttle.h"
#include "csc_adc.h"
#include "csc_rc_spektrum.h"
@@ -97,7 +96,6 @@ static void csc_main_init( void ) {
#endif
motors_init();
csc_throttle_init();
int_enable();
}
@@ -128,7 +126,6 @@ static void csc_main_periodic( void ) {
static void csc_main_event( void ) {
csc_can_event();
csc_throttle_event_task();
#ifdef SPEKTRUM_LINK
spektrum_event_task();
#endif
@@ -168,10 +165,7 @@ static void on_servo_cmd(struct CscServoCmd *cmd)
static void on_motor_cmd(struct CscMotorMsg *msg)
{
// always send to throttle_id zero, only one motorcontrol per csc board
const static uint8_t throttle_id = 0;
csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2);
}
int main( void ) {
sw/airborne/csc/mercury_main.c
+1 -1
View File
@@ -159,7 +159,7 @@ static inline void csc_main_periodic( void )
baro_scp_periodic();
csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED);
!(pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED));
}
sw/airborne/csc/mercury_supervision.h
+19 -1
View File
@@ -3,6 +3,11 @@
#include "airframe.h"
extern pprz_t mercury_supervision_yaw_servo_gain;
extern pprz_t mercury_supervision_yaw_motor_gain;
extern pprz_t mercury_supervision_yaw_comp_slope;
extern pprz_t mercury_supervision_yaw_comp_offset;
/* #if defined SUPERVISION_FRONT_ROTOR_CW */
/* #define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) */
/* #define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) */
@@ -65,10 +70,23 @@
Bound(_mot_cmd[PROP_UPPER_RIGHT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
}
#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; \
} 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); \
_actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 - \
((SERVO_S1_MAX-SERVO_S1_MIN)*bndcmd)/2/400; \
_actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 - \
((SERVO_S2_MAX-SERVO_S2_MIN)*bndcmd)/2/400; \
} \
}
#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) { \
if (_motors_on) { \
SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], _in[COMMAND_YAW], _in[COMMAND_THRUST]); \
SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], (mercury_supervision_yaw_motor_gain*_in[COMMAND_YAW])/20, _in[COMMAND_THRUST]); \
pprz_t min_mot; \
SUPERVISION_FIND_MIN_MOTOR(_out, min_mot); \
if (min_mot < SUPERVISION_MIN_MOTOR) { \