From 69c55a06630c79ea42aea0b65f2080474e6eb24a Mon Sep 17 00:00:00 2001 From: Pietro Campolucci <45997587+pcampolucci@users.noreply.github.com> Date: Wed, 15 Sep 2021 22:26:28 +0200 Subject: [PATCH] EKF2 Optical Flow Interface (#2779) Co-authored-by: Dennis van Wijngaarden --- .vscode/settings.json | 6 + conf/airframes/tudelft/nederquad.xml | 309 ++++++++++++++++++ conf/modules/gps_datalink.xml | 1 + conf/modules/ins_ekf2.xml | 22 +- .../optical_flow_mateksys_3901_l0x.xml | 21 +- conf/userconf/tudelft/conf.xml | 11 + conf/userconf/tudelft/control_panel.xml | 72 ++++ .../modules/optical_flow/mateksys_3901_l0x.c | 137 +++++--- .../modules/optical_flow/mateksys_3901_l0x.h | 35 +- sw/airborne/modules/optical_flow/px4flow.c | 2 + sw/airborne/modules/optical_flow/px4flow.h | 1 + .../modules/optical_flow/px4flow_i2c.c | 3 + sw/airborne/subsystems/abi_sender_ids.h | 4 + sw/airborne/subsystems/gps/gps_datalink.c | 45 ++- sw/airborne/subsystems/ins/ins_ekf2.cpp | 218 ++++++++++-- sw/airborne/subsystems/ins/ins_ekf2.h | 2 + sw/ext/pprzlink | 2 +- 17 files changed, 789 insertions(+), 102 deletions(-) create mode 100644 conf/airframes/tudelft/nederquad.xml diff --git a/.vscode/settings.json b/.vscode/settings.json index 1dba363774..7e8a23ab98 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -8,4 +8,10 @@ "[python]": { "editor.tabSize": 4, }, + "files.associations": { + "mateksys_3901_l0x.h": "c", + "stdlib.h": "c", + "iterator": "cpp", + "tensor": "cpp" + }, } \ No newline at end of file diff --git a/conf/airframes/tudelft/nederquad.xml b/conf/airframes/tudelft/nederquad.xml new file mode 100644 index 0000000000..461a222c3e --- /dev/null +++ b/conf/airframes/tudelft/nederquad.xml @@ -0,0 +1,309 @@ + + + + + + Detouchable Quadcopter for TUDelft Outback Challenge + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + + +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + +
+
+ + + + + + +
+
+ + +
+
+ + + + + +
+
+ + + + + +
+ diff --git a/conf/modules/gps_datalink.xml b/conf/modules/gps_datalink.xml index f918ac7f41..982374f3b6 100644 --- a/conf/modules/gps_datalink.xml +++ b/conf/modules/gps_datalink.xml @@ -6,6 +6,7 @@ Remote GPS via datalink. Parses the REMOTE_GPS and REMOTE_GPS_SMALL datalink messages and publishes it onboard via ABI. + gps,@datalink diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml index 1f15a31bcb..df9f8702c5 100644 --- a/conf/modules/ins_ekf2.xml +++ b/conf/modules/ins_ekf2.xml @@ -5,11 +5,21 @@ simple INS and AHRS using EKF2 from PX4 + + + + + + + + + + + + - - @@ -22,6 +32,7 @@ + @@ -45,9 +56,9 @@ - - - + + + @@ -67,5 +78,6 @@ + diff --git a/conf/modules/optical_flow_mateksys_3901_l0x.xml b/conf/modules/optical_flow_mateksys_3901_l0x.xml index d39f52ae27..08f960a0de 100644 --- a/conf/modules/optical_flow_mateksys_3901_l0x.xml +++ b/conf/modules/optical_flow_mateksys_3901_l0x.xml @@ -12,11 +12,24 @@ + + + + + + + + + + + + +
@@ -32,18 +45,10 @@ - - - - - - - - diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 6960e42f6c..6714f43344 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -309,6 +309,17 @@ gui_color="blue" release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e" /> +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/optical_flow/mateksys_3901_l0x.c b/sw/airborne/modules/optical_flow/mateksys_3901_l0x.c index 1da70121e5..45c4d0a134 100644 --- a/sw/airborne/modules/optical_flow/mateksys_3901_l0x.c +++ b/sw/airborne/modules/optical_flow/mateksys_3901_l0x.c @@ -25,6 +25,7 @@ * */ +#include #include "mateksys_3901_l0x.h" #include "mcu_periph/uart.h" #include "subsystems/abi.h" @@ -39,23 +40,39 @@ // Define configuration parameters #ifndef MATEKSYS_3901_L0X_MOTION_THRES -#define MATEKSYS_3901_L0X_MOTION_THRES +#define MATEKSYS_3901_L0X_MOTION_THRES 100 #endif #ifndef MATEKSYS_3901_L0X_DISTANCE_THRES -#define MATEKSYS_3901_L0X_DISTANCE_THRES +#define MATEKSYS_3901_L0X_DISTANCE_THRES 200 +#endif + +#ifndef MATEKSYS_3901_L0X_MAX_FLOW +#define MATEKSYS_3901_L0X_MAX_FLOW 300 +#endif + +#ifndef MATEKSYS_3901_L0X_MAX_DISTANCE +#define MATEKSYS_3901_L0X_MAX_DISTANCE 3000 #endif #ifndef USE_MATEKSYS_3901_L0X_AGL -#define USE_MATEKSYS_3901_L0X_AGL +#define USE_MATEKSYS_3901_L0X_AGL 1 #endif #ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW -#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW +#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW 1 #endif #ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION -#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION +#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION 1 +#endif + +#ifndef MATEKSYS_3901_L0X_FLOW_X_SCALER +#define MATEKSYS_3901_L0X_FLOW_X_SCALER 1 +#endif + +#ifndef MATEKSYS_3901_L0X_FLOW_Y_SCALER +#define MATEKSYS_3901_L0X_FLOW_Y_SCALER 1 #endif struct Mateksys3901l0X mateksys3901l0x = { @@ -73,15 +90,16 @@ static void mateksys3901l0x_parse(uint8_t byte); static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID, - &mateksys3901l0x.time_sec, - &mateksys3901l0x.sensor_id, - &mateksys3901l0x.motionX_clean, - &mateksys3901l0x.motionY_clean, - &mateksys3901l0x.velocityX, - &mateksys3901l0x.velocityY, - &mateksys3901l0x.motion_quality, - &mateksys3901l0x.distance_clean, - &mateksys3901l0x.distancemm_quality); + &mateksys3901l0x.time_usec, + &mateksys3901l0x.sensor_id, + &mateksys3901l0x.motionX, + &mateksys3901l0x.motionY, + &mateksys3901l0x.velocityX, + &mateksys3901l0x.velocityY, + &mateksys3901l0x.motion_quality, + &mateksys3901l0x.distancemm, + &mateksys3901l0x.distance_compensated, + &mateksys3901l0x.distancemm_quality); } #endif @@ -93,18 +111,40 @@ void mateksys3901l0x_init(void) { mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device); mateksys3901l0x.parse_crc = 0; - mateksys3901l0x.motion_quality = 0; - mateksys3901l0x.motionX = 0; + mateksys3901l0x.motion_quality = 0; + mateksys3901l0x.motionX_temp = 0; + mateksys3901l0x.motionX = 0; + mateksys3901l0x.motionY_temp = 0; mateksys3901l0x.motionY = 0; mateksys3901l0x.distancemm_quality = 0; - mateksys3901l0x.distancemm = 0; + mateksys3901l0x.distancemm_temp = 0; + mateksys3901l0x.distancemm = 0; + mateksys3901l0x.distance_compensated = 0; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; + mateksys3901l0x.scaler_x = MATEKSYS_3901_L0X_FLOW_X_SCALER; + mateksys3901l0x.scaler_y = MATEKSYS_3901_L0X_FLOW_Y_SCALER; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW, mateksys3901l0x_send_optical_flow); #endif } +/** + * Scale the Flow X + */ +void mateksys_3901_l0x_scale_X(float scalex) +{ + mateksys3901l0x.scaler_x = scalex; +} + +/** + * Scale the Flow Y + */ +void mateksys_3901_l0x_scale_Y(float scaley) +{ + mateksys3901l0x.scaler_y = scaley; +} + /** * Receive bytes from the UART port and parse them */ @@ -205,26 +245,26 @@ static void mateksys3901l0x_parse(uint8_t byte) if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) { mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; } else { - mateksys3901l0x.distancemm = byte; + mateksys3901l0x.distancemm_temp = byte; mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; } break; case MATEKSYS_3901_L0X_PARSE_DISTANCE_B2: - mateksys3901l0x.distancemm |= (byte << 8); + mateksys3901l0x.distancemm_temp |= (byte << 8); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_DISTANCE_B3: - mateksys3901l0x.distancemm |= (byte << 16); + mateksys3901l0x.distancemm_temp |= (byte << 16); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_DISTANCE_B4: - mateksys3901l0x.distancemm |= (byte << 24); + mateksys3901l0x.distancemm_temp |= (byte << 24); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_CHECKSUM; break; @@ -240,50 +280,50 @@ static void mateksys3901l0x_parse(uint8_t byte) if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) { mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; } else { - mateksys3901l0x.motionY = byte; + mateksys3901l0x.motionY_temp = byte; mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; } break; case MATEKSYS_3901_L0X_PARSE_MOTIONY_B2: - mateksys3901l0x.motionY |= (byte << 8); + mateksys3901l0x.motionY_temp |= (byte << 8); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONY_B3: - mateksys3901l0x.motionY |= (byte << 16); + mateksys3901l0x.motionY_temp |= (byte << 16); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONY_B4: - mateksys3901l0x.motionY |= (byte << 24); + mateksys3901l0x.motionY_temp |= (byte << 24); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONX_B1: - mateksys3901l0x.motionX = byte; + mateksys3901l0x.motionX_temp = byte; mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONX_B2: - mateksys3901l0x.motionX |= (byte << 8); + mateksys3901l0x.motionX_temp |= (byte << 8); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONX_B3: - mateksys3901l0x.motionX |= (byte << 16); + mateksys3901l0x.motionX_temp |= (byte << 16); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; case MATEKSYS_3901_L0X_PARSE_MOTIONX_B4: - mateksys3901l0x.motionX |= (byte << 24); + mateksys3901l0x.motionX_temp |= (byte << 24); mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_status++; break; @@ -291,41 +331,42 @@ static void mateksys3901l0x_parse(uint8_t byte) case MATEKSYS_3901_L0X_PARSE_CHECKSUM: // When the distance and motion info are valid (max values based on sensor specifications)... - if (mateksys3901l0x.distancemm > 0 && mateksys3901l0x.distancemm <= 3000 && abs(mateksys3901l0x.motionX) <= 300 && abs(mateksys3901l0x.motionY) <= 300) { - // ... compensate AGL measurement for body rotation + if (mateksys3901l0x.distancemm_temp > 0 && mateksys3901l0x.distancemm_temp <= MATEKSYS_3901_L0X_MAX_DISTANCE && abs(mateksys3901l0x.motionX_temp) <= MATEKSYS_3901_L0X_MAX_FLOW && abs(mateksys3901l0x.motionY_temp) <= MATEKSYS_3901_L0X_MAX_FLOW) { + + // pass temporary message and apply calibration parameters + mateksys3901l0x.motionX = mateksys3901l0x.motionX_temp * mateksys3901l0x.scaler_x; + mateksys3901l0x.motionY = mateksys3901l0x.motionY_temp * mateksys3901l0x.scaler_y; + mateksys3901l0x.distancemm = mateksys3901l0x.distancemm_temp; + + // get from ground distance to altitude by compensating for body rotation if (MATEKSYS_3901_L0X_COMPENSATE_ROTATION) { + float phi = stateGetNedToBodyEulers_f()->phi; float theta = stateGetNedToBodyEulers_f()->theta; - float gain = (float)fabs((double)(cosf(phi) * cosf(theta))); - mateksys3901l0x.distancemm = mateksys3901l0x.distancemm * gain; + mateksys3901l0x.distance_compensated = ((mateksys3901l0x.distancemm * cos(phi)) * cos(theta)) * 0.001; + } - // send messages with no error measurements - mateksys3901l0x.distance_clean = mateksys3901l0x.distancemm/1000.f; - mateksys3901l0x.motionX_clean = mateksys3901l0x.motionX; - mateksys3901l0x.motionY_clean = mateksys3901l0x.motionY; - - // estimate velocity and send it to telemetry - mateksys3901l0x.velocityX = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionX_clean)); - mateksys3901l0x.velocityY = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionY_clean)); + // estimate velocity and send it to telemetry (flow not compensated for gyro measurements) + mateksys3901l0x.velocityX = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionY)); // velocity in m/sec + mateksys3901l0x.velocityY = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionX)); // velocity in m/sec // get ticks - uint32_t now_ts = get_sys_time_usec(); - mateksys3901l0x.time_sec = now_ts*1e-6; + mateksys3901l0x.time_usec = get_sys_time_usec(); // send AGL (if requested) if (USE_MATEKSYS_3901_L0X_AGL) { AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID, - now_ts, - mateksys3901l0x.distance_clean); + mateksys3901l0x.time_usec, + mateksys3901l0x.distance_compensated); } // send optical flow (if requested) if (USE_MATEKSYS_3901_L0X_OPTICAL_FLOW) { AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID, - now_ts, - mateksys3901l0x.motionX_clean, - mateksys3901l0x.motionY_clean, + mateksys3901l0x.time_usec, + mateksys3901l0x.motionX, // motion in deg/sec + mateksys3901l0x.motionY, // motion in deg/sec 0, 0, mateksys3901l0x.motion_quality, diff --git a/sw/airborne/modules/optical_flow/mateksys_3901_l0x.h b/sw/airborne/modules/optical_flow/mateksys_3901_l0x.h index c2ff0beaa6..adf013031f 100644 --- a/sw/airborne/modules/optical_flow/mateksys_3901_l0x.h +++ b/sw/airborne/modules/optical_flow/mateksys_3901_l0x.h @@ -22,7 +22,6 @@ /** @file modules/optical_flow/mateksys_3901_l0x.h * @brief Driver for the mateksys_3901_l0x sensor via MSPx protocol output - * */ /* @@ -37,7 +36,6 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2 #include "std.h" #include "stdbool.h" -//#include "filters/median_filter.h" //Who knows we need it ;) #define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF) @@ -45,7 +43,7 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2 #define MSP2_SENSOR_OPTIC_FLOW 0x1F02 enum Mateksys3901l0XParseStatus { - MATEKSYS_3901_L0X_INITIALIZE, // initialization + MATEKSYS_3901_L0X_INITIALIZE, MATEKSYS_3901_L0X_PARSE_HEAD, MATEKSYS_3901_L0X_PARSE_HEAD2, MATEKSYS_3901_L0X_PARSE_DIRECTION, @@ -53,13 +51,13 @@ enum Mateksys3901l0XParseStatus { MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1, MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2, MATEKSYS_3901_L0X_PARSE_SIZE, - MATEKSYS_3901_L0X_PARSE_POINTER, // ?? - MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY, // used if lidar message + MATEKSYS_3901_L0X_PARSE_POINTER, + MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY, MATEKSYS_3901_L0X_PARSE_DISTANCE_B1, MATEKSYS_3901_L0X_PARSE_DISTANCE_B2, MATEKSYS_3901_L0X_PARSE_DISTANCE_B3, MATEKSYS_3901_L0X_PARSE_DISTANCE_B4, - MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY, // used if flow message + MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY, MATEKSYS_3901_L0X_PARSE_MOTIONY_B1, MATEKSYS_3901_L0X_PARSE_MOTIONY_B2, MATEKSYS_3901_L0X_PARSE_MOTIONY_B3, @@ -74,19 +72,22 @@ enum Mateksys3901l0XParseStatus { struct Mateksys3901l0X { struct link_device *device; enum Mateksys3901l0XParseStatus parse_status; - float time_sec; + float time_usec; uint8_t sensor_id; - uint8_t motion_quality; + uint8_t motion_quality; + int32_t motionX_temp; int32_t motionX; + int32_t motionY_temp; int32_t motionY; - int32_t motionX_clean; - int32_t motionY_clean; - uint8_t distancemm_quality; - int32_t distancemm; - float distance_clean; - float velocityX; - float velocityY; - uint8_t parse_crc; + uint8_t distancemm_quality; + int32_t distancemm_temp; + float distancemm; + float distance_compensated; + float velocityX; + float velocityY; + uint8_t parse_crc; + float scaler_x; + float scaler_y; }; extern struct Mateksys3901l0X mateksys3901l0x; @@ -94,6 +95,8 @@ extern struct Mateksys3901l0X mateksys3901l0x; extern void mateksys3901l0x_init(void); extern void mateksys3901l0x_event(void); extern void mateksys3901l0x_downlink(void); +extern void mateksys_3901_l0x_scale_X(float scalex); +extern void mateksys_3901_l0x_scale_Y(float scaley); #endif /* MATEKSYS_3901_L0X_H */ diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c index 0903c0cb42..5c93311add 100644 --- a/sw/airborne/modules/optical_flow/px4flow.c +++ b/sw/airborne/modules/optical_flow/px4flow.c @@ -102,6 +102,7 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u float phi = stateGetNedToBodyEulers_f()->phi; float theta = stateGetNedToBodyEulers_f()->theta; float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); + optical_flow.distance = optical_flow.ground_distance; optical_flow.ground_distance = optical_flow.ground_distance / gain; } @@ -172,6 +173,7 @@ void px4flow_downlink(void) &optical_flow.flow_comp_m_x, &optical_flow.flow_comp_m_y, &optical_flow.quality, + &optical_flow.distance, &optical_flow.ground_distance, &distance_quality); } diff --git a/sw/airborne/modules/optical_flow/px4flow.h b/sw/airborne/modules/optical_flow/px4flow.h index 8050d1b5cb..2115a1ebe3 100644 --- a/sw/airborne/modules/optical_flow/px4flow.h +++ b/sw/airborne/modules/optical_flow/px4flow.h @@ -47,6 +47,7 @@ struct mavlink_optical_flow { float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated [meters/sec] float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated [meters/sec] float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + float distance; ///< Distance measured without compensation in meters int32_t flow_x; ///< Flow in pixels in x-sensor direction int32_t flow_y; ///< Flow in pixels in y-sensor direction uint8_t sensor_id; ///< Sensor ID diff --git a/sw/airborne/modules/optical_flow/px4flow_i2c.c b/sw/airborne/modules/optical_flow/px4flow_i2c.c index 96a0ef9881..f9af7718c0 100644 --- a/sw/airborne/modules/optical_flow/px4flow_i2c.c +++ b/sw/airborne/modules/optical_flow/px4flow_i2c.c @@ -278,6 +278,7 @@ void px4flow_i2c_downlink(void) uint8_t quality = px4flow.i2c_int_frame.qual; float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0; + float distance = 0; #else int32_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum; int32_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum; @@ -287,6 +288,7 @@ void px4flow_i2c_downlink(void) uint8_t quality = px4flow.i2c_frame.qual; float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0; + float distance; #endif DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice, @@ -298,5 +300,6 @@ void px4flow_i2c_downlink(void) &flow_comp_m_y, &quality, &ground_distance, + &distance, &distance_quality); } diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index dd6d844506..7701212bc2 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -209,6 +209,10 @@ #define MAG_RM3100_SENDER_ID 5 #endif +#ifndef MAG_DATALINK_SENDER_ID +#define MAG_DATALINK_SENDER_ID 6 +#endif + #ifndef IMU_MAG_PITOT_ID #define IMU_MAG_PITOT_ID 50 #endif diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 7b4f84f525..2cbdc87e6b 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -1,3 +1,4 @@ + /* * Copyright (C) 2014 Freek van Tienen * @@ -31,7 +32,15 @@ #include "subsystems/gps.h" #include "subsystems/abi.h" +#include "subsystems/imu.h" #include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" + +/** Set to 1 to receive also magnetometer ABI messages */ +#ifndef GPS_DATALINK_USE_MAG +#define GPS_DATALINK_USE_MAG 1 +#endif +PRINT_CONFIG_VAR(GPS_DATALINK_USE_MAG) struct LtpDef_i ltp_def; @@ -57,6 +66,26 @@ void gps_datalink_init(void) ltp_def_from_lla_i(<p_def, &llh_nav0); } +// Send GPS heading info as magnetometer messages +static void send_magnetometer(int32_t course, uint32_t now_ts) +{ + struct Int32Vect3 mag; + struct FloatVect3 mag_real; + // course from gps in [0, 2*Pi]*1e7 (CW/north) + float heading = course/1e7; + mag_real.x = cos(heading); + mag_real.y = -sin(heading); + mag_real.z = 0; + MAGS_BFP_OF_REAL(mag, mag_real); + + // update IMU information + VECT3_COPY(imu.mag_unscaled, mag); + imu_scale_mag(&imu); + + // Send fake ABI for GPS, Magnetometer and Optical Flow for GPS fusion + AbiSendMsgIMU_MAG_INT32(MAG_DATALINK_SENDER_ID, now_ts, &mag); +} + // Parse the REMOTE_GPS_SMALL datalink packet static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow) { @@ -168,8 +197,14 @@ static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; gps_datalink.last_3dfix_time = sys_time.nb_sec; - // publish new GPS data uint32_t now_ts = get_sys_time_usec(); + + // if selected, publish magnetometer data + #if GPS_DATALINK_USE_MAG + send_magnetometer(course, now_ts); + #endif + + // publish new GPS data AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); } @@ -223,8 +258,14 @@ static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z, gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; gps_datalink.last_3dfix_time = sys_time.nb_sec; - // publish new GPS data uint32_t now_ts = get_sys_time_usec(); + + // if selected, publish magnetometer data + #if GPS_DATALINK_USE_MAG + send_magnetometer(course, now_ts); + #endif + + // Publish GPS data AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); } diff --git a/sw/airborne/subsystems/ins/ins_ekf2.cpp b/sw/airborne/subsystems/ins/ins_ekf2.cpp index ae5b9f50c9..f12a4e6c8c 100644 --- a/sw/airborne/subsystems/ins/ins_ekf2.cpp +++ b/sw/airborne/subsystems/ins/ins_ekf2.cpp @@ -72,16 +72,22 @@ PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK) PRINT_CONFIG_VAR(INS_EKF2_AGL_ID) /** Default AGL sensor minimum range */ -#ifndef INS_SONAR_MIN_RANGE -#define INS_SONAR_MIN_RANGE 0.001 +#ifndef INS_EKF2_SONAR_MIN_RANGE +#define INS_EKF2_SONAR_MIN_RANGE 0.001 #endif -PRINT_CONFIG_VAR(INS_SONAR_MIN_RANGE) +PRINT_CONFIG_VAR(INS_EKF2_SONAR_MIN_RANGE) /** Default AGL sensor maximum range */ -#ifndef INS_SONAR_MAX_RANGE -#define INS_SONAR_MAX_RANGE 4.0 +#ifndef INS_EKF2_SONAR_MAX_RANGE +#define INS_EKF2_SONAR_MAX_RANGE 4 #endif -PRINT_CONFIG_VAR(INS_SONAR_MAX_RANGE) +PRINT_CONFIG_VAR(INS_EKF2_SONAR_MAX_RANGE) + +/** If enabled uses radar sensor as primary AGL source, if possible */ +#ifndef INS_EKF2_RANGE_MAIN_AGL +#define INS_EKF2_RANGE_MAIN_AGL 1 +#endif +PRINT_CONFIG_VAR(INS_EKF2_RANGE_MAIN_AGL) /** default barometer to use in INS */ #ifndef INS_EKF2_BARO_ID @@ -117,6 +123,66 @@ PRINT_CONFIG_VAR(INS_EKF2_MAG_ID) #endif PRINT_CONFIG_VAR(INS_EKF2_GPS_ID) +/* default Optical Flow to use in INS */ +#ifndef INS_EKF2_OF_ID +#define INS_EKF2_OF_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_EKF2_OF_ID) + +/* Default flow/radar message delay (in ms) */ +#ifndef INS_EKF2_FLOW_SENSOR_DELAY +#define INS_EKF2_FLOW_SENSOR_DELAY 15 +#endif +PRINT_CONFIG_VAR(INS_FLOW_SENSOR_DELAY) + +/* Default minimum accepted quality (1 to 255) */ +#ifndef INS_EKF2_MIN_FLOW_QUALITY +#define INS_EKF2_MIN_FLOW_QUALITY 100 +#endif +PRINT_CONFIG_VAR(INS_EKF2_MIN_FLOW_QUALITY) + +/* Max flow rate that the sensor can measure (rad/sec) */ +#ifndef INS_EKF2_MAX_FLOW_RATE +#define INS_EKF2_MAX_FLOW_RATE 200 +#endif +PRINT_CONFIG_VAR(INS_EKF2_MAX_FLOW_RATE) + +/* Flow sensor X offset from IMU position in meters */ +#ifndef INS_EKF2_FLOW_OFFSET_X +#define INS_EKF2_FLOW_OFFSET_X 0 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_X) + +/* Flow sensor Y offset from IMU position in meters */ +#ifndef INS_EKF2_FLOW_OFFSET_Y +#define INS_EKF2_FLOW_OFFSET_Y 0 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Y) + +/* Flow sensor Z offset from IMU position in meters */ +#ifndef INS_EKF2_FLOW_OFFSET_Z +#define INS_EKF2_FLOW_OFFSET_Z 0 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Z) + +/* Flow sensor noise in rad/sec */ +#ifndef INS_EKF2_FLOW_NOISE +#define INS_EKF2_FLOW_NOISE 0.03 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE) + +/* Flow sensor noise at qmin in rad/sec */ +#ifndef INS_EKF2_FLOW_NOISE_QMIN +#define INS_EKF2_FLOW_NOISE_QMIN 0.05 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE_QMIN) + +/* Flow sensor innovation gate */ +#ifndef INS_EKF2_FLOW_INNOV_GATE +#define INS_EKF2_FLOW_INNOV_GATE 4 +#endif +PRINT_CONFIG_VAR(INS_EKF2_FLOW_INNOV_GATE) + /* All registered ABI events */ static abi_event agl_ev; static abi_event baro_ev; @@ -125,7 +191,11 @@ static abi_event accel_ev; static abi_event mag_ev; static abi_event gps_ev; static abi_event body_to_imu_ev; +static abi_event optical_flow_ev; + +/* Build optical flow and gps message struct based on flow message defined in common.h */ struct gps_message gps_msg = {}; +struct flow_message flow_msg = {}; /* All ABI callbacks */ static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance); @@ -135,23 +205,43 @@ static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag); static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f); +static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence); -/* Main EKF2 structure for keeping track of the status */ -struct ekf2_t { +/* Main EKF2 structure for keeping track of the status and use cross messaging */ +struct ekf2_t +{ + + // stamp and dt for sensors uint32_t gyro_stamp; uint32_t gyro_dt; uint32_t accel_stamp; uint32_t accel_dt; + uint32_t flow_stamp; + uint32_t flow_dt; + + // gyro and accellerometer values FloatRates gyro; FloatVect3 accel; bool gyro_valid; bool accel_valid; - uint8_t quat_reset_counter; + // optical flow and gyro values + float flow_quality; + float flow_x; + float flow_y; + float gyro_roll; + float gyro_pitch; + float gyro_yaw; + float offset_x; + float offset_y; + float offset_z; + // optical flow takeover + float flow_innov; + + uint8_t quat_reset_counter; uint64_t ltp_stamp; struct LtpDef_i ltp_def; - struct OrientationReps body_to_imu; bool got_imu_data; }; @@ -191,13 +281,30 @@ static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev) ekf.get_ekf_soln_status(&soln_status); uint16_t innov_test_status; - float mag, vel, pos, hgt, tas, hagl, beta, mag_decl; + float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl; + uint8_t terrain_valid, dead_reckoning; ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta); + ekf.get_flow_innov(&flow); ekf.get_mag_decl_deg(&mag_decl); + + uint32_t fix_status = (control_mode >> 2) & 1; + + if (ekf.get_terrain_valid()) { + terrain_valid = 1; + } else { + terrain_valid = 0; + } + + if (ekf.inertial_dead_reckoning()) { + dead_reckoning = 1; + } else { + dead_reckoning = 0; + } + pprz_msg_send_INS_EKF2(trans, dev, AC_ID, - &control_mode, &filter_fault_status, &gps_check_status, &soln_status, - &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta, - &mag_decl); + &fix_status, &filter_fault_status, &gps_check_status, &soln_status, + &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta, + &mag_decl, &terrain_valid, &dead_reckoning); } static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev) @@ -210,8 +317,8 @@ static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *de gps_blocked_b = gps_blocked; pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID, - &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b, - &vibe[0], &vibe[1], &vibe[2]); + &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b, + &vibe[0], &vibe[1], &vibe[2]); } static void send_filter_status(struct transport_tx *trans, struct link_device *dev) @@ -258,7 +365,7 @@ static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev) ekf.get_gyro_bias(gyro_bias); ekf.get_state_delayed(states); - pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2], + pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2], &gyro_bias[0], &gyro_bias[1], &gyro_bias[2], &states[19], &states[20], &states[21]); } #endif @@ -274,17 +381,38 @@ void ins_ekf2_init(void) ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE; ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK; + /* Set optical flow parameters */ + ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY; + ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY; + ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY; + ekf_params->flow_noise = INS_EKF2_FLOW_NOISE; + ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN; + ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE; + + /* Set flow sensor offset from IMU position in xyz (m) */ + ekf2.offset_x = INS_EKF2_FLOW_OFFSET_X; + ekf2.offset_y = INS_EKF2_FLOW_OFFSET_Y; + ekf2.offset_z = INS_EKF2_FLOW_OFFSET_Z; + ekf_params->flow_pos_body = {0.001f*ekf2.offset_x, 0.001f*ekf2.offset_y, 0.001f*ekf2.offset_z}; + + /* Set range as default AGL measurement if possible */ + ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL; + /* Initialize struct */ ekf2.ltp_stamp = 0; ekf2.accel_stamp = 0; ekf2.gyro_stamp = 0; + ekf2.flow_stamp = 0; ekf2.gyro_valid = false; ekf2.accel_valid = false; ekf2.got_imu_data = false; ekf2.quat_reset_counter = 0; /* Initialize the range sensor limits */ - ekf.set_rangefinder_limits(INS_SONAR_MIN_RANGE, INS_SONAR_MAX_RANGE); + ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE); + + /* Initialize the flow sensor limits */ + ekf.set_optical_flow_limits(INS_EKF2_MAX_FLOW_RATE, INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); @@ -305,6 +433,7 @@ void ins_ekf2_init(void) AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb); AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb); } /* Update the INS state */ @@ -386,12 +515,22 @@ void ins_ekf2_update(void) ekf2.got_imu_data = false; } -void ins_ekf2_change_param(int32_t unk) { +void ins_ekf2_change_param(int32_t unk) +{ ekf_params->mag_fusion_type = ekf2_params.mag_fusion_type = unk; } +void ins_ekf2_remove_gps(int32_t mode) +{ + if (mode) { + ekf_params->fusion_mode = ekf2_params.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW); + } else { + ekf_params->fusion_mode = ekf2_params.fusion_mode = INS_EKF2_FUSION_MODE; + } +} + /** Publish the attitude and get the new state - * Directly called after a succeslfull gyro+accel reading + * Directly called after a succeslfull gyro+accel reading */ static void ins_ekf2_publish_attitude(uint32_t stamp) { @@ -471,12 +610,12 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, f { // Calculate the air density float rho = pprz_isa_density_of_pressure(pressure, - 20.0f); // TODO: add temperature compensation now set to 20 degree celcius + 20.0f); // TODO: add temperature compensation now set to 20 degree celcius ekf.set_air_density(rho); // Calculate the height above mean sea level based on pressure float height_amsl_m = pprz_isa_height_of_pressure_full(pressure, - 101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h + 101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h ekf.setBaroData(stamp, height_amsl_m); } @@ -602,3 +741,38 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), { orientationSetQuat_f(&ekf2.body_to_imu, q_b2i_f); } + +/* Update INS based on Optical Flow information */ +static void optical_flow_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, + int32_t flow_x, + int32_t flow_y, + int32_t flow_der_x __attribute__((unused)), + int32_t flow_der_y __attribute__((unused)), + float quality, + float size_divergence __attribute__((unused))) +{ + // update time + ekf2.flow_dt = stamp - ekf2.flow_stamp; + ekf2.flow_stamp = stamp; + + /* Build integrated flow and gyro messages for filter + NOTE: pure rotations should result in same flow_x and + gyro_roll and same flow_y and gyro_pitch */ + ekf2.flow_quality = quality; + ekf2.flow_x = RadOfDeg(flow_y) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X) + ekf2.flow_y = - RadOfDeg(flow_x) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y) + ekf2.gyro_roll = NAN; + ekf2.gyro_pitch = NAN; + ekf2.gyro_yaw = NAN; + + /* once callback initiated, build the + optical flow message with what is received */ + flow_msg.quality = quality; // quality indicator between 0 and 255 + flow_msg.flowdata = Vector2f(ekf2.flow_x, ekf2.flow_y); // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive + flow_msg.gyrodata = Vector3f{ekf2.gyro_roll, ekf2.gyro_pitch, ekf2.gyro_yaw}; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive + flow_msg.dt = ekf2.flow_dt; // amount of integration time (usec) + + // update the optical flow data based on the callback + ekf.setOpticalFlowData(stamp, &flow_msg); +} diff --git a/sw/airborne/subsystems/ins/ins_ekf2.h b/sw/airborne/subsystems/ins/ins_ekf2.h index 8b5937536d..7f8f0c5472 100644 --- a/sw/airborne/subsystems/ins/ins_ekf2.h +++ b/sw/airborne/subsystems/ins/ins_ekf2.h @@ -38,11 +38,13 @@ extern "C" { struct ekf2_parameters_t { int32_t mag_fusion_type; + int32_t fusion_mode; }; extern void ins_ekf2_init(void); extern void ins_ekf2_update(void); extern void ins_ekf2_change_param(int32_t unk); +extern void ins_ekf2_remove_gps(int32_t mode); extern struct ekf2_parameters_t ekf2_params; #ifdef __cplusplus diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index fca8d15eff..c6e88ccbb9 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit fca8d15effafa84e0446bbf3b12a0b450cc62eba +Subproject commit c6e88ccbb9d65330299576fd8886300113baa5f0