diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml new file mode 100644 index 0000000000..2e66e42e27 --- /dev/null +++ b/conf/airframes/tl.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +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 + + + + + + diff --git a/conf/settings/tl.xml b/conf/settings/tl.xml new file mode 100644 index 0000000000..aacfa026b2 --- /dev/null +++ b/conf/settings/tl.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/conf/telemetry/tl.xml b/conf/telemetry/tl.xml new file mode 100644 index 0000000000..ef47927065 --- /dev/null +++ b/conf/telemetry/tl.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c index 1745483af1..ff56e55f85 100644 --- a/sw/airborne/booz_control.c +++ b/sw/airborne/booz_control.c @@ -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. diff --git a/sw/airborne/tl_autopilot.c b/sw/airborne/tl_autopilot.c new file mode 100644 index 0000000000..5d397dc6e8 --- /dev/null +++ b/sw/airborne/tl_autopilot.c @@ -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; */ + } +} diff --git a/sw/airborne/tl_autopilot.h b/sw/airborne/tl_autopilot.h new file mode 100644 index 0000000000..d9af3ab66f --- /dev/null +++ b/sw/airborne/tl_autopilot.h @@ -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 */ diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c new file mode 100644 index 0000000000..cee7db7045 --- /dev/null +++ b/sw/airborne/tl_control.c @@ -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) { + +} diff --git a/sw/airborne/tl_control.h b/sw/airborne/tl_control.h new file mode 100644 index 0000000000..c1f2266a09 --- /dev/null +++ b/sw/airborne/tl_control.h @@ -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 */ diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h new file mode 100644 index 0000000000..46ca2ce40c --- /dev/null +++ b/sw/airborne/tl_estimator.h @@ -0,0 +1,7 @@ +#ifndef TL_ESTIMATOR_H +#define TL_ESTIMATOR_H + +void tl_estimator_init(void); + + +#endif /* TL_ESTIMATOR_H */ diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c new file mode 100644 index 0000000000..b83222dd82 --- /dev/null +++ b/sw/airborne/tl_main.c @@ -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); +} diff --git a/sw/airborne/tl_telemetry.c b/sw/airborne/tl_telemetry.c new file mode 100644 index 0000000000..6839b56079 --- /dev/null +++ b/sw/airborne/tl_telemetry.c @@ -0,0 +1,3 @@ +#include "tl_telemetry.h" + +uint8_t telemetry_mode_Ap; diff --git a/sw/airborne/tl_telemetry.h b/sw/airborne/tl_telemetry.h new file mode 100644 index 0000000000..0e0a4a9fc0 --- /dev/null +++ b/sw/airborne/tl_telemetry.h @@ -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 diff --git a/sw/airborne/uart.h b/sw/airborne/uart.h index 949fa378cf..52524c8bef 100644 --- a/sw/airborne/uart.h +++ b/sw/airborne/uart.h @@ -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 */