diff --git a/sw/airborne/actuators.c b/sw/airborne/actuators.c index 15a409efa7..9845a50a65 100644 --- a/sw/airborne/actuators.c +++ b/sw/airborne/actuators.c @@ -25,6 +25,7 @@ * */ -#include "actuators.h" +#include "firmwares/fixedwing/actuators.h" +//#include "actuators.h" uint16_t actuators[SERVOS_NB]; diff --git a/sw/airborne/setup_actuators.c b/sw/airborne/setup_actuators.c index c9c2299c87..1a5d6fdfd6 100644 --- a/sw/airborne/setup_actuators.c +++ b/sw/airborne/setup_actuators.c @@ -3,7 +3,8 @@ #include "interrupt_hw.h" #include "sys_time.h" #include "led.h" -#include "actuators.h" +#include "firmwares/fixedwing/actuators.h" +//#include "actuators.h" #include "airframe.h" #define DATALINK_C #include "datalink.h" @@ -11,6 +12,7 @@ #include "pprz_transport.h" #include "main_fbw.h" #include "downlink.h" +#include "settings.h" #define IdOfMsg(x) (x[1]) @@ -21,9 +23,26 @@ void dl_parse_msg( void ) { if (msg_id == DL_SET_ACTUATOR) { uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer); uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer); + LED_TOGGLE(2); if (servo_no < SERVOS_NB) SetServo(servo_no, servo_value); } +#ifdef DlSetting + else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_SETTING_index(dl_buffer); + float val = DL_SETTING_value(dl_buffer); + DlSetting(i, val); + LED_TOGGLE(2); + for (int j=0 ; j<8 ; j++) { + SetServo(j,actuators[j]); + } + DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val); + } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_GET_SETTING_index(dl_buffer); + float val = settings_get_value(i); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val); + } +#endif } #define PprzUartInit() Link(Init()) @@ -52,8 +71,8 @@ void periodic_task_fbw(void) { /* t += 1./60.; */ /* uint16_t servo_value = 1500+ 500*sin(t); */ /* SetServo(SERVO_THROTTLE, servo_value); */ - - // RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM)); + + RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM)); RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, SERVOS_NB, actuators )); } @@ -64,6 +83,7 @@ void event_task_fbw(void) { if (pprz_msg_received) { pprz_msg_received = FALSE; pprz_parse_payload(); + LED_TOGGLE(3); } if (dl_msg_available) { dl_parse_msg();