Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Fixed all kinds of fbw related problems

Browse Source
This commit is contained in:
kevindehecker
2016-03-04 15:25:38 +01:00
parent ca21b0a89f
commit 0ec787c365
7 changed files with 92 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/telemetry/default_rotorcraft_slow.xml
+1
View File
@@ -28,6 +28,7 @@
<message name="SURVEY" period="4.5"/>
<message name="OPTIC_FLOW_EST" period="0.5"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="FBW_STATUS" period="2"/>
</mode>
<mode name="ppm">
sw/airborne/firmwares/rotorcraft/main_fbw.c
+27 -13
View File
@@ -47,7 +47,8 @@
#include "firmwares/rotorcraft/main_fbw.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
//#include "generated/modules.h"
#define MODULES_C
#include "generated/modules.h"
/** Fly by wire modes */
typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
@@ -59,7 +60,7 @@ fbw_mode_enum fbw_mode;
//PRINT_CONFIG_VAR(MODULES_FREQUENCY)
tid_t main_periodic_tid; ///< id for main_periodic() timer
//tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
tid_t electrical_tid; ///< id for electrical_periodic() timer
tid_t telemetry_tid; ///< id for telemetry_periodic() timer
@@ -85,17 +86,17 @@ STATIC_INLINE void main_init(void)
mcu_init();
actuators_init();
electrical_init();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
// TODO
//modules_init();
modules_init();
mcu_int_enable();
@@ -103,7 +104,7 @@ STATIC_INLINE void main_init(void)
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
// modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / 20.), NULL);
@@ -115,13 +116,15 @@ STATIC_INLINE void main_init(void)
STATIC_INLINE void handle_periodic_tasks(void)
{
if (sys_time_check_and_ack_timer(main_periodic_tid)) {
main_periodic();
}
//if (sys_time_check_and_ack_timer(modules_tid)) {
// TODO
//modules_periodic_task();
//}
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
}
if (sys_time_check_and_ack_timer(radio_control_tid)) {
radio_control_periodic_task();
}
@@ -175,9 +178,17 @@ STATIC_INLINE void main_periodic(void)
}
}
static uint16_t dv = 0;
// TODO make module out of led blink?
/* set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
SetCommands(commands_failsafe);
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
LED_TOGGLE(FBW_MODE_LED); // toggle instead of on, because then it is still visible when fbw_mode switches very fast
}
/* set actuators */
@@ -208,6 +219,7 @@ static void autopilot_on_rc_frame(void)
/* if manual */
if (fbw_mode == FBW_MODE_MANUAL) {
autopilot_motors_on = TRUE;
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
@@ -216,13 +228,15 @@ static void autopilot_on_rc_frame(void)
}
/* Forward radiocontrol to AP */
intermcu_on_rc_frame();
intermcu_on_rc_frame(fbw_mode);
}
static void autopilot_on_ap_command(void)
{
if (fbw_mode != FBW_MODE_MANUAL) {
SetCommands(intermcu_commands);
} else {
autopilot_motors_on = TRUE;
}
}
@@ -237,6 +251,6 @@ STATIC_INLINE void main_event(void)
// InterMCU
InterMcuEvent(autopilot_on_ap_command);
// TODO Modules
//modules_event_task();
//Modules
modules_event_task();
}
sw/airborne/subsystems/intermcu.h
+2
View File
@@ -33,7 +33,9 @@
#define INTERMCU_AP 0
#define INTERMCU_FBW 1
#ifndef INTERMCU_LOST_CNT
#define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer TODO fixed value */
#endif
enum intermcu_status {
INTERMCU_OK,
sw/airborne/subsystems/intermcu/intermcu_ap.c
+14
View File
@@ -45,9 +45,21 @@ static struct pprz_transport intermcu_transport;
struct intermcu_t inter_mcu;
static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void));
static void send_status(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&(radio_control.status), &(radio_control.frame_rate), &(inter_mcu.status), &electrical.vsupply,
&electrical.current); // due to limitation of GCS, send the electrical from ap as if it comes from fbw...
}
void intermcu_init(void)
{
pprz_transport_init(&intermcu_transport);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status);
}
void intermcu_periodic(void)
@@ -81,6 +93,7 @@ void intermcu_send_spektrum_bind(void)
}
}
static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void))
{
/* Parse the Inter MCU message */
@@ -89,6 +102,7 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_fram
case DL_IMCU_RADIO_COMMANDS: {
uint8_t i;
uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(trans->payload);
inter_mcu.status = DL_IMCU_RADIO_COMMANDS_status(trans->payload);
int16_t *rc_values = DL_IMCU_RADIO_COMMANDS_values(trans->payload);
for (i = 0; i < size; i++) {
radio_control.values[i] = rc_values[i];
sw/airborne/subsystems/intermcu/intermcu_ap.h
+2 -8
View File
@@ -40,18 +40,12 @@ void disable_inter_comm(bool_t value);
#define RADIO_ROLL 1
#define RADIO_PITCH 2
#define RADIO_YAW 3
#define RADIO_GEAR 4
#define RADIO_FLAP 5
#define RADIO_MODE 4
#define RADIO_KILL_SWITCH 5
#define RADIO_AUX1 5
#define RADIO_AUX2 6
#define RADIO_AUX3 7
#define RADIO_CONTROL_NB_CHANNEL 8
#ifndef RADIO_MODE
#define RADIO_MODE RADIO_GEAR
#endif
//#ifndef RADIO_KILL_SWITCH
//#define RADIO_KILL_SWITCH RADIO_FLAP
//#endif
#endif /* INTERMCU_AP_ROTORCRAFT_H */
sw/airborne/subsystems/intermcu/intermcu_fbw.c
+30 -2
View File
@@ -76,10 +76,38 @@ void intermcu_periodic(void)
}
}
void intermcu_on_rc_frame(void)
void intermcu_on_rc_frame(uint8_t fbw_mode)
{
pprz_t values[8];
values[INTERMCU_RADIO_THROTTLE] = radio_control.values[RADIO_THROTTLE];
values[INTERMCU_RADIO_ROLL] = radio_control.values[RADIO_ROLL];
values[INTERMCU_RADIO_PITCH] = radio_control.values[RADIO_PITCH];
values[INTERMCU_RADIO_YAW] = radio_control.values[RADIO_YAW];
#ifdef RADIO_MODE
values[INTERMCU_RADIO_MODE] = radio_control.values[RADIO_MODE];
#endif
#ifdef RADIO_KILL_SWITCH
values[INTERMCU_RADIO_KILL_SWITCH] = radio_control.values[RADIO_KILL];
#endif
#if defined (RADIO_AUX1) && defined (RADIO_KILL_SWITCH)
#warning "RC AUX1 and KILL_SWITCH are on the same channel."
#endif
#ifdef RADIO_AUX1
values[INTERMCU_RADIO_AUX1] = radio_control.values[RADIO_AUX1];
#endif
#ifdef RADIO_AUX2
values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2];
#endif
#ifdef RADIO_AUX3
values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2];
#endif
pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device,
INTERMCU_FBW, 0, RADIO_CONTROL_NB_CHANNEL, radio_control.values); //TODO: Fix status
INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values);
}
void intermcu_send_status(uint8_t mode)
sw/airborne/subsystems/intermcu/intermcu_fbw.h
+15 -1
View File
@@ -29,9 +29,23 @@
#include "subsystems/intermcu.h"
extern uint8_t autopilot_motors_on;
extern pprz_t intermcu_commands[COMMANDS_NB];
void intermcu_on_rc_frame(void);
void intermcu_on_rc_frame(uint8_t fbw_mode);
void intermcu_send_status(uint8_t mode);
void InterMcuEvent(void (*frame_handler)(void));
/* We need radio defines for the Autopilot */
#define INTERMCU_RADIO_THROTTLE 0
#define INTERMCU_RADIO_ROLL 1
#define INTERMCU_RADIO_PITCH 2
#define INTERMCU_RADIO_YAW 3
#define INTERMCU_RADIO_MODE 4
#define INTERMCU_RADIO_KILL_SWITCH 5
#define INTERMCU_RADIO_AUX1 5
#define INTERMCU_RADIO_AUX2 6
#define INTERMCU_RADIO_AUX3 7
#define INTERMCU_RADIO_CONTROL_NB_CHANNEL 8
#endif /* INTERMCU_FBW_ROTORCRAFT_H */