diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml
index 7ebebbb5d9..592b548249 100644
--- a/conf/airframes/Poine/beth.xml
+++ b/conf/airframes/Poine/beth.xml
@@ -104,14 +104,14 @@ main_stm32.ARCHDIR = stm32
#main_stm32.LDSCRIPT = $(SRC_ARCH)/stm32f103re_flash.ld
main_stm32.TARGET = main_stm32
main_stm32.TARGETDIR = main_stm32
-main_stm32.CFLAGS += -I$(SRC_LISA) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+main_stm32.CFLAGS += -I$(SRC_LISA) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_LISA_ARCH) -DPERIPHERALS_AUTO_INIT
main_stm32.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
main_stm32.srcs = $(SRC_BETH)/main_stm32.c \
$(SRC_ARCH)/stm32_exceptions.c \
$(SRC_ARCH)/stm32_vector_table.c
main_stm32.CFLAGS += -DUSE_LED
main_stm32.srcs += $(SRC_ARCH)/led_hw.c
-main_stm32.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=2
+main_stm32.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
main_stm32.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
main_stm32.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
@@ -123,7 +123,7 @@ main_stm32.srcs += downlink.c pprz_transport.c
main_stm32.CFLAGS += -DUSE_OVERO_LINK
main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-main_stm32.CFLAGS += -DOVERO_LINK_LED_OK=5 -DOVERO_LINK_LED_KO=4 -DUSE_DMA1_C2_IRQ
+main_stm32.CFLAGS += -DOVERO_LINK_LED_OK=3 -DOVERO_LINK_LED_KO=4 -DUSE_DMA1_C2_IRQ
main_stm32.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c
#booz IMU
diff --git a/conf/messages.xml b/conf/messages.xml
index c0276a274e..ebca7d3185 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -595,19 +595,23 @@
-
-
-
-
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
@@ -617,15 +621,12 @@
-
-
-
-
+
+
+
+
-
-
-
diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml
index 05fd52d556..a92e5b80d4 100644
--- a/conf/settings/settings_beth.xml
+++ b/conf/settings/settings_beth.xml
@@ -4,9 +4,9 @@
-
+
-
+
diff --git a/sw/airborne/beth/bench_sensors.h b/sw/airborne/beth/bench_sensors.h
index 2b78a47663..de655ad046 100644
--- a/sw/airborne/beth/bench_sensors.h
+++ b/sw/airborne/beth/bench_sensors.h
@@ -11,8 +11,7 @@
#include "can.h"
#include "can_hw.h"
#include
-
-extern uint16_t halfw1,halfw2,halfw3,halfw4;
+extern uint16_t can_err_flags;
#endif
extern void bench_sensors_init(void);
diff --git a/sw/airborne/beth/bench_sensors_can.c b/sw/airborne/beth/bench_sensors_can.c
index 3717bb88a4..052570a1c5 100644
--- a/sw/airborne/beth/bench_sensors_can.c
+++ b/sw/airborne/beth/bench_sensors_can.c
@@ -2,38 +2,53 @@
#include "can.h"
#include "led.h"
-uint16_t halfw1,halfw2,halfw3,halfw4;
+//uint16_t halfw1,halfw2,halfw3,halfw4;
+uint16_t can_err_flags = 0;
struct BenchSensors bench_sensors;
static void can_rx_callback(uint32_t id, uint8_t *buf, int len);
+static uint8_t rx_bd1 = 0;
+static uint8_t rx_bd2 = 0;
void bench_sensors_init(void) {
can_init(can_rx_callback);
}
+//for now Azimuth board(BD1) is slow so we give it more time...
+#define MAX_ALLOWED_SKIPS_CANBD1 (60)
+#define MAX_ALLOWED_SKIPS_CANBD2 (10)
+
void read_bench_sensors(void) {
-
+ //rx_bd1/2 keep track of how long it's been since last receive (when it is set to 0)
+ //if we've lost link for 0.5 second, stop counting (to avoid overflowing our uint8)
+ if (rx_bd1 < 255) rx_bd1++;
+ if (rx_bd2 < 255) rx_bd2++;
+
+ //if we haven't heard from a board for 15 periods (15/512=29ms)
+ //we flag a CAN error to show which board is at cause and how long it's been.
+ can_err_flags = 0;
+ if (rx_bd1 > MAX_ALLOWED_SKIPS_CANBD1) {can_err_flags = 1000 + rx_bd1;}
+ if (rx_bd2 > MAX_ALLOWED_SKIPS_CANBD2) {can_err_flags += (2000 + rx_bd2);}
+ //if ((rx_bd1 > 15) && (rx_bd2 > 15)) {can_err_flags = 3000 + rx_bd2 + rx_bd1;}
}
static void can_rx_callback(uint32_t id, uint8_t *buf, int len) {
//LED_TOGGLE(7);
+ static uint16_t tempangle;
+
bench_sensors.current = id>>7;
if (bench_sensors.current == 2) {
- halfw2 = buf[3];
- halfw2 = (halfw2<<8) + buf[2];
- halfw1 = buf[1];
- halfw1 = (halfw1<<8) + buf[0];
- bench_sensors.angle_2 = halfw1;
- bench_sensors.angle_3 = halfw2;
+ tempangle = (buf[1]<<8) | buf[0];
+ if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_2 = tempangle;}
+ tempangle = (buf[3]<<8) | buf[2];
+ if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_3 = tempangle;}
+ rx_bd2 = 0;
}
else {
- halfw4 = buf[3];
- halfw4 = (halfw4<<8) + buf[2];
- //halfw3 = buf[1];
- //halfw3 = (halfw3<<8) + buf[0];
- bench_sensors.angle_1 = halfw4;
+ tempangle = (buf[3]<<8) | buf[2];
+ if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x10;} else {bench_sensors.angle_1 = tempangle;}
+ rx_bd1 = 0;
}
-
}
diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c
index 21af86d7e1..c09976fbef 100644
--- a/sw/airborne/beth/main_overo.c
+++ b/sw/airborne/beth/main_overo.c
@@ -31,6 +31,7 @@
#include
#include "messages2.h"
+#include "airframe.h"
#include "fms_periodic.h"
#include "fms_debug.h"
@@ -50,14 +51,15 @@ static void main_exit(int sig);
static void main_talk_with_stm32(void);
-static struct AutopilotMessageBethUp msg_in;
-static struct AutopilotMessageBethDown msg_out;
+static struct AutopilotMessageCRCFrame msg_in;
+static struct AutopilotMessageCRCFrame msg_out;
struct BoozImu booz_imu;
struct BoozImuFloat booz_imu_float;
static uint32_t foo = 0;
+static uint8_t spi_crc_ok = 1;
int main(int argc, char *argv[]) {
@@ -77,17 +79,18 @@ int main(int argc, char *argv[]) {
event_init();
control_init();
+ estimator_init();
+ // file_logger_init("my_log.data");
+
+ gcs_com_init();
+
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return -1;
}
- // file_logger_init("my_log.data");
-
- gcs_com_init();
-
- main_parse_cmd_line(argc, argv);
+ //main_parse_cmd_line(argc, argv);
event_dispatch();
//should never occur!
@@ -96,10 +99,10 @@ int main(int argc, char *argv[]) {
return 0;
}
-
+#define PITCH_MAGIC_NUMBER (121)
static void main_periodic(int my_sig_num) {
- static last_state = 0;
+ static uint8_t last_state = 1;
/* static int bar=0;
if (!(foo%2000)) {
if (bar) {
@@ -117,20 +120,39 @@ static void main_periodic(int my_sig_num) {
BoozImuScaleGyro(booz_imu);
- RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
- &msg_in.bench_sensor.z,&foo);});
+ RunOnceEvery(15, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,
+ &msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y,
+ &msg_in.payload.msg_up.bench_sensor.z,&msg_in.payload.msg_up.cnt,
+ &msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs,
+ &msg_in.payload.msg_up.thrust_out,&msg_in.payload.msg_up.pitch_out);});
- estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
+ estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,msg_in.payload.msg_up.bench_sensor.x);
+ if ( msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is lost! %d %d\n",
+ msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
+ if ( msg_in.payload.msg_up.cnt == 1) printf("STM indicates overo link is regained. %d %d\n",
+ msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
+
+ //If the stm32 cut the motors due to an error, we force the state machine into spinup mode.
+ //when the stm32 resumes after the error, the system will need to be rearmed by the user.
+ //if ( (controller.armed != 0) && (msg_in.payload.msg_up.pitch_out == PITCH_MAGIC_NUMBER) ) {
+ if ( msg_in.payload.msg_up.pitch_out == PITCH_MAGIC_NUMBER ) {
+ controller.armed = 0; last_state=1;
+ printf("STM cut motor power. %d %d\n",
+ msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
+ }
switch (controller.armed) {
case 0:
if (last_state == 2) {
controller.armed = 2;
- printf("Entering spinup mode from flight not permitted. Enter standby first.\n");
- controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
+ printf("Not allowed. Enter standby first.\n");
+ //controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
control_run();
} else {
+ if (last_state == 1) {
+ printf("Entering spinup mode.\n");
+ }
controller.cmd_pitch = 1;
controller.cmd_thrust = 1;
last_state=0;
@@ -139,18 +161,25 @@ static void main_periodic(int my_sig_num) {
case 1:
if (last_state != 1) {
printf("Entering standby mode.\n");
- controller.elevation_sp = RadOfDeg(-30);
- controller.tilt_sp = 0;
last_state=1;
+ if (last_state == 0) {
+ controller.elevation_ref = estimator.elevation;
+ controller.elevation_dot_ref = estimator.elevation_dot;
+ }
}
+ controller.elevation_sp = RadOfDeg(-28);
+ controller.tilt_sp = 0;
control_run();
break;
case 2:
if (last_state == 0) {
- printf("Entering flight mode from spinup not permitted. Enter standby first.\n");
+ printf("Not allowed. Enter standby first.\n");
controller.armed = 0;
} else {
- controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
+ if (last_state == 1) {
+ printf("Entering flight mode.\n");
+ }
+ //controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
control_run();
last_state=2;
}
@@ -175,17 +204,17 @@ static void main_periodic(int my_sig_num) {
/* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
- //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
+ //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
- //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
+ //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
*/
/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
- //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
+ //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
@@ -193,7 +222,7 @@ static void main_periodic(int my_sig_num) {
*/
/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
- //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
+ //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
RunOnceEvery(33, gcs_com_periodic());
@@ -245,22 +274,24 @@ static void main_exit(int sig) {
}
+
static void main_talk_with_stm32() {
//static int8_t adder = 1;
//uint8_t *fooptr;
- //msg_out.thrust = 0;
- msg_out.thrust = (int8_t)controller.cmd_thrust;
+ //msg_out.payload.msg_down.thrust = 0;
+ msg_out.payload.msg_down.thrust = (int8_t)controller.cmd_thrust;
//TODO: change motor config to remove minus here.
- msg_out.pitch = -(int8_t)controller.cmd_pitch;
- //msg_out.pitch = 0;
+ msg_out.payload.msg_down.pitch = -(int8_t)controller.cmd_pitch;
+ //msg_out.payload.msg_down.pitch = 0;
- spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);
+
+ spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame) , &msg_in,&spi_crc_ok);
/* for debugging overo/stm spi link
- if (msg_in.bench_sensor.x == 0){
+ if (msg_in.payload.msg_up.bench_sensor.x == 0){
fooptr = &msg_in;
for (int i=0; i 10) pitch = 10; else
- if (pitch < -10) pitch = -10;
+ Bound(pitch_out,-30,30);
+ Bound(thrust_out,0,80);
- booz2_commands[COMMAND_PITCH] = pitch;
- booz2_commands[COMMAND_ROLL] = 0;
- booz2_commands[COMMAND_YAW] = 0;
-
- if ( overo_link.down.msg.thrust < 100) {
- booz2_commands[COMMAND_THRUST] = overo_link.down.msg.thrust;
- } else {
- booz2_commands[COMMAND_THRUST] = 100;
- }
- if (my_cnt == 0) {
+ overo_link.up.msg.thrust_out = thrust_out;
+ overo_link.up.msg.pitch_out = pitch_out;
+
+ booz2_commands[COMMAND_PITCH] = pitch_out;
+ booz2_commands[COMMAND_THRUST] = thrust_out;
+
+ //stop the motors if we've lost SPI or CAN link
+ //If SPI has had CRC error we don't stop motors
+ if ((spi_msg_cnt == 0) || (can_err_flags != 0)) {
+ booz2_commands[COMMAND_PITCH] = 0;
+ booz2_commands[COMMAND_THRUST] = 0;
actuators_set(FALSE);
+ overo_link.up.msg.can_errs = can_err_flags;
+ overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER;
} else {
actuators_set(TRUE);
}
}
-
static inline void main_event( void ) {
BoozImuEvent(on_gyro_accel_event, on_mag_event);
- OveroLinkEvent(main_on_overo_msg_received);
-
+ OveroLinkEvent(main_on_overo_msg_received,main_on_overo_link_error);
}
static inline void main_on_overo_msg_received(void) {
@@ -123,14 +125,6 @@ static inline void main_on_overo_msg_received(void) {
overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2;
overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3;
-/* overo_link.up.msg.accel.x = booz_imu.accel.x;
- overo_link.up.msg.accel.y = booz_imu.accel.y;
- overo_link.up.msg.accel.z = booz_imu.accel.z;
-
- overo_link.up.msg.gyro.p = booz_imu.gyro.p;
- overo_link.up.msg.gyro.q = booz_imu.gyro.q;
- overo_link.up.msg.gyro.r = booz_imu.gyro.r;*/
-
overo_link.up.msg.accel.x = booz_imu.accel_unscaled.x;
overo_link.up.msg.accel.y = booz_imu.accel_unscaled.y;
overo_link.up.msg.accel.z = booz_imu.accel_unscaled.z;
@@ -138,18 +132,22 @@ static inline void main_on_overo_msg_received(void) {
overo_link.up.msg.gyro.p = booz_imu.gyro_unscaled.p;
overo_link.up.msg.gyro.q = booz_imu.gyro_unscaled.q;
overo_link.up.msg.gyro.r = booz_imu.gyro_unscaled.r;
+
+ //can_err_flags (uint16) represents the board number that is not communicating regularly
+ //spi_errors (uint16) reflects the number of crc errors on the spi link
+ //TODO: if >10% of messages are coming in with crc errors, assume something is really wrong
+ //and disable the motors.
+ overo_link.up.msg.can_errs = can_err_flags;
+ overo_link.up.msg.spi_errs = spi_errors;
+
+ //spi_msg_cnt shows number of spi transfers since last link lost event
+ overo_link.up.msg.cnt = spi_msg_cnt++;
- my_cnt=1;
- //actuators_set(TRUE);
}
static inline void main_on_overo_link_lost(void) {
//actuators_set(FALSE);
- my_cnt = 0;
-/* didn't work: */
-// overo_link_arch_prepare_next_transfert();
-// overo_link.status = IDLE;
-
+ spi_msg_cnt = 0;
}
@@ -157,7 +155,7 @@ static inline void on_gyro_accel_event(void) {
BoozImuScaleGyro(booz_imu);
BoozImuScaleAccel(booz_imu);
- LED_TOGGLE(2);
+ //LED_TOGGLE(2);
static uint8_t cnt;
cnt++;
if (cnt > 15) cnt = 0;
@@ -188,7 +186,7 @@ static inline void on_gyro_accel_event(void) {
static inline void on_mag_event(void) {
- BoozImuScaleMag();
+ BoozImuScaleMag(booz_imu);
static uint8_t cnt;
cnt++;
if (cnt > 1) cnt = 0;
@@ -207,3 +205,6 @@ static inline void on_mag_event(void) {
}
}
+static inline void main_on_overo_link_error(void){
+ spi_errors++;
+}
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index 7d92bb3499..028072b8b1 100644
--- a/sw/airborne/beth/overo_controller.c
+++ b/sw/airborne/beth/overo_controller.c
@@ -11,7 +11,7 @@ void control_init(void) {
// controller.kd = 0.01;
controller.tilt_sp = 0.;
- controller.elevation_sp = 0.;
+ controller.elevation_sp = RadOfDeg(10);
controller.omega_tilt_ref = RadOfDeg(200);
controller.omega_elevation_ref = RadOfDeg(120);
@@ -21,7 +21,8 @@ void control_init(void) {
controller.tilt_dot_ref = estimator.tilt_dot;
controller.tilt_ddot_ref = 0.;
- controller.elevation_ref = estimator.elevation;
+ //controller.elevation_ref = estimator.elevation;
+ controller.elevation_ref = RadOfDeg(-28);
controller.elevation_dot_ref = estimator.elevation_dot;
controller.elevation_ddot_ref = 0.;
@@ -82,7 +83,9 @@ void control_run(void) {
controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;
controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
- if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
+ //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
+ Bound(controller.cmd_thrust,0,100);
+ Bound(controller.cmd_pitch,-100,100);
if (!(foo%100)) {
//printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot);
diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c
index 1de16deb6d..c8157f0c24 100644
--- a/sw/airborne/beth/overo_estimator.c
+++ b/sw/airborne/beth/overo_estimator.c
@@ -8,14 +8,14 @@ void estimator_init(void) {
}
-
+//zyx
void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) {
- const int32_t tilt_neutral = 2815;
+ const int32_t tilt_neutral = 1970;
const float tilt_scale = 1./580.;
const int32_t azimuth_neutral = 1500;
const float azimuth_scale = 1./580.;
- const int32_t elevation_neutral = 1050;
+ const int32_t elevation_neutral = 670;
const float elevation_scale = 1./580.;
diff --git a/sw/airborne/beth/overo_gcs_com.c b/sw/airborne/beth/overo_gcs_com.c
index 5ba3867601..02a354300e 100644
--- a/sw/airborne/beth/overo_gcs_com.c
+++ b/sw/airborne/beth/overo_gcs_com.c
@@ -42,7 +42,7 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void
bytes_read = checked_read(fd, buf, sizeof(buf) - 1);
struct DownlinkTransport *tp = (struct DownlinkTransport *) arg;
struct udp_transport *udp_impl = tp->impl;
- printf("on datalink event: %d bytes\n",bytes_read);
+ //printf("on datalink event: %d bytes\n",bytes_read);
int i = 0;
while (i