diff --git a/conf/airframes/gorrazoptere_esc_3DMG.xml b/conf/airframes/gorrazoptere_esc_3DMG.xml
index 105503b5f0..2a4215cb8e 100644
--- a/conf/airframes/gorrazoptere_esc_3DMG.xml
+++ b/conf/airframes/gorrazoptere_esc_3DMG.xml
@@ -61,9 +61,19 @@
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index b874e6cafd..b1b7e9f358 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -23,12 +23,12 @@
-
+
-
+
diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml
index ed9cff37b3..8ffff8bca2 100644
--- a/conf/airframes/twinstar2.xml
+++ b/conf/airframes/twinstar2.xml
@@ -22,6 +22,13 @@
+
+
+
+
+
+
+
diff --git a/conf/airframes/twinstar5.xml b/conf/airframes/twinstar5.xml
index 3cf7cebe1c..d3c6a075cd 100644
--- a/conf/airframes/twinstar5.xml
+++ b/conf/airframes/twinstar5.xml
@@ -15,13 +15,17 @@
-
-
-
-
-
+
+
+
+
+
+
+
+
+
@@ -29,8 +33,8 @@
-
-
+
+
diff --git a/sw/airborne/arm7/servos_4015_hw.c b/sw/airborne/arm7/servos_4015_hw.c
index 57c1c6f815..41334061fa 100644
--- a/sw/airborne/arm7/servos_4015_hw.c
+++ b/sw/airborne/arm7/servos_4015_hw.c
@@ -1,3 +1,4 @@
+#include "servos_4015_hw.h"
#include "command.h"
#include "armVIC.h"
diff --git a/sw/airborne/avr/link_mcu_ap.c b/sw/airborne/avr/link_mcu_ap.c
index a1cd14f7ad..14aea3c70e 100644
--- a/sw/airborne/avr/link_mcu_ap.c
+++ b/sw/airborne/avr/link_mcu_ap.c
@@ -89,7 +89,7 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
/* notify valid frame */
if (crc_in1 == Crc1(crc_in) && tmp == Crc2(crc_in)) {
from_fbw_receive_valid = TRUE;
- link_fbw_fbw_nb_err = from_fbw.nb_err;
+ link_fbw_fbw_nb_err = from_fbw.from_fbw.nb_err;
} else
link_fbw_nb_err++;
/* unselect slave0 */
@@ -110,7 +110,7 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
/* we are sending/receiving payload */
if (idx_buf < FRAME_LENGTH - 2) {
/* place new payload byte in send register */
- tmp = ((uint8_t*)&from_ap)[idx_buf];
+ tmp = ((uint8_t*)&from_ap.from_ap)[idx_buf];
SPI_SEND(tmp);
crc_out = CrcUpdate(crc_out, tmp);
}
@@ -123,6 +123,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
/* read the byte from receive register */
tmp = SPDR;
- ((uint8_t*)&from_fbw)[idx_buf-1] = tmp;
+ ((uint8_t*)&from_fbw.from_fbw)[idx_buf-1] = tmp;
crc_in = CrcUpdate(crc_in, tmp);
}
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index 482bd43d31..65752716b9 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -137,7 +137,10 @@ static inline void radio_control_task(void) {
pitch_dot_pgain = roll_dot_pgain;
pitch_dot_dgain = roll_dot_dgain;
#endif
- commands_of_radio(rc_values);
+
+ pprz_t commands[COMMANDS_NB];
+ CommandsOfRC(commands);
+ command_set(commands);
}
}
@@ -163,7 +166,6 @@ void init_fbw( void ) {
adc_init();
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf);
#if defined IMU_3DMG || defined IMU_ANALOG
- CounterLedInit();
imu_init();
#endif
sys_time_init();
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index 4618ec6513..2243288449 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -82,8 +82,8 @@ let parse_servo = fun c ->
let pprz_value = Str.regexp "@\\([A-Z_0-9]+\\)"
let var_value = Str.regexp "\\$\\([_a-z0-9]+\\)"
-let preprocess_command = fun s ->
- let s = Str.global_replace pprz_value "values[COMMAND_\\1]" s in
+let preprocess_value = fun s v ->
+ let s = Str.global_replace pprz_value (sprintf "%s[COMMAND_\\1]" v) s in
Str.global_replace var_value "_var_\\1" s
let parse_command_laws = fun command ->
@@ -92,7 +92,7 @@ let parse_command_laws = fun command ->
"set" ->
let servo = a "servo"
and value = a "value" in
- let v = preprocess_command value in
+ let v = preprocess_value value "values" in
printf " command_value = %s;\\\n" v;
printf " command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN;\\\n" servo servo;
printf " servo_value = SERVO_%s_NEUTRAL + (int16_t)(command_value);\\\n" servo;
@@ -100,25 +100,25 @@ let parse_command_laws = fun command ->
| "let" ->
let var = a "var"
and value = a "value" in
- let v = preprocess_command value in
+ let v = preprocess_value value "values" in
printf " int16_t _var_%s = %s;\\\n" var v
| "define" ->
parse_element "" command
| _ -> xml_error "set|let"
-let parse_radio_laws = fun rc ->
+let parse_rc_commands = fun rc ->
let a = fun s -> ExtXml.attrib rc s in
match Xml.tag rc with
"set" ->
let com = a "command"
and value = a "value" in
- let v = preprocess_command value in
- printf " values[COMMAND_%s] = %s;\\\n" com v;
+ let v = preprocess_value value "rc_values" in
+ printf " commands[COMMAND_%s] = %s;\\\n" com v;
| "let" ->
let var = a "var"
and value = a "value" in
- let v = preprocess_command value in
+ let v = preprocess_value value "rc_values" in
printf " int16_t _var_%s = %s;\\\n" var v
| "define" ->
parse_element "" rc
@@ -154,9 +154,9 @@ let parse_section = fun s ->
let commands_params = Array.to_list commands_params in
define "COMMANDS_FAILSAFE" (sprint_float_array (List.map (fun x -> string_of_int x.failsafe_value) commands_params));
nl (); nl ()
- | "radio_laws" ->
- printf "#define CommandsOfRC(values) { \\\n";
- List.iter parse_radio_laws (Xml.children s);
+ | "rc_commands" ->
+ printf "#define CommandsOfRC(commands) { \\\n";
+ List.iter parse_rc_commands (Xml.children s);
printf "}\n\n"
| "command_laws" ->
printf "#define CommandsSet(values) { \\\n";