[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5338] Beth updates : Add CAN/SPI error monitoring.
|
From: |
Paul Cox |
|
Subject: |
[paparazzi-commits] [5338] Beth updates : Add CAN/SPI error monitoring. |
|
Date: |
Thu, 12 Aug 2010 14:17:13 +0000 |
Revision: 5338
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5338
Author: paulcox
Date: 2010-08-12 14:17:12 +0000 (Thu, 12 Aug 2010)
Log Message:
-----------
Beth updates : Add CAN/SPI error monitoring. Adjusted arming state machine.
TODO: monitor CAN crc errors.
TODO: update beth board #1 to output at higher rate (important when azimuth
starts to be controlled)
TODO: sin/cos corrections in estimator/controller
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/beth.xml
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/settings/settings_beth.xml
paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
paparazzi3/trunk/sw/airborne/beth/main_overo.c
paparazzi3/trunk/sw/airborne/beth/main_stm32.c
paparazzi3/trunk/sw/airborne/beth/overo_controller.c
paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -104,14 +104,14 @@
#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.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
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-08-12 13:56:13 UTC (rev 5337)
+++ paparazzi3/trunk/conf/messages.xml 2010-08-12 14:17:12 UTC (rev 5338)
@@ -595,19 +595,23 @@
</message>
<message name="BETH" id="106">
- <field name="azimuth" type="uint16"/>
- <field name="elevation" type="uint16"/>
- <field name="tilt" type="uint16"/>
- <field name="other" type="uint16"/>
+ <field name="azimuth" type="int16"/>
+ <field name="elevation" type="int16"/>
+ <field name="tilt" type="int16"/>
+ <field name="counter" type="uint32"/>
+ <field name="can_errs" type="uint16"/>
+ <field name="spi_errs" type="uint16"/>
+ <field name="thrust_out" type="int8"/>
+ <field name="pitch_out" type="int8"/>
</message>
<message name="BETH_ESTIMATOR" id="107">
- <field name="tilt" type="float"/>
- <field name="tilt_dot" type="float"/>
- <field name="elevation" type="float"/>
- <field name="elevation_dot" type="float"/>
- <field name="azimuth" type="float"/>
- <field name="azimuth_dot" type="float"/>
+ <field name="tilt" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
+ <field name="tilt_dot" type="float" alt_unit="deg/s"
alt_unit_coef="57.29578"/>
+ <field name="elevation" type="float" alt_unit="deg"
alt_unit_coef="57.29578"/>
+ <field name="elevation_dot" type="float" alt_unit="deg/s"
alt_unit_coef="57.29578"/>
+ <field name="azimuth" type="float" alt_unit="deg"
alt_unit_coef="57.29578"/>
+ <field name="azimuth_dot" type="float" alt_unit="deg/s"
alt_unit_coef="57.29578"/>
</message>
<message name="BETH_CONTROLLER" id="108">
@@ -617,15 +621,12 @@
<field name="pitch_fb" type="float"/>
<field name="thrust_ff" type="float"/>
<field name="thrust_fb" type="float"/>
- <field name="tilt_ref" type="float"/>
- <field name="tilt_dot_ref" type="float"/>
- <field name="elevation_ref " type="float"/>
- <field name="elevation_dot_ref" type="float"/>
+ <field name="tilt_ref" type="float" alt_unit="deg"
alt_unit_coef="57.29578"/>
+ <field name="tilt_dot_ref" type="float" alt_unit="deg/s"
alt_unit_coef="57.29578"/>
+ <field name="elevation_ref " type="float" alt_unit="deg"
alt_unit_coef="57.29578"/>
+ <field name="elevation_dot_ref" type="float" alt_unit="deg/s"
alt_unit_coef="57.29578"/>
</message>
-
-
-
<message name="DC_SHOT" id="110">
<field name="photo_nr" type="int16" unit=""></field>
<field name="utm_east" type="int32" unit="cm"></field>
Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -4,9 +4,9 @@
<dl_settings>
<dl_settings NAME="Controller">
- <dl_setting var="controller.elevation_sp" min="-15" step="0.5" max="15"
module="beth/overo_controller" shortname="elev_sp" unit="rad" alt_unit="deg"
alt_unit_coef="57.32"/>
+ <dl_setting var="controller.elevation_sp" min="-25" step="1" max="20"
module="beth/overo_controller" shortname="elev_sp" unit="rad" alt_unit="deg"
alt_unit_coef="57.29578"/>
- <dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15"
module="beth/overo_controller" shortname="tilt_sp" unit="rad" alt_unit="deg"
alt_unit_coef="57.32">
+ <dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15"
module="beth/overo_controller" shortname="tilt_sp" unit="rad" alt_unit="deg"
alt_unit_coef="57.29578">
</dl_setting>
<dl_setting var="controller.armed" min="0" step="1" max="2" shortname
="mode"/>
Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors.h 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors.h 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -11,8 +11,7 @@
#include "can.h"
#include "can_hw.h"
#include <stm32/can.h>
-
-extern uint16_t halfw1,halfw2,halfw3,halfw4;
+extern uint16_t can_err_flags;
#endif
extern void bench_sensors_init(void);
Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c 2010-08-12
13:56:13 UTC (rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c 2010-08-12
14:17:12 UTC (rev 5338)
@@ -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;
}
-
}
Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -31,6 +31,7 @@
#include <event.h>
#include "messages2.h"
+#include "airframe.h"
#include "fms_periodic.h"
#include "fms_debug.h"
@@ -50,14 +51,15 @@
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 @@
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 @@
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 @@
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 @@
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 @@
/* 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 @@
*/
/* 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_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<sizeof(msg_in); i++)
printf("%02x ", fooptr[i]);
@@ -269,12 +300,12 @@
foo++;
- booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
- booz_imu.gyro_unscaled.q = (msg_in.gyro.q&0xFFFF);
- booz_imu.gyro_unscaled.r = (msg_in.gyro.r&0xFFFF);
- booz_imu.accel_unscaled.x = (msg_in.accel.x&0xFFFF);
- booz_imu.accel_unscaled.y = (msg_in.accel.y&0xFFFF);
- booz_imu.accel_unscaled.z = (msg_in.accel.z&0xFFFF);
+ booz_imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p&0xFFFF);
+ booz_imu.gyro_unscaled.q = (msg_in.payload.msg_up.gyro.q&0xFFFF);
+ booz_imu.gyro_unscaled.r = (msg_in.payload.msg_up.gyro.r&0xFFFF);
+ booz_imu.accel_unscaled.x = (msg_in.payload.msg_up.accel.x&0xFFFF);
+ booz_imu.accel_unscaled.y = (msg_in.payload.msg_up.accel.y&0xFFFF);
+ booz_imu.accel_unscaled.z = (msg_in.payload.msg_up.accel.z&0xFFFF);
}
Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -23,6 +23,7 @@
#include BOARD_CONFIG
+#include "std.h"
#include "init_hw.h"
#include "can.h"
#include "sys_time.h"
@@ -43,8 +44,10 @@
static inline void main_on_overo_msg_received(void);
static inline void main_on_overo_link_lost(void);
+static inline void main_on_overo_link_error(void);
-static int16_t my_cnt;
+static uint32_t spi_msg_cnt = 0;
+static uint16_t spi_errors = 0;
int main(void) {
main_init();
@@ -65,12 +68,14 @@
booz_imu_init();
overo_link_init();
bench_sensors_init();
- //LED_ON(7);
+ booz2_commands[COMMAND_ROLL] = 0;
+ booz2_commands[COMMAND_YAW] = 0;
}
+#define PITCH_MAGIC_NUMBER (121)
static inline void main_periodic( void ) {
- int8_t pitch;
+ int8_t pitch_out,thrust_out;
booz_imu_periodic();
OveroLinkPeriodic(main_on_overo_link_lost)
@@ -82,39 +87,36 @@
//RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel,
&overo_link.down.msg.foo,&overo_link.down.msg.bar);});
- /*Request reception of values from coder boards :
- When configured for I2C, lisa stm32 is master and requests data from the
- beth board slaves.
- When configured for CAN, data is automatically available as CAN reception
is
- always ongoing, and new data generates a flag by the IST. */
read_bench_sensors();
- pitch = (int8_t)((0xFF) & overo_link.down.msg.pitch);
+ pitch_out = (int8_t)((0xFF) & overo_link.down.msg.pitch);
+ thrust_out = (int8_t)((0xFF) & overo_link.down.msg.thrust);
- if (pitch > 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 @@
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 @@
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;
- my_cnt=1;
- //actuators_set(TRUE);
+ //spi_msg_cnt shows number of spi transfers since last link lost event
+ overo_link.up.msg.cnt = spi_msg_cnt++;
+
}
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 @@
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_mag_event(void) {
- BoozImuScaleMag();
+ BoozImuScaleMag(booz_imu);
static uint8_t cnt;
cnt++;
if (cnt > 1) cnt = 0;
@@ -207,3 +205,6 @@
}
}
+static inline void main_on_overo_link_error(void){
+ spi_errors++;
+}
Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-12
13:56:13 UTC (rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-12
14:17:12 UTC (rev 5338)
@@ -11,7 +11,7 @@
// 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 @@
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 @@
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);
Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -8,14 +8,14 @@
}
-
+//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.;
Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c 2010-08-12 13:56:13 UTC
(rev 5337)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c 2010-08-12 14:17:12 UTC
(rev 5338)
@@ -42,7 +42,7 @@
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<bytes_read) {
parse_udp_dl(udp_impl, buf[i]);
@@ -77,7 +77,7 @@
break;
default :
- printf("did nothing\n");
+ printf("datalink did nothing\n");
break;
}
Modified: paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h 2010-08-12
13:56:13 UTC (rev 5337)
+++ paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h 2010-08-12
14:17:12 UTC (rev 5338)
@@ -31,13 +31,19 @@
struct Int16Rates gyro;
struct Int16Vect3 accel;
struct Int16Vect3 bench_sensor;
+ uint16_t can_errs;
+ uint16_t spi_errs;
uint32_t cnt;
+ int8_t thrust_out;
+ int8_t pitch_out;
};
struct __attribute__ ((packed)) AutopilotMessageBethDown
{
uint8_t thrust;
uint8_t pitch;
+ uint32_t errors;
+ uint32_t cnt;
};
/*
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5338] Beth updates : Add CAN/SPI error monitoring.,
Paul Cox <=