From 55c3ad045d6de5c61ff8dd8d1453ad6b1aab15bc Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 6 Aug 2013 18:34:36 +0200 Subject: [PATCH 01/65] [rotorcraft] set guidance_v_zd_sp to zero in altitude hold for visualization --- sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index c8cd29d6e13..71882d19352 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -202,6 +202,7 @@ void guidance_v_run(bool_t in_flight) { if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) guidance_v_z_sp = fms.input.v_sp.height; #endif + guidance_v_zd_sp = 0; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT @@ -216,6 +217,7 @@ void guidance_v_run(bool_t in_flight) { { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; + guidance_v_zd_sp = 0; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } From 3951bd7f27cfe48cc3e3e0f7d3ade925202b0297 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 7 Aug 2013 00:04:35 +0200 Subject: [PATCH 02/65] [math] add FLOAT_QUAT_OF_ORIENTATION_VECT --- sw/airborne/math/pprz_algebra_float.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index e48aca10dd5..e787b961e3e 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -701,6 +701,22 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { _q.qz = san * _uv.z; \ } +#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) { \ + const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \ + if (ov_norm < 1e-8) { \ + (_q).qi = 1; \ + (_q).qx = 0; \ + (_q).qy = 0; \ + (_q).qz = 0; \ + } else { \ + const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \ + (_q).qi = cosf(ov_norm/2.0); \ + (_q).qx = (_ov).x * s2_normalized; \ + (_q).qy = (_ov).y * s2_normalized; \ + (_q).qz = (_ov).z * s2_normalized; \ + } \ + } + #define FLOAT_QUAT_OF_RMAT(_q, _r) { \ const float tr = RMAT_TRACE(_r); \ if (tr > 0) { \ From 104acd276754eef8850826d00f0dfb6f5fb5bf70 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 7 Aug 2013 13:46:08 +0200 Subject: [PATCH 03/65] [rotorcraft][guidance][stabilization] quat setpoint fixes Don't pretend that the commands from guidance_h are actually real euler angles. Compose a roll/pitch quaternion from simultaneous rotation of roll/pitch, then rotate around resulting body z-axis to align the heading. This should "fix" the setpoint passed to the attitude stabilization if in HOVER or NAV at large angles. Only tested quickly in simulation... --- .../rotorcraft/guidance/guidance_h.c | 14 ++++----- .../stabilization/stabilization_attitude.h | 2 +- .../stabilization_attitude_euler_float.c | 4 +-- .../stabilization_attitude_euler_int.c | 4 +-- .../stabilization_attitude_passthrough.c | 4 +-- .../stabilization_attitude_quat_float.c | 22 ++++++++++++-- .../stabilization_attitude_quat_int.c | 30 ++++++++++++++++--- 7 files changed, 59 insertions(+), 21 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 39995aad2ff..e609a896243 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -253,12 +253,12 @@ void guidance_h_run(bool_t in_flight) { guidance_h_nav_enter(); if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { - struct Int32Eulers sp_euler_i; - sp_euler_i.phi = nav_roll; - sp_euler_i.theta = nav_pitch; + struct Int32Eulers sp_cmd_i; + sp_cmd_i.phi = nav_roll; + sp_cmd_i.theta = nav_pitch; /* FIXME: heading can't be set via attitude block yet, use current heading for now */ - sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi; - stabilization_attitude_set_from_eulers_i(&sp_euler_i); + sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; + stabilization_attitude_set_cmd_i(&sp_cmd_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -362,8 +362,8 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_command_body.phi += guidance_h_rc_sp.phi; guidance_h_command_body.theta += guidance_h_rc_sp.theta; - /* Set attitude setpoint from pseudo-eulers */ - stabilization_attitude_set_from_eulers_i(&guidance_h_command_body); + /* Set attitude setpoint from pseudo-euler commands */ + stabilization_attitude_set_cmd_i(&guidance_h_command_body); } static void guidance_h_hover_enter(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 7a0572f69e1..94b47466e43 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -34,7 +34,7 @@ extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); extern void stabilization_attitude_set_failsafe_setpoint(void); -extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler); +extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd); extern void stabilization_attitude_run(bool_t in_flight); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index b2f9f241784..3a5df194178 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -86,8 +86,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index f435e97a1f6..a86f9c4c80b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -102,8 +102,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 69f7004aca4..5ddcb0e7c3a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -78,7 +78,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index e546692a871..b99400c4920 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -142,9 +142,25 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); - FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); + + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = stab_att_sp_euler.phi; + ov.y = stab_att_sp_euler.theta; + ov.z = 0.0; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /* quaternion with only heading setpoint */ + const float yaw2 = stab_att_sp_euler.psi / 2.0; + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 1bff46cd25a..1c889af5d03 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -92,11 +92,33 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { // copy euler setpoint for debugging - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); - INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); + + /// @todo calc sp_quat in fixed-point + + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi); + ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta); + ov.z = 0.0; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /* quaternion with only heading setpoint */ + const float yaw2 = ANGLE_FLOAT_OF_BFP(sp_cmd->psi) / 2.0; + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + struct FloatQuat q_sp; + FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); + FLOAT_QUAT_WRAP_SHORTEST(q_sp); + + /* convert to fixed point */ + QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) From c32ea43ed2a712e5b03c0d814513275d903a4bc0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 13 Aug 2013 17:20:07 +0200 Subject: [PATCH 04/65] [rotorcraft] guidance_h: normalize psi sp from nav_heading --- sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index e609a896243..1b5cdbf0b22 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -267,6 +267,7 @@ void guidance_h_run(bool_t in_flight) { /* set psi command */ guidance_h_command_body.psi = nav_heading; + INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi); /* compute roll and pitch commands and set final attitude setpoint */ guidance_h_traj_run(in_flight); } From 25609e0c5ac2aec44f261b07cadb49b41fc62934 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 20 Aug 2013 23:43:41 +0200 Subject: [PATCH 05/65] [rotorcraft][stabilization] calculate correct yaw from guidance psi setpoint Instead of using the psi setpoint angle to rotate around the body z-axis, calculate the real angle needed to align the projection of the body x-axis onto the horizontal plane with the psi setpoint. Only makes a few degrees of difference at high roll/pitch angles, but should be correc now. still needs to be optimized... --- .../stabilization_attitude_quat_float.c | 40 +++++++++++++++++- .../stabilization_attitude_quat_int.c | 42 ++++++++++++++++++- 2 files changed, 78 insertions(+), 4 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index b99400c4920..d6c83774b60 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -154,8 +154,44 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { struct FloatQuat q_rp; FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - /* quaternion with only heading setpoint */ - const float yaw2 = stab_att_sp_euler.psi / 2.0; + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) + */ + const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; + const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; + struct FloatVect3 a; + FLOAT_QUAT_VMULT(a, q_rp, xaxis); + + // desired heading vect in earth x-y plane + struct FloatVect3 psi_vect; + psi_vect.x = cosf(stab_att_sp_euler.psi); + psi_vect.y = sinf(stab_att_sp_euler.psi); + psi_vect.z = 0.0; + struct FloatVect3 normal; + FLOAT_QUAT_VMULT(normal, q_rp, zaxis); + // projection of desired heading onto body x-y plane + // b = v - dot(v,n)*n + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, normal, dot); + + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(a, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, a, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + float yaw2 = atan2(nc, dot) / 2.0; + + /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 1c889af5d03..b0b5b3ae6eb 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -107,8 +107,46 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { struct FloatQuat q_rp; FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - /* quaternion with only heading setpoint */ - const float yaw2 = ANGLE_FLOAT_OF_BFP(sp_cmd->psi) / 2.0; + const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); + + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) + */ + const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; + const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; + struct FloatVect3 a; + FLOAT_QUAT_VMULT(a, q_rp, xaxis); + + // desired heading vect in earth x-y plane + struct FloatVect3 psi_vect; + psi_vect.x = cosf(psi_sp); + psi_vect.y = sinf(psi_sp); + psi_vect.z = 0.0; + struct FloatVect3 normal; + FLOAT_QUAT_VMULT(normal, q_rp, zaxis); + // projection of desired heading onto body x-y plane + // b = v - dot(v,n)*n + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, normal, dot); + + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(a, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, a, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + float yaw2 = atan2(nc, dot) / 2.0; + + /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); From 029264bdc2d7e22f44479e9fef6177d0e54091b9 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 28 Aug 2013 18:05:36 +0200 Subject: [PATCH 06/65] [Ardrone] Full Auto2 Flight in RAW Mode --- conf/airframes/ardrone2_raw.xml | 256 ++++++++++++++++---------------- 1 file changed, 131 insertions(+), 125 deletions(-) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 66512c40454..0b6eb33d186 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -4,214 +4,220 @@ - + - - - - - - + + + + + + - - + + - + - - - - - - + + + + + + - - + + - - - - + + + + - - - - + + + +
- - - - - + + + + + - - - - + + + +
- - - - - + + + + +
- - - + + + + + + + + + + + + + - - - - + - + - +
- - - + + +
- +
- - - - - - - + + + + + + + - - - + + + - - - + + + - - - + + +
- - - - - - + + + + + + - - - - + + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + +
- - - - - - - - + + + + + + + + - + - +
- - - - + + + +
- - - + + +
- - - + + +
- - - - - + + + + +
From fb4990234de147fd956b8372dc9f603f790640c4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 17:53:53 +0200 Subject: [PATCH 07/65] [ins] include std.h for PRINT_CONFIG_VAR --- sw/airborne/subsystems/ins/vf_extended_float.c | 1 + sw/airborne/subsystems/ins/vf_float.c | 1 + 2 files changed, 2 insertions(+) diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index 6e34cecfd7c..307c253f05a 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -31,6 +31,7 @@ #include "subsystems/ins/vf_extended_float.h" #include "generated/airframe.h" +#include "std.h" #define DEBUG_VFF_EXTENDED 1 diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c index 7a2d023ac47..9c8cf7300fa 100644 --- a/sw/airborne/subsystems/ins/vf_float.c +++ b/sw/airborne/subsystems/ins/vf_float.c @@ -28,6 +28,7 @@ #include "subsystems/ins/vf_float.h" #include "generated/airframe.h" +#include "std.h" #ifndef INS_PROPAGATE_FREQUENCY #define INS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY From 8c7724cc3351db6e94f81efa6cd477352573a4ab Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 17:57:51 +0200 Subject: [PATCH 08/65] [conf][tests] fix test_baro sources --- conf/firmwares/lisa_test_progs.makefile | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile index bb8f1a53b86..5a183453599 100644 --- a/conf/firmwares/lisa_test_progs.makefile +++ b/conf/firmwares/lisa_test_progs.makefile @@ -203,16 +203,21 @@ LISA_M_BARO ?= BARO_BOARD_BMP085 ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile test_baro.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - test_baro.srcs += $(SRC_BOARD)/baro_ms5611_spi.c + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_spi.c + test_baro.srcs += subsystems/sensors/baro_ms5611_spi.c test_baro.srcs += $(SRC_LISA)/test_baro_spi.c else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c - test_baro.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_i2c.c + test_baro.srcs += subsystems/sensors/baro_ms5611_i2c.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.srcs += peripherals/bmp085.c test_baro.srcs += $(SRC_BOARD)/baro_board.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c endif From 63dbb46d2ea72bc6cd92d805b785caca7408ba85 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 19:33:18 +0200 Subject: [PATCH 09/65] add make-release-tarball script --- make-release-tarball.sh | 255 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 255 insertions(+) create mode 100755 make-release-tarball.sh diff --git a/make-release-tarball.sh b/make-release-tarball.sh new file mode 100755 index 00000000000..5d500331789 --- /dev/null +++ b/make-release-tarball.sh @@ -0,0 +1,255 @@ +#!/bin/bash - +# +# File: git-archive-all.sh +# +# Description: A utility script that builds an archive file(s) of all +# git repositories and submodules in the current path. +# Useful for creating a single tarfile of a git super- +# project that contains other submodules. +# +# Examples: Use git-archive-all.sh to create archive distributions +# from git repositories. To use, simply do: +# +# cd $GIT_DIR; git-archive-all.sh +# +# where $GIT_DIR is the root of your git superproject. +# +# License: GPL3 +# +############################################################################### +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +############################################################################### + +# DEBUGGING +set -e +set -C # noclobber + +# TRAP SIGNALS +trap 'cleanup' QUIT EXIT + +# For security reasons, explicitly set the internal field separator +# to newline, space, tab +OLD_IFS=$IFS +IFS=' + ' + +function cleanup () { + rm -f $TMPFILE + rm -f $TOARCHIVE + IFS="$OLD_IFS" +} + +function usage () { + echo "Usage is as follows:" + echo + echo "$PROGRAM <--version>" + echo " Prints the program version number on a line by itself and exits." + echo + echo "$PROGRAM <--usage|--help|-?>" + echo " Prints this usage output and exits." + echo + echo "$PROGRAM [--format ] [--prefix ] [--verbose|-v] [--separate|-s] [output_file]" + echo " Creates an archive for the entire git superproject, and its submodules" + echo " using the passed parameters, described below." + echo + echo " If '--format' is specified, the archive is created with the named" + echo " git archiver backend. Obviously, this must be a backend that git archive" + echo " understands. The format defaults to 'tar' if not specified." + echo + echo " If '--prefix' is specified, the archive's superproject and all submodules" + echo " are created with the prefix named. The default is to not use one." + echo + echo " If '--separate' or '-s' is specified, individual archives will be created" + echo " for each of the superproject itself and its submodules. The default is to" + echo " concatenate individual archives into one larger archive." + echo + echo " If 'output_file' is specified, the resulting archive is created as the" + echo " file named. This parameter is essentially a path that must be writeable." + echo " When combined with '--separate' ('-s') this path must refer to a directory." + echo " Without this parameter or when combined with '--separate' the resulting" + echo " archive(s) are named with a dot-separated path of the archived directory and" + echo " a file extension equal to their format (e.g., 'superdir.submodule1dir.tar')." + echo + echo " If '--verbose' or '-v' is specified, progress will be printed." +} + +function version () { + echo "$PROGRAM version $VERSION" +} + +# Internal variables and initializations. +readonly PROGRAM=`basename "$0"` +readonly VERSION=0.2 + +OLD_PWD="`pwd`" +TMPDIR=${TMPDIR:-/tmp} +TMPFILE=`mktemp "$TMPDIR/$PROGRAM.XXXXXX"` # Create a place to store our work's progress +TOARCHIVE=`mktemp "$TMPDIR/$PROGRAM.toarchive.XXXXXX"` +OUT_FILE=$OLD_PWD # assume "this directory" without a name change by default +SEPARATE=0 +VERBOSE=0 + +FORMAT=tar +PREFIX= +TREEISH=HEAD + +# RETURN VALUES/EXIT STATUS CODES +readonly E_BAD_OPTION=254 +readonly E_UNKNOWN=255 + +# Process command-line arguments. +while test $# -gt 0; do + case $1 in + --format ) + shift + FORMAT="$1" + shift + ;; + + --prefix ) + shift + PREFIX="$1" + shift + ;; + + --separate | -s ) + shift + SEPARATE=1 + ;; + + --version ) + version + exit + ;; + + --verbose | -v ) + shift + VERBOSE=1 + ;; + + -? | --usage | --help ) + usage + exit + ;; + + -* ) + echo "Unrecognized option: $1" >&2 + usage + exit $E_BAD_OPTION + ;; + + * ) + break + ;; + esac +done + +if [ ! -z "$1" ]; then + OUT_FILE="$1" + shift +fi + +# Validate parameters; error early, error often. +if [ $SEPARATE -eq 1 -a ! -d $OUT_FILE ]; then + echo "When creating multiple archives, your destination must be a directory." + echo "If it's not, you risk being surprised when your files are overwritten." + exit +elif [ `git config -l | grep -q '^core\.bare=false'; echo $?` -ne 0 ]; then + echo "$PROGRAM must be run from a git working copy (i.e., not a bare repository)." + exit +fi + +# Create the superproject's git-archive +if [ $VERBOSE -eq 1 ]; then + echo -n "creating superproject archive..." +fi +git archive --format=$FORMAT --prefix="$PREFIX" $TREEISH > $TMPDIR/$(basename $(pwd)).$FORMAT +if [ $VERBOSE -eq 1 ]; then + echo "done" +fi +echo $TMPDIR/$(basename $(pwd)).$FORMAT >| $TMPFILE # clobber on purpose +superfile=`head -n 1 $TMPFILE` + +if [ $VERBOSE -eq 1 ]; then + echo -n "looking for subprojects..." +fi +# find all '.git' dirs, these show us the remaining to-be-archived dirs +# we only want directories that are below the current directory +find . -mindepth 2 -name '.git' -type d -print | sed -e 's/^\.\///' -e 's/\.git$//' >> $TOARCHIVE +# as of version 1.7.8, git places the submodule .git directories under the superprojects .git dir +# the submodules get a .git file that points to their .git dir. we need to find all of these too +find . -mindepth 2 -name '.git' -type f -print | xargs grep -l "gitdir" | sed -e 's/^\.\///' -e 's/\.git$//' >> $TOARCHIVE +if [ $VERBOSE -eq 1 ]; then + echo "done" + echo " found:" + cat $TOARCHIVE | while read arch + do + echo " $arch" + done +fi + +if [ $VERBOSE -eq 1 ]; then + echo -n "archiving submodules..." +fi +while read path; do + TREEISH=$(git submodule | grep "^ .*${path%/} " | cut -d ' ' -f 2) # git submodule does not list trailing slashes in $path + cd "$path" + git archive --format=$FORMAT --prefix="${PREFIX}$path" ${TREEISH:-HEAD} > "$TMPDIR"/"$(echo "$path" | sed -e 's/\//./g')"$FORMAT + if [ $FORMAT == 'zip' ]; then + # delete the empty directory entry; zipped submodules won't unzip if we don't do this + zip -d "$(tail -n 1 $TMPFILE)" "${PREFIX}${path%/}" >/dev/null # remove trailing '/' + fi + echo "$TMPDIR"/"$(echo "$path" | sed -e 's/\//./g')"$FORMAT >> $TMPFILE + cd "$OLD_PWD" +done < $TOARCHIVE +if [ $VERBOSE -eq 1 ]; then + echo "done" +fi + +if [ $VERBOSE -eq 1 ]; then + echo -n "concatenating archives into single archive..." +fi +# Concatenate archives into a super-archive. +if [ $SEPARATE -eq 0 ]; then + if [ $FORMAT == 'tar' ]; then + sed -e '1d' $TMPFILE | while read file; do + tar --concatenate -f "$superfile" "$file" && rm -f "$file" + done + elif [ $FORMAT == 'zip' ]; then + sed -e '1d' $TMPFILE | while read file; do + # zip incorrectly stores the full path, so cd and then grow + cd `dirname "$file"` + zip -g "$superfile" `basename "$file"` && rm -f "$file" + done + cd "$OLD_PWD" + fi + + echo "$superfile" >| $TMPFILE # clobber on purpose +fi +if [ $VERBOSE -eq 1 ]; then + echo "done" +fi + +if [ $VERBOSE -eq 1 ]; then + echo -n "moving archive to $OUT_FILE..." +fi +while read file; do + mv "$file" "$OUT_FILE" +done < $TMPFILE +if [ $VERBOSE -eq 1 ]; then + echo "done" +fi From 313c647d00a1945257cd02a676936bf825cfc80a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 19:39:26 +0200 Subject: [PATCH 10/65] [conf] imu in nps for krooz --- conf/firmwares/subsystems/shared/imu_krooz_sd.makefile | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile index 878dadfac15..baf9c6ae101 100644 --- a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile @@ -25,3 +25,8 @@ ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) ap.CFLAGS += $(IMU_KROOZ_CFLAGS) ap.srcs += $(IMU_KROOZ_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile From 9572ca5b39bba6ab4eb75da16d8ddd2c17d6754d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 19:39:51 +0200 Subject: [PATCH 11/65] [conf] remove unneeded DT_VFILTER --- .../subsystems/lisa_passthrough/booz_stabilization_int.makefile | 2 +- conf/firmwares/subsystems/rotorcraft/ins_hff.makefile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile index 53d7b44202e..9798cb93cb4 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile @@ -14,7 +14,7 @@ stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins.c stm_passthrough.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c stm_passthrough.srcs += $(SRC_FIRMWARE)/navigation.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -stm_passthrough.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)' +stm_passthrough.CFLAGS += -DUSE_VFF stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" diff --git a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile index 0e1d9dc5efc..fd0ca652280 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile @@ -8,7 +8,7 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' +$(TARGET).CFLAGS += -DUSE_VFF # horizontal filter float version $(TARGET).CFLAGS += -DUSE_HFF From f2a5d9ea5cce1e4ec220877fbdfc836dfc767c5f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 22:47:48 +0200 Subject: [PATCH 12/65] [conf] remove joystick subsystem rather add to your airframe file --- conf/firmwares/subsystems/fixedwing/joystick.makefile | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 conf/firmwares/subsystems/fixedwing/joystick.makefile diff --git a/conf/firmwares/subsystems/fixedwing/joystick.makefile b/conf/firmwares/subsystems/fixedwing/joystick.makefile deleted file mode 100644 index f0787a55676..00000000000 --- a/conf/firmwares/subsystems/fixedwing/joystick.makefile +++ /dev/null @@ -1,6 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -# joystick for fixedwings - -$(TARGET).CFLAGS += -DUSE_JOYSTICK - From 9c279840c28d4dbc7cf37baf8e8d8e6333178ee0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 20:36:05 +0200 Subject: [PATCH 13/65] [fixedwing] add nav_reset_utm_zone start solving issue #508 --- .../subsystems/navigation/common_nav.c | 30 +++++++++++++++++++ .../subsystems/navigation/common_nav.h | 1 + 2 files changed, 31 insertions(+) diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 317fa475b3b..7e641c05d60 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -61,6 +61,36 @@ void compute_dist2_to_home(void) { static float previous_ground_alt; +/** Reset the UTM zone to current GPS fix */ +unit_t nav_reset_utm_zone(void) { + + struct UtmCoor_f utm0_old; + utm0_old.zone = nav_utm_zone0; + utm0_old.north = nav_utm_north0; + utm0_old.east = nav_utm_east0; + utm0_old.alt = ground_alt; + struct LlaCoor_f lla0; + lla_of_utm_f(&lla0, &utm0_old); + +#ifdef GPS_USE_LATLONG + /* Set the real UTM zone */ + nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; +#else + nav_utm_zone0 = gps.utm_pos.zone; +#endif + + struct UtmCoor_f utm0; + utm0.zone = nav_utm_zone0; + utm_of_lla_f(&utm0, &lla0); + + nav_utm_east0 = utm0.east; + nav_utm_north0 = utm0.north; + + stateSetLocalUtmOrigin_f(&utm0); + + return 0; +} + /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { #ifdef GPS_USE_LATLONG diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index c7f03434895..3698d927022 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -61,6 +61,7 @@ extern uint8_t nav_utm_zone0; void compute_dist2_to_home(void); +unit_t nav_reset_utm_zone(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused)); void common_nav_periodic_task_4Hz(void); From d744f8ca2c9be9e957fe6acd034e6a032650bcd5 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 29 Aug 2013 15:22:08 +0200 Subject: [PATCH 14/65] [Messages] Image Analysis Telemetry --- conf/messages.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/conf/messages.xml b/conf/messages.xml index 683bac8e3c0..3c5a9b2a295 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -614,7 +614,11 @@ - + + + + + From 03c3b8f43a77b06894083e474fbd7560fe776211 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Tue, 2 Jul 2013 16:40:40 +0200 Subject: [PATCH 15/65] [superbitrf] Initial commit for superbitrf 2.4GHz radio control --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 4 +- .../shared/radio_control_superbitrf.makefile | 14 + conf/messages.xml | 7 +- conf/telemetry/default_rotorcraft.xml | 1 + sw/airborne/firmwares/rotorcraft/telemetry.h | 12 + sw/airborne/mcu_periph/gpio.h | 4 + sw/airborne/peripherals/cyrf6936.c | 359 ++++++++++++++++++ sw/airborne/peripherals/cyrf6936.h | 70 ++++ sw/airborne/peripherals/cyrf6936_regs.h | 211 ++++++++++ .../subsystems/radio_control/superbitrf.c | 129 +++++++ .../subsystems/radio_control/superbitrf.h | 86 +++++ 11 files changed, 892 insertions(+), 5 deletions(-) create mode 100644 conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile create mode 100644 sw/airborne/peripherals/cyrf6936.c create mode 100644 sw/airborne/peripherals/cyrf6936.h create mode 100644 sw/airborne/peripherals/cyrf6936_regs.h create mode 100644 sw/airborne/subsystems/radio_control/superbitrf.c create mode 100644 sw/airborne/subsystems/radio_control/superbitrf.h diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 8881af6298e..3244ea3474e 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -12,9 +12,7 @@ - - - + diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile new file mode 100644 index 00000000000..791751efde8 --- /dev/null +++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile @@ -0,0 +1,14 @@ +# +# Makefile for shared radio_control superbitrf subsystem +# + +ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf.h\" +ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 + +ifneq ($(RADIO_CONTROL_LED),none) +ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +endif + +ap.srcs += peripherals/cyrf6936.c \ + $(SRC_SUBSYSTEMS)/radio_control.c \ + $(SRC_SUBSYSTEMS)/radio_control/superbitrf.c diff --git a/conf/messages.xml b/conf/messages.xml index 3c5a9b2a295..2999fcab523 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -603,8 +603,11 @@ - - + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 6f83d9b3400..0ed6ab52aef 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -26,6 +26,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index a1b3472812f..e08247b3f62 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -134,6 +134,18 @@ #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif +#ifdef RADIO_CONTROL_TYPE_SUPERBITRF +#include "subsystems/radio_control/superbitrf.h" +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) { \ + DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ + &superbitrf.status, \ + &superbitrf.cyrf6936.status, \ + 6, \ + superbitrf.cyrf6936.mfg_id);} +#else +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) {} +#endif + #ifdef ACTUATORS #define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators) #else diff --git a/sw/airborne/mcu_periph/gpio.h b/sw/airborne/mcu_periph/gpio.h index 1218ae6c05d..0c500b3dd6d 100644 --- a/sw/airborne/mcu_periph/gpio.h +++ b/sw/airborne/mcu_periph/gpio.h @@ -25,6 +25,9 @@ * Some architecture independent helper functions for GPIOs. */ +#ifndef MCU_PERIPH_GPIO_H +#define MCU_PERIPH_GPIO_H + #include "std.h" #include "mcu_periph/gpio_arch.h" @@ -38,3 +41,4 @@ extern void gpio_setup_output(uint32_t port, uint16_t pin); */ extern void gpio_setup_input(uint32_t port, uint16_t pin); +#endif /* MCU_PERIPH_GPIO_H */ diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c new file mode 100644 index 00000000000..9613d4c0959 --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936.c @@ -0,0 +1,359 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/cyrf6936.c + * Driver for the cyrf6936 2.4GHz radio chip + */ + +#include "cyrf6936.h" +#include "mcu_periph/spi.h" +#include "mcu_periph/gpio.h" +#include "mcu_periph/gpio_arch.h" +#include "subsystems/radio_control.h" + +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + +/* Static functions used in the different statuses */ +static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length); +static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr); +static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length); + +//FIXME +void Delay(uint32_t x); +void Delay(uint32_t x) +{ + (void)x; + __asm ("mov r1, #24;" + "mul r0, r0, r1;" + "b _delaycmp;" + "_delayloop:" + "subs r0, r0, #1;" + "_delaycmp:;" + "cmp r0, #0;" + " bne _delayloop;"); +} + +/** + * Initializing the cyrf chip + */ +void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) { + /* Set spi_peripheral and the status */ + cyrf->spi_p = spi_p; + cyrf->status = CYRF6936_UNINIT; + + /* Set the spi transaction */ + cyrf->spi_t.cpol = SPICpolIdleLow; + cyrf->spi_t.cpha = SPICphaEdge1; + cyrf->spi_t.dss = SPIDss8bit; + cyrf->spi_t.bitorder = SPIMSBFirst; + cyrf->spi_t.cdiv = SPIDiv64; + + cyrf->spi_t.input_length = 0; + cyrf->spi_t.output_length = 0; + cyrf->spi_t.input_buf = cyrf->input_buf; + cyrf->spi_t.output_buf = cyrf->output_buf; + cyrf->spi_t.slave_idx = slave_idx; + cyrf->spi_t.select = SPISelectUnselect; + cyrf->spi_t.status = SPITransDone; + + /* Reset the CYRF6936 chip */ + gpio_setup_output(rst_port, rst_pin); + gpio_output_high(rst_port, rst_pin); + Delay(100); + gpio_output_low(rst_port, rst_pin); + Delay(100); + + /* Get the MFG ID */ + cyrf->status = CYRF6936_GET_MFG_ID; + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); +} + +/** + * The on event call for the CYRF6936 chip + */ +void cyrf6936_event(struct Cyrf6936 *cyrf) { + int i; + // Check if cyrf is initialized + if(cyrf->status == CYRF6936_UNINIT) + return; + + // Check if there is still a transaction in progress + if(cyrf->spi_t.status == SPITransPending || cyrf->spi_t.status == SPITransRunning) + return; + + /* Check the status of the cyrf */ + switch (cyrf->status) { + + /* Getting the MFG id */ + case CYRF6936_GET_MFG_ID: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); + break; + case 1: + cyrf6936_read_block(cyrf, CYRF_MFG_ID, 6); + break; + case 2: + // Copy the MFG id + for(i = 0; i < 6; i++) + cyrf->mfg_id[i] = cyrf->input_buf[i+1]; + + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0x00); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* Do a multi write */ + case CYRF6936_MULTIWRITE: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // When we are done writing + if(cyrf->buffer_idx == cyrf->buffer_length) { + cyrf->status = CYRF6936_IDLE; + break; + } + + // Write the next register from the buffer + cyrf6936_write_register(cyrf, + ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][0], + ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][1]); + break; + + /* Do a write of channel, sop, data and crc */ + case CYRF6936_CHAN_SOP_DATA_CRC: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Write the CRC LSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); + break; + case 1: // Write the CRC MSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_MSB, cyrf->buffer[1]); + break; + case 2: // Write the SOP code + cyrf6936_write_block(cyrf, CYRF_SOP_CODE, &(cyrf->buffer[2]), 8); + break; + case 3: // Write the DATA code + cyrf6936_write_block(cyrf, CYRF_DATA_CODE, &(cyrf->buffer[10]), 16); + break; + case 4: // Write the Channel + cyrf6936_write_register(cyrf, CYRF_CHANNEL, cyrf->buffer[26]); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* Do a read of the receive irq status, receive status and the receive packet */ + case CYRF6936_RX_IRQ_STATUS_PACKET: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Read the receive IRQ status + cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); + break; + case 1: // Read the receive status + cyrf->rx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_STATUS); + break; + case 2: // Read the receive packet + cyrf->rx_status = cyrf->input_buf[1]; + cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); + break; + default: + // Copy the receive packet + for(i = 0; i < 16; i++) + cyrf->rx_packet[i] = cyrf->input_buf[i+1]; + + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* This should not happen */ + default: + break; + } +} + +/** + * Write a byte to a register + */ +static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { + return cyrf6936_write_block(cyrf, addr, &data, 1); +} + +/** + * Write multiple bytes to a register + */ +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length) { + uint8_t i; + /* Check if there is already a SPI transaction busy */ + if(cyrf->spi_t.status != SPITransDone) + return FALSE; + + /* Set the buffer and commit the transaction */ + cyrf->spi_t.output_length = length+1; + cyrf->spi_t.input_length = 0; + cyrf->output_buf[0] = addr | CYRF_DIR; + + // Copy the data + for(i = 0; i < length; i++) + cyrf->output_buf[i+1] = data[i]; + + // Submit the transaction + return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); +} + +/** + * Read a byte from a register + */ +static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr) { + return cyrf6936_read_block(cyrf, addr, 1); +} + +/** + * Read multiple bytes from a register + */ +static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length) { + if(cyrf->spi_t.status != SPITransDone) + return FALSE; + + /* Set the buffer and commit the transaction */ + cyrf->spi_t.output_length = 1; + cyrf->spi_t.input_length = length + 1; + cyrf->output_buf[0] = addr; + + // Submit the transaction + return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); +} + + +/** + * Write to multiple registers one byte + */ +bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length) { + uint8_t i; + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_MULTIWRITE; + + /* Set the multi write */ + cyrf->buffer_length = length; + cyrf->buffer_idx = 0; + + // Copy the buffer + for(i = 0; i< length; i++) { + cyrf->buffer[i*2] = data[i][0]; + cyrf->buffer[i*2+1] = data[i][1]; + } + + /* Write the first regiter */ + if(length > 0) + cyrf6936_write_register(cyrf, data[0][0], data[0][1]); + + return TRUE; +} + +/** + * Set the channel, SOP code, DATA code and the CRC seed + */ +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed) { + uint8_t i; + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_CHAN_SOP_DATA_CRC; + + // Copy the CRC + cyrf->buffer[0] = crc_seed & 0xFF; + cyrf->buffer[1] = (crc_seed >> 8) & 0xFF; + + // Copy the SOP code + for(i = 0; i < 8; i++) + cyrf->buffer[i+2] = sop_code[i]; + + // Copy the DATA code + for(i = 0; i < 16; i++) + cyrf->buffer[i+10] = data_code[i]; + + // Copy the channel + cyrf->buffer[26] = chan; + + /* Try to write the CRC LSB */ + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); + + return TRUE; +} + +/** + * Read the RX IRQ status register, the rx status register and the rx packet + */ +bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) { + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_RX_IRQ_STATUS_PACKET; + + /* Try to read the RX status */ + cyrf->buffer_idx = 0; + cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); + + return TRUE; +} diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h new file mode 100644 index 00000000000..b7254ff026e --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936.h @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/yrf6936c.h + * Driver for the cyrf6936 2.4GHz radio chip + */ + +#ifndef CYRF6936_H +#define CYRF6936_H + +#include "cyrf6936_regs.h" +#include "mcu_periph/spi.h" + +#define CYRF6936_MAX_BUFFER 80 /**< The max buffer size in the cyrf6936 structure */ + +/* The different statuses the cyrf6936 chip can be in */ +enum Cyrf6936Status { + CYRF6936_UNINIT, /**< The chip isn't initialized */ + CYRF6936_IDLE, /**< The chip is idle and can be used */ + CYRF6936_GET_MFG_ID, /**< The chip is busy with getting the manufacturer ID */ + CYRF6936_MULTIWRITE, /**< The chip is writing multiple registers */ + CYRF6936_CHAN_SOP_DATA_CRC, /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */ + CYRF6936_RX_IRQ_STATUS_PACKET /**< The chip is getting the receive irq status, receive status and the receive packet */ +}; + +/* The structure for the cyrf6936 chip that handles all the buffers and requests */ +struct Cyrf6936 { + struct spi_periph *spi_p; /**< The SPI peripheral for the connection */ + struct spi_transaction spi_t; /**< The SPI transaction used for the writing and reading of registers */ + volatile enum Cyrf6936Status status; /**< The status of the CYRF6936 chip */ + uint8_t input_buf[17]; /**< The input buffer for the SPI transaction */ + uint8_t output_buf[17]; /**< The output buffer for the SPI transaction */ + + uint8_t buffer[CYRF6936_MAX_BUFFER]; /**< The buffer used to write/read multiple registers */ + uint8_t buffer_length; /**< The length of the buffer used for MULTIWRITE */ + uint8_t buffer_idx; /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */ + + uint8_t mfg_id[6]; /**< The manufacturer id of the CYRF6936 chip */ + uint8_t rx_irq_status; /**< The last receive interrupt status */ + uint8_t rx_status; /**< The last receive status */ + uint8_t rx_packet[16]; /**< The last received packet */ +}; + +extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin); +void cyrf6936_event(struct Cyrf6936 *cyrf); + +bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length); +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed); +bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf); + +#endif /* CYRF6936_H */ diff --git a/sw/airborne/peripherals/cyrf6936_regs.h b/sw/airborne/peripherals/cyrf6936_regs.h new file mode 100644 index 00000000000..c5926771a7e --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936_regs.h @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/cyrf6936_regs.h + * Register defines for the CYRF6936 2.4GHz radio chip + */ + +#ifndef CYRF6936_REGS_H +#define CYRF6936_REGS_H + +/* The SPI interface defines */ +enum { + CYRF_CHANNEL = 0x00, + CYRF_TX_LENGTH = 0x01, + CYRF_TX_CTRL = 0x02, + CYRF_TX_CFG = 0x03, + CYRF_TX_IRQ_STATUS = 0x04, + CYRF_RX_CTRL = 0x05, + CYRF_RX_CFG = 0x06, + CYRF_RX_IRQ_STATUS = 0x07, + CYRF_RX_STATUS = 0x08, + CYRF_RX_COUNT = 0x09, + CYRF_RX_LENGTH = 0x0A, + CYRF_PWR_CTRL = 0x0B, + CYRF_XTAL_CTRL = 0x0C, + CYRF_IO_CFG = 0x0D, + CYRF_GPIO_CTRL = 0x0E, + CYRF_XACT_CFG = 0x0F, + CYRF_FRAMING_CFG = 0x10, + CYRF_DATA32_THOLD = 0x11, + CYRF_DATA64_THOLD = 0x12, + CYRF_RSSI = 0x13, + CYRF_EOP_CTRL = 0x14, + CYRF_CRC_SEED_LSB = 0x15, + CYRF_CRC_SEED_MSB = 0x16, + CYRF_TX_CRC_LSB = 0x17, + CYRF_TX_CRC_MSB = 0x18, + CYRF_RX_CRC_LSB = 0x19, + CYRF_RX_CRC_MSB = 0x1A, + CYRF_TX_OFFSET_LSB = 0x1B, + CYRF_TX_OFFSET_MSB = 0x1C, + CYRF_MODE_OVERRIDE = 0x1D, + CYRF_RX_OVERRIDE = 0x1E, + CYRF_TX_OVERRIDE = 0x1F, + CYRF_TX_BUFFER = 0x20, + CYRF_RX_BUFFER = 0x21, + CYRF_SOP_CODE = 0x22, + CYRF_DATA_CODE = 0x23, + CYRF_PREAMBLE = 0x24, + CYRF_MFG_ID = 0x25, + CYRF_XTAL_CFG = 0x26, + CYRF_CLK_OFFSET = 0x27, + CYRF_CLK_EN = 0x28, + CYRF_RX_ABORT = 0x29, + CYRF_AUTO_CAL_TIME = 0x32, + CYRF_AUTO_CAL_OFFSET = 0x35, + CYRF_ANALOG_CTRL = 0x39, +}; +#define CYRF_DIR (1<<7) /**< Bit for enabling writing */ + +// CYRF_MODE_OVERRIDE +#define CYRF_RST (1<<0) + +// CYRF_CLK_EN +#define CYRF_RXF (1<<1) + +// CYRF_XACT_CFG +enum { + CYRF_MODE_SLEEP = (0x0 <<2), + CYRF_MODE_IDLE = (0x1 <<2), + CYRF_MODE_SYNTH_TX = (0x2 <<2), + CYRF_MODE_SYNTH_RX = (0x3 <<2), + CYRF_MODE_RX = (0x4 <<2), +}; +#define CYRF_FRC_END (1<<5) +#define CYRF_ACK_EN (1<<7) + +// CYRF_IO_CFG +#define CYRF_IRQ_GPIO (1<<0) +#define CYRF_SPI_3PIN (1<<1) +#define CYRF_PACTL_GPIO (1<<2) +#define CYRF_PACTL_OD (1<<3) +#define CYRF_XOUT_OD (1<<4) +#define CYRF_MISO_OD (1<<5) +#define CYRF_IRQ_POL (1<<6) +#define CYRF_IRQ_OD (1<<7) + +// CYRF_FRAMING_CFG +#define CYRF_LEN_EN (1<<5) +#define CYRF_SOP_LEN (1<<6) +#define CYRF_SOP_EN (1<<7) + +// CYRF_RX_STATUS +enum { + CYRF_RX_DATA_MODE_GFSK = 0x00, + CYRF_RX_DATA_MODE_8DR = 0x01, + CYRF_RX_DATA_MODE_DDR = 0x10, + CYRF_RX_DATA_MODE_NV = 0x11, +}; +#define CYRF_RX_CODE (1<<2) +#define CYRF_BAD_CRC (1<<3) +#define CYRF_CRC0 (1<<4) +#define CYRF_EOP_ERR (1<<5) +#define CYRF_PKT_ERR (1<<6) +#define CYRF_RX_ACK (1<<7) + +// CYRF_TX_IRQ_STATUS +#define CYRF_TXE_IRQ (1<<0) +#define CYRF_TXC_IRQ (1<<1) +#define CYRF_TXBERR_IRQ (1<<2) +#define CYRF_TXB0_IRQ (1<<3) +#define CYRF_TXB8_IRQ (1<<4) +#define CYRF_TXB15_IRQ (1<<5) +#define CYRF_LV_IRQ (1<<6) +#define CYRF_OS_IRQ (1<<7) + +// CYRF_RX_IRQ_STATUS +#define CYRF_RXE_IRQ (1<<0) +#define CYRF_RXC_IRQ (1<<1) +#define CYRF_RXBERR_IRQ (1<<2) +#define CYRF_RXB1_IRQ (1<<3) +#define CYRF_RXB8_IRQ (1<<4) +#define CYRF_RXB16_IRQ (1<<5) +#define CYRF_SOPDET_IRQ (1<<6) +#define CYRF_RXOW_IRQ (1<<7) + +// CYRF_TX_CTRL +#define CYRF_TXE_IRQEN (1<<0) +#define CYRF_TXC_IRQEN (1<<1) +#define CYRF_TXBERR_IRQEN (1<<2) +#define CYRF_TXB0_IRQEN (1<<3) +#define CYRF_TXB8_IRQEN (1<<4) +#define CYRF_TXB15_IRQEN (1<<5) +#define CYRF_TX_CLR (1<<6) +#define CYRF_TX_GO (1<<7) + +// CYRF_RX_CTRL +#define CYRF_RXE_IRQEN (1<<0) +#define CYRF_RXC_IRQEN (1<<1) +#define CYRF_RXBERR_IRQEN (1<<2) +#define CYRF_RXB1_IRQEN (1<<3) +#define CYRF_RXB8_IRQEN (1<<4) +#define CYRF_RXB16_IRQEN (1<<5) +#define CYRF_RSVD (1<<6) +#define CYRF_RX_GO (1<<7) + +// CYRF_RX_OVERRIDE +#define CYRF_ACE (1<<1) +#define CYRF_DIS_RXCRC (1<<2) +#define CYRF_DIS_CRC0 (1<<3) +#define CYRF_FRC_RXDR (1<<4) +#define CYRF_MAN_RXACK (1<<5) +#define CYRF_RXTX_DLY (1<<6) +#define CYRF_ACK_RX (1<<7) + +// CYRF_TX_OVERRIDE +#define CYRF_TX_INV (1<<0) +#define CYRF_DIS_TXCRC (1<<2) +#define CYRF_OVRD_ACK (1<<3) +#define CYRF_MAN_TXACK (1<<4) +#define CYRF_FRC_PRE (1<<6) +#define CYRF_ACK_TX (1<<7) + +// CYRF_RX_CFG +#define CYRF_VLD_EN (1<<0) +#define CYRF_RXOW_EN (1<<1) +#define CYRF_FAST_TURN_EN (1<<3) +#define CYRF_HILO (1<<4) +#define CYRF_ATT (1<<5) +#define CYRF_LNA (1<<6) +#define CYRF_AGC_EN (1<<7) + +// CYRF_TX_CFG +enum { + CYRF_PA_M35 = 0x0, + CYRF_PA_M30 = 0x1, + CYRF_PA_M24 = 0x2, + CYRF_PA_M18 = 0x3, + CYRF_PA_M13 = 0x4, + CYRF_PA_M5 = 0x5, + CYRF_PA_0 = 0x6, + CYRF_PA_4 = 0x7, +}; +enum { + CYRF_DATA_MODE_GFSK = (0x0 <<3), + CYRF_DATA_MODE_8DR = (0x1 <<3), + CYRF_DATA_MODE_DDR = (0x2 <<3), + CYRF_DATA_MODE_SDR = (0x3 <<3), +}; +#define CYRF_DATA_CODE_LENGTH (1<<5) + +#endif // CYRF6936_REGS_H diff --git a/sw/airborne/subsystems/radio_control/superbitrf.c b/sw/airborne/subsystems/radio_control/superbitrf.c new file mode 100644 index 00000000000..8f51d733eb0 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf.c @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/radio_control/superbitrf.c + * DSM2 and DSMX implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#include "superbitrf.h" + +#include "subsystems/radio_control.h" +#include "mcu_periph/spi.h" +#include + +/* Default SuperbitRF SPI DEV */ +#ifndef SUPERBITRF_SPI_DEV +#define SUPERBITRF_SPI_DEV spi1 +#endif + +/* Default SuperbitRF RST PORT and PIN */ +#ifndef SUPERBITRF_RST_PORT +#define SUPERBITRF_RST_PORT GPIOC +#endif +#ifndef SUPERBITRF_RST_PIN +#define SUPERBITRF_RST_PIN GPIO12 +#endif + +/* Default SuperbitRF DRDY(IRQ) PORT and PIN */ +#ifndef SUPERBITRF_DRDY_PORT +#define SUPERBITRF_DRDY_PORT GPIOB +#endif +#ifndef SUPERBITRF_DRDY_PIN +#define SUPERBITRF_DRDY_PIN GPIO1 +#endif + +/* The superbitRF structure */ +struct SuperbitRF superbitrf; + +/* The startup configuration for the cyrf6936 */ +static const uint8_t cyrf_stratup_config[][2] = { + {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device + {CYRF_CLK_EN, CYRF_RXF}, // Enable the clock + {CYRF_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization + {CYRF_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization + {CYRF_RX_CFG, CYRF_LNA | CYRF_FAST_TURN_EN}, // Enable low noise amplifier and fast turning + {CYRF_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration + {CYRF_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, // Force in Synth RX mode + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_DATA64_THOLD, 0x0E}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX}, // Set in Synth RX mode (again, really needed?) +}; +/* The binding configuration for the cyrf6936 */ +static const uint8_t cyrf_bind_config[][2] = { + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE + {CYRF_RX_OVERRIDE, CYRF_FRC_RXDR | CYRF_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker + {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 + {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate +}; + +/** + * Initialize the superbitrf + */ +void radio_control_impl_init(void) { + // Set the status to uninitialized + superbitrf.status = SUPERBITRF_UNINIT; + + // Initialize the binding pin + gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); + + // Initialize the cyrf6936 chip + cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 1, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN); +} + +/** + * The superbitrf on event call + */ +void superbitrf_event(void) { + // Check the status + switch(superbitrf.status) { + + /* When the superbitrf isn't initialized */ + case SUPERBITRF_UNINIT: + // Try to write the startup config + if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) { + // Check if need to go to bind or transfer + if(gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) + superbitrf.status = SUPERBITRF_INIT_BINDING; + else + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + } + break; + + /* When the superbitrf is initializing binding */ + case SUPERBITRF_INIT_BINDING: + // Try to write the binding config + if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5)) + superbitrf.status = SUPERBITRF_BINDING; + break; + + /* When the superbitrf is in binding mode */ + case SUPERBITRF_BINDING: + break; + + /* Should not come here */ + default: + break; + } + +} diff --git a/sw/airborne/subsystems/radio_control/superbitrf.h b/sw/airborne/subsystems/radio_control/superbitrf.h new file mode 100644 index 00000000000..a9f7e20dcfb --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf.h @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file subsystems/radio_control/superbitrf.h + * DSM2 and DSMX implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#ifndef RADIO_CONTROL_SUPERBITRF_H +#define RADIO_CONTROL_SUPERBITRF_H + +#include "peripherals/cyrf6936.h" +#include "mcu_periph/gpio.h" + +/* Theoretically you could have 14 channel over DSM2/DSMX */ +#ifndef RADIO_CONTROL_NB_CHANNEL +#define RADIO_CONTROL_NB_CHANNEL 14 +#endif + +/* The channel ordering is always the same for DSM2 and DSMX */ +#define RADIO_THROTTLE 0 +#define RADIO_ROLL 1 +#define RADIO_PITCH 2 +#define RADIO_YAW 3 +#define RADIO_GEAR 4 +#define RADIO_FLAP 5 +#define RADIO_AUX1 5 +#define RADIO_AUX2 6 +#define RADIO_AUX3 7 +#define RADIO_AUX4 8 +#define RADIO_AUX5 9 +#define RADIO_AUX6 10 +#define RADIO_AUX7 11 +#define RADIO_AUX8 12 +#define RADIO_AUX9 13 + +/* Map the MODE default to the gear switch */ +#ifndef RADIO_MODE +#define RADIO_MODE RADIO_GEAR +#endif + +/* The different statuses the superbitRF can be in */ +enum SuperbitRFStatus { + SUPERBITRF_UNINIT, /**< The chip isn't initialized */ + SUPERBITRF_INIT_BINDING, /**< The chip is initializing binding mode */ + SUPERBITRF_INIT_TRANSFER, /**< The chip is initializing transfer mode */ + SUPERBITRF_BINDING, /**< The chip is in binding mode */ + SUPERBITRF_TRANSFER, /**< The chip is in transfer mode */ +}; + +/* The superbitrf structure */ +struct SuperbitRF { + struct Cyrf6936 cyrf6936; /**< The cyrf chip used */ + volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ +}; + +/* The superbitrf functions and structures */ +extern struct SuperbitRF superbitrf; +void superbitrf_event(void); + +/* The radio control event handler */ +#define RadioControlEvent(_received_frame_handler) { \ + cyrf6936_event(&superbitrf.cyrf6936); \ + superbitrf_event(); \ +} + +#endif /* RADIO_CONTROL_SUPERBITRF_H */ From 55add10b2fb86c5b30378699e0bc69b5f980ffcf Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 3 Jul 2013 16:54:58 +0200 Subject: [PATCH 16/65] [superbitrf] Now got a full RC working, still needs testing. Binding could be improved but already works about 30% of the times you try. --- conf/messages.xml | 4 +- conf/telemetry/default_rotorcraft.xml | 2 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 2 + sw/airborne/peripherals/cyrf6936.c | 37 +- sw/airborne/peripherals/cyrf6936.h | 3 + .../subsystems/radio_control/superbitrf.c | 393 +++++++++++++++++- .../subsystems/radio_control/superbitrf.h | 61 ++- 7 files changed, 474 insertions(+), 28 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 2999fcab523..6c9b867c8cd 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -604,8 +604,10 @@ - + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 0ed6ab52aef..b647b9c609d 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -18,6 +18,7 @@ + @@ -26,7 +27,6 @@ - diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index e08247b3f62..afed5e1137e 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -140,6 +140,8 @@ DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ &superbitrf.status, \ &superbitrf.cyrf6936.status, \ + &superbitrf.packet_count, \ + &superbitrf.packet_count, \ 6, \ superbitrf.cyrf6936.mfg_id);} #else diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 9613d4c0959..996285c5f23 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -28,6 +28,7 @@ #include "mcu_periph/spi.h" #include "mcu_periph/gpio.h" #include "mcu_periph/gpio_arch.h" +#include "mcu_periph/sys_time.h" #include "subsystems/radio_control.h" #include "mcu_periph/uart.h" @@ -40,21 +41,6 @@ static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, co static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr); static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length); -//FIXME -void Delay(uint32_t x); -void Delay(uint32_t x) -{ - (void)x; - __asm ("mov r1, #24;" - "mul r0, r0, r1;" - "b _delaycmp;" - "_delayloop:" - "subs r0, r0, #1;" - "_delaycmp:;" - "cmp r0, #0;" - " bne _delayloop;"); -} - /** * Initializing the cyrf chip */ @@ -62,6 +48,7 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_ /* Set spi_peripheral and the status */ cyrf->spi_p = spi_p; cyrf->status = CYRF6936_UNINIT; + cyrf->has_packet = FALSE; /* Set the spi transaction */ cyrf->spi_t.cpol = SPICpolIdleLow; @@ -78,12 +65,12 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_ cyrf->spi_t.select = SPISelectUnselect; cyrf->spi_t.status = SPITransDone; - /* Reset the CYRF6936 chip */ + /* Reset the CYRF6936 chip (busy waiting) */ gpio_setup_output(rst_port, rst_pin); gpio_output_high(rst_port, rst_pin); - Delay(100); + sys_time_usleep(100); gpio_output_low(rst_port, rst_pin); - Delay(100); + sys_time_usleep(100); /* Get the MFG ID */ cyrf->status = CYRF6936_GET_MFG_ID; @@ -156,6 +143,10 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) { ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][1]); break; + /* Do a write of the data code */ + case CYRF6936_DATA_CODE: + break; + /* Do a write of channel, sop, data and crc */ case CYRF6936_CHAN_SOP_DATA_CRC: // When the last transaction isn't failed send the next @@ -213,6 +204,7 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) { for(i = 0; i < 16; i++) cyrf->rx_packet[i] = cyrf->input_buf[i+1]; + cyrf->has_packet = TRUE; cyrf->status = CYRF6936_IDLE; break; } @@ -276,6 +268,15 @@ static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, con return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); } +/** + * Write to one register + */ +bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { + const uint8_t data_multi[][2] = { + {addr, data} + }; + return cyrf6936_multi_write(cyrf, data_multi, 1); +} /** * Write to multiple registers one byte diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h index b7254ff026e..7a7943f5666 100644 --- a/sw/airborne/peripherals/cyrf6936.h +++ b/sw/airborne/peripherals/cyrf6936.h @@ -38,6 +38,7 @@ enum Cyrf6936Status { CYRF6936_IDLE, /**< The chip is idle and can be used */ CYRF6936_GET_MFG_ID, /**< The chip is busy with getting the manufacturer ID */ CYRF6936_MULTIWRITE, /**< The chip is writing multiple registers */ + CYRF6936_DATA_CODE, /**< The chip is writing a data code */ CYRF6936_CHAN_SOP_DATA_CRC, /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */ CYRF6936_RX_IRQ_STATUS_PACKET /**< The chip is getting the receive irq status, receive status and the receive packet */ }; @@ -54,6 +55,7 @@ struct Cyrf6936 { uint8_t buffer_length; /**< The length of the buffer used for MULTIWRITE */ uint8_t buffer_idx; /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */ + bool_t has_packet; /**< When the CYRF6936 is done reading the receive registers */ uint8_t mfg_id[6]; /**< The manufacturer id of the CYRF6936 chip */ uint8_t rx_irq_status; /**< The last receive interrupt status */ uint8_t rx_status; /**< The last receive status */ @@ -63,6 +65,7 @@ struct Cyrf6936 { extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin); void cyrf6936_event(struct Cyrf6936 *cyrf); +bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length); bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed); bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf); diff --git a/sw/airborne/subsystems/radio_control/superbitrf.c b/sw/airborne/subsystems/radio_control/superbitrf.c index 8f51d733eb0..510a16ee0b7 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.c +++ b/sw/airborne/subsystems/radio_control/superbitrf.c @@ -26,8 +26,10 @@ #include "superbitrf.h" +#include #include "subsystems/radio_control.h" #include "mcu_periph/spi.h" +#include "mcu_periph/sys_time.h" #include /* Default SuperbitRF SPI DEV */ @@ -54,6 +56,10 @@ /* The superbitRF structure */ struct SuperbitRF superbitrf; +/* The internal functions */ +static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[]); +static inline void superbitrf_radio_to_channels(char* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); + /* The startup configuration for the cyrf6936 */ static const uint8_t cyrf_stratup_config[][2] = { {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device @@ -76,17 +82,100 @@ static const uint8_t cyrf_bind_config[][2] = { {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate }; +/* The transfer configuration for the cyrf6936 */ +static const uint8_t cyrf_transfer_config[][2] = { + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | CYRF_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_EN | CYRF_SOP_LEN | CYRF_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE + {CYRF_TX_OVERRIDE, 0x00}, // Reset TX overrides + {CYRF_RX_OVERRIDE, 0x00}, // Reset RX overrides +}; +/* Abort the receive of the cyrf6936 */ +const uint8_t cyrf_abort_receive[][2] = { + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, + {CYRF_RX_ABORT, 0x00} +}; +/* Start the receive of the cyrf6936 */ +const uint8_t cyrf_start_receive[][2] = { + {CYRF_RX_IRQ_STATUS, CYRF_RXOW_IRQ}, // Clear the IRQ + {CYRF_RX_CTRL, CYRF_RX_GO | CYRF_RXC_IRQEN | CYRF_RXE_IRQEN} // Start receiving and set the IRQ +}; + +/* The PN codes used for DSM2 and DSMX channel hopping */ +static const uint8_t pn_codes[5][9][8] = { +{ /* Row 0 */ + /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, + /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, + /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, + /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, + /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, + /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, + /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, + /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, + /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} +}, +{ /* Row 1 */ + /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, + /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, + /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, + /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, + /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, + /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, + /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, + /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, + /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} +}, +{ /* Row 2 */ + /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, + /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, + /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, + /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, + /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, + /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, + /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, + /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, + /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} +}, +{ /* Row 3 */ + /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, + /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, + /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, + /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, + /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, + /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, + /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, + /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, + /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} +}, +{ /* Row 4 */ + /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, + /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, + /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, + /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, + /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, + /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, + /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, + /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, + /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} +}, +}; +static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x84 }; /** * Initialize the superbitrf */ void radio_control_impl_init(void) { - // Set the status to uninitialized + // Set the status to uninitialized and set the timer to 0 superbitrf.status = SUPERBITRF_UNINIT; + superbitrf.state = 0; + superbitrf.timer = 0; + superbitrf.packet_count = 0; // Initialize the binding pin gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); + // Initialize the IRQ/DRDY pin + gpio_setup_input(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN); + // Initialize the cyrf6936 chip cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 1, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN); } @@ -95,7 +184,30 @@ void radio_control_impl_init(void) { * The superbitrf on event call */ void superbitrf_event(void) { - // Check the status + uint8_t data_code[16]; + + // Check if the cyrf6936 isn't busy + if(superbitrf.cyrf6936.status != CYRF6936_IDLE) + return; + + // First handle the IRQ + if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { + // Receive the packet + cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); + } + + /* Check if it is a valid receive */ + if(superbitrf.cyrf6936.has_packet && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { + superbitrf.packet_count++; + + // Handle the packet received + superbitrf_receive_packet_cb(superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); + + // Reset the packet receiving + superbitrf.cyrf6936.has_packet = FALSE; + } + + // Check the status of the superbitrf switch(superbitrf.status) { /* When the superbitrf isn't initialized */ @@ -112,18 +224,291 @@ void superbitrf_event(void) { /* When the superbitrf is initializing binding */ case SUPERBITRF_INIT_BINDING: - // Try to write the binding config - if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5)) + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // Try to write the binding config + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5); + superbitrf.state++; + break; + case 1: + // Set the data code and channel + memcpy(data_code, pn_codes[0][8], 8); + memcpy(data_code + 8, pn_bind, 8); + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, 1, pn_codes[0][0], data_code, 0x0000); + superbitrf.status = SUPERBITRF_BINDING; + break; + default: + // Should not happen + superbitrf.state = 0; + break; + } break; + /* When the superbitrf is initializing transfer */ + case SUPERBITRF_INIT_TRANSFER: + // Try to write the transfer config + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4); + superbitrf.status = SUPERBITRF_SYNCING_A; + break; + /* When the superbitrf is in binding mode */ case SUPERBITRF_BINDING: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) { + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + } + break; + case 1: + // Switch channel + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define + cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel); + + superbitrf.state++; + break; + case 2: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } + break; + + /* When the receiver is synchronizing with the transmitter */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) { + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + } + break; + case 1: + // Switch channel, sop code, data code and crc + superbitrf.channel = (superbitrf.channel + 1) % 0x4F; //TODO fix define + superbitrf.crc_seed = ~ superbitrf.crc_seed; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[superbitrf.channel % 5][superbitrf.sop_col], + pn_codes[superbitrf.channel % 5][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 2: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_SYNC_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } + break; + + /* Normal transfer mode */ + case SUPERBITRF_TRANSFER: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.status = SUPERBITRF_SYNCING_A; + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 2: + // Switch channel, sop code, data code and crc + superbitrf.channel_idx = (superbitrf.channel_idx + 1) %2; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + superbitrf.crc_seed = ~ superbitrf.crc_seed; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[superbitrf.channel % 5][superbitrf.sop_col], + pn_codes[superbitrf.channel % 5][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 3: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } break; /* Should not come here */ default: break; } +} + +/** + * When we receive a packet this callback is called + */ +static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[]) { + int i; + uint16_t sum; + + /* Switch on the status of the superbitRF */ + switch (superbitrf.status) { + + /* When we are in binding mode */ + case SUPERBITRF_BINDING: + // Check if the MFG id is exactly the same + if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) { + // Start receiving without changing channel + superbitrf.state = 2; + break; + } + // Calculate the first sum + sum = 384 - 0x10; + for (i = 0; i < 8; i++) + sum += packet[i]; + + // Check the first sum + if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 2; + break; + } + + // Calculate second sum + for (i = 8; i < 14; i++) + sum += packet[i]; + + // Check the second sum + if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 2; + break; + } + + // Update the mfg id, number of channels and protocol + superbitrf.bind_mfg_id[0] = ~packet[0]; + superbitrf.bind_mfg_id[1] = ~packet[1]; + superbitrf.bind_mfg_id[2] = ~packet[2]; + superbitrf.bind_mfg_id[3] = ~packet[3]; + superbitrf.num_channels = packet[11]; + superbitrf.protocol = packet[12]; + superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; + + // Calculate some values based on the bind MFG id + superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]); + superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07; + superbitrf.data_col = 7 - superbitrf.sop_col; + + // Update the status of the receiver + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + break; + + /* When we receive a packet during syncing first channel A */ + case SUPERBITRF_SYNCING_A: + // Check the MFG id + if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + break; + + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channels[1] = superbitrf.channel; + + superbitrf.status = SUPERBITRF_SYNCING_B; //TODO: Fix it + break; + + /* When we receive a packet during syncing second channel B */ + case SUPERBITRF_SYNCING_B: + // Check the MFG id + if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + break; + + // Set the channel + if (superbitrf.crc_seed == ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channel_idx = 0; + } + else { + superbitrf.channels[1] = superbitrf.channel; + superbitrf.channel_idx = 1; + } + + // When the channels aren't the same go to transfer mode + if(superbitrf.channels[1] != superbitrf.channels[0]) { + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + } + break; + + /* When we receive a packet during transfer */ + case SUPERBITRF_TRANSFER: + // Check the MFG id + if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + break; + + // Parse the packet + superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); + superbitrf.frame_available = TRUE; + + // Go to next receive + superbitrf.state = 1; + break; + + /* Should not come here */ + default: + break; + } +} + +/** + * Parse a radio channel packet + */ +static inline void superbitrf_radio_to_channels(char* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) { + int i; + uint8_t bit_shift = (is_11bit)? 11:10; + int16_t value_max = (is_11bit)? 0x07FF: 0x03FF; + + for (i=0; i<7; i++) { + const int16_t tmp = ((data[2*i]<<8) + data[2*i+1]) & 0x7FFF; + const uint8_t chan = (tmp >> bit_shift) & 0x0F; + const int16_t val = (tmp&value_max); + + if(chan < nb_channels) { + channels[chan] = val; + + // Scale the channel + if(is_11bit) { + channels[chan] -= 0x400; + channels[chan] *= MAX_PPRZ/0x2AC; + } else { + channels[chan] -= 0x200; + channels[chan] *= MAX_PPRZ/0x156; + } + } + } } diff --git a/sw/airborne/subsystems/radio_control/superbitrf.h b/sw/airborne/subsystems/radio_control/superbitrf.h index a9f7e20dcfb..65a6771b6cb 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.h +++ b/sw/airborne/subsystems/radio_control/superbitrf.h @@ -17,7 +17,6 @@ * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. - * */ /** @@ -58,29 +57,83 @@ #define RADIO_MODE RADIO_GEAR #endif +/* The timings in microseconds */ +#define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ +#define SUPERBITRF_SYNC_RECV_TIME 12000 /**< The time to wait for a sync packet on a channel in microseconds */ +#define SUPERBITRF_RECV_TIME 24000 /**< The time to wait for a transfer packet on a channel in microseconds */ + /* The different statuses the superbitRF can be in */ enum SuperbitRFStatus { SUPERBITRF_UNINIT, /**< The chip isn't initialized */ SUPERBITRF_INIT_BINDING, /**< The chip is initializing binding mode */ SUPERBITRF_INIT_TRANSFER, /**< The chip is initializing transfer mode */ SUPERBITRF_BINDING, /**< The chip is in binding mode */ + SUPERBITRF_SYNCING_A, /**< The chip is in synchronizing mode for channel A */ + SUPERBITRF_SYNCING_B, /**< The chip is in synchronizing mode for channel B */ SUPERBITRF_TRANSFER, /**< The chip is in transfer mode */ }; +/* The different resolutions a transmitter can be in */ +enum dsm_resolution { + SUPERBITRF_10_BIT_RESOLUTION = 0x00, /**< The transmitter has a 10 bit resolution */ + SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */ +}; + /* The superbitrf structure */ struct SuperbitRF { struct Cyrf6936 cyrf6936; /**< The cyrf chip used */ volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ + uint8_t state; /**< The states each status can be in */ + uint32_t timer; /**< The timer in usec */ + + uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ + uint8_t channel_idx; /**< The current channel index */ + uint8_t channel; /**< The current channel number */ + uint32_t packet_count; /**< How many packets are received(also the invalid) */ + + uint8_t bind_mfg_id[4]; /**< The MFG id where the receiver is bound to */ + uint8_t num_channels; /**< The number of channels the transmitter has */ + uint8_t protocol; /**< The protocol the transmitter uses */ + volatile enum dsm_resolution resolution; /**< The resolution that the transmitter has */ + uint16_t crc_seed; /**< The CRC seed that is calculated with the bind MFG id */ + uint8_t sop_col; /**< The sop code column number calculated with the bind MFG id */ + uint8_t data_col; /**< The data code column number calculated with the bind MFG id */ + + bool_t frame_available; /**< When a frame is available */ + int16_t rc_values[14]; /**< The rc values from the packet */ }; /* The superbitrf functions and structures */ extern struct SuperbitRF superbitrf; void superbitrf_event(void); +/* Macro that normalize superbitrf rc_values to radio values */ +#define NormalizeRcDl(_in, _out, _count) { \ + uint8_t i; \ + for(i = 0; i < _count; i++) { \ + if(i == RADIO_THROTTLE) { \ + _out[i] = (_in[i] + MAX_PPRZ) / 2; \ + Bound(_out[i], 0, MAX_PPRZ); \ + } else { \ + _out[i] = -_in[i]; \ + Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ + } \ + } \ +} + /* The radio control event handler */ -#define RadioControlEvent(_received_frame_handler) { \ - cyrf6936_event(&superbitrf.cyrf6936); \ - superbitrf_event(); \ +#define RadioControlEvent(_received_frame_handler) { \ + cyrf6936_event(&superbitrf.cyrf6936); \ + superbitrf_event(); \ + if(superbitrf.frame_available) { \ + radio_control.frame_cpt++; \ + radio_control.time_since_last_frame = 0; \ + radio_control.radio_ok_cpt = 0; \ + radio_control.status = RC_OK; \ + NormalizeRcDl(superbitrf.rc_values,radio_control.values); \ + _received_frame_handler(); \ + superbitrf.frame_available = FALSE; \ + } \ } #endif /* RADIO_CONTROL_SUPERBITRF_H */ From 7c69924c1c94d29bad5e105b152a2fdc147b138b Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 3 Jul 2013 17:20:41 +0200 Subject: [PATCH 17/65] [superbitrf] Less RC Lost and fixed message Implemented to make it only resyncing when it lost 2 packets of data. Also fixed the message to show the correct status of the CYRF6936. --- conf/messages.xml | 2 +- sw/airborne/subsystems/radio_control/superbitrf.c | 11 ++++++++++- sw/airborne/subsystems/radio_control/superbitrf.h | 1 + 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 6c9b867c8cd..6d9c56b387a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -605,7 +605,7 @@ - + diff --git a/sw/airborne/subsystems/radio_control/superbitrf.c b/sw/airborne/subsystems/radio_control/superbitrf.c index 510a16ee0b7..8ccd83521f3 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.c +++ b/sw/airborne/subsystems/radio_control/superbitrf.c @@ -329,8 +329,16 @@ void superbitrf_event(void) { switch (superbitrf.state) { case 0: // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.timeouts++; + superbitrf.state++; + } + + // We really lost it + if(superbitrf.timeouts > 1) { + superbitrf.state = 0; superbitrf.status = SUPERBITRF_SYNCING_A; + } break; case 1: // Abort the receive @@ -477,6 +485,7 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] // Go to next receive superbitrf.state = 1; + superbitrf.timeouts = 0; break; /* Should not come here */ diff --git a/sw/airborne/subsystems/radio_control/superbitrf.h b/sw/airborne/subsystems/radio_control/superbitrf.h index 65a6771b6cb..d6d408f7b22 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.h +++ b/sw/airborne/subsystems/radio_control/superbitrf.h @@ -85,6 +85,7 @@ struct SuperbitRF { volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ uint8_t state; /**< The states each status can be in */ uint32_t timer; /**< The timer in usec */ + uint8_t timeouts; /**< The amount of timeouts */ uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ uint8_t channel_idx; /**< The current channel index */ From 66c80469a431e7b1443dbf772ebfda695e464117 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Tue, 9 Jul 2013 16:17:44 +0200 Subject: [PATCH 18/65] [superbitrf] Downlink is now functional Still needs some extensive testing to make it more relaible. --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 44 ++--- ...e => radio_control_superbitrf_rc.makefile} | 7 +- .../shared/telemetry_superbitrf.makefile | 11 ++ conf/messages.xml | 4 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 8 +- sw/airborne/peripherals/cyrf6936.c | 65 ++++++- sw/airborne/peripherals/cyrf6936.h | 9 +- sw/airborne/subsystems/datalink/datalink.h | 3 +- sw/airborne/subsystems/datalink/downlink.h | 1 + .../{radio_control => datalink}/superbitrf.c | 183 +++++++++++++----- .../{radio_control => datalink}/superbitrf.h | 96 +++------ .../subsystems/radio_control/superbitrf_rc.c | 37 ++++ .../subsystems/radio_control/superbitrf_rc.h | 89 +++++++++ 13 files changed, 405 insertions(+), 152 deletions(-) rename conf/firmwares/subsystems/shared/{radio_control_superbitrf.makefile => radio_control_superbitrf_rc.makefile} (57%) create mode 100644 conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile rename sw/airborne/subsystems/{radio_control => datalink}/superbitrf.c (75%) rename sw/airborne/subsystems/{radio_control => datalink}/superbitrf.h (55%) create mode 100644 sw/airborne/subsystems/radio_control/superbitrf_rc.c create mode 100644 sw/airborne/subsystems/radio_control/superbitrf_rc.h diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 3244ea3474e..4d08f9e5ec5 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -12,7 +12,7 @@ - + @@ -28,7 +28,7 @@ - + @@ -39,10 +39,10 @@ - - - - + + + + @@ -59,9 +59,9 @@ - - - + + + @@ -88,7 +88,7 @@ - +
@@ -153,22 +153,22 @@ - - - + + + - - - + + + - - - + + + - - - + + +
diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile similarity index 57% rename from conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile rename to conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile index 791751efde8..3774d22f3bf 100644 --- a/conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile @@ -2,13 +2,14 @@ # Makefile for shared radio_control superbitrf subsystem # -ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf.h\" -ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 +ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf_rc.h\" +ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1 ifneq ($(RADIO_CONTROL_LED),none) ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) endif ap.srcs += peripherals/cyrf6936.c \ + $(SRC_SUBSYSTEMS)/datalink/superbitrf.c\ $(SRC_SUBSYSTEMS)/radio_control.c \ - $(SRC_SUBSYSTEMS)/radio_control/superbitrf.c + $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c diff --git a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile new file mode 100644 index 00000000000..20781731ecf --- /dev/null +++ b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile @@ -0,0 +1,11 @@ +# +# The superbitRF module as telemetry downlink/uplink +# +# + +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF +ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1 +ap.srcs += peripherals/cyrf6936.c +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c +ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c diff --git a/conf/messages.xml b/conf/messages.xml index 6d9c56b387a..f2199afb15a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -605,8 +605,10 @@ - + + + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index afed5e1137e..80f5ab8366e 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -134,14 +134,16 @@ #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif -#ifdef RADIO_CONTROL_TYPE_SUPERBITRF -#include "subsystems/radio_control/superbitrf.h" +#ifdef USE_SUPERBITRF +#include "subsystems/datalink/superbitrf.h" #define PERIODIC_SEND_SUPERBITRF(_trans, _dev) { \ DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ &superbitrf.status, \ &superbitrf.cyrf6936.status, \ &superbitrf.packet_count, \ - &superbitrf.packet_count, \ + &superbitrf.timing1, \ + &superbitrf.timing2, \ + &superbitrf.bind_mfg_id32, \ 6, \ superbitrf.cyrf6936.mfg_id);} #else diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 996285c5f23..1ea90971df9 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -48,7 +48,7 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_ /* Set spi_peripheral and the status */ cyrf->spi_p = spi_p; cyrf->status = CYRF6936_UNINIT; - cyrf->has_packet = FALSE; + cyrf->has_irq = FALSE; /* Set the spi transaction */ cyrf->spi_t.cpol = SPICpolIdleLow; @@ -191,11 +191,15 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) { case 0: // Read the receive IRQ status cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); break; - case 1: // Read the receive status + case 1: // Read the send IRQ status cyrf->rx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS); + break; + case 2: // Read the receive status + cyrf->tx_irq_status = cyrf->input_buf[1]; cyrf6936_read_register(cyrf, CYRF_RX_STATUS); break; - case 2: // Read the receive packet + case 3: // Read the receive packet cyrf->rx_status = cyrf->input_buf[1]; cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); break; @@ -204,7 +208,35 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) { for(i = 0; i < 16; i++) cyrf->rx_packet[i] = cyrf->input_buf[i+1]; - cyrf->has_packet = TRUE; + cyrf->has_irq = TRUE; + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* The CYRF6936 is busy sending a packet */ + case CYRF6936_SEND: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Set the packet length + cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); + break; + case 1: // Clear the TX buffer + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR); + break; + case 2: // Write the send packet + cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16); + break; + case 3: // Send the packet + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN); + break; + default: cyrf->status = CYRF6936_IDLE; break; } @@ -358,3 +390,28 @@ bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) { return TRUE; } + +/** + * Send a packet with a certain length + */ +bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) { + int i; + + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_SEND; + + // Copy the length and the data + cyrf->buffer[0] = length; + for(i = 0; i < length; i++) + cyrf->buffer[i+1] = data[i]; + + /* Try to set the packet length */ + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); + + return TRUE; +} diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h index 7a7943f5666..2336257a023 100644 --- a/sw/airborne/peripherals/cyrf6936.h +++ b/sw/airborne/peripherals/cyrf6936.h @@ -20,7 +20,7 @@ */ /** - * @file peripherals/yrf6936c.h + * @file peripherals/cyrf6936.h * Driver for the cyrf6936 2.4GHz radio chip */ @@ -40,7 +40,8 @@ enum Cyrf6936Status { CYRF6936_MULTIWRITE, /**< The chip is writing multiple registers */ CYRF6936_DATA_CODE, /**< The chip is writing a data code */ CYRF6936_CHAN_SOP_DATA_CRC, /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */ - CYRF6936_RX_IRQ_STATUS_PACKET /**< The chip is getting the receive irq status, receive status and the receive packet */ + CYRF6936_RX_IRQ_STATUS_PACKET, /**< The chip is getting the receive irq status, receive status and the receive packet */ + CYRF6936_SEND /**< The chip is busy sending a packet */ }; /* The structure for the cyrf6936 chip that handles all the buffers and requests */ @@ -55,8 +56,9 @@ struct Cyrf6936 { uint8_t buffer_length; /**< The length of the buffer used for MULTIWRITE */ uint8_t buffer_idx; /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */ - bool_t has_packet; /**< When the CYRF6936 is done reading the receive registers */ + bool_t has_irq; /**< When the CYRF6936 is done reading the irq */ uint8_t mfg_id[6]; /**< The manufacturer id of the CYRF6936 chip */ + uint8_t tx_irq_status; /**< The last send interrupt status */ uint8_t rx_irq_status; /**< The last receive interrupt status */ uint8_t rx_status; /**< The last receive status */ uint8_t rx_packet[16]; /**< The last received packet */ @@ -69,5 +71,6 @@ bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t d bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length); bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed); bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf); +bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length); #endif /* CYRF6936_H */ diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 1259ba9ff1e..990f51cddb4 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -46,6 +46,7 @@ #define PPRZ 1 #define XBEE 2 #define UDP 3 +#define SUPERBITRF 4 EXTERN bool_t dl_msg_available; /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/ @@ -90,7 +91,7 @@ EXTERN void dl_parse_msg(void); #elif defined DATALINK && DATALINK == UDP #define DatalinkEvent() { \ - UdpCheckAndParse(); \ + UdpCheckAndParse(); \ DlCheckAndParse(); \ } diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 39883053b63..50c8e562c9d 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -50,6 +50,7 @@ #include "subsystems/datalink/pprz_transport.h" #include "subsystems/datalink/xbee.h" #include "subsystems/datalink/w5100.h" +#include "subsystems/datalink/superbitrf.h" #if USE_AUDIO_TELEMETRY #include "subsystems/datalink/audio_telemetry.h" #endif diff --git a/sw/airborne/subsystems/radio_control/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c similarity index 75% rename from sw/airborne/subsystems/radio_control/superbitrf.c rename to sw/airborne/subsystems/datalink/superbitrf.c index 8ccd83521f3..87b8d332e13 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -10,24 +10,24 @@ * * paparazzi is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to + * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ /** - * @file subsystems/radio_control/superbitrf.c - * DSM2 and DSMX implementation for the cyrf6936 2.4GHz radio chip trough SPI + * @file subsystems/datalink/superbitrf.c + * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI */ -#include "superbitrf.h" +#include "subsystems/datalink/superbitrf.h" #include -#include "subsystems/radio_control.h" +#include "paparazzi.h" #include "mcu_periph/spi.h" #include "mcu_periph/sys_time.h" #include @@ -57,8 +57,9 @@ struct SuperbitRF superbitrf; /* The internal functions */ -static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[]); -static inline void superbitrf_radio_to_channels(char* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); +static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); +static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]); +static inline void superbitrf_send_packet_cb(bool_t error); /* The startup configuration for the cyrf6936 */ static const uint8_t cyrf_stratup_config[][2] = { @@ -163,13 +164,17 @@ static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x8 /** * Initialize the superbitrf */ -void radio_control_impl_init(void) { +void superbitrf_init(void) { // Set the status to uninitialized and set the timer to 0 superbitrf.status = SUPERBITRF_UNINIT; superbitrf.state = 0; superbitrf.timer = 0; superbitrf.packet_count = 0; + // Setup the transmit buffer + superbitrf.tx_insert_idx = 0; + superbitrf.tx_extract_idx = 0; + // Initialize the binding pin gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); @@ -184,27 +189,38 @@ void radio_control_impl_init(void) { * The superbitrf on event call */ void superbitrf_event(void) { - uint8_t data_code[16]; + uint8_t i, packet_size, data_code[16], tx_packet[16]; - // Check if the cyrf6936 isn't busy + // Check if the cyrf6936 isn't busy and the uperbitrf is initialized if(superbitrf.cyrf6936.status != CYRF6936_IDLE) return; - // First handle the IRQ - if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { - // Receive the packet - cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); - } + // When the device is initialized handle the IRQ + if(superbitrf.status != SUPERBITRF_UNINIT) { + // First handle the IRQ + if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { + // Receive the packet + cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); + } + + /* Check if it is a valid receive */ + if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { + // Handle the packet received + superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); + superbitrf.packet_count++; - /* Check if it is a valid receive */ - if(superbitrf.cyrf6936.has_packet && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { - superbitrf.packet_count++; + // Reset the packet receiving + superbitrf.cyrf6936.has_irq = FALSE; + } - // Handle the packet received - superbitrf_receive_packet_cb(superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); + /* Check if it has a valid send */ + if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) { + // Handle the send packet + superbitrf_send_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_TXE_IRQ)); - // Reset the packet receiving - superbitrf.cyrf6936.has_packet = FALSE; + // Reset the packet receiving + superbitrf.cyrf6936.has_irq = FALSE; + } } // Check the status of the superbitrf @@ -248,9 +264,9 @@ void superbitrf_event(void) { /* When the superbitrf is initializing transfer */ case SUPERBITRF_INIT_TRANSFER: - // Try to write the transfer config - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4); - superbitrf.status = SUPERBITRF_SYNCING_A; + // Try to write the transfer config + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4); + superbitrf.status = SUPERBITRF_SYNCING_A; break; /* When the superbitrf is in binding mode */ @@ -259,20 +275,29 @@ void superbitrf_event(void) { switch (superbitrf.state) { case 0: // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) { - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + if (superbitrf.timer < get_sys_time_usec()) superbitrf.state++; - } break; case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + + superbitrf.state++; + break; + case 2: // Switch channel superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel); + superbitrf.state += 2; // Already aborted + break; + case 3: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; break; - case 2: + case 4: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; @@ -292,16 +317,18 @@ void superbitrf_event(void) { switch (superbitrf.state) { case 0: // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) { - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + if (superbitrf.timer < get_sys_time_usec()) superbitrf.state++; - } break; case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 2: // Switch channel, sop code, data code and crc - superbitrf.channel = (superbitrf.channel + 1) % 0x4F; //TODO fix define - superbitrf.crc_seed = ~ superbitrf.crc_seed; + superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define + superbitrf.crc_seed = ~superbitrf.crc_seed; cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, pn_codes[superbitrf.channel % 5][superbitrf.sop_col], @@ -310,7 +337,7 @@ void superbitrf_event(void) { superbitrf.state++; break; - case 2: + case 3: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; @@ -346,6 +373,24 @@ void superbitrf_event(void) { superbitrf.state++; break; case 2: + // Send a packet + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; + + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + superbitrf.state++; + break; + case 3: + //TODO: check timeout? (Waiting for send) + break; + case 4: // Switch channel, sop code, data code and crc superbitrf.channel_idx = (superbitrf.channel_idx + 1) %2; superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; @@ -358,7 +403,7 @@ void superbitrf_event(void) { superbitrf.state++; break; - case 3: + case 5: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; @@ -380,7 +425,7 @@ void superbitrf_event(void) { /** * When we receive a packet this callback is called */ -static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[]) { +static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]) { int i; uint16_t sum; @@ -392,7 +437,7 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] // Check if the MFG id is exactly the same if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) { // Start receiving without changing channel - superbitrf.state = 2; + superbitrf.state = 3; break; } @@ -404,7 +449,7 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] // Check the first sum if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) { // Start receiving without changing channel - superbitrf.state = 2; + superbitrf.state = 3; break; } @@ -415,7 +460,7 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] // Check the second sum if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) { // Start receiving without changing channel - superbitrf.state = 2; + superbitrf.state = 3; break; } @@ -424,6 +469,8 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] superbitrf.bind_mfg_id[1] = ~packet[1]; superbitrf.bind_mfg_id[2] = ~packet[2]; superbitrf.bind_mfg_id[3] = ~packet[3]; + superbitrf.bind_mfg_id32 = ((superbitrf.bind_mfg_id[3] &0xFF) << 24 | (superbitrf.bind_mfg_id[2] &0xFF) << 16 | + (superbitrf.bind_mfg_id[1] &0xFF) << 8 | (superbitrf.bind_mfg_id[0] &0xFF)) &0xFFFFFFFF; superbitrf.num_channels = packet[11]; superbitrf.protocol = packet[12]; superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; @@ -441,23 +488,34 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] /* When we receive a packet during syncing first channel A */ case SUPERBITRF_SYNCING_A: // Check the MFG id - if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + if((error && !(status & CYRF_BAD_CRC)) || + packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) break; + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) + superbitrf.crc_seed = ~superbitrf.crc_seed; + superbitrf.channels[0] = superbitrf.channel; superbitrf.channels[1] = superbitrf.channel; - superbitrf.status = SUPERBITRF_SYNCING_B; //TODO: Fix it + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_SYNCING_B; break; /* When we receive a packet during syncing second channel B */ case SUPERBITRF_SYNCING_B: // Check the MFG id - if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + if((error && !(status & CYRF_BAD_CRC)) || + packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) break; + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) + superbitrf.crc_seed = ~superbitrf.crc_seed; + // Set the channel - if (superbitrf.crc_seed == ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + if(superbitrf.crc_seed == ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { superbitrf.channels[0] = superbitrf.channel; superbitrf.channel_idx = 0; } @@ -470,6 +528,7 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] if(superbitrf.channels[1] != superbitrf.channels[0]) { superbitrf.state = 1; superbitrf.status = SUPERBITRF_TRANSFER; + superbitrf.timeouts = 0; } break; @@ -481,7 +540,13 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] // Parse the packet superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); - superbitrf.frame_available = TRUE; + superbitrf.rc_frame_available = TRUE; + + // Calculate the timing (seperately for the channel switches) + if(superbitrf.channels[0] == superbitrf.channel) + superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); + else + superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); // Go to next receive superbitrf.state = 1; @@ -494,10 +559,27 @@ static inline void superbitrf_receive_packet_cb(uint8_t status, uint8_t packet[] } } +static inline void superbitrf_send_packet_cb(bool_t error) { + /* Switch on the status of the superbitRF */ + switch (superbitrf.status) { + + /* When we are in transfer mode */ + case SUPERBITRF_TRANSFER: + // When we successfully or unsuccessfully send a packet + if(superbitrf.state == 3) + superbitrf.state++; + break; + + /* Should not come here */ + default: + break; + } +} + /** * Parse a radio channel packet */ -static inline void superbitrf_radio_to_channels(char* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) { +static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) { int i; uint8_t bit_shift = (is_11bit)? 11:10; int16_t value_max = (is_11bit)? 0x07FF: 0x03FF; @@ -521,3 +603,6 @@ static inline void superbitrf_radio_to_channels(char* data, uint8_t nb_channels, } } } + + + diff --git a/sw/airborne/subsystems/radio_control/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h similarity index 55% rename from sw/airborne/subsystems/radio_control/superbitrf.h rename to sw/airborne/subsystems/datalink/superbitrf.h index d6d408f7b22..d3c965c825c 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -10,52 +10,27 @@ * * paparazzi is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to + * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ /** - * @file subsystems/radio_control/superbitrf.h - * DSM2 and DSMX implementation for the cyrf6936 2.4GHz radio chip trough SPI + * @file subsystems/datalink/superbitrf.h + * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI */ -#ifndef RADIO_CONTROL_SUPERBITRF_H -#define RADIO_CONTROL_SUPERBITRF_H +#ifndef DATALINK_SUPERBITRF_H +#define DATALINK_SUPERBITRF_H -#include "peripherals/cyrf6936.h" #include "mcu_periph/gpio.h" - -/* Theoretically you could have 14 channel over DSM2/DSMX */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL 14 -#endif - -/* The channel ordering is always the same for DSM2 and DSMX */ -#define RADIO_THROTTLE 0 -#define RADIO_ROLL 1 -#define RADIO_PITCH 2 -#define RADIO_YAW 3 -#define RADIO_GEAR 4 -#define RADIO_FLAP 5 -#define RADIO_AUX1 5 -#define RADIO_AUX2 6 -#define RADIO_AUX3 7 -#define RADIO_AUX4 8 -#define RADIO_AUX5 9 -#define RADIO_AUX6 10 -#define RADIO_AUX7 11 -#define RADIO_AUX8 12 -#define RADIO_AUX9 13 - -/* Map the MODE default to the gear switch */ -#ifndef RADIO_MODE -#define RADIO_MODE RADIO_GEAR -#endif +#include "peripherals/cyrf6936.h" +#include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/pprz_transport.h" /* The timings in microseconds */ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ @@ -84,7 +59,7 @@ struct SuperbitRF { struct Cyrf6936 cyrf6936; /**< The cyrf chip used */ volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ uint8_t state; /**< The states each status can be in */ - uint32_t timer; /**< The timer in usec */ + uint32_t timer; /**< The timer in microseconds */ uint8_t timeouts; /**< The amount of timeouts */ uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ @@ -93,6 +68,7 @@ struct SuperbitRF { uint32_t packet_count; /**< How many packets are received(also the invalid) */ uint8_t bind_mfg_id[4]; /**< The MFG id where the receiver is bound to */ + uint32_t bind_mfg_id32; /**< The MFG id where the receiver is bound to in uint32 */ uint8_t num_channels; /**< The number of channels the transmitter has */ uint8_t protocol; /**< The protocol the transmitter uses */ volatile enum dsm_resolution resolution; /**< The resolution that the transmitter has */ @@ -100,41 +76,29 @@ struct SuperbitRF { uint8_t sop_col; /**< The sop code column number calculated with the bind MFG id */ uint8_t data_col; /**< The data code column number calculated with the bind MFG id */ - bool_t frame_available; /**< When a frame is available */ + bool_t rc_frame_available; /**< When a RC frame is available */ + uint32_t timing1; /**< Time between last receive in microseconds */ + uint32_t timing2; /**< Time between second last receive in microseconds */ int16_t rc_values[14]; /**< The rc values from the packet */ + + uint8_t tx_buffer[128]; /**< The transmit buffer */ + uint8_t tx_insert_idx; /**< The transmit buffer insert index */ + uint8_t tx_extract_idx; /**< The transmit buffer extract index */ }; /* The superbitrf functions and structures */ extern struct SuperbitRF superbitrf; +void superbitrf_init(void); void superbitrf_event(void); -/* Macro that normalize superbitrf rc_values to radio values */ -#define NormalizeRcDl(_in, _out, _count) { \ - uint8_t i; \ - for(i = 0; i < _count; i++) { \ - if(i == RADIO_THROTTLE) { \ - _out[i] = (_in[i] + MAX_PPRZ) / 2; \ - Bound(_out[i], 0, MAX_PPRZ); \ - } else { \ - _out[i] = -_in[i]; \ - Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ - } \ - } \ -} - -/* The radio control event handler */ -#define RadioControlEvent(_received_frame_handler) { \ - cyrf6936_event(&superbitrf.cyrf6936); \ - superbitrf_event(); \ - if(superbitrf.frame_available) { \ - radio_control.frame_cpt++; \ - radio_control.time_since_last_frame = 0; \ - radio_control.radio_ok_cpt = 0; \ - radio_control.status = RC_OK; \ - NormalizeRcDl(superbitrf.rc_values,radio_control.values); \ - _received_frame_handler(); \ - superbitrf.frame_available = FALSE; \ - } \ -} - -#endif /* RADIO_CONTROL_SUPERBITRF_H */ +/* The datalink defines */ +#define SuperbitRFInit() { }//superbitrf_init(); } +#define SuperbitRFCheckFreeSpace(_x) (((superbitrf.tx_insert_idx+1) %128) != superbitrf.tx_extract_idx) +#define SuperbitRFTransmit(_x) { \ + superbitrf.tx_buffer[superbitrf.tx_insert_idx] = _x; \ + superbitrf.tx_insert_idx = (superbitrf.tx_insert_idx+1) %128; \ + } +#define SuperbitRFSendMessage() { } +#define SuperbitRFCheckAndParse() { } + +#endif /* DATALINK_SUPERBITRF_H */ diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.c b/sw/airborne/subsystems/radio_control/superbitrf_rc.c new file mode 100644 index 00000000000..f3609ed3b83 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.c @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/radio_control/superbitrf_rc.c + * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#include "superbitrf_rc.h" +#include "subsystems/radio_control.h" + +/** + * Initialization + */ +//#if DATALINK == SUPERBITRF +//void radio_control_impl_init(void) {} +//#else +void radio_control_impl_init(void) { superbitrf_init(); } +//#endif diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.h b/sw/airborne/subsystems/radio_control/superbitrf_rc.h new file mode 100644 index 00000000000..de410a56d32 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.h @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/radio_control/superbitrf_rc.h + * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#ifndef RADIO_CONTROL_SUPERBITRF_RC_H +#define RADIO_CONTROL_SUPERBITRF_RC_H + +#include "subsystems/datalink/superbitrf.h" + +/* Theoretically you could have 14 channel over DSM2/DSMX */ +#ifndef RADIO_CONTROL_NB_CHANNEL +#define RADIO_CONTROL_NB_CHANNEL 14 +#endif + +/* The channel ordering is always the same for DSM2 and DSMX */ +#define RADIO_THROTTLE 0 +#define RADIO_ROLL 1 +#define RADIO_PITCH 2 +#define RADIO_YAW 3 +#define RADIO_GEAR 4 +#define RADIO_FLAP 5 +#define RADIO_AUX1 5 +#define RADIO_AUX2 6 +#define RADIO_AUX3 7 +#define RADIO_AUX4 8 +#define RADIO_AUX5 9 +#define RADIO_AUX6 10 +#define RADIO_AUX7 11 +#define RADIO_AUX8 12 +#define RADIO_AUX9 13 + +/* Map the MODE default to the gear switch */ +#ifndef RADIO_MODE +#define RADIO_MODE RADIO_GEAR +#endif + +/* Macro that normalize superbitrf rc_values to radio values */ +#define NormalizeRcDl(_in, _out, _count) { \ + uint8_t i; \ + for(i = 0; i < _count; i++) { \ + if(i == RADIO_THROTTLE) { \ + _out[i] = (_in[i] + MAX_PPRZ) / 2; \ + Bound(_out[i], 0, MAX_PPRZ); \ + } else { \ + _out[i] = -_in[i]; \ + Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ + } \ + } \ +} + +/* The radio control event handler */ +#define RadioControlEvent(_received_frame_handler) { \ + cyrf6936_event(&superbitrf.cyrf6936); \ + superbitrf_event(); \ + if(superbitrf.rc_frame_available) { \ + radio_control.frame_cpt++; \ + radio_control.time_since_last_frame = 0; \ + radio_control.radio_ok_cpt = 0; \ + radio_control.status = RC_OK; \ + NormalizeRcDl(superbitrf.rc_values,radio_control.values \ + ,superbitrf.num_channels); \ + _received_frame_handler(); \ + superbitrf.rc_frame_available = FALSE; \ + } \ +} + +#endif /* RADIO_CONTROL_SUPERBITRF_RC_H */ From 1964c097220780737da115cca3876fba66f06420 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Mon, 15 Jul 2013 18:21:25 +0200 Subject: [PATCH 19/65] DSMX implementation --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 4 +- conf/messages.xml | 4 + conf/telemetry/default_rotorcraft.xml | 2 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 4 + sw/airborne/peripherals/cyrf6936.c | 6 +- sw/airborne/peripherals/cyrf6936.h | 1 + sw/airborne/subsystems/datalink/datalink.h | 6 + sw/airborne/subsystems/datalink/superbitrf.c | 308 +++++++++++++++--- sw/airborne/subsystems/datalink/superbitrf.h | 26 +- 9 files changed, 316 insertions(+), 45 deletions(-) diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 4d08f9e5ec5..e347f81c0a4 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -159,7 +159,7 @@ - + @@ -189,7 +189,7 @@
- + diff --git a/conf/messages.xml b/conf/messages.xml index f2199afb15a..75f5034bf90 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -607,6 +607,10 @@ + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index b647b9c609d..976962721cc 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -18,7 +18,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 80f5ab8366e..e5f9df37e6a 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -141,6 +141,10 @@ &superbitrf.status, \ &superbitrf.cyrf6936.status, \ &superbitrf.packet_count, \ + &superbitrf.transfer_timeouts, \ + &superbitrf.resync_count, \ + &superbitrf.uplink_count, \ + &superbitrf.rc_count, \ &superbitrf.timing1, \ &superbitrf.timing2, \ &superbitrf.bind_mfg_id32, \ diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 1ea90971df9..5bcd9951556 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -199,8 +199,12 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) { cyrf->tx_irq_status = cyrf->input_buf[1]; cyrf6936_read_register(cyrf, CYRF_RX_STATUS); break; - case 3: // Read the receive packet + case 3: // Set the packet length cyrf->rx_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_COUNT); + break; + case 4: // Read the receive packet + cyrf->rx_count = cyrf->input_buf[1]; cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); break; default: diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h index 2336257a023..c384a9878b8 100644 --- a/sw/airborne/peripherals/cyrf6936.h +++ b/sw/airborne/peripherals/cyrf6936.h @@ -61,6 +61,7 @@ struct Cyrf6936 { uint8_t tx_irq_status; /**< The last send interrupt status */ uint8_t rx_irq_status; /**< The last receive interrupt status */ uint8_t rx_status; /**< The last receive status */ + uint8_t rx_count; /**< The length of the received packet */ uint8_t rx_packet[16]; /**< The last received packet */ }; diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 990f51cddb4..ad1f8bb08c5 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -95,6 +95,12 @@ EXTERN void dl_parse_msg(void); DlCheckAndParse(); \ } +#elif defined DATALINK && DATALINK == SUPERBITRF + +#define DatalinkEvent() { \ + SuperbitRFCheckAndParse(); \ + DlCheckAndParse(); \ + } #else diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 87b8d332e13..cf50df99189 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -53,6 +53,11 @@ #define SUPERBITRF_DRDY_PIN GPIO1 #endif +/* Default forcing in DSM2 mode is false */ +#ifndef SUPERBITRF_FORCE_DSM2 +#define SUPERBITRF_FORCE_DSM2 FALSE +#endif + /* The superbitRF structure */ struct SuperbitRF superbitrf; @@ -60,6 +65,7 @@ struct SuperbitRF superbitrf; static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]); static inline void superbitrf_send_packet_cb(bool_t error); +static inline void superbitrf_gen_dsmx_channels(void); /* The startup configuration for the cyrf6936 */ static const uint8_t cyrf_stratup_config[][2] = { @@ -189,7 +195,7 @@ void superbitrf_init(void) { * The superbitrf on event call */ void superbitrf_event(void) { - uint8_t i, packet_size, data_code[16], tx_packet[16]; + uint8_t i, pn_row, packet_size, data_code[16], tx_packet[16]; // Check if the cyrf6936 isn't busy and the uperbitrf is initialized if(superbitrf.cyrf6936.status != CYRF6936_IDLE) @@ -264,8 +270,12 @@ void superbitrf_event(void) { /* When the superbitrf is initializing transfer */ case SUPERBITRF_INIT_TRANSFER: + // Generate the DSMX channels + superbitrf_gen_dsmx_channels(); + // Try to write the transfer config cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4); + superbitrf.resync_count = 0; superbitrf.status = SUPERBITRF_SYNCING_A; break; @@ -326,18 +336,65 @@ void superbitrf_event(void) { superbitrf.state++; break; case 2: + // When DSMX we don't need to switch + if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { + superbitrf.state++; + break; + } + // Switch channel, sop code, data code and crc - superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define + superbitrf.channel_idx = (superbitrf.channel_idx + 1) %2; + pn_row = superbitrf.channels[superbitrf.channel_idx] % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx], + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + superbitrf.state++; + break; + case 3: + // Send a packet + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3])+1; + } + + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; + + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + superbitrf.state++; + break; + case 4: + //TODO: check timeout? (Waiting for send) + break; + case 5: + // Switch channel, sop code, data code and crc + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) + superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define + else { + superbitrf.channel_idx = (superbitrf.channel_idx + 1) %23; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + } superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, - pn_codes[superbitrf.channel % 5][superbitrf.sop_col], - pn_codes[superbitrf.channel % 5][superbitrf.data_col], + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], superbitrf.crc_seed); superbitrf.state++; break; - case 3: + case 6: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; @@ -357,13 +414,16 @@ void superbitrf_event(void) { case 0: // When there is a timeout if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 2) %2 : (superbitrf.channel_idx + 2) %23; + superbitrf.transfer_timeouts++; superbitrf.timeouts++; superbitrf.state++; } - // We really lost it - if(superbitrf.timeouts > 1) { + // We really lost the communication + if(superbitrf.timeouts > 2) { superbitrf.state = 0; + superbitrf.resync_count++; superbitrf.status = SUPERBITRF_SYNCING_A; } break; @@ -371,11 +431,34 @@ void superbitrf_event(void) { // Abort the receive cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); superbitrf.state++; + + // When we don't have enough time + if(superbitrf.timeouts > 0 || superbitrf.timing2 < 10000 || superbitrf.timing1 > 10000) { + superbitrf.state = 8; + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; + break; + } + + // Set the timer for sending + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATAWAIT_TIME) % 0xFFFFFFFF; break; case 2: + // Wait before sending + superbitrf.state++; + //if (superbitrf.timer < get_sys_time_usec()) + //superbitrf.state++; + break; + case 3: // Send a packet - tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3])+1; + } + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); if(packet_size > 14) packet_size = 14; @@ -387,30 +470,49 @@ void superbitrf_event(void) { superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; superbitrf.state++; break; - case 3: + case 4: //TODO: check timeout? (Waiting for send) break; - case 4: + case 5: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + + // Set the timer + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATAWAIT_TIME + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; + break; + case 6: + // Waiting for data receive + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; + break; + case 7: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 8: // Switch channel, sop code, data code and crc - superbitrf.channel_idx = (superbitrf.channel_idx + 1) %2; + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; - superbitrf.crc_seed = ~ superbitrf.crc_seed; + superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, - pn_codes[superbitrf.channel % 5][superbitrf.sop_col], - pn_codes[superbitrf.channel % 5][superbitrf.data_col], + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], superbitrf.crc_seed); superbitrf.state++; break; - case 5: + case 9: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; break; default: // Set the timer - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; superbitrf.state = 0; break; } @@ -488,34 +590,68 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui /* When we receive a packet during syncing first channel A */ case SUPERBITRF_SYNCING_A: // Check the MFG id - if((error && !(status & CYRF_BAD_CRC)) || - packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + if(error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } // If the CRC is wrong invert if (error && (status & CYRF_BAD_CRC)) superbitrf.crc_seed = ~superbitrf.crc_seed; - superbitrf.channels[0] = superbitrf.channel; - superbitrf.channels[1] = superbitrf.channel; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channels[1] = superbitrf.channel; - superbitrf.state = 1; - superbitrf.status = SUPERBITRF_SYNCING_B; + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_SYNCING_B; + } else { + superbitrf.timeouts = 0; + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + } break; /* When we receive a packet during syncing second channel B */ case SUPERBITRF_SYNCING_B: // Check the MFG id - if((error && !(status & CYRF_BAD_CRC)) || - packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + if(error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } // If the CRC is wrong invert if (error && (status & CYRF_BAD_CRC)) superbitrf.crc_seed = ~superbitrf.crc_seed; // Set the channel - if(superbitrf.crc_seed == ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + if(superbitrf.channels[0] != superbitrf.channel) { superbitrf.channels[0] = superbitrf.channel; superbitrf.channel_idx = 0; } @@ -535,22 +671,62 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui /* When we receive a packet during transfer */ case SUPERBITRF_TRANSFER: // Check the MFG id - if(packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)) + if(error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; + } + /*if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + }*/ + + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) + superbitrf.crc_seed = ~superbitrf.crc_seed; - // Parse the packet - superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); - superbitrf.rc_frame_available = TRUE; + // Check if it is a RC packet + if(packet[1] == (~superbitrf.bind_mfg_id[3]&0xFF) || packet[1] == (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.rc_count++; - // Calculate the timing (seperately for the channel switches) - if(superbitrf.channels[0] == superbitrf.channel) + // Parse the packet + superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); + superbitrf.rc_frame_available = TRUE; + + // Calculate the timing (seperately for the channel switches) + superbitrf.timing2 = superbitrf.timing1; superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); - else - superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); - // Go to next receive - superbitrf.state = 1; - superbitrf.timeouts = 0; + // Go to next receive + superbitrf.state = 1; + superbitrf.timeouts = 0; + } else if(superbitrf.state == 5) { + superbitrf.uplink_count++; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + + // Update the state + superbitrf.state = 7; + } break; /* Should not come here */ @@ -563,10 +739,18 @@ static inline void superbitrf_send_packet_cb(bool_t error) { /* Switch on the status of the superbitRF */ switch (superbitrf.status) { + /* When we are synchronizing */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + // When we successfully or unsuccessfully send a data packet + if(superbitrf.state == 4) + superbitrf.state++; + break; + /* When we are in transfer mode */ case SUPERBITRF_TRANSFER: // When we successfully or unsuccessfully send a packet - if(superbitrf.state == 3) + if(superbitrf.state == 4) superbitrf.state++; break; @@ -604,5 +788,53 @@ static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channe } } +/** + * Generate the channels + */ +static inline void superbitrf_gen_dsmx_channels(void) { + // Calculate the DSMX channels + int idx = 0; + uint32_t id = ~((superbitrf.bind_mfg_id[0] << 24) | (superbitrf.bind_mfg_id[1] << 16) | + (superbitrf.bind_mfg_id[2] << 8) | (superbitrf.bind_mfg_id[3] << 0)); + uint32_t id_tmp = id; + + // While not all channels are set + while(idx < 23) { + int i; + int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0; + + id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization + uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3 + if (((next_ch ^ id) & 0x01 ) == 0) + continue; + + // Go trough all already set channels + for (i = 0; i < idx; i++) { + // Channel is already used + if(superbitrf.channels[i] == next_ch) + break; + + // Count the channel groups + if(superbitrf.channels[i] <= 27) + count_3_27++; + else if (superbitrf.channels[i] <= 51) + count_28_51++; + else + count_52_76++; + } + + // When channel is already used continue + if (i != idx) + continue; + + // Set the channel when channel groups aren't full + if ((next_ch < 28 && count_3_27 < 8) // Channels 3-27: max 8 + || (next_ch >= 28 && next_ch < 52 && count_28_51 < 7) // Channels 28-52: max 7 + || (next_ch >= 52 && count_52_76 < 8)) { // Channels 52-76: max 8 + superbitrf.channels[idx++] = next_ch; + } + } +} + diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index d3c965c825c..2f7a786e1b5 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -34,8 +34,10 @@ /* The timings in microseconds */ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ -#define SUPERBITRF_SYNC_RECV_TIME 12000 /**< The time to wait for a sync packet on a channel in microseconds */ -#define SUPERBITRF_RECV_TIME 24000 /**< The time to wait for a transfer packet on a channel in microseconds */ +#define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ +#define SUPERBITRF_RECV_TIME 25000 /**< The time to wait for a transfer packet on a channel in microseconds */ +#define SUPERBITRF_DATAWAIT_TIME 500 /**< The time to wait after RC receive to send a data packet in microseconds */ +#define SUPERBITRF_DATARECV_TIME 2000 /**< The time to wait for a data packet on a channel in microseconds */ /* The different statuses the superbitRF can be in */ enum SuperbitRFStatus { @@ -54,6 +56,18 @@ enum dsm_resolution { SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */ }; +/* The different protocols a transmitter can send */ +enum dsm_protocol { + DSM_DSM2_1 = 0x01, /**< The original DSM2 protocol with 1 packet of data */ + DSM_DSM2_2 = 0x02, /**< The original DSM2 protocol with 2 packets of data */ + DSM_DSM2P = 0x10, /**< Our own DSM2 Paparazzi protocol */ + DSM_DSMXP = 0x11, /**< Our own DSMX Paparazzi protocol */ + DSM_DSMX_1 = 0xA2, /**< The original DSMX protocol with 1 packet of data */ + DSM_DSMX_2 = 0xB2, /**< The original DSMX protocol with 2 packets of data */ +}; +#define IS_DSM2(x) (x == DSM_DSM2P || x == DSM_DSM2_1 || x == DSM_DSM2_2) +#define IS_DSMX(x) (!IS_DSM2(x)) + /* The superbitrf structure */ struct SuperbitRF { struct Cyrf6936 cyrf6936; /**< The cyrf chip used */ @@ -61,16 +75,20 @@ struct SuperbitRF { uint8_t state; /**< The states each status can be in */ uint32_t timer; /**< The timer in microseconds */ uint8_t timeouts; /**< The amount of timeouts */ + uint32_t transfer_timeouts; /**< The amount of timeouts during transfer */ + uint32_t resync_count; /**< The amount of resyncs needed during transfer */ uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ uint8_t channel_idx; /**< The current channel index */ uint8_t channel; /**< The current channel number */ uint32_t packet_count; /**< How many packets are received(also the invalid) */ + uint32_t uplink_count; /**< How many valid uplink packages are received */ + uint32_t rc_count; /**< How many valid RC packages are received */ uint8_t bind_mfg_id[4]; /**< The MFG id where the receiver is bound to */ uint32_t bind_mfg_id32; /**< The MFG id where the receiver is bound to in uint32 */ uint8_t num_channels; /**< The number of channels the transmitter has */ - uint8_t protocol; /**< The protocol the transmitter uses */ + volatile enum dsm_protocol protocol; /**< The protocol the transmitter uses */ volatile enum dsm_resolution resolution; /**< The resolution that the transmitter has */ uint16_t crc_seed; /**< The CRC seed that is calculated with the bind MFG id */ uint8_t sop_col; /**< The sop code column number calculated with the bind MFG id */ @@ -81,6 +99,8 @@ struct SuperbitRF { uint32_t timing2; /**< Time between second last receive in microseconds */ int16_t rc_values[14]; /**< The rc values from the packet */ + struct pprz_transport rx_transport; /**< The receive transport */ + uint8_t tx_buffer[128]; /**< The transmit buffer */ uint8_t tx_insert_idx; /**< The transmit buffer insert index */ uint8_t tx_extract_idx; /**< The transmit buffer extract index */ From 06df13cf95ea4007e12c2ef48e068b00c113a55a Mon Sep 17 00:00:00 2001 From: fvantienen Date: Tue, 16 Jul 2013 20:13:20 +0200 Subject: [PATCH 20/65] Some small fixes --- sw/airborne/subsystems/datalink/superbitrf.c | 84 +++++++++++++++----- sw/airborne/subsystems/datalink/superbitrf.h | 2 +- 2 files changed, 63 insertions(+), 23 deletions(-) diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index cf50df99189..08e1a3128ec 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -336,15 +336,9 @@ void superbitrf_event(void) { superbitrf.state++; break; case 2: - // When DSMX we don't need to switch - if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { - superbitrf.state++; - break; - } - // Switch channel, sop code, data code and crc - superbitrf.channel_idx = (superbitrf.channel_idx + 1) %2; - pn_row = superbitrf.channels[superbitrf.channel_idx] % 5; + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channels[superbitrf.channel_idx] % 5 : (superbitrf.channels[superbitrf.channel_idx]-2) % 5; cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx], pn_codes[pn_row][superbitrf.sop_col], @@ -377,13 +371,15 @@ void superbitrf_event(void) { //TODO: check timeout? (Waiting for send) break; case 5: - // Switch channel, sop code, data code and crc - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) - superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define - else { - superbitrf.channel_idx = (superbitrf.channel_idx + 1) %23; + // When DSMX we don't need to switch + if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { + superbitrf.state++; superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + break; } + + // Switch channel, sop code, data code and crc + superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define superbitrf.crc_seed = ~superbitrf.crc_seed; pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; @@ -596,13 +592,15 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui break; } if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; @@ -612,6 +610,25 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui if (error && (status & CYRF_BAD_CRC)) superbitrf.crc_seed = ~superbitrf.crc_seed; + // When we receive a data packet + if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.uplink_count++; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + break; + } + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { superbitrf.channels[0] = superbitrf.channel; superbitrf.channels[1] = superbitrf.channel; @@ -634,13 +651,15 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui break; } if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; @@ -650,6 +669,25 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui if (error && (status & CYRF_BAD_CRC)) superbitrf.crc_seed = ~superbitrf.crc_seed; + // When we receive a data packet + if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.uplink_count++; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + break; + } + // Set the channel if(superbitrf.channels[0] != superbitrf.channel) { superbitrf.channels[0] = superbitrf.channel; @@ -676,18 +714,20 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } - /*if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF))) { + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || packet[1] != (superbitrf.bind_mfg_id[3]&0xFF))) { + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; - }*/ + } // If the CRC is wrong invert if (error && (status & CYRF_BAD_CRC)) @@ -708,7 +748,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui // Go to next receive superbitrf.state = 1; superbitrf.timeouts = 0; - } else if(superbitrf.state == 5) { + } else { superbitrf.uplink_count++; // When it is a data packet, parse the packet if not busy already diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index 2f7a786e1b5..c6ac544d6e7 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -37,7 +37,7 @@ #define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ #define SUPERBITRF_RECV_TIME 25000 /**< The time to wait for a transfer packet on a channel in microseconds */ #define SUPERBITRF_DATAWAIT_TIME 500 /**< The time to wait after RC receive to send a data packet in microseconds */ -#define SUPERBITRF_DATARECV_TIME 2000 /**< The time to wait for a data packet on a channel in microseconds */ +#define SUPERBITRF_DATARECV_TIME 9000 /**< The time to wait for a data packet on a channel in microseconds */ /* The different statuses the superbitRF can be in */ enum SuperbitRFStatus { From bfc5598ad88b9fd116e51e5ee82467c9eaab061a Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Tue, 23 Jul 2013 23:20:37 -0700 Subject: [PATCH 21/65] Initial commit for Lisa/S V0.1 support. --- conf/airframes/esden/calib/ls01-default.xml | 34 ++ conf/airframes/esden/quady_ls01pwm.xml | 233 ++++++++++ conf/boards/lisa_s_0.1.makefile | 72 +++ conf/firmwares/rotorcraft.makefile | 10 + .../shared/imu_lisa_s_v0.1.makefile | 70 +++ sw/airborne/arch/stm32/lisa-s.ld | 36 ++ sw/airborne/boards/lisa_s/baro_board.h | 67 +++ sw/airborne/boards/lisa_s_0.1.h | 421 ++++++++++++++++++ 8 files changed, 943 insertions(+) create mode 100644 conf/airframes/esden/calib/ls01-default.xml create mode 100644 conf/airframes/esden/quady_ls01pwm.xml create mode 100644 conf/boards/lisa_s_0.1.makefile create mode 100644 conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile create mode 100644 sw/airborne/arch/stm32/lisa-s.ld create mode 100644 sw/airborne/boards/lisa_s/baro_board.h create mode 100644 sw/airborne/boards/lisa_s_0.1.h diff --git a/conf/airframes/esden/calib/ls01-default.xml b/conf/airframes/esden/calib/ls01-default.xml new file mode 100644 index 00000000000..a7171f01581 --- /dev/null +++ b/conf/airframes/esden/calib/ls01-default.xml @@ -0,0 +1,34 @@ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml new file mode 100644 index 00000000000..9dc6032179e --- /dev/null +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -0,0 +1,233 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ + + +
+ + + +
+ +
+ + + +
+ +
+ +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/boards/lisa_s_0.1.makefile b/conf/boards/lisa_s_0.1.makefile new file mode 100644 index 00000000000..ffaeaf9a6c2 --- /dev/null +++ b/conf/boards/lisa_s_0.1.makefile @@ -0,0 +1,72 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# lisa_s_0.1.makefile +# +# http://paparazzi.enac.fr/wiki/Lisa/S +# + +BOARD=lisa_s +BOARD_VERSION=0.1 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/lisa-s.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via usb dfu bootloader (luftboot) +# other possibilities: JTAG, SWD, SERIAL +FLASH_MODE ?= SWD + +# +# +# some default values shared between different firmwares +# +# + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= 3 +SYS_TIME_LED ?= 1 + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 +RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= none + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART3 +GPS_BAUD ?= B38400 + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm + +# Thish should be disabled as we don't have adc inputs on Lisa/S +ifndef ADC_IR1 +ADC_IR1 = 1 +ADC_IR1_CHAN = 0 +endif +ifndef ADC_IR2 +ADC_IR2 = 2 +ADC_IR2_CHAN = 1 +endif +ifndef ADC_IR3 +ADC_IR_TOP = 3 +ADC_IR_TOP_CHAN = 2 +endif +ADC_IR_NB_SAMPLES ?= 16 diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index b5cd13d64e4..575c28dabb8 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -187,6 +187,16 @@ LISA_M_BARO ?= BARO_BOARD_BMP085 endif ap.CFLAGS += -D$(LISA_M_BARO) +# Lisa/S baro +else ifeq ($(BOARD), lisa_s) +# defaults to SPI baro MS5611 on the board + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 + ap.CFLAGS += -DMS5611_SPI_DEV=spi1 + ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 + ap.srcs += boards/lisa_m/baro_ms5611_spi.c + ap.CFLAGS += -DBARO_MS5611_SPI + # Lia baro (no bmp onboard) else ifeq ($(BOARD), lia) # fixme, reuse the baro drivers in lisa_m dir diff --git a/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile new file mode 100644 index 00000000000..536031592b3 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile @@ -0,0 +1,70 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Common part for Aspirin IMU v2.1 and v2.2 +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU +endif + +IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\" +IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c + +include $(CFG_SHARED)/spi_master.makefile + +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0 +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1 + +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 +# Slave select configuration +# SLAVE0 is on PA4 (NSS) (MPU600 CS) +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0 + +# SLAVE1 is on PC4, which is the baro CS +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1 + +ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_2_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/arch/stm32/lisa-s.ld b/sw/airborne/arch/stm32/lisa-s.ld new file mode 100644 index 00000000000..a919196043a --- /dev/null +++ b/sw/airborne/arch/stm32/lisa-s.ld @@ -0,0 +1,36 @@ +/* + * Hey Emacs, this is a -*- makefile -*- + * + * Copyright (C) 2012 Piotr Esden-Tempski + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* Linker script for Lisa-S (STM32F103REY6, 512K flash, 64K RAM). */ + +/* Define memory regions. */ +MEMORY +{ + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + /* Leaving 2k of space at the end of rom for stored settings */ + rom (rx) : ORIGIN = 0x08000000, LENGTH = 512K +} + +/* Include the common ld script. */ +INCLUDE libopencm3_stm32f1.ld + diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h new file mode 100644 index 00000000000..42014ddeee5 --- /dev/null +++ b/sw/airborne/boards/lisa_s/baro_board.h @@ -0,0 +1,67 @@ + +/* + * board specific functions for the lisa_m board + * + */ + +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_BARO_H + +#include "std.h" + +// for right now we abuse this file for the ms5611 baro on aspirin as well +#if !BARO_MS5611_I2C && !BARO_MS5611 + +#include "mcu_periph/i2c.h" + +// absolute addr +#define BMP085_ADDR 0xEE +// Over sample setting (0-3) +#define BMP085_OSS 3 + +enum LisaBaroStatus { + LBS_UNINITIALIZED, + LBS_REQUEST, + LBS_READING, + LBS_READ, + LBS_REQUEST_TEMP, + LBS_READING_TEMP, + LBS_READ_TEMP, +}; + +struct BaroBoard { + enum LisaBaroStatus status; +}; + +struct bmp085_baro_calibration { + // These values come from EEPROM on sensor + int16_t ac1; + int16_t ac2; + int16_t ac3; + uint16_t ac4; + uint16_t ac5; + uint16_t ac6; + int16_t b1; + int16_t b2; + int16_t mb; + int16_t mc; + int16_t md; + + // These values are calculated + int32_t b5; +}; + +extern struct BaroBoard baro_board; +extern struct i2c_transaction baro_trans; +extern struct bmp085_baro_calibration calibration; + +extern void baro_board_send_reset(void); +extern void baro_board_send_config(void); + +#endif // !BARO_MS5611_xx + +extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)); + +#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler) + +#endif /* BOARDS_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lisa_s_0.1.h b/sw/airborne/boards/lisa_s_0.1.h new file mode 100644 index 00000000000..fa3c3d49cbc --- /dev/null +++ b/sw/airborne/boards/lisa_s_0.1.h @@ -0,0 +1,421 @@ +#ifndef CONFIG_LISA_S_0_1_H +#define CONFIG_LISA_S_0_1_H + +#define BOARD_LISA_S + +/* Lisa/M has a 12MHz external clock and 72MHz internal. */ +#define EXT_CLK 12000000 +#define AHB_CLK 72000000 + +/* + * Onboard LEDs + */ + +/* red, on PA8 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOC +#define LED_1_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_1_GPIO_PIN GPIO10 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* green, shared with JTAG_TRST */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOC +#define LED_2_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_2_GPIO_PIN GPIO11 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */ +#ifndef USE_LED_3 +#define USE_LED_3 1 +#endif +#define LED_3_GPIO GPIOD +#define LED_3_GPIO_CLK RCC_APB2ENR_IOPDEN +#define LED_3_GPIO_PIN GPIO2 +#define LED_3_GPIO_ON gpio_clear +#define LED_3_GPIO_OFF gpio_set +#define LED_3_AFIO_REMAP ((void)0) + +/* + * not actual LEDS, used as GPIOs + */ + +/* PB1, DRDY on EXT SPI connector*/ +#define LED_BODY_GPIO GPIOB +#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN +#define LED_BODY_GPIO_PIN GPIO1 +#define LED_BODY_GPIO_ON gpio_set +#define LED_BODY_GPIO_OFF gpio_clear +#define LED_BODY_AFIO_REMAP ((void)0) + +/* PC12, on GPIO connector*/ +#define LED_12_GPIO GPIOC +#define LED_12_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_12_GPIO_PIN GPIO12 +#define LED_12_GPIO_ON gpio_clear +#define LED_12_GPIO_OFF gpio_set +#define LED_12_AFIO_REMAP ((void)0) + + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + + +#define DefaultVoltageOfAdc(adc) (0.0045*adc) + +/* Onboard ADCs */ +/* + ADC1 PC3/ADC13 + ADC2 PC0/ADC10 + ADC3 PC1/ADC11 + ADC4 PC5/ADC15 + ADC6 PC2/ADC12 + BATT PC4/ADC14 +*/ +/* We should remove the first three adc channels, as Lisa/S does not provide those. */ +#define BOARD_ADC_CHANNEL_1 10 +#define BOARD_ADC_CHANNEL_2 11 +#define BOARD_ADC_CHANNEL_3 12 +// we can only use ADC1,2,3; the last channel is for bat monitoring +#define BOARD_ADC_CHANNEL_4 2 + +/* provide defines that can be used to access the ADC_x in the code or airframe file + * these directly map to the index number of the 4 adc channels defined above + * 4th (index 3) is used for bat monitoring by default + */ +#define ADC_1 0 +#define ADC_2 1 +#define ADC_3 2 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY 3 +#endif + +/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ +// FIXME, this is not very nice, is also locm3 lib specific +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO2); \ + } +#endif // USE_AD1 + +#define BOARD_HAS_BARO 1 + +/* SPI slave mapping */ + +/* IMU_MPU_CS */ +#define SPI_SELECT_SLAVE0_PORT GPIOA +#define SPI_SELECT_SLAVE0_PIN GPIO4 + +/* IMU_BARO_CS */ +#define SPI_SELECT_SLAVE1_PORT GPIOC +#define SPI_SELECT_SLAVE1_PIN GPIO4 + +/* RADIO_SS */ +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO12 + +/* + * UART pin configuration + * + * sets on which pins the UARTs are connected + */ +/* DIAG port */ +#define UART1_GPIO_AF 0 +#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX +#define UART1_GPIO_RX GPIO_USART1_RX +#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX +#define UART1_GPIO_TX GPIO_USART1_TX + +/* GPS */ +#define UART3_GPIO_AF 0 +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX +#define UART3_GPIO_RX GPIO_USART3_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX +#define UART3_GPIO_TX GPIO_USART3_TX + +/* LED1 & LED2 */ +/* +#define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX +#define UART3_GPIO_RX GPIO_USART3_PR_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX +#define UART3_GPIO_TX GPIO_USART3_PR_TX +*/ + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +#define SPEKTRUM_BIND_PIN GPIO2 +#define SPEKTRUM_BIND_PIN_PORT GPIOB + +/* Diag Port RX */ +#define SPEKTRUM_UART1_RCC_REG &RCC_APB2ENR +#define SPEKTRUM_UART1_RCC_DEV RCC_APB2ENR_USART1EN +#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX +#define SPEKTRUM_UART1_PIN GPIO_USART1_RX +#define SPEKTRUM_UART1_AF 0 +#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART1_ISR usart1_isr +#define SPEKTRUM_UART1_DEV USART1 + +/* AUX Radio RX */ +#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN +#define SPEKTRUM_UART2_BANK GPIO_BANK_USART2_RX +#define SPEKTRUM_UART2_PIN GPIO_USART2_RX +#define SPEKTRUM_UART2_AF 0 +#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ +#define SPEKTRUM_UART2_ISR usart2_isr +#define SPEKTRUM_UART2_DEV USART2 + +/* LED2 */ +#define SPEKTRUM_UART3_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART3_RCC_DEV RCC_APB1ENR_USART3EN +#define SPEKTRUM_UART3_BANK GPIO_BANK_USART3_PR_RX +#define SPEKTRUM_UART3_PIN GPIO_USART3_PR_RX +#define SPEKTRUM_UART3_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP +#define SPEKTRUM_UART3_IRQ NVIC_USART3_IRQ +#define SPEKTRUM_UART3_ISR usart3_isr +#define SPEKTRUM_UART3_DEV USART3 + +/* LED3 */ +#define SPEKTRUM_UART5_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART5_RCC_DEV RCC_APB1ENR_UART5EN +#define SPEKTRUM_UART5_BANK GPIO_BANK_UART5_RX +#define SPEKTRUM_UART5_PIN GPIO_UART5_RX +#define SPEKTRUM_UART5_AF 0 +#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ +#define SPEKTRUM_UART5_ISR uart5_isr +#define SPEKTRUM_UART5_DEV UART5 + +/* PPM + */ + +/* + * On Lisa/S there is no really dedicated port available. But we could use a + * bunch of different pins to do PPM Input. + */ + +/* + * Default is PPM config 2, input on GPIO01 (Servo pin 6) + */ + +#ifndef PPM_CONFIG +#define PPM_CONFIG 3 +#endif + + +#if PPM_CONFIG == 1 +/* input on PA01 (UART1_RX) Diag port */ +#define USE_PPM_TIM1 1 +#define PPM_CHANNEL TIM_IC3 +#define PPM_TIMER_INPUT TIM_IC_IN_TI3 +#define PPM_IRQ NVIC_TIM1_UP_IRQ +#define PPM_IRQ2 NVIC_TIM1_CC_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC3IE +#define PPM_CC_IF TIM_SR_CC3IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO10 +#define PPM_GPIO_AF 0 + +#elif PPM_CONFIG == 2 +/* input on PA1 (Servo 6 pin) + On Lisa/S we could also use TIM5 IC2 instead. */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC2 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC2IE +#define PPM_CC_IF TIM_SR_CC2IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO1 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#elif PPM_CONFIG == 3 +/* input on PA3 (Aux RX pin) + On Lisa/S we could also use TIM5 IC4 instead. */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC4 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC4IE +#define PPM_CC_IF TIM_SR_CC4IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO3 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#else +#error "Unknown PPM config" + +#endif // PPM_CONFIG + +/* ADC */ + +// active ADC +#define USE_AD1 1 +#define USE_AD1_1 1 +#define USE_AD1_2 1 +#define USE_AD1_3 1 +#define USE_AD1_4 1 + +/* + * I2C + * + */ +/* Servo1 & Servo2 */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO6 +#define I2C1_GPIO_SDA GPIO7 + +/* GPS TX & RX */ +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + +/* + * PWM + * + */ +#define PWM_USE_TIM4 1 +#define PWM_USE_TIM5 1 + + +#if USE_SERVOS_1AND2 + #if USE_I2C1 + #error "You cannot USE_SERVOS_1AND2 and USE_I2C1 at the same time" + #else + #define ACTUATORS_PWM_NB 6 + #define USE_PWM1 1 + #define USE_PWM2 1 + #define PWM_USE_TIM4 1 + #endif +#else + #define ACTUATORS_PWM_NB 4 +#endif + +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 +#define USE_PWM6 1 + +// Servo numbering on LisaM silkscreen/docs starts with 1 + +/* PWM_SERVO_x is the index of the servo in the actuators_pwm_values array */ +/* Because PWM1 and 2 can be disabled we put them at the index 4 & 5 so that it + * is easy to disable. + */ +#if USE_PWM1 +#define PWM_SERVO_1 4 +#define PWM_SERVO_1_TIMER TIM4 +#define PWM_SERVO_1_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_1_GPIO GPIOB +#define PWM_SERVO_1_PIN GPIO6 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 5 +#define PWM_SERVO_2_TIMER TIM4 +#define PWM_SERVO_2_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_2_GPIO GPIOB +#define PWM_SERVO_2_PIN GPIO7 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 0 +#define PWM_SERVO_3_TIMER TIM4 +#define PWM_SERVO_3_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO8 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 1 +#define PWM_SERVO_4_TIMER TIM4 +#define PWM_SERVO_4_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO9 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 2 +#define PWM_SERVO_5_TIMER TIM5 +#define PWM_SERVO_5_RCC_IOP RCC_APB2ENR_IOPAEN +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO0 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + +#if USE_PWM6 +#define PWM_SERVO_6 3 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_RCC_IOP RCC_APB2ENR_IOPAEN +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO1 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +/* servos 1-4 or 3-4 on TIM4 depending on USE_SERVOS_1AND2 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) +/* servos 5-6 on TIM5 */ +#define PWM_TIM5_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT) + +#endif /* CONFIG_LISA_S_0_1_H */ From 83492da11e950ed7c20210dd3e46c9f302819faa Mon Sep 17 00:00:00 2001 From: fvantienen Date: Tue, 20 Aug 2013 15:43:58 +0200 Subject: [PATCH 22/65] Walkera Heli flying good. --- conf/airframes/esden/quady_ls01pwm.xml | 173 +++++++------- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 2 +- conf/airframes/walkera_heli.xml | 218 ++++++++++++++++++ conf/boards/lisa_s_0.1.makefile | 4 +- .../radio_control_superbitrf_rc.makefile | 2 +- .../shared/telemetry_superbitrf.makefile | 7 +- conf/messages.xml | 4 +- conf/telemetry/default_rotorcraft.xml | 15 ++ sw/airborne/boards/lisa_s_0.1.h | 8 + sw/airborne/firmwares/rotorcraft/telemetry.h | 4 +- sw/airborne/subsystems/datalink/superbitrf.c | 72 ++++-- sw/airborne/subsystems/datalink/superbitrf.h | 6 +- 12 files changed, 408 insertions(+), 107 deletions(-) create mode 100644 conf/airframes/walkera_heli.xml diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 9dc6032179e..23609269749 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -12,25 +12,25 @@ - - - - + + + + - - - + + + - - - - + + + +
@@ -39,91 +39,95 @@ - - - + + +
- + - +
- - + +
- +
- - - + + + - + + + + +
- - - - - - - + + + + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - + - +
@@ -131,19 +135,19 @@
- - - - + + + + - - - + + + - + - - + +
@@ -159,11 +163,11 @@
- +
- +
@@ -174,10 +178,11 @@ - - - - + + + + + @@ -185,21 +190,21 @@ - + - + - - - + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ +
+ + + +
+ +
+ +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/boards/lisa_s_0.1.makefile b/conf/boards/lisa_s_0.1.makefile index ffaeaf9a6c2..51d99547e3c 100644 --- a/conf/boards/lisa_s_0.1.makefile +++ b/conf/boards/lisa_s_0.1.makefile @@ -28,10 +28,10 @@ FLASH_MODE ?= SWD # # default LED configuration # -RADIO_CONTROL_LED ?= none +RADIO_CONTROL_LED ?= 3 BARO_LED ?= none AHRS_ALIGNER_LED ?= 2 -GPS_LED ?= 3 +GPS_LED ?= none SYS_TIME_LED ?= 1 # diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile index 3774d22f3bf..deac945a9ac 100644 --- a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile @@ -3,7 +3,7 @@ # ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf_rc.h\" -ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1 +ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 ifneq ($(RADIO_CONTROL_LED),none) ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) diff --git a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile index 20781731ecf..01f59768ea3 100644 --- a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile @@ -2,10 +2,15 @@ # The superbitRF module as telemetry downlink/uplink # # +ap.CFLAGS += -DUSE_$(MODEM_PORT) +ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF -ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1 + +#ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF +#ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF +#ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 ap.srcs += peripherals/cyrf6936.c ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c diff --git a/conf/messages.xml b/conf/messages.xml index 75f5034bf90..0704620416d 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -606,7 +606,9 @@ - + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 976962721cc..30b46ac09f3 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -6,6 +6,21 @@ + + + + + + + + + + + + + + + diff --git a/sw/airborne/boards/lisa_s_0.1.h b/sw/airborne/boards/lisa_s_0.1.h index fa3c3d49cbc..1182c1954e2 100644 --- a/sw/airborne/boards/lisa_s_0.1.h +++ b/sw/airborne/boards/lisa_s_0.1.h @@ -418,4 +418,12 @@ /* servos 5-6 on TIM5 */ #define PWM_TIM5_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT) +/* SuperbitRF mounted */ +#define SUPERBITRF_SPI_DEV spi2 +#define SUPERBITRF_RST_PORT GPIOC +#define SUPERBITRF_RST_PIN GPIO9 +#define SUPERBITRF_DRDY_PORT GPIOC +#define SUPERBITRF_DRDY_PIN GPIO6 +#define SUPERBITRF_FORCE_DSM2 FALSE + #endif /* CONFIG_LISA_S_0_1_H */ diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index e5f9df37e6a..4891f0a0dfa 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -140,7 +140,9 @@ DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ &superbitrf.status, \ &superbitrf.cyrf6936.status, \ - &superbitrf.packet_count, \ + &superbitrf.irq_count, \ + &superbitrf.rx_packet_count, \ + &superbitrf.tx_packet_count, \ &superbitrf.transfer_timeouts, \ &superbitrf.resync_count, \ &superbitrf.uplink_count, \ diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 08e1a3128ec..3557037123f 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -36,27 +36,33 @@ #ifndef SUPERBITRF_SPI_DEV #define SUPERBITRF_SPI_DEV spi1 #endif +PRINT_CONFIG_VAR(SUPERBITRF_SPI_DEV); /* Default SuperbitRF RST PORT and PIN */ #ifndef SUPERBITRF_RST_PORT #define SUPERBITRF_RST_PORT GPIOC #endif +PRINT_CONFIG_VAR(SUPERBITRF_RST_PORT); #ifndef SUPERBITRF_RST_PIN #define SUPERBITRF_RST_PIN GPIO12 #endif +PRINT_CONFIG_VAR(SUPERBITRF_RST_PIN); /* Default SuperbitRF DRDY(IRQ) PORT and PIN */ #ifndef SUPERBITRF_DRDY_PORT #define SUPERBITRF_DRDY_PORT GPIOB #endif +PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PORT); #ifndef SUPERBITRF_DRDY_PIN #define SUPERBITRF_DRDY_PIN GPIO1 #endif +PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PIN); /* Default forcing in DSM2 mode is false */ #ifndef SUPERBITRF_FORCE_DSM2 #define SUPERBITRF_FORCE_DSM2 FALSE #endif +PRINT_CONFIG_VAR(SUPERBITRF_FORCE_DSM2); /* The superbitRF structure */ struct SuperbitRF superbitrf; @@ -175,7 +181,8 @@ void superbitrf_init(void) { superbitrf.status = SUPERBITRF_UNINIT; superbitrf.state = 0; superbitrf.timer = 0; - superbitrf.packet_count = 0; + superbitrf.rx_packet_count = 0; + superbitrf.tx_packet_count = 0; // Setup the transmit buffer superbitrf.tx_insert_idx = 0; @@ -188,7 +195,7 @@ void superbitrf_init(void) { gpio_setup_input(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN); // Initialize the cyrf6936 chip - cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 1, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN); + cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 2, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN); } /** @@ -196,6 +203,7 @@ void superbitrf_init(void) { */ void superbitrf_event(void) { uint8_t i, pn_row, packet_size, data_code[16], tx_packet[16]; + static bool_t start_transfer = TRUE; // Check if the cyrf6936 isn't busy and the uperbitrf is initialized if(superbitrf.cyrf6936.status != CYRF6936_IDLE) @@ -207,13 +215,14 @@ void superbitrf_event(void) { if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { // Receive the packet cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); + superbitrf.irq_count++; } /* Check if it is a valid receive */ if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { // Handle the packet received superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); - superbitrf.packet_count++; + superbitrf.rx_packet_count++; // Reset the packet receiving superbitrf.cyrf6936.has_irq = FALSE; @@ -223,6 +232,7 @@ void superbitrf_event(void) { if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) { // Handle the send packet superbitrf_send_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_TXE_IRQ)); + superbitrf.tx_packet_count++; // Reset the packet receiving superbitrf.cyrf6936.has_irq = FALSE; @@ -238,9 +248,9 @@ void superbitrf_event(void) { if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) { // Check if need to go to bind or transfer if(gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) - superbitrf.status = SUPERBITRF_INIT_BINDING; - else - superbitrf.status = SUPERBITRF_INIT_TRANSFER; + start_transfer = FALSE; + + superbitrf.status = SUPERBITRF_INIT_BINDING; } break; @@ -268,16 +278,18 @@ void superbitrf_event(void) { } break; - /* When the superbitrf is initializing transfer */ - case SUPERBITRF_INIT_TRANSFER: - // Generate the DSMX channels - superbitrf_gen_dsmx_channels(); + /* When the superbitrf is initializing transfer */ + case SUPERBITRF_INIT_TRANSFER: + // Generate the DSMX channels + superbitrf_gen_dsmx_channels(); - // Try to write the transfer config - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4); + // Try to write the transfer config + if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) { superbitrf.resync_count = 0; superbitrf.status = SUPERBITRF_SYNCING_A; - break; + superbitrf.state = 1; + } + break; /* When the superbitrf is in binding mode */ case SUPERBITRF_BINDING: @@ -313,6 +325,38 @@ void superbitrf_event(void) { superbitrf.state++; break; default: + // Check if need to go to transfer + if(start_transfer) { + // Initialize the binding values + #ifdef RADIO_TRANSMITTER_ID + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_ID); + superbitrf.bind_mfg_id32 = RADIO_TRANSMITTER_ID; + superbitrf.bind_mfg_id[0] = (superbitrf.bind_mfg_id32 &0xFF); + superbitrf.bind_mfg_id[1] = (superbitrf.bind_mfg_id32 >>8 &0xFF); + superbitrf.bind_mfg_id[2] = (superbitrf.bind_mfg_id32 >>16 &0xFF); + superbitrf.bind_mfg_id[3] = (superbitrf.bind_mfg_id32 >>24 &0xFF); + + // Calculate some values based on the bind MFG id + superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]); + superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07; + superbitrf.data_col = 7 - superbitrf.sop_col; + #endif + #ifdef RADIO_TRANSMITTER_CHAN + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_CHAN); + superbitrf.num_channels = RADIO_TRANSMITTER_CHAN; + #endif + #ifdef RADIO_TRANSMITTER_PROTOCOL + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_PROTOCOL); + superbitrf.protocol = RADIO_TRANSMITTER_PROTOCOL; + superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; + #endif + + // Start transfer + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + break; + } + // Set the timer superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF; superbitrf.state = 0; @@ -568,7 +612,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui superbitrf.bind_mfg_id[2] = ~packet[2]; superbitrf.bind_mfg_id[3] = ~packet[3]; superbitrf.bind_mfg_id32 = ((superbitrf.bind_mfg_id[3] &0xFF) << 24 | (superbitrf.bind_mfg_id[2] &0xFF) << 16 | - (superbitrf.bind_mfg_id[1] &0xFF) << 8 | (superbitrf.bind_mfg_id[0] &0xFF)) &0xFFFFFFFF; + (superbitrf.bind_mfg_id[1] &0xFF) << 8 | (superbitrf.bind_mfg_id[0] &0xFF)); superbitrf.num_channels = packet[11]; superbitrf.protocol = packet[12]; superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index c6ac544d6e7..f3b19b4498d 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -37,7 +37,7 @@ #define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ #define SUPERBITRF_RECV_TIME 25000 /**< The time to wait for a transfer packet on a channel in microseconds */ #define SUPERBITRF_DATAWAIT_TIME 500 /**< The time to wait after RC receive to send a data packet in microseconds */ -#define SUPERBITRF_DATARECV_TIME 9000 /**< The time to wait for a data packet on a channel in microseconds */ +#define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ /* The different statuses the superbitRF can be in */ enum SuperbitRFStatus { @@ -81,7 +81,9 @@ struct SuperbitRF { uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ uint8_t channel_idx; /**< The current channel index */ uint8_t channel; /**< The current channel number */ - uint32_t packet_count; /**< How many packets are received(also the invalid) */ + uint32_t irq_count; /**< How many interrupts are made */ + uint32_t rx_packet_count; /**< How many packets are received(also the invalid) */ + uint32_t tx_packet_count; /**< How many packets are send(also the invalid) */ uint32_t uplink_count; /**< How many valid uplink packages are received */ uint32_t rc_count; /**< How many valid RC packages are received */ From 2064e65ae710c34a40a8f2c185fb38cb5820f606 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 21 Aug 2013 14:36:05 +0200 Subject: [PATCH 23/65] Fixed binding --- conf/airframes/esden/quady_ls01pwm.xml | 6 +- conf/airframes/walkera_heli.xml | 51 +++- .../walkera_heli.xml.2013-08-21_140323 | 249 ++++++++++++++++++ conf/telemetry/default_rotorcraft.xml | 8 +- sw/airborne/peripherals/cyrf6936.c | 4 +- sw/airborne/subsystems/datalink/superbitrf.c | 107 +++++--- sw/airborne/subsystems/datalink/superbitrf.h | 4 +- 7 files changed, 368 insertions(+), 61 deletions(-) create mode 100644 conf/airframes/walkera_heli.xml.2013-08-21_140323 diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 23609269749..b008276b187 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -179,9 +179,9 @@ - - - + + + diff --git a/conf/airframes/walkera_heli.xml b/conf/airframes/walkera_heli.xml index 7d73c38286b..3fbc382ca28 100644 --- a/conf/airframes/walkera_heli.xml +++ b/conf/airframes/walkera_heli.xml @@ -27,8 +27,8 @@ - - + + @@ -40,21 +40,52 @@ - -
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- +
+
@@ -131,9 +162,9 @@ - - - + + + @@ -167,7 +198,7 @@ - + @@ -180,7 +211,7 @@ - + diff --git a/conf/airframes/walkera_heli.xml.2013-08-21_140323 b/conf/airframes/walkera_heli.xml.2013-08-21_140323 new file mode 100644 index 00000000000..b31e3d58822 --- /dev/null +++ b/conf/airframes/walkera_heli.xml.2013-08-21_140323 @@ -0,0 +1,249 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 30b46ac09f3..51a5588909b 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -99,13 +99,7 @@ - - - - - - - + diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 5bcd9951556..691049baac5 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -67,9 +67,9 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_ /* Reset the CYRF6936 chip (busy waiting) */ gpio_setup_output(rst_port, rst_pin); - gpio_output_high(rst_port, rst_pin); + gpio_set(rst_port, rst_pin); sys_time_usleep(100); - gpio_output_low(rst_port, rst_pin); + gpio_clear(rst_port, rst_pin); sys_time_usleep(100); /* Get the MFG ID */ diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 3557037123f..4231d1dd901 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -202,7 +202,8 @@ void superbitrf_init(void) { * The superbitrf on event call */ void superbitrf_event(void) { - uint8_t i, pn_row, packet_size, data_code[16], tx_packet[16]; + uint8_t i, pn_row, data_code[16]; + static uint8_t packet_size, tx_packet[16]; static bool_t start_transfer = TRUE; // Check if the cyrf6936 isn't busy and the uperbitrf is initialized @@ -286,6 +287,8 @@ void superbitrf_event(void) { // Try to write the transfer config if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) { superbitrf.resync_count = 0; + superbitrf.packet_loss = FALSE; + superbitrf.packet_loss_bit = 0; superbitrf.status = SUPERBITRF_SYNCING_A; superbitrf.state = 1; } @@ -391,24 +394,32 @@ void superbitrf_event(void) { superbitrf.state++; break; case 3: - // Send a packet - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { - tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; - } else { - tx_packet[0] = superbitrf.bind_mfg_id[2]; - tx_packet[1] = (superbitrf.bind_mfg_id[3])+1; - } + // Create a new packet when no packet loss + if(!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } - packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); - if(packet_size > 14) - packet_size = 14; + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; - for(i = 0; i < packet_size; i++) - tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + } + // Send a packet cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); - superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + + // Update the packet extraction + if(!superbitrf.packet_loss) + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + superbitrf.state++; break; case 4: @@ -485,29 +496,37 @@ void superbitrf_event(void) { break; case 2: // Wait before sending - superbitrf.state++; - //if (superbitrf.timer < get_sys_time_usec()) - //superbitrf.state++; + //superbitrf.state++; + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; break; case 3: - // Send a packet - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { - tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1; - } else { - tx_packet[0] = superbitrf.bind_mfg_id[2]; - tx_packet[1] = (superbitrf.bind_mfg_id[3])+1; - } + // Create a new packet when no packet loss + if(!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } - packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); - if(packet_size > 14) - packet_size = 14; + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; - for(i = 0; i < packet_size; i++) - tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + } + // Send a packet cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); - superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + + // Update the packet extraction + if(!superbitrf.packet_loss) + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + superbitrf.state++; break; case 4: @@ -658,8 +677,14 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { superbitrf.uplink_count++; + // Check if it is a data loss packet + if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) + superbitrf.packet_loss = TRUE; + else + superbitrf.packet_loss = FALSE; + // When it is a data packet, parse the packet if not busy already - if(!dl_msg_available) { + if(!dl_msg_available && !superbitrf.packet_loss) { for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { parse_pprz(&superbitrf.rx_transport, packet[i]); @@ -696,14 +721,14 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui } if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; @@ -760,14 +785,14 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui } if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; } if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; @@ -795,8 +820,14 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui } else { superbitrf.uplink_count++; + // Check if it is a data loss packet + if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) + superbitrf.packet_loss = TRUE; + else + superbitrf.packet_loss = FALSE; + // When it is a data packet, parse the packet if not busy already - if(!dl_msg_available) { + if(!dl_msg_available && !superbitrf.packet_loss) { for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { parse_pprz(&superbitrf.rx_transport, packet[i]); diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index f3b19b4498d..4312c0f3002 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -36,7 +36,7 @@ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ #define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ #define SUPERBITRF_RECV_TIME 25000 /**< The time to wait for a transfer packet on a channel in microseconds */ -#define SUPERBITRF_DATAWAIT_TIME 500 /**< The time to wait after RC receive to send a data packet in microseconds */ +#define SUPERBITRF_DATAWAIT_TIME 100 /**< The time to wait after RC receive to send a data packet in microseconds */ #define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ /* The different statuses the superbitRF can be in */ @@ -77,6 +77,8 @@ struct SuperbitRF { uint8_t timeouts; /**< The amount of timeouts */ uint32_t transfer_timeouts; /**< The amount of timeouts during transfer */ uint32_t resync_count; /**< The amount of resyncs needed during transfer */ + uint8_t packet_loss_bit; /**< The packet loss indicating bit */ + bool_t packet_loss; /**< When we have packet loss last packet */ uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ uint8_t channel_idx; /**< The current channel index */ From d3ab30d97e5d4cca9164f3635174b2ace4575f30 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 21 Aug 2013 14:47:51 +0200 Subject: [PATCH 24/65] Edited config example --- conf/conf.xml.example | 252 ++++++++++++++++++++++-------------------- 1 file changed, 133 insertions(+), 119 deletions(-) diff --git a/conf/conf.xml.example b/conf/conf.xml.example index eac6d746c4b..44f7ffad911 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -1,49 +1,14 @@ - - - - - - - - - + + + + - - - - - - + - + + + + + + From 0e94c634a96f9d95e362206ce598d7f30381dc73 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Thu, 22 Aug 2013 16:26:56 +0200 Subject: [PATCH 25/65] Better Body to IMU and superbitrf telemetry --- conf/airframes/esden/quady_ls01pwm.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index b008276b187..07c92c0557b 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -48,9 +48,9 @@
- - - + + +
@@ -200,7 +200,7 @@ - + From 5f34ae6e8ee4ab0395f2dbbd1c7a2b442ad9e038 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Tue, 27 Aug 2013 19:31:51 +0200 Subject: [PATCH 26/65] Fast commit --- conf/airframes/walkera_V120D02S.xml | 248 +++++++++++++++++++ conf/settings/superbitrf.xml | 14 ++ sw/airborne/subsystems/datalink/superbitrf.c | 14 +- sw/airborne/subsystems/datalink/superbitrf.h | 2 +- 4 files changed, 270 insertions(+), 8 deletions(-) create mode 100644 conf/airframes/walkera_V120D02S.xml create mode 100644 conf/settings/superbitrf.xml diff --git a/conf/airframes/walkera_V120D02S.xml b/conf/airframes/walkera_V120D02S.xml new file mode 100644 index 00000000000..d25f606cd21 --- /dev/null +++ b/conf/airframes/walkera_V120D02S.xml @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/superbitrf.xml b/conf/settings/superbitrf.xml new file mode 100644 index 00000000000..93596bdab4d --- /dev/null +++ b/conf/settings/superbitrf.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 4231d1dd901..f26ede432e5 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -60,7 +60,7 @@ PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PIN); /* Default forcing in DSM2 mode is false */ #ifndef SUPERBITRF_FORCE_DSM2 -#define SUPERBITRF_FORCE_DSM2 FALSE +#define SUPERBITRF_FORCE_DSM2 TRUE #endif PRINT_CONFIG_VAR(SUPERBITRF_FORCE_DSM2); @@ -434,7 +434,7 @@ void superbitrf_event(void) { } // Switch channel, sop code, data code and crc - superbitrf.channel = (superbitrf.channel + 1) % 0x62; //TODO fix define + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define superbitrf.crc_seed = ~superbitrf.crc_seed; pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; @@ -483,8 +483,8 @@ void superbitrf_event(void) { cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); superbitrf.state++; - // When we don't have enough time - if(superbitrf.timeouts > 0 || superbitrf.timing2 < 10000 || superbitrf.timing1 > 10000) { + // Only send on channel 2 + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { superbitrf.state = 8; // Set the timer superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; @@ -506,10 +506,10 @@ void superbitrf_event(void) { superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + tx_packet[1] = ((~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; } else { tx_packet[0] = superbitrf.bind_mfg_id[2]; - tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + tx_packet[1] = ((superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; } packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); @@ -678,7 +678,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui superbitrf.uplink_count++; // Check if it is a data loss packet - if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) + if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF) superbitrf.packet_loss = TRUE; else superbitrf.packet_loss = FALSE; diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index 4312c0f3002..907cbb52228 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -35,7 +35,7 @@ /* The timings in microseconds */ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ #define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ -#define SUPERBITRF_RECV_TIME 25000 /**< The time to wait for a transfer packet on a channel in microseconds */ +#define SUPERBITRF_RECV_TIME 22000 /**< The time to wait for a transfer packet on a channel in microseconds */ #define SUPERBITRF_DATAWAIT_TIME 100 /**< The time to wait after RC receive to send a data packet in microseconds */ #define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ From 664df087fd511cc1aef4bac0d8ca395b956b1eca Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 27 Aug 2013 20:17:17 +0200 Subject: [PATCH 27/65] [airframes] MicroMavRick --- conf/airframes/CDW/MicroMavRickLisaS.xml | 191 +++++++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 conf/airframes/CDW/MicroMavRickLisaS.xml diff --git a/conf/airframes/CDW/MicroMavRickLisaS.xml b/conf/airframes/CDW/MicroMavRickLisaS.xml new file mode 100644 index 00000000000..18ebcc75834 --- /dev/null +++ b/conf/airframes/CDW/MicroMavRickLisaS.xml @@ -0,0 +1,191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ + From 9a672b08692e87144c6cb976d47815020a1e08da Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 28 Aug 2013 12:54:41 +0200 Subject: [PATCH 28/65] Magneto axis correction --- conf/airframes/esden/quady_ls01pwm.xml | 35 ++++++++++++++++--- .../shared/imu_lisa_s_v0.1.makefile | 2 +- sw/airborne/subsystems/datalink/superbitrf.c | 17 ++++++++- sw/airborne/subsystems/datalink/superbitrf.h | 1 + .../subsystems/imu/imu_aspirin_2_spi.c | 6 ++++ 5 files changed, 54 insertions(+), 7 deletions(-) diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 07c92c0557b..c3b9d8fcb2f 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -45,12 +45,37 @@
- -
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
@@ -179,9 +204,9 @@ - - - + + + diff --git a/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile index 536031592b3..7f75e5c3a98 100644 --- a/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile @@ -53,7 +53,7 @@ include $(CFG_SHARED)/spi_master.makefile IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0 IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1 -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 -DLISA_S # Slave select configuration # SLAVE0 is on PA4 (NSS) (MPU600 CS) IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0 diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index f26ede432e5..40276314d00 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -426,6 +426,19 @@ void superbitrf_event(void) { //TODO: check timeout? (Waiting for send) break; case 5: + superbitrf.state = 7; + break; + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECVB_TIME) % 0xFFFFFFFF; + superbitrf.state++; + break; + case 6: + // Wait for telemetry data + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; + break; + case 7: // When DSMX we don't need to switch if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { superbitrf.state++; @@ -445,7 +458,7 @@ void superbitrf_event(void) { superbitrf.state++; break; - case 6: + case 8: // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; @@ -826,6 +839,8 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui else superbitrf.packet_loss = FALSE; + superbitrf.packet_loss = FALSE; + // When it is a data packet, parse the packet if not busy already if(!dl_msg_available && !superbitrf.packet_loss) { for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index 907cbb52228..8baa147f1a0 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -38,6 +38,7 @@ #define SUPERBITRF_RECV_TIME 22000 /**< The time to wait for a transfer packet on a channel in microseconds */ #define SUPERBITRF_DATAWAIT_TIME 100 /**< The time to wait after RC receive to send a data packet in microseconds */ #define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ +#define SUPERBITRF_DATARECVB_TIME 6000 /**< The time to wait for a data packet on a channel during bind in microseconds */ /* The different statuses the superbitRF can be in */ enum SuperbitRFStatus { diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 0fcd038a6e5..207b0df6aba 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -174,10 +174,16 @@ void imu_aspirin2_event(void) -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); +#else +#ifdef LISA_S + RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); + VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); + VECT3_COPY(imu.mag_unscaled, mag); #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) +#endif #endif imu_aspirin2.mpu.data_available = FALSE; imu_aspirin2.gyro_valid = TRUE; From b58bf64e7fb2cab4162d54d10b6f1ec311fb9a59 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 28 Aug 2013 12:55:55 +0200 Subject: [PATCH 29/65] Config file with airframes added --- conf/conf.xml.example | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/conf/conf.xml.example b/conf/conf.xml.example index 44f7ffad911..f51432fe663 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -19,16 +19,6 @@ settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" gui_color="blue" /> - + Date: Wed, 28 Aug 2013 15:48:18 +0200 Subject: [PATCH 30/65] channels reversed --- conf/airframes/CDW/MicroMavRickLisaS.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/airframes/CDW/MicroMavRickLisaS.xml b/conf/airframes/CDW/MicroMavRickLisaS.xml index 18ebcc75834..c183d7664ca 100644 --- a/conf/airframes/CDW/MicroMavRickLisaS.xml +++ b/conf/airframes/CDW/MicroMavRickLisaS.xml @@ -39,8 +39,8 @@ - - + + From 8a9011f9007dc75898ffe5030e4c887b2b69aeb0 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Wed, 28 Aug 2013 16:04:28 +0200 Subject: [PATCH 31/65] Airframes walkera update --- conf/airframes/esden/quady_ls01pwm.xml | 6 ++++++ conf/airframes/walkera_V120D02S.xml | 20 ++++++++++---------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index c3b9d8fcb2f..ca1279a830d 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -76,6 +76,11 @@ + + + + +
@@ -85,6 +90,7 @@
+
diff --git a/conf/airframes/walkera_V120D02S.xml b/conf/airframes/walkera_V120D02S.xml index d25f606cd21..950f93139f2 100644 --- a/conf/airframes/walkera_V120D02S.xml +++ b/conf/airframes/walkera_V120D02S.xml @@ -12,9 +12,9 @@ - - - + + + @@ -34,9 +34,9 @@ - - - + + + @@ -195,10 +195,10 @@ - - - - + + + + From 97563ded23bcee916f7e7c233620ed352825c2f8 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Thu, 29 Aug 2013 14:56:02 +0200 Subject: [PATCH 32/65] [superbitrf] Cleanup --- conf/airframes/TUDelft/IMAV2013/conf.xml | 45 +++ .../IMAV2013/mavrick_lisa_s.xml} | 26 +- .../IMAV2013/quadrotor_lisa_s.xml} | 143 +++++----- .../IMAV2013}/walkera_V120D02S.xml | 53 +--- .../IMAV2013/walkera_genius_v1.xml} | 50 +--- conf/airframes/esden/quady_ls01pwm.xml | 210 ++++++-------- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 46 +-- conf/airframes/examples/quadrotor_lisa_s.xml | 239 ++++++++++++++++ conf/conf.xml.example | 262 +++++++++--------- conf/firmwares/rotorcraft.makefile | 4 +- conf/settings/superbitrf.xml | 14 - conf/telemetry/default_rotorcraft.xml | 25 +- sw/airborne/boards/lisa_s/baro_board.h | 55 +--- 13 files changed, 645 insertions(+), 527 deletions(-) create mode 100644 conf/airframes/TUDelft/IMAV2013/conf.xml rename conf/airframes/{CDW/MicroMavRickLisaS.xml => TUDelft/IMAV2013/mavrick_lisa_s.xml} (90%) rename conf/airframes/{walkera_heli.xml.2013-08-21_140323 => TUDelft/IMAV2013/quadrotor_lisa_s.xml} (62%) rename conf/airframes/{ => TUDelft/IMAV2013}/walkera_V120D02S.xml (80%) rename conf/airframes/{walkera_heli.xml => TUDelft/IMAV2013/walkera_genius_v1.xml} (81%) create mode 100644 conf/airframes/examples/quadrotor_lisa_s.xml delete mode 100644 conf/settings/superbitrf.xml diff --git a/conf/airframes/TUDelft/IMAV2013/conf.xml b/conf/airframes/TUDelft/IMAV2013/conf.xml new file mode 100644 index 00000000000..76978844ac1 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/conf.xml @@ -0,0 +1,45 @@ + + + + + + + + + diff --git a/conf/airframes/CDW/MicroMavRickLisaS.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml similarity index 90% rename from conf/airframes/CDW/MicroMavRickLisaS.xml rename to conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml index c183d7664ca..bc5da2d9bca 100644 --- a/conf/airframes/CDW/MicroMavRickLisaS.xml +++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml @@ -1,24 +1,26 @@ - + - + - - - - - - - - + + + + + + diff --git a/conf/airframes/walkera_heli.xml.2013-08-21_140323 b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml similarity index 62% rename from conf/airframes/walkera_heli.xml.2013-08-21_140323 rename to conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index b31e3d58822..8a89ed12d7a 100644 --- a/conf/airframes/walkera_heli.xml.2013-08-21_140323 +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -2,21 +2,20 @@ - + - - - - - + + + + @@ -27,23 +26,29 @@ - - - - - - - - - - - + + + + + +
+ + + + + + + + + +
+
- - - + + + @@ -64,41 +69,45 @@ - - - - - - - - - - - - + + + + + + + + + + + +
- +
- +
- - - + + + - + + + + +
@@ -130,15 +139,15 @@ - - - + + + - - - + + + - + @@ -188,6 +197,12 @@
+
+ + + +
+ @@ -195,31 +210,33 @@ - - - - + + + + + + + + + + - - + - + - - @@ -235,15 +252,5 @@ - diff --git a/conf/airframes/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml similarity index 80% rename from conf/airframes/walkera_V120D02S.xml rename to conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 950f93139f2..5df82f5f459 100644 --- a/conf/airframes/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -1,15 +1,15 @@ - + - + @@ -64,18 +64,13 @@ - + - - - - -
@@ -85,13 +80,12 @@
- +
- @@ -99,7 +93,6 @@ -
@@ -195,8 +188,8 @@ - - + + @@ -216,33 +209,5 @@ - - - - - - - - - - - - - - - - - - diff --git a/conf/airframes/walkera_heli.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml similarity index 81% rename from conf/airframes/walkera_heli.xml rename to conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 3fbc382ca28..3c66aae2b2e 100644 --- a/conf/airframes/walkera_heli.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -1,15 +1,15 @@ - + - + @@ -91,7 +91,6 @@
- @@ -99,7 +98,6 @@ -
@@ -195,10 +193,10 @@ - - - - + + + + @@ -211,39 +209,11 @@ - + - - - - - - - - - - - - - - - - - - diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index ca1279a830d..9dc6032179e 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -12,25 +12,25 @@ - - - - + + + + - - - + + + - - - - + + + +
@@ -39,126 +39,91 @@ - - - + + +
+ +
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + +
- - + +
- - +
- - - + + + - - - - - +
- - - - - - - + + + + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - + - +
@@ -166,19 +131,19 @@
- - - - + + + + - - - + + + - + - - + +
@@ -194,11 +159,11 @@
- +
- +
@@ -209,11 +174,10 @@ - - - - - + + + + @@ -221,21 +185,21 @@ - + - + - - - + + + - + @@ -39,10 +41,10 @@ - - - - + + + + @@ -59,9 +61,9 @@ - - - + + +
@@ -88,7 +90,7 @@ - +
@@ -153,22 +155,22 @@ - - - + + + - - - + + + - - - + + + - - - + + +
@@ -189,7 +191,7 @@
- + diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml new file mode 100644 index 00000000000..a46f343030e --- /dev/null +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/conf.xml.example b/conf/conf.xml.example index f51432fe663..893aba9cec0 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -1,14 +1,59 @@ + - + + + + + + + + + - - + + + + + + - - - - - - - - - diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 575c28dabb8..75aeb5062e8 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -194,7 +194,9 @@ else ifeq ($(BOARD), lisa_s) ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 ap.CFLAGS += -DMS5611_SPI_DEV=spi1 ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 - ap.srcs += boards/lisa_m/baro_ms5611_spi.c + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c ap.CFLAGS += -DBARO_MS5611_SPI # Lia baro (no bmp onboard) diff --git a/conf/settings/superbitrf.xml b/conf/settings/superbitrf.xml deleted file mode 100644 index 93596bdab4d..00000000000 --- a/conf/settings/superbitrf.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 51a5588909b..2a79fd2c049 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -6,21 +6,6 @@ - - - - - - - - - - - - - - - @@ -33,7 +18,7 @@ - + @@ -99,7 +84,13 @@ - + + + + + + + diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h index 42014ddeee5..7581ef4de33 100644 --- a/sw/airborne/boards/lisa_s/baro_board.h +++ b/sw/airborne/boards/lisa_s/baro_board.h @@ -9,59 +9,8 @@ #include "std.h" -// for right now we abuse this file for the ms5611 baro on aspirin as well -#if !BARO_MS5611_I2C && !BARO_MS5611 +extern void baro_event(void (*b_abs_handler)(void)); -#include "mcu_periph/i2c.h" - -// absolute addr -#define BMP085_ADDR 0xEE -// Over sample setting (0-3) -#define BMP085_OSS 3 - -enum LisaBaroStatus { - LBS_UNINITIALIZED, - LBS_REQUEST, - LBS_READING, - LBS_READ, - LBS_REQUEST_TEMP, - LBS_READING_TEMP, - LBS_READ_TEMP, -}; - -struct BaroBoard { - enum LisaBaroStatus status; -}; - -struct bmp085_baro_calibration { - // These values come from EEPROM on sensor - int16_t ac1; - int16_t ac2; - int16_t ac3; - uint16_t ac4; - uint16_t ac5; - uint16_t ac6; - int16_t b1; - int16_t b2; - int16_t mb; - int16_t mc; - int16_t md; - - // These values are calculated - int32_t b5; -}; - -extern struct BaroBoard baro_board; -extern struct i2c_transaction baro_trans; -extern struct bmp085_baro_calibration calibration; - -extern void baro_board_send_reset(void); -extern void baro_board_send_config(void); - -#endif // !BARO_MS5611_xx - -extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler) +#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) #endif /* BOARDS_LISA_M_BARO_H */ From e613e79fbe9c0c387cecd161877a8c802b2967a0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Aug 2013 15:28:33 +0200 Subject: [PATCH 33/65] [tools] calibration: automatically choose noise_threshold --- sw/tools/calibration/calibrate.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 2fb739ff186..9aaf3940b6e 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -20,6 +20,7 @@ # Boston, MA 02111-1307, USA. # +from __future__ import print_function import sys import os @@ -66,12 +67,12 @@ def main(): sensor_ref = 9.81 sensor_res = 10 noise_window = 20; - noise_threshold = 40; + #noise_threshold = 40; elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 noise_window = 10; - noise_threshold = 1000; + #noise_threshold = 1000; if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -86,12 +87,20 @@ def main(): if options.verbose: print("found "+str(len(measurements))+" records") + # estimate the noise threshold + # find the median of measurement vector lenght + meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) + # set noise threshold to be below 10% of that + noise_threshold = meas_median * 0.1 + if options.verbose: + print("Using noise threshold of", noise_threshold, "for filtering.") + # filter out noisy measurements flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold) if options.verbose: - print("remaining "+str(len(flt_meas))+" after low pass") + print("remaining "+str(len(flt_meas))+" after filtering") if len(flt_meas) == 0: - print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after low pass!") + print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after filtering!") sys.exit(1) # get an initial min/max guess From a244d214a5ba1c6bddc99e6edd66daf33aafd509 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 29 Aug 2013 17:44:00 +0200 Subject: [PATCH 34/65] [ARDrone] LED driver fixed --- .../boards/ardrone/actuators_ardrone2_raw.c | 48 +++++++++++++++++-- .../boards/ardrone/actuators_ardrone2_raw.h | 2 + sw/airborne/boards/ardrone/baro_board.c | 2 +- sw/airborne/boards/ardrone/navdata.c | 4 +- 4 files changed, 50 insertions(+), 6 deletions(-) diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 353c5a5050b..1989b492e73 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -123,7 +123,10 @@ void actuators_ardrone_init(void) gpio_set(107,1); //all leds green -// actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN); + //actuators_ardrone_set_leds(0,0,0,0); + //actuators_ardrone_set_leds(MOT_LEDRED, MOT_LEDRED, MOT_LEDRED, MOT_LEDRED); + //actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN); + actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); } int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { @@ -136,6 +139,13 @@ void actuators_ardrone_commit(void) actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]); } +uint8_t ardrone_error_flag = 0; + +void actuators_ardrone_error(void) +{ + ardrone_error_flag = 1; +} + /** * Write motor speed command * cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0 @@ -149,17 +159,49 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7); cmd[4] = ((pwm3&0x1ff)<<1); write(mot_fd, cmd, 5); + + if (ardrone_error_flag == 1) + { + static uint8_t blink = 0; + blink++; + if (blink == 80) + { + actuators_ardrone_set_leds(MOT_LEDRED, MOT_LEDRED, MOT_LEDRED, MOT_LEDRED); + blink = 0; + } + else if (blink == 40) + { + actuators_ardrone_set_leds(MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF); + } + else if (blink == 20) + { + //actuators_ardrone_set_leds(MOT_LEDORANGE, MOT_LEDORANGE, MOT_LEDOFF, MOT_LEDORANGE); + } + } + } /** * Write LED command * cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format) */ + +/* + +led0 = RearLeft +led1 = RearRight +led2 = FrontRight +led3 = FrontLeft + +*/ + void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) { uint8_t cmd[2]; - cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1); - cmd[1]=((led2&3)<<7) | ((led3&3)<<5); + + cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); + cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); + write(mot_fd, cmd, 2); } diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h index 2a66ba5dde9..12b03089a6b 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h @@ -52,6 +52,8 @@ uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB]; extern void actuators_ardrone_commit(void); extern void actuators_ardrone_init(void); +extern void actuators_ardrone_error(void); + int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen); void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3); diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index ce0a1591a62..4320e0a3a19 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -69,7 +69,7 @@ static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) return (baro_calibration.b5 + 8) >> 4; } -void baro_periodic(void) +void baro_periodic(void) { } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 683426fabaa..7d0e185dcb9 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -185,7 +185,7 @@ void baro_update_logic(void) { // temp was updated temp_or_press_was_updated_last = 1; - + // This means that press must remain constant if (lastpressval != 0) { @@ -240,7 +240,7 @@ void navdata_update() // if ( navdata_checksum() == 0 ) { memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE); - + // Invert byte order so that TELEMETRY works better uint8_t tmp; uint8_t* p = (uint8_t*) &(navdata->pressure); From 271931bbfc6db1ff5bccc26b2b0a6fa91651cf8d Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 29 Aug 2013 21:05:09 +0200 Subject: [PATCH 35/65] [airframes] Gyro and Accelero sensitivity from datasheet by default, gyro neutral = automatic by filter, acc neutral and mag neutral need to be calibrated --- .../TUDelft/IMAV2013/mavrick_lisa_s.xml | 15 ------------- .../TUDelft/IMAV2013/quadrotor_lisa_s.xml | 15 ------------- .../TUDelft/IMAV2013/walkera_V120D02S.xml | 21 +++---------------- .../TUDelft/IMAV2013/walkera_genius_v1.xml | 21 +++---------------- conf/airframes/examples/quadrotor_lisa_s.xml | 15 ------------- 5 files changed, 6 insertions(+), 81 deletions(-) diff --git a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml index bc5da2d9bca..4955b2c0889 100644 --- a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml @@ -81,25 +81,10 @@ - - - - - - - - - - - - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 8a89ed12d7a..69108b84fad 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -50,25 +50,10 @@ - - - - - - - - - - - - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 5df82f5f459..a11d9f889b7 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -34,9 +34,9 @@ - - - + + + @@ -45,25 +45,10 @@ - - - - - - - - - - - - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 3c66aae2b2e..aa31e8d4ec8 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -34,9 +34,9 @@ - - - + + + @@ -45,25 +45,10 @@ - - - - - - - - - - - - - - - diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index a46f343030e..640ec29d447 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -50,25 +50,10 @@ - - - - - - - - - - - - - - - From 8cc90461ecabb3b42020c0f4c6505add13e3ed61 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 10:17:17 +0200 Subject: [PATCH 36/65] [modules] baro_ms5611_spi: fix update macro --- sw/airborne/modules/sensors/baro_ms5611_spi.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h index 7bab9d92e85..70b83b2b9b5 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.h +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -50,8 +50,8 @@ extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); -#define BaroMs5611UpdatePressure(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; baro_ms5611.data_available = FALSE; } } +#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } -#define BaroMs5611UpdateAlt(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; baro_ms5611.data_available = FALSE; } } +#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } } #endif From 9919870bf9a953871f6fed78e6d46779059d88c4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 12:07:08 +0200 Subject: [PATCH 37/65] [datalink] only include superbitrf if actually used fixes compilation on ar_drone/omap --- sw/airborne/subsystems/datalink/downlink.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 50c8e562c9d..851414b4947 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -46,17 +46,21 @@ #endif #else /** SITL */ + #include "subsystems/datalink/udp.h" #include "subsystems/datalink/pprz_transport.h" #include "subsystems/datalink/xbee.h" #include "subsystems/datalink/w5100.h" +#if USE_SUPERBITRF #include "subsystems/datalink/superbitrf.h" +#endif #if USE_AUDIO_TELEMETRY #include "subsystems/datalink/audio_telemetry.h" #endif #ifdef USE_USB_SERIAL #include "mcu_periph/usb_serial.h" #endif + #endif /** !SITL */ #ifndef DefaultChannel From 2ccb9d4f6c1c4ddc3f8608d5e0ca688f55972144 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 12:07:58 +0200 Subject: [PATCH 38/65] [superbitrf] minor include fixes --- sw/airborne/peripherals/cyrf6936.c | 1 - sw/airborne/subsystems/datalink/superbitrf.c | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 691049baac5..1b1e678a48a 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -27,7 +27,6 @@ #include "cyrf6936.h" #include "mcu_periph/spi.h" #include "mcu_periph/gpio.h" -#include "mcu_periph/gpio_arch.h" #include "mcu_periph/sys_time.h" #include "subsystems/radio_control.h" diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 40276314d00..5c6d27ecfcd 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -30,7 +30,7 @@ #include "paparazzi.h" #include "mcu_periph/spi.h" #include "mcu_periph/sys_time.h" -#include +#include "mcu_periph/gpio.h" /* Default SuperbitRF SPI DEV */ #ifndef SUPERBITRF_SPI_DEV From ef84f4c8df4cd49d07468b4f715e681667a3075c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 12:08:41 +0200 Subject: [PATCH 39/65] [omap] add empty gpio_arch.h --- sw/airborne/arch/omap/mcu_periph/gpio_arch.h | 49 ++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 sw/airborne/arch/omap/mcu_periph/gpio_arch.h diff --git a/sw/airborne/arch/omap/mcu_periph/gpio_arch.h b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h new file mode 100644 index 00000000000..795f0e18f42 --- /dev/null +++ b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file arch/omap/mcu_periph/gpio_arch.h + * + * GPIO helper functions for linux/omap. + * @todo implement gpio_set|clear + */ + +#ifndef GPIO_ARCH_H +#define GPIO_ARCH_H + +#include "std.h" + +/** + * Set a gpio output to high level. + */ +static inline void gpio_set(uint32_t port, uint16_t pin) { + +} + +/** + * Clear a gpio output to low level. + */ +static inline void gpio_clear(uint32_t port, uint16_t pin) { + +} + + +#endif /* GPIO_ARCH_H */ From 45c61055c3bd14595b6742d8638e78f1aae1835f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 13:10:58 +0200 Subject: [PATCH 40/65] [modules] baro_ms5611: don't send MS5611_COEFF by default to send the MS5611_COEFF message either set BARO_MS5611_SEND_COEFF to TRUE to send it every 5s or explicitly call baro_ms5611_send_coeff --- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 29 +++++++++++------- sw/airborne/modules/sensors/baro_ms5611_spi.c | 30 ++++++++++++------- 2 files changed, 37 insertions(+), 22 deletions(-) diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index d3b3a3d88c5..e44e3044f80 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -72,17 +72,10 @@ void baro_ms5611_periodic_check( void ) { ms5611_i2c_periodic_check(&baro_ms5611); - if (baro_ms5611.initialized) { - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); - } +#if BARO_MS5611_SEND_COEFF + // send coeff every 5s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#endif } /// trigger new measurement or initialize if needed @@ -110,3 +103,17 @@ void baro_ms5611_event( void ) { #endif } } + +void baro_ms5611_send_coeff(void) { + if (baro_ms5611.initialized) { + DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &baro_ms5611.data.c[0], + &baro_ms5611.data.c[1], + &baro_ms5611.data.c[2], + &baro_ms5611.data.c[3], + &baro_ms5611.data.c[4], + &baro_ms5611.data.c[5], + &baro_ms5611.data.c[6], + &baro_ms5611.data.c[7]); + } +} diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index c91d713153d..7da2575f413 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -72,17 +72,11 @@ void baro_ms5611_periodic_check( void ) { ms5611_spi_periodic_check(&baro_ms5611); - if (baro_ms5611.initialized) { - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); - } +#if BARO_MS5611_SEND_COEFF + // send coeff every 5s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#endif + } /// trigger new measurement or initialize if needed @@ -110,3 +104,17 @@ void baro_ms5611_event( void ) { #endif } } + +void baro_ms5611_send_coeff(void) { + if (baro_ms5611.initialized) { + DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &baro_ms5611.data.c[0], + &baro_ms5611.data.c[1], + &baro_ms5611.data.c[2], + &baro_ms5611.data.c[3], + &baro_ms5611.data.c[4], + &baro_ms5611.data.c[5], + &baro_ms5611.data.c[6], + &baro_ms5611.data.c[7]); + } +} From 4765f35f937885b1bd0813ea008769045a042f7a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 13:15:51 +0200 Subject: [PATCH 41/65] [modules] add baro_ms5611_send_coeff declaration --- sw/airborne/modules/sensors/baro_ms5611_i2c.h | 1 + sw/airborne/modules/sensors/baro_ms5611_spi.h | 1 + 2 files changed, 2 insertions(+) diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index f30b28debc6..549b6bf7313 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -21,6 +21,7 @@ extern void baro_ms5611_init(void); extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); +extern void baro_ms5611_send_coeff(void); #define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h index 70b83b2b9b5..7be9f9c2e58 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.h +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -49,6 +49,7 @@ extern void baro_ms5611_init(void); extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); +extern void baro_ms5611_send_coeff(void); #define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } From ec0592e846f99c66dc1d00b3ac6458c0ff7a55c8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 14:29:10 +0200 Subject: [PATCH 42/65] [rotorcraft] MODULES_FREQUENCY defaults to PERIODIC_FREQUENCY --- sw/airborne/firmwares/rotorcraft/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 060367961db..beb36fcb31a 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -83,7 +83,7 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) #ifndef MODULES_FREQUENCY -#define MODULES_FREQUENCY 512 +#define MODULES_FREQUENCY PERIODIC_FREQUENCY #endif PRINT_CONFIG_VAR(MODULES_FREQUENCY) From b1eb25e4f0abb2a4a94d433599ac84833093865c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 14:58:01 +0200 Subject: [PATCH 43/65] [dox] create dox makefile target to easily create doxygen docs --- Makefile | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 39e02ab1145..b845818f72d 100644 --- a/Makefile +++ b/Makefile @@ -248,6 +248,15 @@ paparazzi: chmod a+x $@ +# +# doxygen html documentation +# +dox: + $(Q)PAPARAZZI_HOME=$(PAPARAZZI_HOME) sw/tools/doxygen_gen/gen_modules_doc.py -pv + @echo "Generationg doxygen html documentation in doc/generated/html" + $(Q)( cat Doxyfile ; echo "PROJECT_NUMBER=$(./paparazzi_version)"; echo "QUIET=YES") | doxygen - + @echo "Done. Open doc/generated/html/index.html in your browser to view it." + # # Cleaning # @@ -296,7 +305,7 @@ run_tests: test: all replace_current_conf_xml run_tests restore_conf_xml -.PHONY: all print_build_version update_google_version ground_segment ground_segment.opt \ +.PHONY: all print_build_version update_google_version dox ground_segment ground_segment.opt \ subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt tools\ static sim_static lpctools commands \ clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \ From ad8c7d636bf2e13c552ce80f5d630be7d571a3be Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 14:58:33 +0200 Subject: [PATCH 44/65] [dox][modules] update sys_mon description --- conf/modules/sys_mon.xml | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/conf/modules/sys_mon.xml b/conf/modules/sys_mon.xml index 22eccb45111..b6a7dcd9233 100644 --- a/conf/modules/sys_mon.xml +++ b/conf/modules/sys_mon.xml @@ -2,7 +2,29 @@ - System monitor + + System monitor. + The sys_mon module gives you some information about the timing of the periodic tasks and a rough estimate of cpu load (averaged over 1 sec). + + The SYS_MON message contains the following information (all times are given in microseconds): + - @b periodic_time : time between two calls of the modules_periodic_task (averaged over 1s) + - @b periodic_time_min : minimum time between two calls of the modules_periodic_task() during the last second + - @b periodic_time_max : maximum time between two calls of the modules_periodic_task() during the last second + - @b periodic_cycle : time it took to execute the main periodic functions (averaged over 1s) + - @b periodic_cycle_min : minimum time it took to execute the main periodic functions during the last second + - @b periodic_cycle_max : maximum time it took to execute the main periodic functions during the last second + - @b event_number : number of times the event loop was called during the last second + - @b cpu_load : rough estimate of cpu load (averaged over 1 sec) + + + So your periodic_time should be 1/MODULES_FREQUENCY, which should be the same as 1/PERIODIC_FREQUENCY + The periodic_cycle_max should not be over the periodic_time, otherwise in at least one cycle it took longer to calculate everything and the next one was slightly delayed. + + + The sys_mon module has to run at the full main frequency! + + So either don't specify a main_freq parameter for the modules node or set your actual main frequency +
From e99410998bf2804e4089295e77174b9f6e1df54c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 15:11:14 +0200 Subject: [PATCH 45/65] [dox] fix file headers for sensors/baro_ms5611_* --- sw/airborne/subsystems/sensors/baro_ms5611_i2c.c | 2 +- sw/airborne/subsystems/sensors/baro_ms5611_spi.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c index a996cee2dc3..a3c446e4bdd 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c @@ -21,7 +21,7 @@ */ /** - * @file boards/lisa_m/baro_ms5611_i2c.c + * @file subsystems/sensors/baro_ms5611_i2c.c * * Driver for MS5611 baro via I2C. * diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c index a86ab818f7a..bd8603ea432 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c @@ -21,7 +21,7 @@ */ /** - * @file boards/lisa_m/baro_ms5611_spi.c + * @file subsystems/sensors/baro_ms5611_spi.c * * Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI. * From e216c612446a3a6838ef45d6d7619c31da3ebad5 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 30 Aug 2013 16:24:37 +0200 Subject: [PATCH 46/65] [dox] fix sys_mon module description rendering --- conf/modules/sys_mon.xml | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/conf/modules/sys_mon.xml b/conf/modules/sys_mon.xml index b6a7dcd9233..88964bba1c1 100644 --- a/conf/modules/sys_mon.xml +++ b/conf/modules/sys_mon.xml @@ -3,27 +3,27 @@ - System monitor. - The sys_mon module gives you some information about the timing of the periodic tasks and a rough estimate of cpu load (averaged over 1 sec). +System monitor. +The sys_mon module gives you some information about the timing of the periodic tasks and a rough estimate of cpu load (averaged over 1 sec). - The SYS_MON message contains the following information (all times are given in microseconds): - - @b periodic_time : time between two calls of the modules_periodic_task (averaged over 1s) - - @b periodic_time_min : minimum time between two calls of the modules_periodic_task() during the last second - - @b periodic_time_max : maximum time between two calls of the modules_periodic_task() during the last second - - @b periodic_cycle : time it took to execute the main periodic functions (averaged over 1s) - - @b periodic_cycle_min : minimum time it took to execute the main periodic functions during the last second - - @b periodic_cycle_max : maximum time it took to execute the main periodic functions during the last second - - @b event_number : number of times the event loop was called during the last second - - @b cpu_load : rough estimate of cpu load (averaged over 1 sec) +The SYS_MON message contains the following information (all times are given in microseconds): +- @b periodic_time : time between two calls of the modules_periodic_task (averaged over 1s) +- @b periodic_time_min : minimum time between two calls of the modules_periodic_task() during the last second +- @b periodic_time_max : maximum time between two calls of the modules_periodic_task() during the last second +- @b periodic_cycle : time it took to execute the main periodic functions (averaged over 1s) +- @b periodic_cycle_min : minimum time it took to execute the main periodic functions during the last second +- @b periodic_cycle_max : maximum time it took to execute the main periodic functions during the last second +- @b event_number : number of times the event loop was called during the last second +- @b cpu_load : rough estimate of cpu load (averaged over 1 sec) - So your periodic_time should be 1/MODULES_FREQUENCY, which should be the same as 1/PERIODIC_FREQUENCY - The periodic_cycle_max should not be over the periodic_time, otherwise in at least one cycle it took longer to calculate everything and the next one was slightly delayed. +So your periodic_time should be 1/MODULES_FREQUENCY, which should be the same as 1/PERIODIC_FREQUENCY +The periodic_cycle_max should not be over the periodic_time, otherwise in at least one cycle it took longer to calculate everything and the next one was slightly delayed. - The sys_mon module has to run at the full main frequency! +The sys_mon module has to run at the full main frequency! - So either don't specify a main_freq parameter for the modules node or set your actual main frequency +So either don't specify a main_freq parameter for the modules node or set your actual main frequency
From e6ae39d9d96a019aea084eed1bcd6996ee856022 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 12:52:31 +0200 Subject: [PATCH 47/65] [Ardrone] Takeoff without joystick is possible --- conf/airframes/ardrone2_raw.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 0b6eb33d186..d6b09dea15d 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -207,9 +207,10 @@
+ - +
From 06e466face91cc3ad8e7990f68c15b38c9deec95 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 17:41:48 +0200 Subject: [PATCH 48/65] [ArDrone][FIX] New pin numbers for motor selection --- .../boards/ardrone/actuators_ardrone2_raw.c | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 1989b492e73..dc8037afaf1 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -81,34 +81,34 @@ void actuators_ardrone_init(void) tcsetattr(mot_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set(106,-1); - gpio_set(107,0); - gpio_set(107,1); + gpio_set(176,-1); + gpio_set(175,0); + gpio_set(175,1); //all select lines inactive - gpio_set(68,1); - gpio_set(69,1); - gpio_set(70,1); - gpio_set(71,1); + gpio_set(171,1); + gpio_set(172,1); + gpio_set(173,1); + gpio_set(174,1); //configure motors uint8_t reply[256]; for(int m=0;m<4;m++) { - gpio_set(68+m,-1); + gpio_set(171+m,-1); actuators_ardrone_cmd(0xe0,reply,2); if(reply[0]!=0xe0 || reply[1]!=0x00) { printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]); } actuators_ardrone_cmd(m+1,reply,1); - gpio_set(68+m,1); + gpio_set(171+m,1); } //all select lines active - gpio_set(68,-1); - gpio_set(69,-1); - gpio_set(70,-1); - gpio_set(71,-1); + gpio_set(171,-1); + gpio_set(172,-1); + gpio_set(173,-1); + gpio_set(174,-1); //start multicast actuators_ardrone_cmd(0xa0,reply,1); @@ -118,9 +118,9 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set(106,-1); - gpio_set(107,0); - gpio_set(107,1); + gpio_set(176,-1); + gpio_set(175,0); + gpio_set(175,1); //all leds green //actuators_ardrone_set_leds(0,0,0,0); From 32104f3a1668abb831f7faa86ec6bd31c9e7bcb0 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 18:00:33 +0200 Subject: [PATCH 49/65] [ArDrone][BLDC] Read Interrupt: be able to take-off again after a motor IRQ --- .../boards/ardrone/actuators_ardrone2_raw.c | 52 +++++++++++-------- sw/airborne/boards/ardrone/gpio_ardrone.c | 12 +++++ sw/airborne/boards/ardrone/gpio_ardrone.h | 1 + 3 files changed, 44 insertions(+), 21 deletions(-) diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index dc8037afaf1..3a11e8c0b35 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -117,15 +117,12 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); actuators_ardrone_cmd(0xa0,reply,1); - //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 + //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 gpio_set(176,-1); gpio_set(175,0); gpio_set(175,1); - //all leds green - //actuators_ardrone_set_leds(0,0,0,0); - //actuators_ardrone_set_leds(MOT_LEDRED, MOT_LEDRED, MOT_LEDRED, MOT_LEDRED); - //actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN); + // Left Red, Right Green actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); } @@ -134,13 +131,34 @@ int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { return read(mot_fd, reply, replylen); } +#include "autopilot.h" + +void actuators_ardrone_motor_status(void); +void actuators_ardrone_motor_status(void) +{ + // If a motor IRQ lines is set + if (gpio_get(176) == 1) + { + if (autopilot_motors_on) + { + // Tell paparazzi that one motor has stalled + autopilot_set_motors_on(FALSE); + + // Toggle Flipflop reset so motors can be re-enabled + gpio_set(175,0); + gpio_set(175,1); + } + } +} + + void actuators_ardrone_commit(void) { actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]); + RunOnceEvery(100,actuators_ardrone_motor_status()); } uint8_t ardrone_error_flag = 0; - void actuators_ardrone_error(void) { ardrone_error_flag = 1; @@ -173,28 +191,20 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint { actuators_ardrone_set_leds(MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF); } - else if (blink == 20) - { - //actuators_ardrone_set_leds(MOT_LEDORANGE, MOT_LEDORANGE, MOT_LEDOFF, MOT_LEDORANGE); - } } - } /** * Write LED command - * cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format) + * cmd = 011rrrr0 000gggg0 (this is ardrone1 format, we need ardrone2 format) + * + * + * led0 = RearLeft + * led1 = RearRight + * led2 = FrontRight + * led3 = FrontLeft */ -/* - -led0 = RearLeft -led1 = RearRight -led2 = FrontRight -led3 = FrontLeft - -*/ - void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) { uint8_t cmd[2]; diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 3a08b1c2675..cd76054a798 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -37,3 +37,15 @@ int gpio_set(int nr,int val) else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr); return system(cmdline); } + +#define WE_HAVE_NO_CLUE_YET + + +#ifdef WE_HAVE_NO_CLUE_YET + +int gpio_get(int nr) +{ + return 0; +} + +#endif diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h index 55563f2e098..56dee38d43c 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.h +++ b/sw/airborne/boards/ardrone/gpio_ardrone.h @@ -29,5 +29,6 @@ //val=1 -> set gpio output hi //val=-1 -> set gpio as input (output hi-Z) int gpio_set(int nr,int val); +int gpio_get(int nr); #endif /* GPIO_ARDRONE_H */ From d6cc3e5820ce1d9dcbfabb02dfe1d73a4b97d480 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 18:00:52 +0200 Subject: [PATCH 50/65] typo --- sw/airborne/firmwares/rotorcraft/autopilot.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 41c21a33436..1b52e9a4496 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -106,7 +106,7 @@ void autopilot_init(void) { guidance_v_init(); stabilization_init(); - /* set startup mode, propagats through to guidance h/v */ + /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); } From fab4e87f7d33dad8ed294bbc3deb3bb988a8e80f Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 18:13:26 +0200 Subject: [PATCH 51/65] [ArDrone] Working routine to read GPIO pin --- sw/airborne/boards/ardrone/gpio_ardrone.c | 93 ++++++++++++++++++++++- 1 file changed, 92 insertions(+), 1 deletion(-) diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index cd76054a798..3f8e1ce40dc 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -38,7 +38,15 @@ int gpio_set(int nr,int val) return system(cmdline); } -#define WE_HAVE_NO_CLUE_YET +// Option 1: +//#define WE_HAVE_NO_CLUE_YET + +// Option 2: +#define WE_MUST_TO_USE_THE_TERRIBLE_HACK + +// Option 3: +//#define WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO + #ifdef WE_HAVE_NO_CLUE_YET @@ -49,3 +57,86 @@ int gpio_get(int nr) } #endif + + + + +#ifdef WE_MUST_TO_USE_THE_TERRIBLE_HACK + +FILE* ardrone_system_pipe = 0; + +int gpio_get(int nr) +{ + + if (ardrone_system_pipe == 0) + { + char cmdline[200]; + sprintf(cmdline,"/usr/sbin/gpio %d -r",nr); + ardrone_system_pipe = popen(cmdline,"r"); + if (!ardrone_system_pipe) + { + return -1; + } + } + else + { + // TODO: we now call this with a large delay expecting that all data is present + // if (!feof(pipe)) // Still busy + + char buff[128]; + char* ret = fgets(buff, 128, ardrone_system_pipe); + ret = fgets(buff, 128, ardrone_system_pipe); + pclose(ardrone_system_pipe); + ardrone_system_pipe = 0; + + if (ret == NULL) + { + return -2; + } + + int pin = ret[25] - '0'; + + printf("GPIO_GET: %d '%d' \n", nr, pin); + + return pin; + } + return -3; +} + +#endif + + + + + +#ifdef WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO + +#include /* File control definitions */ +#include /* Error number definitions */ +#include + + +#define GPIO_IOCTL_COUNT 0 +#define GPIO_IOCTL_GET 2 + +int gpiofp = 0; +int gpio_get(int nr) +{ + if (gpiofp == 0) + { + gpiofp = open("/dev/gpio",O_RDWR); + printf("GPIO open %d\n", gpiofp); + // printf("%s", errno() ); + } + else + { + int gpio = nr; + int ret = ioctl(gpiofp, GPIO_IOCTL_GET, &gpio); + printf("GPIO_ %d = %d %d \n",nr,gpio, ret); + } + + // We don't know yet + return 0; +} + +#endif From 32ee3bbe3a8b54f4e64f123b378564dd691bd642 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 30 Aug 2013 19:46:59 +0200 Subject: [PATCH 52/65] Line Endings --- .../subsystems/shared/radio_control_superbitrf_rc.makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile index deac945a9ac..85a80e4fb0f 100644 --- a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile @@ -12,4 +12,4 @@ endif ap.srcs += peripherals/cyrf6936.c \ $(SRC_SUBSYSTEMS)/datalink/superbitrf.c\ $(SRC_SUBSYSTEMS)/radio_control.c \ - $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c + $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c From a7fb3bf7cd7c73462a63fcce80ffd8586d5189a8 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Fri, 30 Aug 2013 20:28:50 +0200 Subject: [PATCH 53/65] [ardrone2] GPIO hack removed --- .../boards/ardrone/actuators_ardrone2_raw.c | 11 +- sw/airborne/boards/ardrone/gpio_ardrone.c | 155 ++++++------------ sw/airborne/boards/ardrone/gpio_ardrone.h | 4 +- 3 files changed, 59 insertions(+), 111 deletions(-) diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 3a11e8c0b35..5848b716433 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -81,7 +81,7 @@ void actuators_ardrone_init(void) tcsetattr(mot_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set(176,-1); + gpio_set_input(176); gpio_set(175,0); gpio_set(175,1); @@ -105,10 +105,10 @@ void actuators_ardrone_init(void) } //all select lines active - gpio_set(171,-1); - gpio_set(172,-1); - gpio_set(173,-1); - gpio_set(174,-1); + gpio_set(171,0); + gpio_set(172,0); + gpio_set(173,0); + gpio_set(174,0); //start multicast actuators_ardrone_cmd(0xa0,reply,1); @@ -118,7 +118,6 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 - gpio_set(176,-1); gpio_set(175,0); gpio_set(175,1); diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 3f8e1ce40dc..b9172c7cdf8 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -22,121 +22,70 @@ * ardrone GPIO driver */ -#include -#include +#include /* File control definitions */ +#include /* Error number definitions */ +#include #include "gpio_ardrone.h" -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -//val=-1 -> set gpio as input (output hi-Z) -int gpio_set(int nr,int val) -{ - char cmdline[200]; - if(val<0) sprintf(cmdline,"/usr/sbin/gpio %d -d i",nr); - else if(val>0) sprintf(cmdline,"/usr/sbin/gpio %d -d ho 1",nr); - else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr); - return system(cmdline); -} - -// Option 1: -//#define WE_HAVE_NO_CLUE_YET - -// Option 2: -#define WE_MUST_TO_USE_THE_TERRIBLE_HACK - -// Option 3: -//#define WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO +#define GPIO_MAGIC 'p' +#define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction) +#define GPIO_READ _IOWR(GPIO_MAGIC, 1, struct gpio_data) +#define GPIO_WRITE _IOW(GPIO_MAGIC, 2, struct gpio_data) +int gpiofp = 0; +struct gpio_data { + int pin; + int value; +}; +enum gpio_mode { + GPIO_INPUT = 0, //!< Pin configured for input + GPIO_OUTPUT_LOW, //!< Pin configured for output with low level + GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level +}; -#ifdef WE_HAVE_NO_CLUE_YET +struct gpio_direction { + int pin; + enum gpio_mode mode; +}; -int gpio_get(int nr) +//val=0 -> set gpio output lo +//val=1 -> set gpio output hi +void gpio_set(int nr, int val) { - return 0; + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = nr; + data.value = val; + ioctl(gpiofp, GPIO_WRITE, &data); } -#endif - - - - -#ifdef WE_MUST_TO_USE_THE_TERRIBLE_HACK - -FILE* ardrone_system_pipe = 0; - -int gpio_get(int nr) +void gpio_set_input(int nr) { - - if (ardrone_system_pipe == 0) - { - char cmdline[200]; - sprintf(cmdline,"/usr/sbin/gpio %d -r",nr); - ardrone_system_pipe = popen(cmdline,"r"); - if (!ardrone_system_pipe) - { - return -1; - } - } - else - { - // TODO: we now call this with a large delay expecting that all data is present - // if (!feof(pipe)) // Still busy - - char buff[128]; - char* ret = fgets(buff, 128, ardrone_system_pipe); - ret = fgets(buff, 128, ardrone_system_pipe); - pclose(ardrone_system_pipe); - ardrone_system_pipe = 0; - - if (ret == NULL) - { - return -2; - } - - int pin = ret[25] - '0'; - - printf("GPIO_GET: %d '%d' \n", nr, pin); - - return pin; - } - return -3; + struct gpio_direction dir; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + dir.pin = nr; + dir.mode = GPIO_INPUT; + ioctl(gpiofp, GPIO_DIRECTION, &dir); } -#endif - - - - - -#ifdef WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO - -#include /* File control definitions */ -#include /* Error number definitions */ -#include - - -#define GPIO_IOCTL_COUNT 0 -#define GPIO_IOCTL_GET 2 - -int gpiofp = 0; int gpio_get(int nr) { - if (gpiofp == 0) - { - gpiofp = open("/dev/gpio",O_RDWR); - printf("GPIO open %d\n", gpiofp); - // printf("%s", errno() ); - } - else - { - int gpio = nr; - int ret = ioctl(gpiofp, GPIO_IOCTL_GET, &gpio); - printf("GPIO_ %d = %d %d \n",nr,gpio, ret); - } - - // We don't know yet - return 0; + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = nr; + ioctl(gpiofp, GPIO_READ, &data); + return data.value; } - -#endif diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h index 56dee38d43c..f455d33004a 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.h +++ b/sw/airborne/boards/ardrone/gpio_ardrone.h @@ -27,8 +27,8 @@ //val=0 -> set gpio output lo //val=1 -> set gpio output hi -//val=-1 -> set gpio as input (output hi-Z) -int gpio_set(int nr,int val); +void gpio_set(int nr,int val); +void gpio_set_input(int nr); int gpio_get(int nr); #endif /* GPIO_ARDRONE_H */ From 1fac821b79897c84cd436753734d5dbd0f0d5526 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sat, 31 Aug 2013 16:02:49 +0200 Subject: [PATCH 54/65] [ArDrone] GPIO rewritten to make paparazzi API more consistent --- sw/airborne/arch/omap/mcu_periph/gpio_arch.h | 11 ++-- .../boards/ardrone/actuators_ardrone2_raw.c | 48 ++++++++++------- sw/airborne/boards/ardrone/gpio_ardrone.c | 53 +++++++++++++++---- sw/airborne/boards/ardrone/gpio_ardrone.h | 34 ------------ 4 files changed, 77 insertions(+), 69 deletions(-) delete mode 100644 sw/airborne/boards/ardrone/gpio_ardrone.h diff --git a/sw/airborne/arch/omap/mcu_periph/gpio_arch.h b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h index 795f0e18f42..1f7c9a94879 100644 --- a/sw/airborne/arch/omap/mcu_periph/gpio_arch.h +++ b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h @@ -34,16 +34,17 @@ /** * Set a gpio output to high level. */ -static inline void gpio_set(uint32_t port, uint16_t pin) { - -} +extern void gpio_set(uint32_t port, uint16_t pin); /** * Clear a gpio output to low level. */ -static inline void gpio_clear(uint32_t port, uint16_t pin) { +extern void gpio_clear(uint32_t port, uint16_t pin); -} +/** + * Read a gpio value. + */ +uint16_t gpio_get(uint32_t gpioport, uint16_t gpios); #endif /* GPIO_ARCH_H */ diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 5848b716433..d5fe722d090 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -30,7 +30,7 @@ #include "subsystems/actuators.h" #include "actuators_ardrone2_raw.h" -#include "gpio_ardrone.h" +#include "mcu_periph/gpio.h" #include /* Standard input/output definitions */ #include /* String function definitions */ @@ -52,6 +52,16 @@ */ int mot_fd; /**< File descriptor for the port */ +#define ARDRONE_GPIO_PORT 0 // Dummy for paparazzi compatibility + +#define ARDRONE_GPIO_PIN_MOTOR1 171 +#define ARDRONE_GPIO_PIN_MOTOR2 172 +#define ARDRONE_GPIO_PIN_MOTOR3 173 +#define ARDRONE_GPIO_PIN_MOTOR4 174 + +#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 +#define ARDRONE_GPIO_PIN_IRQ_INPUT 176 + void actuators_ardrone_init(void) { //open mot port @@ -81,34 +91,34 @@ void actuators_ardrone_init(void) tcsetattr(mot_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set_input(176); - gpio_set(175,0); - gpio_set(175,1); + gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); //all select lines inactive - gpio_set(171,1); - gpio_set(172,1); - gpio_set(173,1); - gpio_set(174,1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //configure motors uint8_t reply[256]; for(int m=0;m<4;m++) { - gpio_set(171+m,-1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); actuators_ardrone_cmd(0xe0,reply,2); if(reply[0]!=0xe0 || reply[1]!=0x00) { printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]); } actuators_ardrone_cmd(m+1,reply,1); - gpio_set(171+m,1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); } //all select lines active - gpio_set(171,0); - gpio_set(172,0); - gpio_set(173,0); - gpio_set(174,0); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //start multicast actuators_ardrone_cmd(0xa0,reply,1); @@ -118,8 +128,8 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 - gpio_set(175,0); - gpio_set(175,1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); // Left Red, Right Green actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); @@ -136,7 +146,7 @@ void actuators_ardrone_motor_status(void); void actuators_ardrone_motor_status(void) { // If a motor IRQ lines is set - if (gpio_get(176) == 1) + if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) { if (autopilot_motors_on) { @@ -144,8 +154,8 @@ void actuators_ardrone_motor_status(void) autopilot_set_motors_on(FALSE); // Toggle Flipflop reset so motors can be re-enabled - gpio_set(175,0); - gpio_set(175,1); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); } } } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index b9172c7cdf8..9222cd27aef 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -25,7 +25,8 @@ #include /* File control definitions */ #include /* Error number definitions */ #include -#include "gpio_ardrone.h" + +#include "mcu_periph/gpio.h" #define GPIO_MAGIC 'p' #define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction) @@ -49,22 +50,36 @@ struct gpio_direction { enum gpio_mode mode; }; -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -void gpio_set(int nr, int val) + +void gpio_set(uint32_t port, uint16_t pin) { struct gpio_data data; // Open the device if not open if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = pin; + data.value = 1; + ioctl(gpiofp, GPIO_WRITE, &data); +} + + +void gpio_clear(uint32_t port, uint16_t pin) +{ + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - data.pin = nr; - data.value = val; + data.pin = pin; + data.value = 0; ioctl(gpiofp, GPIO_WRITE, &data); } -void gpio_set_input(int nr) + +void gpio_setup_input(uint32_t port, uint16_t pin) { struct gpio_direction dir; // Open the device if not open @@ -72,12 +87,28 @@ void gpio_set_input(int nr) gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - dir.pin = nr; + dir.pin = pin; dir.mode = GPIO_INPUT; ioctl(gpiofp, GPIO_DIRECTION, &dir); } -int gpio_get(int nr) + +void gpio_setup_output(uint32_t port, uint16_t pin) +{ + struct gpio_direction dir; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + dir.pin = pin; + dir.mode = GPIO_OUTPUT_HIGH; + ioctl(gpiofp, GPIO_DIRECTION, &dir); +} + + + +uint16_t gpio_get(uint32_t gpioport, uint16_t pin) { struct gpio_data data; // Open the device if not open @@ -85,7 +116,7 @@ int gpio_get(int nr) gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - data.pin = nr; + data.pin = pin; ioctl(gpiofp, GPIO_READ, &data); return data.value; } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h deleted file mode 100644 index f455d33004a..00000000000 --- a/sw/airborne/boards/ardrone/gpio_ardrone.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301 USA. - */ - -/** - * @file boards/ardrone/gpio_ardrone.h - * ardrone GPIO driver - */ - -#ifndef GPIO_ARDRONE_H -#define GPIO_ARDRONE_H - -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -void gpio_set(int nr,int val); -void gpio_set_input(int nr); -int gpio_get(int nr); - -#endif /* GPIO_ARDRONE_H */ From 0a8d097a994baec71fe78b949731f8b0b3644753 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sat, 31 Aug 2013 23:19:39 +0200 Subject: [PATCH 55/65] [ArDrone] Add protection to lowlevel driver that can short-circuit the board. --- sw/airborne/boards/ardrone/actuators_ardrone2_raw.c | 2 +- sw/airborne/boards/ardrone/gpio_ardrone.c | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index d5fe722d090..0131db45ba7 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -52,7 +52,7 @@ */ int mot_fd; /**< File descriptor for the port */ -#define ARDRONE_GPIO_PORT 0 // Dummy for paparazzi compatibility +#define ARDRONE_GPIO_PORT 0x32524 #define ARDRONE_GPIO_PIN_MOTOR1 171 #define ARDRONE_GPIO_PIN_MOTOR2 172 diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 9222cd27aef..9e3786722b2 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -53,6 +53,7 @@ struct gpio_direction { void gpio_set(uint32_t port, uint16_t pin) { + if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open if (gpiofp == 0) @@ -67,6 +68,7 @@ void gpio_set(uint32_t port, uint16_t pin) void gpio_clear(uint32_t port, uint16_t pin) { + if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open if (gpiofp == 0) @@ -81,6 +83,7 @@ void gpio_clear(uint32_t port, uint16_t pin) void gpio_setup_input(uint32_t port, uint16_t pin) { + if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_direction dir; // Open the device if not open if (gpiofp == 0) @@ -95,6 +98,7 @@ void gpio_setup_input(uint32_t port, uint16_t pin) void gpio_setup_output(uint32_t port, uint16_t pin) { + if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_direction dir; // Open the device if not open if (gpiofp == 0) @@ -108,8 +112,9 @@ void gpio_setup_output(uint32_t port, uint16_t pin) -uint16_t gpio_get(uint32_t gpioport, uint16_t pin) +uint16_t gpio_get(uint32_t port, uint16_t pin) { + if (port != 0x32524) return 0; // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open if (gpiofp == 0) From d04315e08ea3eeda7a7396e23fb75f133671c628 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sat, 31 Aug 2013 23:37:10 +0200 Subject: [PATCH 56/65] [ArDrone] Redirected paparazzi LED to ArDrone motor-controller LED --- sw/airborne/arch/omap/led_hw.h | 12 ++-- .../boards/ardrone/actuators_ardrone2_raw.c | 61 +++++++++---------- .../boards/ardrone/actuators_ardrone2_raw.h | 1 - 3 files changed, 36 insertions(+), 38 deletions(-) diff --git a/sw/airborne/arch/omap/led_hw.h b/sw/airborne/arch/omap/led_hw.h index 884d9581b3b..6aecc2884d5 100644 --- a/sw/airborne/arch/omap/led_hw.h +++ b/sw/airborne/arch/omap/led_hw.h @@ -28,10 +28,14 @@ #ifndef LED_HW_H_ #define LED_HW_H_ -#define LED_INIT(i) { } -#define LED_ON(i) { } -#define LED_OFF(i) { } -#define LED_TOGGLE(i) { } +#include + +extern uint32_t led_hw_values; + +#define LED_INIT(i) { led_hw_values &= ~(1< /* Standard input/output definitions */ #include /* String function definitions */ @@ -50,7 +51,7 @@ * 190 2.5 * 130 3.0 */ -int mot_fd; /**< File descriptor for the port */ +int actuator_ardrone2_raw_fd; /**< File descriptor for the port */ #define ARDRONE_GPIO_PORT 0x32524 @@ -62,22 +63,26 @@ int mot_fd; /**< File descriptor for the port */ #define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 #define ARDRONE_GPIO_PIN_IRQ_INPUT 176 +uint32_t led_hw_values; + void actuators_ardrone_init(void) { + led_hw_values = 0; + //open mot port - mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY); - if (mot_fd == -1) + actuator_ardrone2_raw_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY); + if (actuator_ardrone2_raw_fd == -1) { perror("open_port: Unable to open /dev/ttyO0 - "); return; } - fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking - fcntl(mot_fd, F_GETFL, 0); + fcntl(actuator_ardrone2_raw_fd, F_SETFL, 0); //read calls are non blocking + fcntl(actuator_ardrone2_raw_fd, F_GETFL, 0); //set port options struct termios options; //Get the current options for the port - tcgetattr(mot_fd, &options); + tcgetattr(actuator_ardrone2_raw_fd, &options); //Set the baud rates to 115200 cfsetispeed(&options, B115200); cfsetospeed(&options, B115200); @@ -88,7 +93,7 @@ void actuators_ardrone_init(void) options.c_oflag &= ~OPOST; //clear output options (raw output) //Set the new options for the port - tcsetattr(mot_fd, TCSANOW, &options); + tcsetattr(actuator_ardrone2_raw_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); @@ -136,8 +141,8 @@ void actuators_ardrone_init(void) } int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { - write(mot_fd, &cmd, 1); - return read(mot_fd, reply, replylen); + write(actuator_ardrone2_raw_fd, &cmd, 1); + return read(actuator_ardrone2_raw_fd, reply, replylen); } #include "autopilot.h" @@ -160,6 +165,16 @@ void actuators_ardrone_motor_status(void) } } +void actuators_ardrone_led_run(void); +void actuators_ardrone_led_run(void) +{ + static uint32_t previous_led_hw_values; + if (previous_led_hw_values != led_hw_values) + { + previous_led_hw_values = led_hw_values; + actuators_ardrone_set_leds(led_hw_values & 0x01, led_hw_values & 0x02, led_hw_values & 0x04, led_hw_values & 0x08); + } +} void actuators_ardrone_commit(void) { @@ -167,12 +182,6 @@ void actuators_ardrone_commit(void) RunOnceEvery(100,actuators_ardrone_motor_status()); } -uint8_t ardrone_error_flag = 0; -void actuators_ardrone_error(void) -{ - ardrone_error_flag = 1; -} - /** * Write motor speed command * cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0 @@ -185,22 +194,8 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6); cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7); cmd[4] = ((pwm3&0x1ff)<<1); - write(mot_fd, cmd, 5); - - if (ardrone_error_flag == 1) - { - static uint8_t blink = 0; - blink++; - if (blink == 80) - { - actuators_ardrone_set_leds(MOT_LEDRED, MOT_LEDRED, MOT_LEDRED, MOT_LEDRED); - blink = 0; - } - else if (blink == 40) - { - actuators_ardrone_set_leds(MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF, MOT_LEDOFF); - } - } + write(actuator_ardrone2_raw_fd, cmd, 5); + RunOnceEvery(20,actuators_ardrone_led_run()); } /** @@ -221,10 +216,10 @@ void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_ cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); - write(mot_fd, cmd, 2); + write(actuator_ardrone2_raw_fd, cmd, 2); } void actuators_ardrone_close(void) { - close(mot_fd); + close(actuator_ardrone2_raw_fd); } diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h index 12b03089a6b..c564dd0eccf 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h @@ -52,7 +52,6 @@ uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB]; extern void actuators_ardrone_commit(void); extern void actuators_ardrone_init(void); -extern void actuators_ardrone_error(void); int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen); From eb17544e65967dffeea3fa00ed87e64652806327 Mon Sep 17 00:00:00 2001 From: softsr Date: Fri, 30 Aug 2013 01:34:52 -0700 Subject: [PATCH 57/65] [subsystems] Introduced bat low and bat critical check in electrical --- sw/airborne/subsystems/electrical.c | 42 +++++++++++++++++++++++++++++ sw/airborne/subsystems/electrical.h | 8 +++--- 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index d0075cdd0ca..1ddf5783e4e 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -19,6 +19,21 @@ #define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST #endif +#ifndef BAT_CHECKER_DELAY +#define BAT_CHECKER_DELAY 5 +#endif + +#ifndef CRITIC_BAT_LEVEL +#error You must define CRITIC_BAT_LEVEL in airframe setup! +#endif + +#ifndef LOW_BAT_LEVEL +#error You must define LOW_BAT_LEVEL in airframe setup! +#endif + +#define BAT_LOW_LEVEL (LOW_BAT_LEVEL*10) +#define BAT_CRIRIC_LEVEL (CRITIC_BAT_LEVEL*10) + struct Electrical electrical; #if defined ADC_CHANNEL_VSUPPLY || defined ADC_CHANNEL_CURRENT || defined MILLIAMP_AT_FULL_THROTTLE @@ -49,6 +64,9 @@ static struct { void electrical_init(void) { electrical.vsupply = 0; electrical.current = 0; + + electrical.bat_low = FALSE; + electrical.bat_critic = FALSE; #if defined ADC_CHANNEL_VSUPPLY adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE); @@ -64,6 +82,8 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) } void electrical_periodic(void) { + static uint8_t bat_low_counter = 0; + static uint8_t bat_critic_counter = 0; #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample)); #endif @@ -93,4 +113,26 @@ void electrical_periodic(void) { electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); #endif /* ADC_CHANNEL_CURRENT */ + + if(electrical.vsupply < BAT_LOW_LEVEL) { + if(bat_low_counter) + bat_low_counter--; + } else + bat_low_counter = BAT_CHECKER_DELAY * 10; + + if(!bat_low_counter) + electrical.bat_low = TRUE; + else + electrical.bat_low = FALSE; + + if(electrical.vsupply < BAT_CRIRIC_LEVEL) { + if(bat_critic_counter) + bat_critic_counter--; + } else + bat_critic_counter = BAT_CHECKER_DELAY * 10; + + if(!bat_critic_counter) + electrical.bat_critic = TRUE; + else + electrical.bat_critic = FALSE; } diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h index d9b70f9a2fc..f90089bb46c 100644 --- a/sw/airborne/subsystems/electrical.h +++ b/sw/airborne/subsystems/electrical.h @@ -5,9 +5,11 @@ struct Electrical { - uint16_t vsupply; ///< supply voltage in decivolts - int32_t current; ///< current in milliamps - int32_t consumed; ///< consumption in mAh + uint16_t vsupply; ///< supply voltage in decivolts + int32_t current; ///< current in milliamps + int32_t consumed; ///< consumption in mAh + bool_t bat_low; ///< battery low status + bool_t bat_critic; ///< battery critical status }; From 1a3b60b9ef5e10b2273002e16b6d4a1406ffe796 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 1 Sep 2013 15:52:10 +0200 Subject: [PATCH 58/65] [electrical] bat_low and bat_critical checks with defaults, used in bat_checker module closes #519 --- sw/airborne/modules/bat_checker/bat_checker.c | 48 +++-------- sw/airborne/modules/bat_checker/bat_checker.h | 8 ++ sw/airborne/subsystems/electrical.c | 86 ++++++++++++------- sw/airborne/subsystems/electrical.h | 42 ++++++++- 4 files changed, 117 insertions(+), 67 deletions(-) diff --git a/sw/airborne/modules/bat_checker/bat_checker.c b/sw/airborne/modules/bat_checker/bat_checker.c index 7949d8bd9bc..bdc716cb4da 100644 --- a/sw/airborne/modules/bat_checker/bat_checker.c +++ b/sw/airborne/modules/bat_checker/bat_checker.c @@ -19,34 +19,24 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file modules/bat_checker/bat_checker.c + * + * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level. + * At LOW_BAT_LEVEL the buzzer will be activated periodically. + * At CRITIC_BAT_LEVEL the buzzer will be activated permanently. + */ + #include "bat_checker.h" #include "generated/airframe.h" #include "generated/modules.h" #include "subsystems/electrical.h" #include "led.h" -#ifndef CRITIC_BAT_LEVEL -#error You must define CRITIC_BAT_LEVEL to use this module! -#endif - -#ifndef LOW_BAT_LEVEL -#error You must define LOW_BAT_LEVEL to use this module! -#endif - #ifndef BAT_CHECKER_LED #error You must define BAT_CHECKER_LED in your airframe file. #endif -#ifndef BAT_CHECKER_DELAY -#define BAT_CHECKER_DELAY 5 -#endif -PRINT_CONFIG_VAR(BAT_CHECKER_DELAY) - -// at this level, the buzzer will be activated periodically -#define WARN_BAT_LEVEL1 (LOW_BAT_LEVEL*10) - -// at this level, the buzzer will be activated permanently -#define WARN_BAT_LEVEL2 (CRITIC_BAT_LEVEL*10) void init_bat_checker(void) { LED_INIT(BAT_CHECKER_LED); @@ -54,22 +44,12 @@ void init_bat_checker(void) { } void bat_checker_periodic(void) { - static uint8_t bat_low_counter = 0; - if(electrical.vsupply < WARN_BAT_LEVEL1) { - if(bat_low_counter) - bat_low_counter--; - } else { - bat_low_counter = BAT_CHECKER_DELAY * BAT_CHECKER_PERIODIC_FREQ; - } - if(!bat_low_counter) { - if(electrical.vsupply < WARN_BAT_LEVEL2) { - LED_ON(BAT_CHECKER_LED); - } else if(electrical.vsupply < WARN_BAT_LEVEL1) { - LED_TOGGLE(BAT_CHECKER_LED); - } - } else { + if (electrical.bat_critical) + LED_ON(BAT_CHECKER_LED); + else if (electrical.bat_low) + LED_TOGGLE(BAT_CHECKER_LED); + else LED_OFF(BAT_CHECKER_LED); - } -} +} diff --git a/sw/airborne/modules/bat_checker/bat_checker.h b/sw/airborne/modules/bat_checker/bat_checker.h index 77c07fe41d5..5a3304e9fa6 100644 --- a/sw/airborne/modules/bat_checker/bat_checker.h +++ b/sw/airborne/modules/bat_checker/bat_checker.h @@ -19,6 +19,14 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file modules/bat_checker/bat_checker.c + * + * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level. + * At LOW_BAT_LEVEL the buzzer will be activated periodically. + * At CRITIC_BAT_LEVEL the buzzer will be activated permanently. + */ + #ifndef BAT_CHECKER_H #define BAT_CHECKER_H diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 1ddf5783e4e..915bf2f603c 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -1,3 +1,30 @@ +/* + * Copyright (C) 2010-2013 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/electrical.c + * + * Implemnetation for electrical status: supply voltage, current, battery status, etc. + */ + #include "subsystems/electrical.h" #include "mcu_periph/adc.h" @@ -23,16 +50,7 @@ #define BAT_CHECKER_DELAY 5 #endif -#ifndef CRITIC_BAT_LEVEL -#error You must define CRITIC_BAT_LEVEL in airframe setup! -#endif - -#ifndef LOW_BAT_LEVEL -#error You must define LOW_BAT_LEVEL in airframe setup! -#endif - -#define BAT_LOW_LEVEL (LOW_BAT_LEVEL*10) -#define BAT_CRIRIC_LEVEL (CRITIC_BAT_LEVEL*10) +#define ELECTRICAL_PERIODIC_FREQ 10 struct Electrical electrical; @@ -64,7 +82,7 @@ static struct { void electrical_init(void) { electrical.vsupply = 0; electrical.current = 0; - + electrical.bat_low = FALSE; electrical.bat_critic = FALSE; @@ -83,7 +101,8 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) void electrical_periodic(void) { static uint8_t bat_low_counter = 0; - static uint8_t bat_critic_counter = 0; + static uint8_t bat_critical_counter = 0; + #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample)); #endif @@ -113,26 +132,29 @@ void electrical_periodic(void) { electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); #endif /* ADC_CHANNEL_CURRENT */ - - if(electrical.vsupply < BAT_LOW_LEVEL) { - if(bat_low_counter) - bat_low_counter--; - } else - bat_low_counter = BAT_CHECKER_DELAY * 10; - if(!bat_low_counter) - electrical.bat_low = TRUE; - else + if (electrical.vsupply < LOW_BAT_LEVEL * 10) { + if (bat_low_counter > 0) + bat_low_counter--; + if (bat_low_counter == 0) + electrical.bat_low = TRUE; + } + else { + // reset battery low status and counter + bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; electrical.bat_low = FALSE; - - if(electrical.vsupply < BAT_CRIRIC_LEVEL) { - if(bat_critic_counter) - bat_critic_counter--; - } else - bat_critic_counter = BAT_CHECKER_DELAY * 10; - - if(!bat_critic_counter) - electrical.bat_critic = TRUE; - else - electrical.bat_critic = FALSE; + } + + if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) { + if (bat_critical_counter > 0) + bat_critical_counter--; + if (bat_critical_counter == 0) + electrical.bat_critical = TRUE; + } + else { + // reset battery critical status and counter + bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; + electrical.bat_critical = FALSE; + } + } diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h index f90089bb46c..81d70fbda2b 100644 --- a/sw/airborne/subsystems/electrical.h +++ b/sw/airborne/subsystems/electrical.h @@ -1,7 +1,47 @@ +/* + * Copyright (C) 2010-2013 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/electrical.h + * + * Interface for electrical status: supply voltage, current, battery status, etc. + */ + #ifndef SUBSYSTEMS_ELECTRICAL_H #define SUBSYSTEMS_ELECTRICAL_H #include "std.h" +#include "generated/airframe.h" + +/** low battery level in Volts (for 3S LiPo) */ +#ifndef LOW_BAT_LEVEL +#define LOW_BAT_LEVEL 10.5 +#endif + + +/** critical battery level in Volts (for 3S LiPo) */ +#ifndef CRITIC_BAT_LEVEL +#define CRITIC_BAT_LEVEL 9.8 +#endif + struct Electrical { @@ -9,7 +49,7 @@ struct Electrical { int32_t current; ///< current in milliamps int32_t consumed; ///< consumption in mAh bool_t bat_low; ///< battery low status - bool_t bat_critic; ///< battery critical status + bool_t bat_critical; ///< battery critical status }; From 9f98227800d5f080479214da31be97d1d3a87b57 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 1 Sep 2013 16:35:12 +0200 Subject: [PATCH 59/65] [electrical] fix typo --- sw/airborne/subsystems/electrical.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 915bf2f603c..5700a0a15ec 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -84,7 +84,7 @@ void electrical_init(void) { electrical.current = 0; electrical.bat_low = FALSE; - electrical.bat_critic = FALSE; + electrical.bat_critical = FALSE; #if defined ADC_CHANNEL_VSUPPLY adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE); From e3082044c9c2a0295c42cb58c8000f192f304188 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Jun 2013 23:09:40 +0200 Subject: [PATCH 60/65] [boards] add initial px4fmu_1.7 board and imu files --- conf/airframes/examples/quadrotor_px4fmu.xml | 218 ++++++++++++ conf/boards/px4fmu_1.7.makefile | 55 +++ conf/firmwares/rotorcraft.makefile | 6 + .../shared/imu_px4fmu_v1.7.makefile | 70 ++++ sw/airborne/boards/px4fmu/baro_board.h | 14 + sw/airborne/boards/px4fmu_1.7.h | 313 ++++++++++++++++++ sw/airborne/subsystems/imu/imu_px4fmu.c | 122 +++++++ sw/airborne/subsystems/imu/imu_px4fmu.h | 83 +++++ 8 files changed, 881 insertions(+) create mode 100644 conf/airframes/examples/quadrotor_px4fmu.xml create mode 100644 conf/boards/px4fmu_1.7.makefile create mode 100644 conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile create mode 100644 sw/airborne/boards/px4fmu/baro_board.h create mode 100644 sw/airborne/boards/px4fmu_1.7.h create mode 100644 sw/airborne/subsystems/imu/imu_px4fmu.c create mode 100644 sw/airborne/subsystems/imu/imu_px4fmu.h diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml new file mode 100644 index 00000000000..7d9b21aaebd --- /dev/null +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/boards/px4fmu_1.7.makefile b/conf/boards/px4fmu_1.7.makefile new file mode 100644 index 00000000000..798d46d7a5a --- /dev/null +++ b/conf/boards/px4fmu_1.7.makefile @@ -0,0 +1,55 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# px4fmu_1.7.makefile +# +# + +BOARD=px4fmu +BOARD_VERSION=1.7 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +ARCH_L=f4 +ARCH_DIR=stm32 +SRC_ARCH=arch/$(ARCH_DIR) +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld + +HARD_FLOAT=yes + +# default flash mode is via usb dfu bootloader +# possibilities: DFU, SWD +FLASH_MODE ?= SWD + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= none +SYS_TIME_LED ?= 1 + +# +# default UART configuration (modem, gps, spektrum) +# + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART6 +GPS_BAUD ?= B38400 + +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 75aeb5062e8..42ae9455685 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -232,6 +232,12 @@ ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_i2c.c ap.srcs += subsystems/sensors/baro_ms5611_i2c.c +else ifeq ($(BOARD), px4fmu) +ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 +ap.srcs += peripherals/ms5611.c +ap.srcs += peripherals/ms5611_i2c.c +ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + # apogee baro else ifeq ($(BOARD), apogee) ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile new file mode 100644 index 00000000000..68a7bc0099a --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile @@ -0,0 +1,70 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# MPU6000 via SPI and HMC5883 via I2C on the PX4FMU v1.7 board +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_PX4FMU_CFLAGS = -DUSE_IMU +endif + +include $(CFG_SHARED)/spi_master.makefile + +IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu.h\" +IMU_PX4FMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_PX4FMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu.c +IMU_PX4FMU_SRCS += peripherals/mpu60x0.c +IMU_PX4FMU_SRCS += peripherals/mpu60x0_spi.c +IMU_PX4FMU_CFLAGS += -DUSE_SPI1 +IMU_PX4FMU_CFLAGS += -DUSE_SPI_SLAVE0 -DUSE_SPI_SLAVE1 -DUSE_SPI_SLAVE2 + +# Magnetometer +IMU_PX4FMU_SRCS += peripherals/hmc58xx.c +IMU_PX4FMU_CFLAGS += -DUSE_I2C2 + + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets +# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example + +ap.CFLAGS += $(IMU_PX4FMU_CFLAGS) +ap.srcs += $(IMU_PX4FMU_SRCS) + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h new file mode 100644 index 00000000000..4aff1cc1c7f --- /dev/null +++ b/sw/airborne/boards/px4fmu/baro_board.h @@ -0,0 +1,14 @@ + +/* + * board specific fonctions for the PX4FMU board + * + */ + +#ifndef BOARDS_PX4FMU_BARO_H +#define BOARDS_PX4FMU_BARO_H + +extern void baro_event(void (*b_abs_handler)(void)); + +#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) + +#endif /* BOARDS_PX4FMU_BARO_H */ diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h new file mode 100644 index 00000000000..deb299a1d6e --- /dev/null +++ b/sw/airborne/boards/px4fmu_1.7.h @@ -0,0 +1,313 @@ +#ifndef CONFIG_PX4FMU_1_7_H +#define CONFIG_PX4FMU_1_7_H + +#define BOARD_PX4FMU + +/* PX4 has a 16MHz external clock and 168MHz internal. */ +#define EXT_CLK 16000000 +#define AHB_CLK 168000000 + + +/* + * Onboard LEDs + */ +/* red/amber , on PB14 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOB +#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_1_GPIO_PIN GPIO14 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* blue, on PB15 */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOB +#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_2_GPIO_PIN GPIO15 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + + + +/* + * UART + */ +#define UART1_GPIO_AF GPIO_AF7 +#define UART1_GPIO_PORT_RX GPIOB +#define UART1_GPIO_RX GPIO7 +#define UART1_GPIO_PORT_TX GPIOB +#define UART1_GPIO_TX GPIO6 + +#define UART2_GPIO_AF GPIO_AF7 +#define UART2_GPIO_PORT_RX GPIOA +#define UART2_GPIO_RX GPIO3 +#define UART2_GPIO_PORT_TX GPIOA +#define UART2_GPIO_TX GPIO2 + +#define UART6_GPIO_AF GPIO_AF8 +#define UART6_GPIO_PORT_RX GPIOC +#define UART6_GPIO_RX GPIO7 +#define UART6_GPIO_PORT_TX GPIOC +#define UART6_GPIO_TX GPIO6 + + +/* + * SPI + */ +/* SPI1 for MPU and accel/gyro if populated */ +#define SPI1_GPIO_AF GPIO_AF5 +#define SPI1_GPIO_PORT_MISO GPIOA +#define SPI1_GPIO_MISO GPIO6 +#define SPI1_GPIO_PORT_MOSI GPIOA +#define SPI1_GPIO_MOSI GPIO7 +#define SPI1_GPIO_PORT_SCK GPIOA +#define SPI1_GPIO_SCK GPIO5 + +/* SPI3 on microSD connector */ +#define SPI3_GPIO_AF GPIO_AF5 +#define SPI3_GPIO_PORT_MISO GPIOC +#define SPI3_GPIO_MISO GPIO11 +#define SPI3_GPIO_PORT_MOSI GPIOB +#define SPI3_GPIO_MOSI GPIO5 +#define SPI3_GPIO_PORT_SCK GPIOC +#define SPI3_GPIO_SCK GPIO10 + +/* + * SPI slave pin declaration + */ +/* GYRO_CS on SPI1 */ +#define SPI_SELECT_SLAVE0_PORT GPIOC +#define SPI_SELECT_SLAVE0_PIN GPIO14 + +/* ACCEL_CS on SPI1 */ +#define SPI_SELECT_SLAVE1_PORT GPIOC +#define SPI_SELECT_SLAVE1_PIN GPIO15 + +/* MPU_CS on SPI1 */ +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO0 + +/* SPI3 NSS on microSD connector */ +#define SPI_SELECT_SLAVE3_PORT GPIOA +#define SPI_SELECT_SLAVE3_PIN GPIO4 + + +/* Onboard ADCs */ +#define USE_AD_TIM4 1 + +#define BOARD_ADC_CHANNEL_1 11 +#define BOARD_ADC_CHANNEL_2 12 +#define BOARD_ADC_CHANNEL_3 13 +#define BOARD_ADC_CHANNEL_4 10 + +#ifndef USE_AD1 +#define USE_AD1 1 +#endif +/* provide defines that can be used to access the ADC_x in the code or airframe file + * these directly map to the index number of the 4 adc channels defined above + * 4th (index 3) is used for bat monitoring by default + */ +#define ADC_1 ADC1_C1 +#ifdef USE_ADC_1 +#ifndef ADC_1_GPIO_CLOCK_PORT +#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_1_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1) +#endif +#define USE_AD1_1 1 +#else +#define ADC_1_GPIO_CLOCK_PORT 0 +#define ADC_1_INIT() {} +#endif + +#define ADC_2 ADC1_C2 +#ifdef USE_ADC_2 +#ifndef ADC_2_GPIO_CLOCK_PORT +#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO2) +#endif +#define USE_AD1_2 1 +#else +#define ADC_2_GPIO_CLOCK_PORT 0 +#define ADC_2_INIT() {} +#endif + +#define ADC_3 ADC1_C3 +#ifdef USE_ADC_3 +#ifndef ADC_3_GPIO_CLOCK_PORT +#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO3) +#endif +#define USE_AD1_3 1 +#else +#define ADC_3_GPIO_CLOCK_PORT 0 +#define ADC_3_INIT() {} +#endif + +#define ADC_4 ADC1_C4 +#ifndef ADC_4_GPIO_CLOCK_PORT +#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_4_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0) +#endif +#define USE_AD1_4 1 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY ADC_4 +#endif +#define DefaultVoltageOfAdc(adc) (0.006185*adc) + +#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT) + +/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + ADC_1_INIT(); \ + ADC_2_INIT(); \ + ADC_3_INIT(); \ + ADC_4_INIT(); \ +} +#endif // USE_AD1 + + +/* + * I2C mapping + */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO8 +#define I2C1_GPIO_SDA GPIO9 + +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + +#define I2C3_GPIO_PORT_SCL GPIOA +#define I2C3_GPIO_SCL GPIO8 +#define I2C3_GPIO_PORT_SDA GPIOC +#define I2C3_GPIO_SDA GPIO9 + + +/* + * PPM + */ +#define USE_PPM_TIM1 1 + +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM1_CC_IRQ +#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO10 +#define PPM_GPIO_AF GPIO_AF1 + + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +/* GPIO_EXT1 on PX4FMU */ +#define SPEKTRUM_BIND_PIN GPIO4 +#define SPEKTRUM_BIND_PIN_PORT GPIOC + +#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN +#define SPEKTRUM_UART2_BANK GPIOA +#define SPEKTRUM_UART2_PIN GPIO3 +#define SPEKTRUM_UART2_AF GPIO_AF7 +#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ +#define SPEKTRUM_UART2_ISR usart2_isr +#define SPEKTRUM_UART2_DEV USART2 + + + +/* Activate onboard baro */ +#define BOARD_HAS_BARO 1 + + + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + +/* PWM */ +#define PWM_USE_TIM2 1 + +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 + +// Servo numbering on the PX4 starts with 1 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +// SRV1 is also UART2_CTS +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM2 +#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_1_GPIO GPIOA +#define PWM_SERVO_1_PIN GPIO0 +#define PWM_SERVO_1_AF GPIO_AF1 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +// SRV2 is also UART2_RTS +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM2 +#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_2_GPIO GPIOA +#define PWM_SERVO_2_PIN GPIO1 +#define PWM_SERVO_2_AF GPIO_AF1 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +// SRV3 is also UART2_TX +#if USE_PWM3 +#define PWM_SERVO_3_IDX 2 +#define PWM_SERVO_3_TIMER TIM2 +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_3_GPIO GPIOA +#define PWM_SERVO_3_PIN GPIO2 +#define PWM_SERVO_3_AF GPIO_AF1 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +// SRV4 is also UART2_RX +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM2 +#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_4_GPIO GPIOA +#define PWM_SERVO_4_PIN GPIO3 +#define PWM_SERVO_4_AF GPIO_AF1 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + + +#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) + + +#endif /* CONFIG_PX4FMU_1_7_H */ diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.c b/sw/airborne/subsystems/imu/imu_px4fmu.c new file mode 100644 index 00000000000..73be88f4756 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu.c @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/imu/imu_px4fmu.c + * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883. + */ + +#include "subsystems/imu.h" + +#include "mcu_periph/spi.h" +#include "peripherals/hmc58xx_regs.h" + + +/* MPU60x0 gyro/accel internal lowpass frequency */ +#if !defined PX4FMU_LOWPASS_FILTER && !defined PX4FMU_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define PX4FMU_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") +#elif PERIODIC_FREQUENCY == 512 +/* Accelerometer: Bandwidth 260Hz, Delay 0ms + * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz + */ +#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define PX4FMU_SMPLRT_DIV 3 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#else +#error Non-default PERIODIC_FREQUENCY: please define PX4FMU_LOWPASS_FILTER and PX4FMU_SMPLRT_DIV. +#endif +#endif +PRINT_CONFIG_VAR(PX4FMU_LOWPASS_FILTER) +PRINT_CONFIG_VAR(PX4FMU_SMPLRT_DIV) + +#ifndef PX4FMU_GYRO_RANGE +#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 +#endif +PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE) + +#ifndef PX4FMU_ACCEL_RANGE +#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G +#endif +PRINT_CONFIG_VAR(PX4FMU_ACCEL_RANGE) + +struct ImuPx4fmu imu_px4fmu; + +void imu_impl_init(void) +{ + imu_px4fmu.accel_valid = FALSE; + imu_px4fmu.gyro_valid = FALSE; + imu_px4fmu.mag_valid = FALSE; + + /* MPU is on spi1 and CS is SLAVE2 */ + mpu60x0_spi_init(&imu_px4fmu.mpu, &spi1, SPI_SLAVE2); + // change the default configuration + imu_px4fmu.mpu.config.smplrt_div = PX4FMU_SMPLRT_DIV; + imu_px4fmu.mpu.config.dlpf_cfg = PX4FMU_LOWPASS_FILTER; + imu_px4fmu.mpu.config.gyro_range = PX4FMU_GYRO_RANGE; + imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE; + + /* initialize mag on i2c2 and set default options */ + hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR); +} + + +void imu_periodic(void) +{ + mpu60x0_spi_periodic(&imu_px4fmu.mpu); + + // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz) + RunOnceEvery(10, hmc58xx_periodic(&imu_px4fmu.hmc)); +} + +void imu_px4fmu_event(void) +{ + mpu60x0_spi_event(&imu_px4fmu.mpu); + if (imu_px4fmu.mpu.data_available) { + RATES_ASSIGN(imu.gyro_unscaled, + imu_px4fmu.mpu.data_rates.rates.q, + imu_px4fmu.mpu.data_rates.rates.p, + -imu_px4fmu.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + imu_px4fmu.mpu.data_accel.vect.y, + imu_px4fmu.mpu.data_accel.vect.x, + -imu_px4fmu.mpu.data_accel.vect.z); + imu_px4fmu.mpu.data_available = FALSE; + imu_px4fmu.gyro_valid = TRUE; + imu_px4fmu.accel_valid = TRUE; + } + + /* HMC58XX event task */ + hmc58xx_event(&imu_px4fmu.hmc); + if (imu_px4fmu.hmc.data_available) { + imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y; + imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x; + imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z; + imu_px4fmu.hmc.data_available = FALSE; + imu_px4fmu.mag_valid = TRUE; + } +} + diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.h b/sw/airborne/subsystems/imu/imu_px4fmu.h new file mode 100644 index 00000000000..1620b9892c1 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu.h @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/imu/imu_px4fmu.h + * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883. + */ + +#ifndef IMU_PX4FMU_SPI_H +#define IMU_PX4FMU_SPI_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "subsystems/imu/imu_mpu60x0_defaults.h" +#include "peripherals/mpu60x0_spi.h" +#include "peripherals/hmc58xx.h" + +#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN +#define IMU_GYRO_P_SIGN 1 +#define IMU_GYRO_Q_SIGN 1 +#define IMU_GYRO_R_SIGN 1 +#endif +#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN +#define IMU_ACCEL_X_SIGN 1 +#define IMU_ACCEL_Y_SIGN 1 +#define IMU_ACCEL_Z_SIGN 1 +#endif +#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN +#define IMU_MAG_X_SIGN 1 +#define IMU_MAG_Y_SIGN 1 +#define IMU_MAG_Z_SIGN 1 +#endif + +struct ImuPx4fmu { + volatile bool_t gyro_valid; + volatile bool_t accel_valid; + volatile bool_t mag_valid; + struct Mpu60x0_Spi mpu; + struct Hmc58xx hmc; +}; + +extern struct ImuPx4fmu imu_px4fmu; + +extern void imu_px4fmu_event(void); + + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_px4fmu_event(); + if (imu_px4fmu.gyro_valid) { + imu_px4fmu.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_px4fmu.accel_valid) { + imu_px4fmu.accel_valid = FALSE; + _accel_handler(); + } + if (imu_px4fmu.mag_valid) { + imu_px4fmu.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_PX4FMU_H */ From 9489857f01be337b791ef84ee88cb74e139740b3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 1 Sep 2013 17:00:43 +0200 Subject: [PATCH 61/65] [rotorcraft] add FAILSAFE_ON_BAT_CRITICAL set FAILSAFE_ON_BAT_CRITICAL to TRUE if you want to go into FAILSAFE mode (neutral attitude, slowly descend) if battery status is critical (vsupply voltage below CRITIC_BAT_LEVEL for a while) --- sw/airborne/firmwares/rotorcraft/main.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index beb36fcb31a..8dfd160743f 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -222,6 +222,14 @@ STATIC_INLINE void failsafe_check( void ) { autopilot_set_mode(AP_MODE_FAILSAFE); } +#if FAILSAFE_ON_BAT_CRITICAL + if (autopilot_mode != AP_MODE_KILL && + electrical.bat_critical) + { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + #if USE_GPS if (autopilot_mode == AP_MODE_NAV && autopilot_motors_on && From 21df20cae8fb9a2298f102c68190d7769485a705 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sun, 1 Sep 2013 22:59:13 +0200 Subject: [PATCH 62/65] [ArDrone] Default LEDS --- conf/boards/ardrone2_raw.makefile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/conf/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile index 5ab2d5f7cc0..9e960525940 100644 --- a/conf/boards/ardrone2_raw.makefile +++ b/conf/boards/ardrone2_raw.makefile @@ -35,8 +35,8 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW # ----------------------------------------------------------------------- # default LED configuration -RADIO_CONTROL_LED ?= none +RADIO_CONTROL_LED ?= 4 BARO_LED ?= none -AHRS_ALIGNER_LED ?= none -GPS_LED ?= none -SYS_TIME_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= 3 +SYS_TIME_LED ?= 1 From 6d33e8553dc9c621460e73cf1a885978b485dcbb Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sun, 1 Sep 2013 23:06:07 +0200 Subject: [PATCH 63/65] [ArDrone] Extra Safety --- sw/airborne/boards/ardrone/gpio_ardrone.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 9e3786722b2..6d2202557b0 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -22,6 +22,8 @@ * ardrone GPIO driver */ +#ifdef ARDRONE2_RAW + #include /* File control definitions */ #include /* Error number definitions */ #include @@ -125,3 +127,5 @@ uint16_t gpio_get(uint32_t port, uint16_t pin) ioctl(gpiofp, GPIO_READ, &data); return data.value; } + +#endif From d44b40255f4ddd06f7f9d257d02aab056965b2a5 Mon Sep 17 00:00:00 2001 From: softsr Date: Sun, 1 Sep 2013 22:45:47 -0700 Subject: [PATCH 64/65] Krooz imu fix --- sw/airborne/boards/krooz/imu_krooz.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index e120cbbea39..8d369be25a9 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -180,7 +180,7 @@ void imu_krooz_event( void ) // If the HMC5883 I2C transaction has succeeded: convert the data hmc58xx_event(&imu_krooz.hmc); if (imu_krooz.hmc.data_available) { - VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.z, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.y); + VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z); UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); imu_krooz.hmc.data_available = FALSE; imu_krooz.mag_valid = TRUE; From 2782e11ad9b5f2c50416f45f4d04d795fbe90943 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sun, 1 Sep 2013 23:12:17 +0200 Subject: [PATCH 65/65] [airframes] CDW --- conf/airframes/CDW/asctec_cdw.xml | 11 +++++++---- conf/airframes/CDW/conf.xml | 22 ++++++++++++++++++++++ conf/airframes/CDW/tricopter_cdw.xml | 4 +++- 3 files changed, 32 insertions(+), 5 deletions(-) create mode 100644 conf/airframes/CDW/conf.xml diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index 300eb6eb119..e28e7f538b0 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -1,12 +1,14 @@ - + - + + + @@ -61,6 +63,7 @@ + @@ -194,7 +197,7 @@ - +
@@ -221,4 +224,4 @@
- + diff --git a/conf/airframes/CDW/conf.xml b/conf/airframes/CDW/conf.xml new file mode 100644 index 00000000000..c8ba362e02e --- /dev/null +++ b/conf/airframes/CDW/conf.xml @@ -0,0 +1,22 @@ + + + + diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 00d9368d88d..48e2192956c 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -2,6 +2,8 @@ - +