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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-01-27 18:30:41 +00:00
parent e97f91250b
commit cabb5a1b02
13 changed files with 446 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tl.xml
+79
View File
@@ -0,0 +1,79 @@
<airframe name="TwistedLogic">
<servos min="0" neutral="0" max="0xff">
<servo name="AILERON" no="2" min="900" neutral="1500" max="2100"/>
<servo name="ELEVATOR" no="1" min="900" neutral="1500" max="2100"/>
<servo name="MOTOR_RIGHT" no="6" min="1250" neutral="1250" max="1810"/>
<servo name="MOTOR_LEFT" no="3" min="1250" neutral="1250" max="1810"/>
<servo name="CAM" no="0" min="900" neutral="1500" max="2100"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="CAM" failsafe_value="0"/>
</commands>
<command_laws>
<let var="roll" value="1.0 * @ROLL"/>
<let var="pitch" value="1.0 * @PITCH"/>
<let var="yaw" value="0.1 * @YAW"/>
<let var="throttle" value="1.0 * @THROTTLE"/>
<let var="right" value="$throttle + $yaw"/><!-- add trim term for Right -->
<let var="left" value="$throttle - $yaw"/><!-- .06*thrsubtract trim term for Right -->
<set servo="AILERON" value="$roll"/>
<set servo="ELEVATOR" value="$pitch"/>
<set servo="MOTOR_LEFT" value="$left - Max(0, $right - MAX_PPRZ)"/>
<set servo="MOTOR_RIGHT" value="$right - Max(0, $left - MAX_PPRZ)"/>
<set servo="CAM" value="@CAM"/>
</command_laws>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
<set command="CAM" value="@GAIN1"/>
</rc_commands>
<makefile>
ARCHI=arm7
FLASH_MODE = IAP
ap.ARCHDIR = $(ARCHI)
ap.ARCH = arm7tdmi
ap.TARGET = flt
ap.TARGETDIR = flt
ap.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(15e-3)'
ap.srcs = tl_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
ap.CFLAGS += -DLED -DLED -DTIME_LED=1
ap.srcs += commands.c
ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
# transparent
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c tl_telemetry.c
ap.srcs += tl_autopilot.c tl_control.c
# ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
# ap.srcs += gps_ubx.c gps.c latlong.c
</makefile>
</airframe>
conf/settings/tl.xml
+7
View File
@@ -0,0 +1,7 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings>
<dl_settings>
</dl_settings>
</settings>
conf/telemetry/tl.xml
+15
View File
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Ap">
<mode name="default">
<message name="PPM" period="1"/>
<message name="RC" period="1"/>
<message name="COMMANDS" period="1"/>
<message name="ACTUATORS" period="1"/> <!-- For trimming -->
</mode>
</process>
</telemetry>
sw/airborne/booz_control.c
+1 -1
View File
@@ -25,7 +25,7 @@ pprz_t booz_control_commands[COMMANDS_NB];
#define BOOZ_CONTROL_RATE_R_DGAIN 5.
/* setpoints for max stick throw in degres per second */
#define BOOZ_CONTROL_RATE_PQ_MAX_SP 200.
#define BOOZ_CONTROL_RATE_PQ_MAX_SP 120.
#define BOOZ_CONTROL_RATE_R_MAX_SP 100.
sw/airborne/tl_autopilot.c
+56
View File
@@ -0,0 +1,56 @@
#include "tl_autopilot.h"
#include "radio_control.h"
#include "commands.h"
#include "tl_control.h"
// #include "tl_nav.h"
uint8_t tl_autopilot_mode;
void tl_autopilot_init(void) {
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
}
void tl_autopilot_periodic_task(void) {
switch (tl_autopilot_mode) {
case TL_AP_MODE_FAILSAFE:
case TL_AP_MODE_KILL:
SetCommands(commands_failsafe);
break;
case TL_AP_MODE_RATE:
tl_control_rate_run();
SetCommands(tl_control_commands);
break;
/* case TL_AP_MODE_ATTITUDE: */
/* tl_control_attitude_run(); */
/* SetCommands(tl_control_commands); */
/* break; */
/* case TL_AP_MODE_NAV: */
/* tl_nav_run(); */
/* SetCommands(tl_control_commands); */
/* break; */
}
}
void tl_autopilot_on_rc_event(void) {
/* I think this should be hidden in rc code */
/* the ap gets a mode everytime - the rc filters it */
if (rc_values_contains_avg_channels) {
tl_autopilot_mode = TL_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
rc_values_contains_avg_channels = FALSE;
}
switch (tl_autopilot_mode) {
case TL_AP_MODE_RATE:
tl_control_rate_read_setpoints_from_rc();
break;
/* case TL_AP_MODE_ATTITUDE: */
/* tl_control_attitude_read_setpoints_from_rc(); */
/* break; */
/* case TL_AP_MODE_NAV: */
/* tl_nav_read_setpoints_from_rc(); */
/* break; */
}
}
sw/airborne/tl_autopilot.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef TL_AUTOPILOT_H
#define TL_AUTOPILOT_H
#include "std.h"
#define TL_AP_MODE_FAILSAFE 0
#define TL_AP_MODE_KILL 1
#define TL_AP_MODE_RATE 2
#define TL_AP_MODE_ATTITUDE 3
#define TL_AP_MODE_NAV 4
extern uint8_t tl_autopilot_mode;
extern void tl_autopilot_init(void);
extern void tl_autopilot_periodic_task(void);
extern void tl_autopilot_on_rc_event(void);
#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2)
#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2)
#define TL_AP_MODE_OF_PPRZ(mode) \
((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_RATE : \
(mode) < TRESHOLD_ATTITUDE_PPRZ ? TL_AP_MODE_ATTITUDE : \
TL_AP_MODE_NAV )
#endif /* TL_AUTOPILOT_H */
sw/airborne/tl_control.c
+103
View File
@@ -0,0 +1,103 @@
#include "tl_control.h"
#include "tl_estimator.h"
#include "radio_control.h"
// output
float tl_control_p_sp;
float tl_control_q_sp;
float tl_control_r_sp;
float tl_control_power_sp;
float tl_control_rate_r_pgain;
float tl_control_rate_r_dgain;
float tl_control_rate_last_err_r;
pprz_t tl_control_commands[COMMANDS_NB];
#define TL_CONTROL_RATE_R_PGAIN -600.
#define TL_CONTROL_RATE_R_DGAIN 5.
/* setpoints for max stick throw in degres per second */
#define TL_CONTROL_RATE_R_MAX_SP 100.
/* float tl_control_attitude_phi_sp; */
/* float tl_control_attitude_theta_sp; */
/* float tl_control_attitude_psi_sp; */
/* float tl_control_attitude_phi_theta_pgain; */
/* float tl_control_attitude_phi_theta_dgain; */
/* float tl_control_attitude_psi_pgain; */
/* float tl_control_attitude_psi_dgain; */
/* #define TL_CONTROL_ATTITUDE_PHI_THETA_PGAIN -1250. */
/* #define TL_CONTROL_ATTITUDE_PHI_THETA_DGAIN -700. */
/* #define TL_CONTROL_ATTITUDE_PSI_PGAIN -1050. */
/* #define TL_CONTROL_ATTITUDE_PSI_DGAIN -850. */
/* setpoints for max stick throw in degres */
/* #define TL_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30. */
/* #define TL_CONTROL_ATTITUDE_PSI_MAX_SP 45. */
void tl_control_init(void) {
tl_control_p_sp = 0.;
tl_control_q_sp = 0.;
tl_control_r_sp = 0.;
tl_control_power_sp = 0.;
tl_control_rate_last_err_r = 0.;
tl_control_rate_r_pgain = TL_CONTROL_RATE_R_PGAIN;
tl_control_rate_r_dgain = TL_CONTROL_RATE_R_DGAIN;
/* tl_control_attitude_phi_sp = 0.; */
/* tl_control_attitude_theta_sp =0.; */
/* tl_control_attitude_psi_sp =0.; */
/* tl_control_attitude_phi_theta_pgain = TL_CONTROL_ATTITUDE_PHI_THETA_PGAIN; */
/* tl_control_attitude_phi_theta_dgain = TL_CONTROL_ATTITUDE_PHI_THETA_DGAIN; */
/* tl_control_attitude_psi_pgain = TL_CONTROL_ATTITUDE_PSI_PGAIN; */
/* tl_control_attitude_psi_dgain = TL_CONTROL_ATTITUDE_PSI_DGAIN; */
}
void tl_control_rate_read_setpoints_from_rc(void) {
tl_control_p_sp = -rc_values[RADIO_ROLL];
tl_control_q_sp = rc_values[RADIO_PITCH];
// tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
tl_control_r_sp = -rc_values[RADIO_YAW];
tl_control_power_sp = rc_values[RADIO_THROTTLE];
}
void tl_control_rate_run(void) {
const float cmd_p = tl_control_p_sp;
const float cmd_q = tl_control_q_sp;
// const float rate_err_r = tl_estimator_uf_r - tl_control_r_sp;
// const float rate_d_err_r = rate_err_r - tl_control_rate_last_err_r;
// tl_control_rate_last_err_r = rate_err_r;
// const float cmd_r = tl_control_rate_r_pgain * ( rate_err_r + tl_control_rate_r_dgain * rate_d_err_r );
const float cmd_r = tl_control_r_sp;
tl_control_commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)cmd_p);
tl_control_commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)cmd_q);
tl_control_commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)cmd_r);
tl_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
}
void tl_control_attitude_read_setpoints_from_rc(void) {
}
void tl_control_attitude_run(void) {
}
sw/airborne/tl_control.h
+45
View File
@@ -0,0 +1,45 @@
#ifndef TL_CONTROL_H
#define TL_CONTROL_H
#include "std.h"
#include "paparazzi.h"
extern void tl_control_init(void);
extern void tl_control_rate_read_setpoints_from_rc(void);
extern void tl_control_rate_run(void);
extern void tl_control_attitude_read_setpoints_from_rc(void);
extern void tl_control_attitude_run(void);
extern float tl_control_p_sp;
extern float tl_control_q_sp;
extern float tl_control_r_sp;
extern float tl_control_power_sp;
extern float tl_control_rate_pq_pgain;
extern float tl_control_rate_pq_dgain;
extern float tl_control_rate_r_pgain;
extern float tl_control_rate_r_dgain;
extern float tl_control_attitude_phi_sp;
extern float tl_control_attitude_theta_sp;
extern float tl_control_attitude_psi_sp;
extern float tl_control_attitude_phi_theta_pgain;
extern float tl_control_attitude_phi_theta_dgain;
extern float tl_control_attitude_psi_pgain;
extern float tl_control_attitude_psi_dgain;
#define TlControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \
tl_control_attitude_phi_sp = _phi_sp; \
tl_control_attitude_theta_sp = _theta_sp; \
tl_control_attitude_psi_sp = _psi_sp; \
tl_control_power_sp = _power_sp; \
}
#include "airframe.h"
extern pprz_t tl_control_commands[COMMANDS_NB];
#endif /* TL_CONTROL_H */
sw/airborne/tl_estimator.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef TL_ESTIMATOR_H
#define TL_ESTIMATOR_H
void tl_estimator_init(void);
#endif /* TL_ESTIMATOR_H */
sw/airborne/tl_main.c
+76
View File
@@ -0,0 +1,76 @@
#include "std.h"
#include "init_hw.h"
#include "interrupt_hw.h"
#include "sys_time.h"
#include "led.h"
#include "commands.h"
#include "actuators.h"
#include "radio_control.h"
#include "uart.h"
#include "messages.h"
#include "downlink.h"
#include "tl_telemetry.h"
#include "datalink.h"
#include "tl_autopilot.h"
static inline void tl_main_init( void );
static inline void tl_main_periodic_task( void );
static inline void tl_main_event_task( void );
int main( void ) {
tl_main_init();
while(1) {
if (sys_time_periodic())
tl_main_periodic_task();
tl_main_event_task();
}
return 0;
}
static inline void tl_main_init( void ) {
hw_init();
led_init();
sys_time_init();
actuators_init();
SetCommands(commands_failsafe);
ppm_init();
radio_control_init();
uart0_init_tx();
// uart1_init_tx();
int_enable();
DOWNLINK_SEND_BOOT(&cpu_time_sec);
}
static inline void tl_main_periodic_task( void ) {
/* run control loops */
tl_autopilot_periodic_task();
SetActuatorsFromCommands(commands);
radio_control_periodic_task();
if (rc_status != RC_OK)
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
tl_telemetry_periodic_task();
}
static inline void tl_main_event_task( void ) {
//DlEventCheckAndHandle();
RadioControlEventCheckAndHandle(tl_autopilot_on_rc_event);
}
sw/airborne/tl_telemetry.c
+3
View File
@@ -0,0 +1,3 @@
#include "tl_telemetry.h"
uint8_t telemetry_mode_Ap;
sw/airborne/tl_telemetry.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef TL_TELEMETRY_H
#include "std.h"
#include "messages.h"
#include "periodic.h"
#include "uart.h"
#include "downlink.h"
#include "radio_control.h"
#include "commands.h"
#include "actuators.h"
#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
#define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(PPM_NB_PULSES, ppm_pulses)
#define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
extern uint8_t telemetry_mode_Ap;
static inline void tl_telemetry_periodic_task(void) {
PeriodicSendAp()
}
#define TL_TELEMETRY_H
#endif //TL_TELEMETRY_H
sw/airborne/uart.h
+3
View File
@@ -69,4 +69,7 @@ bool_t uart1_check_free_space( uint8_t len);
#define Uart1TxRunning uart1_tx_running
#define Uart1InitParam uart1_init_param
#define Uart0TxRunning uart0_tx_running
#define Uart0InitParam uart0_init_param
#endif /* UART_H */