Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

updated to new can callback code, main_overo now logs to a file and shuts down in a controlled manner on receiving control-c

Browse Source
This commit is contained in:
Paul Cox
2010-08-03 14:35:47 +00:00
parent b18ec9731d
commit 6b738bed8d
5 changed files with 44 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/beth.xml
+7 -1
View File
@@ -156,7 +156,13 @@ main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
#main_stm32.CFLAGS += -DUSE_I2C2
#main_stm32.srcs += $(SRC_BETH)/bench_sensors_i2c.c
main_stm32.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ
main_stm32.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DUSE_CAN_EXT_ID
main_stm32.CFLAGS += \
-DCAN_PRESCALER=11 \
-DCAN_SJW_TQ=CAN_SJW_1tq \
-DCAN_BS1_TQ=CAN_BS1_3tq \
-DCAN_BS2_TQ=CAN_BS2_5tq \
-DCAN_ERR_RESUME=DISABLE
main_stm32.srcs += can.c $(SRC_ARCH)/can_hw.c
main_stm32.srcs += $(SRC_BETH)/bench_sensors_can.c
sw/airborne/beth/bench_sensors.h
+1 -30
View File
@@ -12,9 +12,7 @@
#include "can_hw.h"
#include <stm32/can.h>
extern volatile uint8_t CAN_RX_FLAG;
extern CanRxMsg can_rx_msg;
extern uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
extern uint16_t halfw1,halfw2,halfw3,halfw4;
#endif
extern void bench_sensors_init(void);
@@ -45,31 +43,4 @@ extern struct BenchSensors bench_sensors;
}
#endif
#ifdef USE_CAN1
#define BenchSensorsEvent( _handler) { \
if (CAN_RX_FLAG == 1) { \
tempid = (uint16_t)(can_rx_msg.ExtId>>7); \
if (tempid == 2) { \
bench_sensors.current = 2;\
halfw2 = can_rx_msg.Data[3]; \
halfw2 = (halfw2<<8) + can_rx_msg.Data[2]; \
halfw1 = can_rx_msg.Data[1]; \
halfw1 = (halfw1<<8) + can_rx_msg.Data[0]; \
bench_sensors.angle_2 = halfw1; \
bench_sensors.angle_3 = halfw2; \
} else { \
bench_sensors.current = 1;\
halfw4 = can_rx_msg.Data[3]; \
halfw4 = (halfw4<<8) + can_rx_msg.Data[2]; \
halfw3 = can_rx_msg.Data[1]; \
halfw3 = (halfw3<<8) + can_rx_msg.Data[0]; \
bench_sensors.angle_1 = halfw4; \
} \
CAN_RX_FLAG = 0; \
_handler(); \
} \
}
#endif
#endif /* BENCH_SENSORS_H */
sw/airborne/beth/bench_sensors_can.c
+11 -11
View File
@@ -1,25 +1,26 @@
#include "bench_sensors.h"
#include "can.h"
#include "led.h"
uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
uint16_t halfw1,halfw2,halfw3,halfw4;
struct BenchSensors bench_sensors;
static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len);
static void can_rx_callback(uint32_t id, uint8_t *buf, int len);
void bench_sensors_init(void) {
can_init(can_rx_callback_t);
can_init(can_rx_callback);
}
void read_bench_sensors(void) {
}
static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len) {
if (id == 2) {
bench_sensors.current = 2;
static void can_rx_callback(uint32_t id, uint8_t *buf, int len) {
//LED_TOGGLE(7);
bench_sensors.current = id>>7;
if (bench_sensors.current == 2) {
halfw2 = buf[3];
halfw2 = (halfw2<<8) + buf[2];
halfw1 = buf[1];
@@ -28,11 +29,10 @@ static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len) {
bench_sensors.angle_3 = halfw2;
}
else {
bench_sensors.current = 1;
halfw4 = buf[3];
halfw4 = (halfw4<<8) + buf[2];
halfw3 = buf[1];
halfw3 = (halfw3<<8) + buf[0];
//halfw3 = buf[1];
//halfw3 = (halfw3<<8) + buf[0];
bench_sensors.angle_1 = halfw4;
}
sw/airborne/beth/main_overo.c
+24 -7
View File
@@ -26,6 +26,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include "fms_network.h"
#include "fms_periodic.h"
@@ -53,6 +54,8 @@ static struct AutopilotMessageBethUp msg_in;
static struct AutopilotMessageBethDown msg_out;
static void send_message(void);
static void PID(void);
void ex_program(int sig);
FILE *outfile;
struct BoozImu booz_imu;
struct BoozImuFloat booz_imu_float;
@@ -64,12 +67,12 @@ static uint32_t foo = 0;
static void main_periodic(int my_sig_num) {
RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
PID();
send_message();
RunOnceEvery(10, {DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
RunOnceEvery(50, {DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
&msg_in.bench_sensor.z,&foo);});
booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
@@ -81,6 +84,9 @@ static void main_periodic(int my_sig_num) {
BoozImuScaleGyro();
fprintf(outfile,"%f %d IMU_ACCEL_RAW %d %d %d\n",foo/500.,42,booz_imu.accel_unscaled.x,booz_imu.accel_unscaled.y,booz_imu.accel_unscaled.z);
fprintf(outfile,"%f %d IMU_GYRO_RAW %d %d %d\n",foo/500.,42,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r);
RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
//&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
@@ -90,23 +96,23 @@ static void main_periodic(int my_sig_num) {
//&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
//&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
//&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
RunOnceEvery(10, {UdpTransportPeriodic();});
RunOnceEvery(33, {UdpTransportPeriodic();});
}
static int8_t pitchval = 0;
static float kp, ki, kd;
int8_t presp,dresp;
static uint16_t tilt_sp=2600;
static uint16_t tilt_sp=2770;
static float piderror,piderrorold;
static void PID(){
@@ -125,6 +131,13 @@ static void PID(){
}
}
void ex_program(int sig) {
fprintf(outfile,"%d\n",foo);
printf("Closing down\n");
fclose(outfile);
exit(EXIT_SUCCESS);
}
int main(int argc, char *argv[]) {
if (argc>1){
@@ -143,6 +156,10 @@ int main(int argc, char *argv[]) {
}
ki=0.0;
outfile = fopen("output.data","w+");
(void) signal(SIGINT, ex_program);
RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
sw/airborne/beth/main_stm32.c
+1 -7
View File
@@ -43,7 +43,6 @@ static inline void on_mag_event(void);
static inline void main_on_overo_msg_received(void);
static inline void main_on_overo_link_lost(void);
static inline void main_on_bench_sensors( void );
static int16_t my_cnt;
@@ -66,7 +65,7 @@ static inline void main_init( void ) {
booz_imu_init();
overo_link_init();
bench_sensors_init();
LED_ON(7);
//LED_ON(7);
}
@@ -114,7 +113,6 @@ static inline void main_event( void ) {
BoozImuEvent(on_gyro_accel_event, on_mag_event);
OveroLinkEvent(main_on_overo_msg_received);
BenchSensorsEvent(main_on_bench_sensors);
}
static inline void main_on_overo_msg_received(void) {
@@ -207,7 +205,3 @@ static inline void on_mag_event(void) {
}
}
static inline void main_on_bench_sensors( void ) {
}