diff --git a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml index 5544a96a32..79d27ad291 100644 --- a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml +++ b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml @@ -51,13 +51,39 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -69,12 +95,13 @@ - - + + + - - - + + + @@ -82,6 +109,7 @@ + @@ -92,21 +120,27 @@ + +
- +
+ + + - + + + - @@ -126,27 +160,28 @@ Normalized Local magnetic field obtained from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm Magnetic field intensity (North-East-vertical) / total field strength H_x = INTENSITY_x / Total Magnetic Field - Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2) - Calculated for LAGONISI ATTIKI 21 AUGUST 2020 + Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2) + Calculated for LAGONISI ATTIKI 23 October 2020 -->
- - - - + + + +
+ + + - - @@ -154,23 +189,31 @@ - - - - + + + + + + + - + + + + + + +
-
@@ -243,9 +286,11 @@
+ + - - + +
@@ -263,16 +308,16 @@ - + - - +
- +
diff --git a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml index 07391d8bd4..c1eb065afd 100644 --- a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml +++ b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml @@ -27,11 +27,9 @@ - - - + @@ -47,28 +45,53 @@ - - + + - + + - + - - - - - + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -80,12 +103,13 @@ - - + + + - - - + + + @@ -93,6 +117,7 @@ + @@ -103,22 +128,27 @@ - + +
- +
+ + + - + + + - @@ -128,7 +158,6 @@ --> - @@ -139,26 +168,27 @@ Magnetic field intensity (North-East-vertical) / total field strength H_x = INTENSITY_x / Total Magnetic Field Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2) - Calculated for LAGONISI ATTIKI 21 AUGUST 2020 + Calculated for LAGONISI ATTIKI 23 October 2020 -->
- - - - + + + +
+ + + - - @@ -166,22 +196,37 @@ - - - - + + + + + + + + + + + + + + +
+
+ + +
@@ -198,7 +243,7 @@ - + @@ -255,9 +300,11 @@
+ + - - + +
@@ -275,16 +322,16 @@ - + - - +
- +
@@ -335,7 +382,7 @@ - +
diff --git a/conf/flight_plans/lagonisi_beach_survey.xml b/conf/flight_plans/lagonisi_beach_survey.xml new file mode 100644 index 0000000000..78730414c9 --- /dev/null +++ b/conf/flight_plans/lagonisi_beach_survey.xml @@ -0,0 +1,155 @@ + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/gcs/gcs_with_video.xml b/conf/gcs/gcs_with_video.xml new file mode 100644 index 0000000000..69125dc906 --- /dev/null +++ b/conf/gcs/gcs_with_video.xml @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/board_matek_wing.xml b/conf/modules/board_matek_wing.xml index 8a1b014b83..bdd8b30050 100644 --- a/conf/modules/board_matek_wing.xml +++ b/conf/modules/board_matek_wing.xml @@ -27,10 +27,8 @@ - - - + + - diff --git a/conf/modules/pca9685.xml b/conf/modules/pca9685.xml new file mode 100644 index 0000000000..21304c21d0 --- /dev/null +++ b/conf/modules/pca9685.xml @@ -0,0 +1,22 @@ + + + + + + pca9685 additional servo driver + in a c code file write pca9865_set_servo(12, 1500); to set servo number 12. + In the flight plan you can use call_once fun="pca9865_set_servo(12, 1500)" + + + + +
+ +
+ + + + + + + diff --git a/conf/modules/uav_recovery.xml b/conf/modules/uav_recovery.xml new file mode 100644 index 0000000000..899c29b78c --- /dev/null +++ b/conf/modules/uav_recovery.xml @@ -0,0 +1,32 @@ + + + + + Parachute + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + diff --git a/conf/radios/openTX_radio_8ch.xml b/conf/radios/openTX_radio_8ch.xml index febe4959fe..ac62be1e88 100644 --- a/conf/radios/openTX_radio_8ch.xml +++ b/conf/radios/openTX_radio_8ch.xml @@ -38,14 +38,14 @@ -- Note: a command may be reversed by exchanging min and max values --> - - - - - - - - - + + + + + + + + + diff --git a/conf/telemetry/telemetry_openlrsng_19200.xml b/conf/telemetry/telemetry_openlrsng_19200.xml index 619a007e01..37f082a4e5 100644 --- a/conf/telemetry/telemetry_openlrsng_19200.xml +++ b/conf/telemetry/telemetry_openlrsng_19200.xml @@ -3,32 +3,35 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + - - + + + - - + - + @@ -40,13 +43,14 @@ - + + - - + + diff --git a/sw/airborne/boards/matek_f405_wing_v1.h b/sw/airborne/boards/matek_f405_wing_v1.h index 74b9a73a6c..08e57b3ae8 100644 --- a/sw/airborne/boards/matek_f405_wing_v1.h +++ b/sw/airborne/boards/matek_f405_wing_v1.h @@ -251,8 +251,8 @@ #define ADC_CHANNEL_VSUPPLY ADC_1 #endif -#ifndef CURRENT_ADC_IN -#define CURRENT_ADC_IN ADC_2 +#ifndef ADC_CHANNEL_CURRENT +#define ADC_CHANNEL_CURRENT ADC_2 #endif /* no voltage divider on board, adjust VoltageOfAdc in airframe file */ diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c index 8479645d32..eb9044bad0 100644 --- a/sw/airborne/modules/display/max7456.c +++ b/sw/airborne/modules/display/max7456.c @@ -47,6 +47,9 @@ #else #include "firmwares/rotorcraft/navigation.h" #endif +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" +#endif // Peripherials #include "modules/display/max7456.h" @@ -55,6 +58,49 @@ #define OSD_STRING_SIZE 31 #define osd_sprintf _osd_sprintf +#if !defined(OSD_USE_FLOAT_LOW_PASS_FILTERING) +#define OSD_USE_FLOAT_LOW_PASS_FILTERING +#endif + +//LOW PASS filter strength, cannot be 0, MAX=16 +#if !defined(AMPS_LOW_PASS_FILTER_STRENGTH) || AMPS_LOW_PASS_FILTER_STRENGTH == 0 +#define AMPS_LOW_PASS_FILTER_STRENGTH 6 +#endif + +#if !defined(SPEED_LOW_PASS_FILTER_STRENGTH) || SPEED_LOW_PASS_FILTER_STRENGTH == 0 +#define SPEED_LOW_PASS_FILTER_STRENGTH 6 +#endif + +#if !defined(BAT_CAPACITY) +#pragma message "BAT_CAPACITY not defined, 5000 mah will be used." +#define BAT_CAPACITY 5000.0 +#endif + +#if AP + +#if !defined(LOITER_BAT_CURRENT) +#pragma message "LOITER_BAT_CURRENT not defined, 10 Amps will be used for LOITER current draw." +#define LOITER_BAT_CURRENT 10.0 +#endif + +#if !defined(STALL_AIRSPEED) +#pragma message "STALL_AIRSPEED not defined, 10 m/s will be used." +#define STALL_AIRSPEED 10.0 +#endif + +#endif + +#if !defined(IMU_MAG_X_SIGN) +#define IMU_MAG_X_SIGN 1 +#endif +#if !defined(IMU_MAG_X_SIGN) +#define IMU_MAG_Y_SIGN 1 +#endif +#if !defined(IMU_MAG_X_SIGN) +#define IMU_MAG_Z_SIGN 1 +#endif + + typedef struct { float fx; float fy; @@ -67,11 +113,16 @@ typedef struct { float fz1; float fz2; float fz3; } MATRIX; - +#if AP +static void mag_compass(void); +#endif +static void send_mag_heading(struct transport_tx *trans, struct link_device *dev); static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC); static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC); static float home_direction(void); static char ascii_to_osd_c(char c); +static void calc_flight_time_left(void); +static void draw_osd(void); static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column); static bool _osd_sprintf(char *buffer, char *string, float value); @@ -113,8 +164,95 @@ uint8_t max7456_osd_status = OSD_UNINIT; uint8_t osd_enable = true; uint8_t osd_enable_val = OSD_IMAGE_ENABLE; uint8_t osd_stat_reg = 0; -float home_dir = 0; +uint32_t max_flyable_distance_left = 0; +float mag_course_deg = 0; +float mag_heading_rad = 0; +float gps_course_deg = 0; +float home_dir_deg = 0; + +#if AP +// Periodic function called with a frequency defined in the module .xml file +void mag_compass(void) +{ + + struct FloatEulers *att = stateGetNedToBodyEulers_f(); + struct Int32Vect3 mag; + struct Int32Vect3 mag_neutrals; + int32_t x = 0, y = 0, z = 0; + float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; float mag_x; float mag_y; + static float mag_declination = 0; + static bool declination_calculated = false; + + VECT3_COPY(mag, imu.mag_unscaled); + VECT3_COPY(mag_neutrals, imu.mag_neutral); +#if (defined(IMU_MAG_X_SENS) && defined(IMU_MAG_Y_SENS) && defined(IMU_MAG_Z_SENS)) && !defined(USE_MAGNETOMETER) + x = ((mag.x - mag_neutrals.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; + y = ((mag.y - mag_neutrals.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; + z = ((mag.z - mag_neutrals.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; +#else + x = (mag.x - mag_neutrals.x) * IMU_MAG_X_SIGN; + y = (mag.y - mag_neutrals.y) * IMU_MAG_Y_SIGN; + z = (mag.z - mag_neutrals.z) * IMU_MAG_Z_SIGN; +#endif + + cos_roll = cosf(att->phi); + sin_roll = sinf(att->phi); + cos_pitch = cosf(att->theta); + sin_pitch = sinf(att->theta); + // Pitch&Roll Compensation: + mag_x = x * cos_pitch + y * sin_roll * sin_pitch + z * cos_roll * sin_pitch; + mag_y = y * cos_roll - z * sin_roll; + + // Magnetic Heading N = 0, E = 90, S = +-180, W = -90 + mag_heading_rad = atan2(-mag_y, mag_x); +#if defined(AHRS_MAG_DECLINATION) + if (AHRS_MAG_DECLINATION != 0.0) { + //conversion from degrees to radians is done in the airframe.h file + mag_heading_rad = mag_heading_rad + AHRS_MAG_DECLINATION; + } +#endif + if (mag_heading_rad > M_PI) { // Angle normalization (-180 deg to 180 deg) + mag_heading_rad -= (2.0 * M_PI); + } else if (mag_heading_rad < -M_PI) { mag_heading_rad += (2.0 * M_PI); } + + if (declination_calculated == false) { +#if defined(NOMINAL_AIRSPEED) + if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > (float)NOMINAL_AIRSPEED) { +#else + if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 10.0) { +#endif + mag_declination = stateGetHorizontalSpeedDir_f(); + if (mag_declination > M_PI) { // Angle normalization (-180 deg to 180 deg) + mag_declination -= (2.0 * M_PI); + } else if (mag_declination < -M_PI) { mag_declination += (2.0 * M_PI); } + mag_declination -= mag_heading_rad; + declination_calculated = true; + if (fabs(mag_declination) > RadOfDeg(10.)) { mag_declination = 0; declination_calculated = false; } + } + } + mag_heading_rad = mag_heading_rad + mag_declination; + if (mag_heading_rad > M_PI) { // Angle normalization (-180 deg to 180 deg) + mag_heading_rad -= (2.0 * M_PI); + } else if (mag_heading_rad < -M_PI) { mag_heading_rad += (2.0 * M_PI); } + // Magnetic COMPASS Heading N = 0, E = 90, S = 180, W = 270 + mag_course_deg = DegOfRad(mag_heading_rad); + if (mag_course_deg < 0) { mag_course_deg += 360; } + + return; +} +#endif + + +static void send_mag_heading(struct transport_tx *trans, struct link_device *dev) +{ + +#if DOWNLINK + pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &mag_course_deg, &gps_course_deg, &home_dir_deg); +#endif + + return; +} //******************************************************************* // function name: vSubtractVectors @@ -144,12 +282,10 @@ static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC) static float home_direction(void) { - static VECTOR svPlanePosition, - Home_Position, - Home_PositionForPlane, - Home_PositionForPlane2; - + static VECTOR svPlanePosition, Home_Position, Home_PositionForPlane, Home_PositionForPlane2; static MATRIX smRotation; + float home_dir = 0; + /* By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle) @@ -157,67 +293,72 @@ static float home_direction(void) applied to the rotation matrices (in radians). In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left and +90 is to the top (counterclockwise rotation). - When reading back the actual rotated coordinates fx has the y coordinate and fy has the x + When reading back the actual rotated coordinates fx has the y coordinate and fy has the x when represented on a circle in standard mathematical notation. */ - - svPlanePosition.fx = stateGetPositionEnu_f()->y; - svPlanePosition.fy = stateGetPositionEnu_f()->x; - svPlanePosition.fz = stateGetPositionUtm_f()->alt; + if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 5.0) { //Only when flying + svPlanePosition.fx = stateGetPositionEnu_f()->y; + svPlanePosition.fy = stateGetPositionEnu_f()->x; + svPlanePosition.fz = stateGetPositionUtm_f()->alt; #ifdef AP - Home_Position.fx = WaypointY(WP_HOME); - Home_Position.fy = WaypointX(WP_HOME); - Home_Position.fz = ground_alt; + Home_Position.fx = WaypointY(WP_HOME); + Home_Position.fy = WaypointX(WP_HOME); + Home_Position.fz = GetAltRef(); #else - Home_Position.fx = waypoint_get_x(WP_HOME); - Home_Position.fy = waypoint_get_y(WP_HOME); - Home_Position.fz = 0; + Home_Position.fx = waypoint_get_x(WP_HOME); + Home_Position.fy = waypoint_get_y(WP_HOME); + Home_Position.fz = 0; #endif - /* distance between plane and object */ - vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition); + /* distance between plane and object */ + vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition); - /* yaw */ - smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f()); - smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f()); - smRotation.fx3 = 0.; - smRotation.fy1 = -smRotation.fx2; - smRotation.fy2 = smRotation.fx1; - smRotation.fy3 = 0.; - smRotation.fz1 = 0.; - smRotation.fz2 = 0.; - smRotation.fz3 = 1.; + /* yaw */ + smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f()); + smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f()); + smRotation.fx3 = 0.; + smRotation.fy1 = -smRotation.fx2; + smRotation.fy2 = smRotation.fx1; + smRotation.fy3 = 0.; + smRotation.fz1 = 0.; + smRotation.fz2 = 0.; + smRotation.fz3 = 1.; - vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane); + vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane); - /* DEFAULT ORIENTATION IS 0 = FRONT, 90 = RIGHT, 180 = BACK, -90 = LEFT - * - * WHEN home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx))); - * - * plane front - * 0˚ - * ^ - * I - * -45˚ I 45˚ - * \ I / - * \I/ - * -90˚-------I------- 90˚ - * /I\ - * / I \ - * -135˚ I 135˚ - * I - * 180 - * plane back - * - * - */ + /* DEFAULT ORIENTATION IS 0 = FRONT, 90 = RIGHT, 180 = BACK, -90 = LEFT + * + * WHEN home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx))); + * + * plane front + * 0˚ + * ^ + * I + * -45˚ I 45˚ + * \ I / + * \I/ + * -90˚-------I------- 90˚ + * /I\ + * / I \ + * -135˚ I 135˚ + * I + * 180 + * plane back + * + * + * When the home_dir variable goes to 0 the aircraft is headed straight back home + */ - /* fixed to the plane*/ - home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx))); - home_dir = DegOfRad(home_dir); - if (home_dir < 0) { home_dir += 360; } + /* fixed to the plane*/ + home_dir = atan2(Home_PositionForPlane2.fy, Home_PositionForPlane2.fx); + if (home_dir > M_PI) { // Angle normalization (-180 deg to 180 deg but still in radians) + home_dir -= (2.0 * M_PI); + } else if (home_dir < -M_PI) { home_dir += (2.0 * M_PI); } + home_dir_deg = DegOfRad(home_dir); // Now convert radians to degrees. - return (home_dir); + } // END OF if (gps.fix == GPS_FIX_3D) statement. + + return (home_dir_deg); } static char ascii_to_osd_c(char c) @@ -229,6 +370,7 @@ static char ascii_to_osd_c(char c) return (c); #else + PRINT_CONFIG_MSG("OSD USES THE STANDARF MAX7456 OSD CHIP") if (c >= '0' && c <= '9') { if (c == '0') { c -= 38; } else { c -= 48; } @@ -246,6 +388,7 @@ static char ascii_to_osd_c(char c) case (';'): c = 0x43; break; case (':'): c = 0x44; break; case (','): c = 0x45; break; + //case('''): c = 0x46; break; case ('/'): c = 0x47; break; case ('"'): c = 0x48; break; case ('-'): c = 0x49; break; @@ -263,6 +406,77 @@ static char ascii_to_osd_c(char c) #endif } +static void calc_flight_time_left(void) +{ + float current_amps = 0; + float horizontal_speed = 0; + float bat_capacity_left = 0; + static float bat_capacity_used = 0; + + + current_amps = electrical.current; + bat_capacity_used += (current_amps * 1000.) / (3600. * (float)MAX7456_PERIODIC_FREQ); + bat_capacity_left = (float)BAT_CAPACITY - bat_capacity_used; + if (bat_capacity_left < 0) { bat_capacity_left = 0; } + horizontal_speed = stateGetHorizontalSpeedNorm_f(); + +#if AP + if (stateGetHorizontalSpeedNorm_f() < 5.0 || autopilot.launch == false) { + current_amps = LOITER_BAT_CURRENT; + horizontal_speed = MINIMUM_AIRSPEED; + } +#else // #if AP + current_amps = 1.0; // FIXME, Find how to tell if the rotorcraft is on the ground or it is flying. + horizontal_speed = 10.0; +#endif + +#if defined(OSD_USE_FLOAT_LOW_PASS_FILTERING) + + static double current_amps_sum = 0; + static double horizontal_speed_sum = 0; + static float current_amps_filtered = 0; + static float horizontal_speed_filtered = 0; + + current_amps_sum = (current_amps_sum - current_amps_filtered) + current_amps; + current_amps_filtered = current_amps_sum / (1 << AMPS_LOW_PASS_FILTER_STRENGTH); + current_amps = current_amps_filtered; + + horizontal_speed_sum = (horizontal_speed_sum - horizontal_speed_filtered) + horizontal_speed; + horizontal_speed_filtered = horizontal_speed_sum / (1 << SPEED_LOW_PASS_FILTER_STRENGTH); + horizontal_speed = horizontal_speed_filtered; + +#else + + uint32_t current_amps_int = 0; + uint32_t horizontal_speed_int = 0; + static uint64_t current_amps_sum = 0; + static uint64_t horizontal_speed_sum = 0; + static uint32_t current_amps_filtered = 0; + static uint32_t horizontal_speed_filtered = 0; + + // LOW PASS FILTERS for making the OSD 'max_flyable_distance_left' var change more gently. + current_amps_int = (uint32_t)(current_amps * 1000.0); + horizontal_speed_int = (uint32_t)(horizontal_speed * 1000.0); + + current_amps_sum = (current_amps_sum - current_amps_filtered) + current_amps_int; + current_amps_filtered = (current_amps_sum + (1 << (AMPS_LOW_PASS_FILTER_STRENGTH - 1))) >> + (AMPS_LOW_PASS_FILTER_STRENGTH); + + horizontal_speed_sum = (horizontal_speed_sum - horizontal_speed_filtered) + horizontal_speed_int; + horizontal_speed_filtered = (horizontal_speed_sum + (1 << (SPEED_LOW_PASS_FILTER_STRENGTH - 1))) >> + (SPEED_LOW_PASS_FILTER_STRENGTH); + + current_amps = (float)(current_amps_filtered) / 1000.; + horizontal_speed = (float)(horizontal_speed_filtered) / 1000.; + +#endif + + max_flyable_distance_left = ((bat_capacity_left / (current_amps * 1000)) * 3600) * horizontal_speed; + + return; +} + + static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column) { @@ -328,7 +542,6 @@ static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t static bool _osd_sprintf(char *buffer, char *string, float value) { - uint8_t param_start = 0; uint8_t param_end = 0; uint8_t frac_nb = 0; @@ -347,28 +560,27 @@ static bool _osd_sprintf(char *buffer, char *string, float value) //copy the string passed as parameter to a buffer for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = *(string + x); if (string_buf[x] == '\0') { break; } } - x = 0; - param_start = 0; - param_end = 0; -//do { - //Now check for any special character - while (string_buf[x] != '\0') { - // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0' - if (string_buf[x] == '%') { if (string_buf[x + 4] == 'c') { (param_start = x + 1); param_end = x + 3; break; } } - x++; - } - if (param_end - param_start) { - //load the special character value where the % character was - string_buf[x] = ((string_buf[param_start] - 48) * 100) + ((string_buf[param_start + 1] - 48) * 10) + - (string_buf[param_start + 2] - 48); - x++; // increment x to the next character which should be the first special character's digit - //Move the rest of the buffer forward so only the special character remains, - // for example in %170c '%' now has the special character's code and x now points to '1' - // which will be overwritten with the rest of the string after the 'c' - for (y = (x + 4); y <= sizeof(string_buf); y++) { string_buf[x++] = string_buf[y]; } - } - -//}while((param_end-param_start > 0)); + do { + x = 0; + param_start = 0; + param_end = 0; + //Now check for any special character + while (string_buf[x] != '\0') { + // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0' + if (string_buf[x] == '%') { if (string_buf[x + 4] == 'c') { (param_start = x + 1); param_end = x + 3; break; } } + x++; + } + if (param_end - param_start) { + //load the special character value where the % character was + string_buf[x] = ((string_buf[param_start] - 48) * 100) + ((string_buf[param_start + 1] - 48) * 10) + + (string_buf[param_start + 2] - 48); + x++; // increment x to the next character which should be the first special character's digit + //Move the rest of the buffer forward so only the special character remains, + // for example in %170c '%' now has the special character's code and x now points to '1' + // which will be overwritten with the rest of the string after the 'c' + for (y = (x + 4); y <= sizeof(string_buf); y++) { string_buf[x++] = string_buf[y]; } + } + } while ((param_end - param_start > 0)); // RESET THE USED VARIABLES JUST TO BE SAFE. x = 0; @@ -436,11 +648,287 @@ static bool _osd_sprintf(char *buffer, char *string, float value) } while (string_buf[param_end] != '\0'); //Write the rest of the string including the terminating char. - } // End of if (param_end - param_start) + // End of if (param_end - param_start) + } else { + for (x = 0; x < sizeof(string_buf); x++) { + *(buffer + x) = string_buf[x]; //Write the rest of the string including the terminating char. + if (*(buffer + x) == '\0') { break; } + } + } return (0); } +void draw_osd(void) +{ + float temp = 0; + float altitude = 0; + float distance_to_home = 0; + static float home_direction_degrees = 0; +#if defined(BARO_ALTITUDE_VAR) + static float baro_alt_correction = 0; +#endif + + struct FloatEulers *att = stateGetNedToBodyEulers_f(); + struct EnuCoor_f *pos = stateGetPositionEnu_f(); +#if AP + float ph_x = waypoints[WP_HOME].x - pos->x; + float ph_y = waypoints[WP_HOME].y - pos->y; + float stall_speed = STALL_AIRSPEED; +#else // FOR ROTORCRAFTS + float ph_x = waypoint_get_x(WP_HOME) - pos->x; + float ph_y = waypoint_get_y(WP_HOME) - pos->y; +#endif // #if AP + + distance_to_home = (float)(sqrt(ph_x * ph_x + ph_y * ph_y)); + calc_flight_time_left(); +#if AP + mag_compass(); +#endif + + //THE SWITCH STATEMENT ENSURES THAT ONLY ONE SPI TRANSACTION WILL OCUUR IN EVERY PERIODIC FUNCTION CALL + switch (step) { + + case (0): + if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 10.0) { //Only when flying + gps_course_deg = (float)gps.course; + gps_course_deg = DegOfRad(gps_course_deg / 1e7); + } else { +#if AP + gps_course_deg = mag_course_deg; +#else + gps_course_deg = stateGetNedToBodyEulers_f()->psi; +#endif + } + osd_sprintf(osd_string, "%.0f", gps_course_deg); + osd_put_s(osd_string, C_JUST, 3, 1, 15); + step = 10; + break; + + case (10): + //Only when flying because i need this indication to remain stable if the GPS is lost. + // This way i can still have the synchronized magnetic compass heading and the last bearing home. + if (gps.fix == GPS_FIX_3D && gps.pdop < 1000 && stateGetHorizontalSpeedNorm_f() > 5.0) { //Only when flying + home_direction_degrees = gps_course_deg + home_direction(); //home_direction returns degrees -180 to +180 + if (home_direction_degrees < 0) { home_direction_degrees += 360; } // translate the -180, +180 to 0-360. + if (home_direction_degrees >= 360) { home_direction_degrees -= 360; } + } +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + // All special character codes must be in 3 digit format! + osd_sprintf(osd_string, "%191c%.0f", home_direction_degrees); // 0 when heading straight back home. + osd_put_s(osd_string, C_JUST, 5, 2, 15); // "false = L_JUST +#else + osd_sprintf(osd_string, "H%.0f", home_direction_degrees); + osd_put_s(osd_string, C_JUST, 5, 2, 15); // "false = L_JUST + +#endif + step = 20; + break; + + case (20): + temp = ((float)electrical.vsupply); + osd_sprintf(osd_string, "%.1fV", temp); + if (temp > LOW_BAT_LEVEL) { + osd_put_s(osd_string, L_JUST, 5, 1, 1); + + } else { osd_put_s(osd_string, (L_JUST | BLINK | INVERT), 5, 1, 1); } + step = 30; + break; + + case (30): + if (gps.fix == GPS_FIX_3D) { +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + //Since we only send one special character the float variable is replaced by a zero. + osd_sprintf(osd_string, "%030c%031c", 0); + osd_put_s(osd_string, false, 2, 2, 1); +#else + osd_put_s("**", false, 2, 2, 1); +#endif + } else { +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + //Since we only send one special character the float variable is replaced by a zero. + osd_sprintf(osd_string, "%030c%031c", 0); // ALL special osd chars must have 3 digits. + osd_put_s(osd_string, (L_JUST | BLINK), 2, 2, 1); +#else + osd_put_s("**", (L_JUST | BLINK), 2, 2, 1); +#endif + } + step = 40; + break; + + case (40): +#if AP + if (autopilot.mode == AP_MODE_AUTO2) { + osd_put_s("A2", L_JUST, 2, 2, 3); + } else if (autopilot.mode == AP_MODE_AUTO1) { + osd_put_s("A1", L_JUST, 2, 2, 3); + } else { + osd_put_s("MAN", L_JUST, 3, 2, 3); + } +#endif + step = 50; + break; + + case (50): +#if AP + osd_sprintf(osd_string, "THR%.0f", (((float)ap_state->commands[COMMAND_THROTTLE] / (float)MAX_PPRZ) * 100.)); +#else + osd_sprintf(osd_string, "THR%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / (float)MAX_PPRZ) * 100.)); +#endif + osd_put_s(osd_string, L_JUST, 6, 3, 1); + step = 60; + break; + + case (60): +#if AP +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + if ((fabs(stateGetHorizontalSpeedNorm_f() * cos(att->phi))) < stall_speed) { + osd_sprintf(osd_string, "STALL!", 0); + osd_put_s(osd_string, (R_JUST | BLINK), 6, 1, 30); + } else { + osd_sprintf(osd_string, "%.0f%161c", (stateGetHorizontalSpeedNorm_f() * 3.6)); + osd_put_s(osd_string, R_JUST, 6, 1, 30); + } +#else + if ((fabs(stateGetHorizontalSpeedNorm_f() * cos(att->phi))) < stall_speed) { + osd_sprintf(osd_string, "STALL!", 0); + osd_put_s(osd_string, (R_JUST | BLINK), 6, 1, 30); + } else { + osd_sprintf(osd_string, "%.0fKM", (stateGetHorizontalSpeedNorm_f() * 3.6)); + osd_put_s(osd_string, R_JUST, 6, 1, 30); + } +#endif + +#else // #if AP + +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + osd_sprintf(osd_string, "%.0f%161c", (stateGetHorizontalSpeedNorm_f() * 3.6)); +#else + osd_sprintf(osd_string, "%.0fKM", (stateGetHorizontalSpeedNorm_f() * 3.6)); +#endif + osd_put_s(osd_string, R_JUST, 6, 1, 30); + +#endif + step = 70; + break; + + case (70): +#if defined(BARO_ALTITUDE_VAR) + if (gps.fix == GPS_FIX_3D && gps.pdop < 1000) { + RunOnceEvery((MAX7456_PERIODIC_FREQ * 300), { baro_alt_correction = GetPosAlt() - BARO_ALTITUDE_VAR;}); + altitude = GetPosAlt(); + } else { + altitude = BARO_ALTITUDE_VAR + baro_alt_correction; + } +#else + altitude = GetPosAlt(); +#endif +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + osd_sprintf(osd_string, "%.0f%177c", altitude); +#else + osd_sprintf(osd_string, "%.0fM", altitude); +#endif + osd_put_s(osd_string, R_JUST, 6, 2, 30); // "false = L_JUST + step = 80; + break; + + case (80): +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + osd_sprintf(osd_string, "%.1f%159c", stateGetSpeedEnu_f()->z); +#else + osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z); +#endif + if (stateGetSpeedEnu_f()->z > 3.0) { + osd_put_s(osd_string, (R_JUST | BLINK), 6, 3, 30); + } else { + osd_put_s(osd_string, R_JUST, 6, 3, 30); + } + step = 90; + break; + + case (90): +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!! + // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER + osd_sprintf(osd_string, "%160c%.1fK%012c", (distance_to_home / 1000)); + osd_put_s(osd_string, L_JUST, 7, 15, 1); +#else + osd_sprintf(osd_string, "%.1fKM", (distance_to_home / 1000)); + osd_put_s(osd_string, L_JUST, 7, 15, 1); +#endif + step = 100; + break; + + case (100): +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + osd_sprintf(osd_string, "%147c%.1fK%012c", (max_flyable_distance_left / 1000)); +#else + osd_sprintf(osd_string, "%.1fKM", (max_flyable_distance_left / 1000)); +#endif + if ((max_flyable_distance_left + 1000.0) < distance_to_home) { + osd_put_s(osd_string, (R_JUST | BLINK), 7, 15, 30); + } else { + osd_put_s(osd_string, (R_JUST), 7, 15, 30); + } + step = 110; + break; + + // A Text PFD as graphics are not the strong point of the MAX7456 + // In order to level the aircraft while fpving + // just move the stick to the opposite direction from the angles shown on the osd + // and that's why positive pitch (UP) is shown below the OSD center + case (110): + if (DegOfRad(att->theta) > 3) { + osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta)); + osd_put_s(osd_string, C_JUST, 5, 6, 15); + + } else { osd_put_s(" ", C_JUST, 5, 6, 15); } + step = 112; + break; + + case (112): + if (DegOfRad(att->theta) < -3) { + osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta)); + osd_put_s(osd_string, C_JUST, 5, 10, 15); + + } else { osd_put_s(" ", C_JUST, 5, 10, 15); } + step = 114; + break; + + case (114): + if (DegOfRad(att->phi) > 3) { + osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi)); + osd_put_s(osd_string, false, 5, 8, 18); + + } else { osd_put_s(" ", false, 5, 8, 18); } + step = 116; + break; + + case (116): + if (DegOfRad(att->phi) < -3) { + osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi))); + osd_put_s(osd_string, R_JUST, 5, 8, 13); + + } else { osd_put_s(" ", R_JUST, 5, 8, 13); } + step = 120; + break; + + case (120): +#if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1 + osd_sprintf(osd_string, "%126c", 0); + osd_put_s(osd_string, false, 1, 8, 15); // false = L_JUST +#else + osd_put_s("+", false, 1, 8, 15); // false = L_JUST +#endif + step = 0; + break; + + default: step = 0; break; + } // End of switch statement. + + return; +} + void max7456_init(void) { @@ -462,24 +950,19 @@ void max7456_init(void) osd_enable_val = OSD_IMAGE_ENABLE; max7456_osd_status = OSD_UNINIT; +#if DOWNLINK + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag_heading); +#endif + home_direction(); + return; } void max7456_periodic(void) { - float temp = 0; - struct FloatEulers *att = stateGetNedToBodyEulers_f(); - struct EnuCoor_f *pos = stateGetPositionEnu_f(); -#if AP - float ph_x = waypoints[WP_HOME].x - pos->x; - float ph_y = waypoints[WP_HOME].y - pos->y; -#else - float ph_x = waypoint_get_x(WP_HOME) - pos->x; - float ph_y = waypoint_get_y(WP_HOME) - pos->y; -#endif -//This code is executed always and checks if the "osd_enable" var has been changed by telemetry. -//If yes then it commands a reset but this time turns on or off the osd overlay, not the video. + //This code is executed always and checks if the "osd_enable" var has been changed by telemetry. + //If yes then it commands a reset but this time turns on or off the osd overlay, not the video. if (max7456_osd_status == OSD_IDLE) { if (osd_enable > 1) { osd_enable = 1; @@ -498,6 +981,7 @@ void max7456_periodic(void) //This operation needs at least 100us but when the periodic function will be invoked again //sufficient time will have elapsed even with at a periodic frequency of 1000 Hz max7456_trans.output_buf[1] = OSD_RESET; + //We give an extra delay step by going to the event function and back here for the Reset to complete. max7456_osd_status = OSD_INIT1; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); } else if (max7456_osd_status == OSD_INIT2) { @@ -507,144 +991,11 @@ void max7456_periodic(void) max7456_osd_status = OSD_INIT3; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { - switch (step) { - case (0): - osd_put_s("HDG", FALSE, 3, 1, 14); - step = 1; - break; - case (1): -#if !defined USE_MATEK_TYPE_OSD_CHIP || USE_MATEK_TYPE_OSD_CHIP == 0 - osd_put_s("DISTANCE", FALSE, 8, 14, 12); -#endif - step = 10; - break; - case (10): - osd_put_s("( )", FALSE, 3, 8, 14); - step = 20; - break; - case (20): - temp = ((float)electrical.vsupply); - osd_sprintf(osd_string, "%.1fV", temp); - if (temp > LOW_BAT_LEVEL) { - osd_put_s(osd_string, L_JUST, 5, 1, 1); - - } else { osd_put_s(osd_string, L_JUST | BLINK | INVERT, 5, 1, 1); } - step = 30; - break; - case (30): -#if OSD_USE_MAG_COMPASS && !defined(SITL) - PRINT_CONFIG_MSG("OSD USES THE MAGNETIC HEADING") - temp = DegOfRad(MAG_Heading); - if (temp < 0) { temp += 360; } -#else - PRINT_CONFIG_MSG("OSD USES THE GPS HEADING") - temp = DegOfRad(state.h_speed_dir_f); - if (temp < 0) { temp += 360; } -#endif - osd_sprintf(osd_string, "%.0f", temp); - osd_put_s(osd_string, C_JUST, 3, 1, 15); - step = 40; - break; - case (40): - osd_sprintf(osd_string, "%.0f KM", (state.h_speed_norm_f * 3.6)); - osd_put_s(osd_string, R_JUST, 6, 1, 29); - step = 42; - break; - case (42): -#if AP - osd_sprintf(osd_string, "%.0fTHR", (((float)ap_state->commands[COMMAND_THROTTLE] / MAX_PPRZ) * 100)); -#else - osd_sprintf(osd_string, "%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ) * 100)); -#endif - osd_put_s(osd_string, R_JUST, 5, 2, 29); - step = 50; - break; - case (50): -#if OSD_USE_BARO_ALTITUDE && !defined(SITL) - PRINT_CONFIG_MSG("OSD ALTITUDE IS COMING FROM BAROMETER") -#if defined BARO_ALTITUDE_VAR - osd_sprintf(osd_string, "%.0fM", BARO_ALTITUDE_VAR); -#else - PRINT_CONFIG_MSG("OSD USES THE DEFAULT BARO ALTITUDE VARIABLE") - osd_sprintf(osd_string, "%.0fM", baro_alt); -#endif -#else - PRINT_CONFIG_MSG("ALTITUDE IS COMING FROM GPS") - osd_sprintf(osd_string, "%.0fM", GetPosAlt()); -#endif - osd_put_s(osd_string, L_JUST, 6, 14, 1); // "FALSE = L_JUST - step = 52; - break; - case (52): -#if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1 - // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!! - // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER - osd_sprintf(osd_string, "%191c%.0f", home_direction()); - osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST -#else - osd_sprintf(osd_string, "H%.0f", home_direction()); - osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST - -#endif - step = 60; - break; - - case (60): -#if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1 - // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!! - // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER - osd_sprintf(osd_string, "%160c%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y))); - osd_put_s(osd_string, C_JUST, 6, 14, 15); -#else - osd_sprintf(osd_string, "%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y))); - osd_put_s(osd_string, C_JUST, 6, 14, 15); -#endif - step = 70; - break; - case (70): - osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z); - osd_put_s(osd_string, R_JUST, 9, 14, 29); - step = 80; - break; - // A Text PFD as graphics are not the strong point of the MAX7456 - // In order to level the aircraft while fpving - // just move the stick to the opposite direction from the angles shown on the osd - // and that's why positive pitch (UP) is shown below the OSD center - case (80): - if (DegOfRad(att->theta) > 2) { - osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta)); - osd_put_s(osd_string, C_JUST, 5, 6, 15); - - } else { osd_put_s(" ", C_JUST, 5, 6, 15); } - step = 90; - break; - case (90): - if (DegOfRad(att->theta) < -2) { - osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta)); - osd_put_s(osd_string, C_JUST, 5, 10, 15); - - } else { osd_put_s(" ", C_JUST, 5, 10, 15); } - step = 100; - break; - case (100): - if (DegOfRad(att->phi) > 2) { - osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi)); - osd_put_s(osd_string, FALSE, 5, 8, 18); - - } else { osd_put_s(" ", FALSE, 5, 8, 18); } - step = 110; - break; - case (110): - if (DegOfRad(att->phi) < -2) { - osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi))); - osd_put_s(osd_string, R_JUST, 5, 8, 13); - - } else { osd_put_s(" ", R_JUST, 5, 8, 13); } - step = 10; - break; - default: step = 10; break; - } // End of switch statement. + draw_osd(); } // End of if (max7456_osd_status == OSD_UNINIT) + + + return; } @@ -664,13 +1015,16 @@ void max7456_event(void) max7456_trans.output_length = 2; max7456_trans.input_length = 0; max7456_trans.output_buf[0] = OSD_OSDBL_REG; - max7456_trans.output_buf[1] = max7456_trans.input_buf[0] & (~(1 << 4)); + //Max7456 requires that you read first and then change only bit4 of the OSDBL register. + //Reading was started in the periodic function and now we rerwrite the OSDBL register. + max7456_trans.output_buf[1] = max7456_trans.input_buf[1] & (~(1 << 4)); max7456_osd_status = OSD_INIT4; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); break; case (OSD_INIT4): max7456_trans.output_buf[0] = OSD_VM0_REG; #if USE_PAL_FOR_OSD_VIDEO +#pragma message "Camera and OSD must be both PAL or NTSC otherwise only the camera picture will be visible." max7456_trans.output_buf[1] = OSD_VIDEO_MODE_PAL | osd_enable_val; #else max7456_trans.output_buf[1] = osd_enable_val; diff --git a/sw/airborne/modules/display/max7456.h b/sw/airborne/modules/display/max7456.h index b2e47a0af6..b23e3e3438 100644 --- a/sw/airborne/modules/display/max7456.h +++ b/sw/airborne/modules/display/max7456.h @@ -36,6 +36,9 @@ extern void max7456_periodic(void); extern void max7456_event(void); extern uint8_t osd_enable; +extern float mag_course_deg; +extern float mag_heading_rad; +extern float home_dir_deg; #endif //MAX7456_H diff --git a/sw/airborne/modules/pca9685/pca9685_i2c.c b/sw/airborne/modules/pca9685/pca9685_i2c.c new file mode 100644 index 0000000000..ebb5e96178 --- /dev/null +++ b/sw/airborne/modules/pca9685/pca9685_i2c.c @@ -0,0 +1,471 @@ +/* + * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com + * + * 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 modules/pca9685/pca9685.c + * PCA9685 LED DRIVER USED AS A 16 ADDITIONAL PWM SERVO DRIVER. + * + */ + + +//################################################################################################### +// I N C L U D E D H E A D E R F I L E S +//################################################################################################### +#include "modules/pca9685/pca9685_i2c.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/sys_time.h" + +#include "math/pprz_isa.h" +#include "mcu_periph/sys_time.h" +#include "subsystems/abi.h" +#include "mcu_periph/uart.h" + +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" +#endif + + + + + +//################################################################################################### +// P R E P R O C E S S O R D I R E C T I V E S A N D D E F I N I T I O N S +//################################################################################################### +// BIG ENDIAN DEFINITIONS +//#define BUF2INT(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) +//#define INT2BUF(_int,_buf,_idx) { _buf[_idx] = (_int>>8); _buf[_idx+1] = _int; } + +// LITTLE ENDIAN DEFINITIONS USED FOR AvR CPU DUE TO MORE EFFICIENT DATA TRANSFERS +#define BUF2INT(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) +#define INT2BUF(_int,_buf,_idx) { _buf[_idx] = _int; _buf[_idx+1] = (_int>>8); } + +#ifndef PCA9685_I2C_DEV +#define PCA9685_I2C_DEV i2c2 +#endif +PRINT_CONFIG_VAR(PCA9685_I2C_DEV) + +#ifndef PCA9865_SRV_RESOLUTION +#define PCA9865_SRV_RESOLUTION 1. // in microseconds +#endif + +#ifndef PCA9865_SRV_DEFAULT_VAL_US +#define PCA9865_SRV_DEFAULT_VAL_US 1000. // in microseconds +#endif + +#ifndef PCA9865_SRV_NUMBER +#define PCA9865_SRV_NUMBER 16 // in microseconds +#endif + +// I2C Addreses +#ifndef PCA9685_I2C_SLAVE_ADDR +#define PCA9685_I2C_SLAVE_ADDR 0xE0 +#endif +PRINT_CONFIG_VAR(PCA9685_I2C_SLAVE_ADDR) + +#ifndef PCA9685_I2C_ALLCALL_ADDR +#define PCA9685_I2C_ALLCALL_ADDR 0xE0 +#endif +#ifndef PCA9685_I2C_RESET_ADDR +#define PCA9685_I2C_RESET_ADDR 0x06 +#endif +#ifndef PCA9685_I2C_GEN_CALL_ADDR +#define PCA9685_I2C_GEN_CALL_ADDR 0x00 +#endif + +// Register addresses +#define PCA9685_MODE1_REG_ADDR 0x00 +#define PCA9685_MODE2_REG_ADDR 0x01 +#define PCA9685_ALLCALL_ADDR 0x05 + +#define PCA9685_LED0_ON_L_REG_ADDR 0X06 +#define PCA9685_LED0_ON_H_REG_ADDR 0X07 +#define PCA9685_LED0_OFF_L_REG_ADDR 0X08 +#define PCA9685_LED0_OFF_H_REG_ADDR 0X09 + +#define PCA9685_LED1_ON_L_REG_ADDR 0X0A +#define PCA9685_LED1_ON_H_REG_ADDR 0X0B +#define PCA9685_LED1_OFF_L_REG_ADDR 0X0C +#define PCA9685_LED1_OFF_H_REG_ADDR 0X0D + +#define PCA9685_LED2_ON_L_REG_ADDR 0X0E +#define PCA9685_LED2_ON_H_REG_ADDR 0X0F +#define PCA9685_LED2_OFF_L_REG_ADDR 0X10 +#define PCA9685_LED2_OFF_H_REG_ADDR 0X11 + +#define PCA9685_LED3_ON_L_REG_ADDR 0X12 +#define PCA9685_LED3_ON_H_REG_ADDR 0X13 +#define PCA9685_LED3_OFF_L_REG_ADDR 0X14 +#define PCA9685_LED3_OFF_H_REG_ADDR 0X15 + +#define PCA9685_LED4_ON_L_REG_ADDR 0X16 +#define PCA9685_LED4_ON_H_REG_ADDR 0X17 +#define PCA9685_LED4_OFF_L_REG_ADDR 0X18 +#define PCA9685_LED4_OFF_H_REG_ADDR 0X19 + +#define PCA9685_LED5_ON_L_REG_ADDR 0X1A +#define PCA9685_LED5_ON_H_REG_ADDR 0X1B +#define PCA9685_LED5_OFF_L_REG_ADDR 0X1C +#define PCA9685_LED5_OFF_H_REG_ADDR 0X1D + +#define PCA9685_LED6_ON_L_REG_ADDR 0X1E +#define PCA9685_LED6_ON_H_REG_ADDR 0X1F +#define PCA9685_LED6_OFF_L_REG_ADDR 0X20 +#define PCA9685_LED6_OFF_H_REG_ADDR 0X21 + +#define PCA9685_LED7_ON_L_REG_ADDR 0X22 +#define PCA9685_LED7_ON_H_REG_ADDR 0X23 +#define PCA9685_LED7_OFF_L_REG_ADDR 0X24 +#define PCA9685_LED7_OFF_H_REG_ADDR 0X25 + + +#define PCA9685_LED8_ON_L_REG_ADDR 0X26 +#define PCA9685_LED8_ON_H_REG_ADDR 0X27 +#define PCA9685_LED8_OFF_L_REG_ADDR 0X28 +#define PCA9685_LED8_OFF_H_REG_ADDR 0X29 + +#define PCA9685_LED9_ON_L_REG_ADDR 0X2A +#define PCA9685_LED9_ON_H_REG_ADDR 0X2B +#define PCA9685_LED9_OFF_L_REG_ADDR 0X2C +#define PCA9685_LED9_OFF_H_REG_ADDR 0X2D + +#define PCA9685_LED10_ON_L_REG_ADDR 0X2E +#define PCA9685_LED10_ON_H_REG_ADDR 0X2F +#define PCA9685_LED10_OFF_L_REG_ADDR 0X30 +#define PCA9685_LED10_OFF_H_REG_ADDR 0X31 + +#define PCA9685_LED11_ON_L_REG_ADDR 0X32 +#define PCA9685_LED11_ON_H_REG_ADDR 0X33 +#define PCA9685_LED11_OFF_L_REG_ADDR 0X34 +#define PCA9685_LED11_OFF_H_REG_ADDR 0X35 + +#define PCA9685_LED12_ON_L_REG_ADDR 0X36 +#define PCA9685_LED12_ON_H_REG_ADDR 0X37 +#define PCA9685_LED12_OFF_L_REG_ADDR 0X38 +#define PCA9685_LED12_OFF_H_REG_ADDR 0X39 + +#define PCA9685_LED13_ON_L_REG_ADDR 0X3A +#define PCA9685_LED13_ON_H_REG_ADDR 0X3B +#define PCA9685_LED13_OFF_L_REG_ADDR 0X3C +#define PCA9685_LED13_OFF_H_REG_ADDR 0X3D + +#define PCA9685_LED14_ON_L_REG_ADDR 0X3E +#define PCA9685_LED14_ON_H_REG_ADDR 0X3F +#define PCA9685_LED14_OFF_L_REG_ADDR 0X40 +#define PCA9685_LED14_OFF_H_REG_ADDR 0X41 + +#define PCA9685_LED15_ON_L_REG_ADDR 0X42 +#define PCA9685_LED15_ON_H_REG_ADDR 0X43 +#define PCA9685_LED15_OFF_L_REG_ADDR 0X44 +#define PCA9685_LED15_OFF_H_REG_ADDR 0X45 + +#define PCA9685_ALL_LED_ON_L_REG_ADDR 0XFA +#define PCA9685_ALL_LED_ON_H_REG_ADDR 0XFB +#define PCA9685_ALL_LED_OFF_L_REG_ADDR 0XFC +#define PCA9685_ALL_LED_OFF_H_REG_ADDR 0XFD + +#define PCA9685_PRESCALER_REG_ADDR 0XFE + +//Bit positions +//MODE 0 REGISTER +#define PCA9865_RESTART_BIT 7 // 1= RESTART ENABLE, DEFAULT = 0 +#define PCA9865_AUTO_INCREMENT_BIT 5 // 1 = AUTO INCREMENT, DEFAULT = 0 +#define PCA9865_SLEEP_BIT 4 // 1 = LOW POWER MODE (OSC OFF), 0=NORMAL MODE, DEFAULT=0 +#define PCA9865_ALLCALL_BIT 0 // 1 = ALL LED ENABLE, DEFAULT = 1 +//MODE 1 REGISTER +#define PCA9865_OUTDRV_BIT 2 // 1 = TOTEM POLE, 0 = OPEN DRAIN, DEFAULT = 1 + + + + +#if !defined(SITL) +//################################################################################################### +// P R I V A T E F U N C T I O N P R O T O T Y P E S +//################################################################################################### +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 +static void pca9685_send_servo_values(struct transport_tx *trans, struct link_device *dev); +#endif + + + + +//################################################################################################### +// G L O B A L V A R I A B L E S +//################################################################################################### + +enum { + PCA9685_I2C_STATUS_UNINIT, + PCA9685_I2C_STATUS_INITIALIZED, + PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY, + PCA9865_I2C_STATUS_WRITE_LED_REG_READY, + PCA9865_I2C_STATUS_READ_LED_REG_BUSY, + PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED +}; + +struct i2c_transaction pca9685_i2c_trans; +uint8_t pca9685_i2c_status = 0; +uint8_t srv_cnt = 0; +uint16_t pca9865_write_servo_vals[PCA9865_SRV_NUMBER]; +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 +PRINT_CONFIG_MSG("PCA9685 servo values can be found in the ""BARO_WORDS"" message") +uint16_t pca9865_read_servo_vals[PCA9865_SRV_NUMBER]; +#endif +uint8_t pca9865_reg_nb[16]; + + +//################################################################################################### +// P R I V A T E F U N C T I O N D E F I N I T I O N S +//################################################################################################### + +//111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 +static void pca9685_send_servo_values(struct transport_tx *trans, struct link_device *dev) +{ + +// For debugging purposes we send anly the first 4 servo values using the BARO_WORDS message. +// Those values are read back from PCA9865 so they indicate if the module is working as intended. + pprz_msg_send_CSC_SERVO_CMD( + trans, dev, AC_ID, + &pca9865_read_servo_vals[0], &pca9865_read_servo_vals[1], + &pca9865_read_servo_vals[2], &pca9865_read_servo_vals[3] + ); + + + return; +} +#endif + + +//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +// P U B L I C F U N C T I O N D E F I N I T I O N S +//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ + +//111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val) +{ + uint16_t servo_value = 0; + + if (srv_nb < 16) { + servo_value = (uint16_t)((float)srv_val / (float)PCA9865_SRV_RESOLUTION); + pca9865_write_servo_vals[srv_nb] = servo_value; + pca9865_reg_nb[srv_nb] = (srv_nb * 4) + 8; + + } + + return (FALSE); +} + + + +//222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222 +void pca9685_i2c_init(void) +{ + + uint16_t reg_value = 0; + uint32_t timer0 = 0; + +// clear all arrays to zero. + for (srv_cnt = 0; srv_cnt < PCA9865_SRV_NUMBER; srv_cnt++) { + pca9865_write_servo_vals[srv_cnt] = 0; +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 + pca9865_read_servo_vals[srv_cnt] = 0; +#endif + pca9865_reg_nb[srv_cnt] = 0; + } + srv_cnt = 0; + pca9685_i2c_trans.status = I2CTransDone; + pca9685_i2c_status = PCA9685_I2C_STATUS_UNINIT; + + pca9685_i2c_trans.buf[0] = PCA9685_I2C_RESET_ADDR; + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_GEN_CALL_ADDR, 1); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + +// Internal Oscillator Off, the oscillator is off by default on power up. + pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR; + pca9685_i2c_trans.buf[1] = (1 << PCA9865_SLEEP_BIT) | (1 << PCA9865_ALLCALL_BIT); // OSC off + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + + reg_value = (uint16_t)((float)PCA9865_SRV_RESOLUTION / 0.04); //0.04 = 1/25 Mhz + if (reg_value > 0xFF) { reg_value = 0xFF; } // Sanity check. + pca9685_i2c_trans.buf[0] = PCA9685_PRESCALER_REG_ADDR; + pca9685_i2c_trans.buf[1] = (uint8_t)reg_value; + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + +// Internal Oscillator On, Auto Increment on, AllCall enabled, 500 microseconds needed. + pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR; + pca9685_i2c_trans.buf[1] = (1 << PCA9865_AUTO_INCREMENT_BIT) | (1 << PCA9865_ALLCALL_BIT); + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + + do { + pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR; + i2c_transceive(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 1, 1); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + + } while ((pca9685_i2c_trans.buf[0] & (1 << PCA9865_RESTART_BIT)) != 0); + + pca9685_i2c_trans.buf[0] = PCA9685_ALL_LED_ON_L_REG_ADDR; + pca9685_i2c_trans.buf[1] = 0; + pca9685_i2c_trans.buf[2] = 0; + reg_value = (uint16_t)((float)PCA9865_SRV_DEFAULT_VAL_US / (float)PCA9865_SRV_RESOLUTION); + pca9685_i2c_trans.buf[3] = (uint8_t)reg_value; + pca9685_i2c_trans.buf[4] = (uint8_t)((reg_value >> 8) & 0x000F); + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_ALLCALL_ADDR, 5); + timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2); + while (pca9685_i2c_trans.status == I2CTransPending) { + if (sys_time.nb_tick > timer0) { break; } + } + + + pca9685_i2c_trans.status = I2CTransDone; + pca9685_i2c_status = PCA9685_I2C_STATUS_INITIALIZED; + +#if DOWNLINK +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CSC_SERVO_CMD, pca9685_send_servo_values); +#endif +#endif + +// Initialize all 16 servo with the default values + pca9865_set_servo(0, 1500); + pca9865_set_servo(1, 1500); + pca9865_set_servo(2, 1000); + pca9865_set_servo(3, 1500); + for (srv_cnt = 4; srv_cnt < PCA9865_SRV_NUMBER; srv_cnt++) { + pca9865_set_servo(srv_cnt, 1000); + } + srv_cnt = 0; + + return; +} + + + +//333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333 +void pca9685_i2c_periodic(void) +{ +// If a servo change was requested write the appropriate pca9685 registers. +// Since those servos are not intented for flight surface control they can be updated slower +// at about one servo each module cycle. This way i2c load is minimized. +// +// Start searching for a servo change request either after intialization or after all +// servo channels have been searched (looping). +// This loop will be executed always until a servo value change is requested. +// Then it will break with "srv_cnt" having the servo number to be changed. + +// DELAY IN ORDER TO GIVE TIME TO THE INTERNAL PCA9865 OSCILLATOR TO STABILIZE. +//if (sys_time.nb_sec > 3){ + if (srv_cnt >= 16 || pca9685_i2c_status == PCA9685_I2C_STATUS_INITIALIZED) { + for (srv_cnt = 0; srv_cnt < 16; srv_cnt++) { + if (pca9865_reg_nb[srv_cnt] > 0) { + pca9685_i2c_status = PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED; + break; + } + } + } + + // Change the above found register in order to update the servo pwm value. + // The search for another servo to change is completed in the "event" function. + if (srv_cnt < 16) { + if (pca9685_i2c_status == PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED) { + pca9685_i2c_trans.buf[0] = pca9865_reg_nb[srv_cnt]; + pca9685_i2c_trans.buf[1] = (uint8_t)pca9865_write_servo_vals[srv_cnt]; + pca9685_i2c_trans.buf[2] = (uint8_t)(pca9865_write_servo_vals[srv_cnt] >> 8) & 0x0F; + i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 3); + pca9685_i2c_status = PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY; + } + } +//} + + return; +} + + + +//444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444 +void pca9685_i2c_event(void) +{ + +//-------------------------------------------------------------------------------------------------- +// IF SUCCESFULL +//-------------------------------------------------------------------------------------------------- + if (pca9685_i2c_trans.status == I2CTransSuccess) { + + if (pca9685_i2c_status == PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY) { + // Read back the changed value just to be sure... + pca9685_i2c_trans.buf[0] = pca9865_reg_nb[srv_cnt]; + i2c_transceive(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 1, 2); + pca9685_i2c_status = PCA9865_I2C_STATUS_READ_LED_REG_BUSY; + + } else if (pca9685_i2c_status == PCA9865_I2C_STATUS_READ_LED_REG_BUSY) { +#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1 + pca9865_read_servo_vals[srv_cnt] = BUF2INT(pca9685_i2c_trans.buf, 0); +#endif + if (BUF2INT(pca9685_i2c_trans.buf, 0) != pca9865_write_servo_vals[srv_cnt]) { + //pca9685_i2c_init(); + return; + } + pca9685_i2c_status = PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED; + // Delete this request. + pca9865_reg_nb[srv_cnt] = 0; + // Search for the next changed servo value. + // If a value > 0 is detected x will contain the new servo number to be changed. + srv_cnt++; + for (; srv_cnt < 16; srv_cnt++) { if (pca9865_reg_nb[srv_cnt] > 0) { break; } } + } +//------------------------------------------------------------------------------------------------- +// ELSE IF NOT SUCCESFULL +//------------------------------------------------------------------------------------------------- + }//else{ pca9685_i2c_init(); } + + return; +} + +#else +// just to stop the compiler complaining i manipulte srv_nb and srv_val a little. +bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val) { srv_nb = srv_val; srv_nb /= 2; return (FALSE); } +void pca9685_i2c_init(void) { return; } +void pca9685_i2c_periodic(void) { return; } +void pca9685_i2c_event(void) { return; } + +#endif // #if !defined(SITL) + +/************************************************************************************************/ +// T H E E N D +/************************************************************************************************/ + diff --git a/sw/airborne/modules/pca9685/pca9685_i2c.h b/sw/airborne/modules/pca9685/pca9685_i2c.h new file mode 100644 index 0000000000..7fbc590214 --- /dev/null +++ b/sw/airborne/modules/pca9685/pca9685_i2c.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com + * + * 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 modules/pca9685/pca9685.h + * PCA9685 LED DRIVER USED AS A 16 ADDITIONAL PWM SERVO DRIVER. + * + */ + + +#ifndef PCA9685_I2C_H +#define PCA9685_I2C_H + +#include "std.h" + +#define PCA9865_SRV0 PCA9685_LED0_OFF_L_REG_ADDR +#define PCA9865_SRV1 PCA9685_LED1_OFF_L_REG_ADDR +#define PCA9865_SRV2 PCA9685_LED2_OFF_L_REG_ADDR +#define PCA9865_SRV3 PCA9685_LED3_OFF_L_REG_ADDR +#define PCA9865_SRV4 PCA9685_LED4_OFF_L_REG_ADDR +#define PCA9865_SRV5 PCA9685_LED5_OFF_L_REG_ADDR +#define PCA9865_SRV6 PCA9685_LED6_OFF_L_REG_ADDR +#define PCA9865_SRV7 PCA9685_LED7_OFF_L_REG_ADDR +#define PCA9865_SRV8 PCA9685_LED8_OFF_L_REG_ADDR +#define PCA9865_SRV9 PCA9685_LED9_OFF_L_REG_ADDR +#define PCA9865_SRV10 PCA9685_LED10_OFF_L_REG_ADDR +#define PCA9865_SRV11 PCA9685_LED11_OFF_L_REG_ADDR +#define PCA9865_SRV12 PCA9685_LED12_OFF_L_REG_ADDR +#define PCA9865_SRV13 PCA9685_LED13_OFF_L_REG_ADDR +#define PCA9865_SRV14 PCA9685_LED14_OFF_L_REG_ADDR +#define PCA9865_SRV15 PCA9685_LED15_OFF_L_REG_ADDR + +extern void pca9685_i2c_init(void); +extern void pca9685_i2c_periodic(void); +extern void pca9685_i2c_event(void); + +bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val); + +#endif diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.c b/sw/airborne/modules/sensors/baro_bmp280_i2c.c index e62d522b93..4840ac204a 100644 --- a/sw/airborne/modules/sensors/baro_bmp280_i2c.c +++ b/sw/airborne/modules/sensors/baro_bmp280_i2c.c @@ -19,7 +19,7 @@ */ /** - * @file modules/sensors/baro_bmp280.c + * @file modules/sensors/baro_bmp280_i2c.c * Bosch BMP280 I2C sensor interface. * * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C. @@ -34,19 +34,44 @@ #include "subsystems/datalink/downlink.h" #include "math/pprz_isa.h" +#if DOWNLINK && !defined(BMP280_SYNC_SEND) +#include "subsystems/datalink/telemetry.h" +#endif + /** default slave address */ #ifndef BMP280_SLAVE_ADDR #define BMP280_SLAVE_ADDR BMP280_I2C_ADDR #endif float baro_alt = 0; +float baro_temp = 0; +float baro_press = 0; bool baro_alt_valid = 0; + struct Bmp280_I2c baro_bmp280; +#if DOWNLINK && !defined(BMP280_SYNC_SEND) +static void send_baro_bmp_data(struct transport_tx *trans, struct link_device *dev) +{ + int32_t baro_altitude = (int32_t)(baro_alt * 100); + int32_t baro_pressure = (int32_t)(baro_press); + int32_t baro_temperature = (int32_t)(baro_temp * 100); + + pprz_msg_send_BMP_STATUS(trans, dev, AC_ID, &baro_altitude, &baro_altitude , &baro_pressure, & baro_temperature); + + return; +} +#endif + + void baro_bmp280_init(void) { + bmp280_i2c_init(&baro_bmp280, &BMP280_I2C_DEV, BMP280_SLAVE_ADDR); +#if DOWNLINK && !defined(BMP280_SYNC_SEND) + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BMP_STATUS, send_baro_bmp_data); +#endif } void baro_bmp280_periodic(void) @@ -64,12 +89,14 @@ void baro_bmp280_event(void) AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, now_ts, baro_bmp280.pressure); AbiSendMsgTEMPERATURE(BARO_BMP_SENDER_ID, baro_bmp280.temperature); baro_bmp280.data_available = false; - baro_alt = pprz_isa_altitude_of_pressure((float)baro_bmp280.pressure); - baro_alt_valid = TRUE; + baro_alt = pprz_isa_altitude_of_pressure(baro_bmp280.pressure); + baro_alt_valid = true; + baro_press = (float)baro_bmp280.pressure; + baro_temp = ((float)baro_bmp280.temperature) / 100; -#ifdef BMP280_SYNC_SEND - int32_t up = (int32_t)(baro_alt * 100.0); - int32_t ut = (int32_t) baro_bmp280.raw_temperature; +#if defined(BMP280_SYNC_SEND) + int32_t up = (int32_t)(baro_bmp280.raw_pressure); + int32_t ut = (int32_t)(baro_bmp280.raw_temperature); int32_t p = (int32_t) baro_bmp280.pressure; int32_t t = (int32_t)(baro_bmp280.temperature); DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &up, &ut, &p, &t); diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.h b/sw/airborne/modules/sensors/baro_bmp280_i2c.h index d6487e6647..0dfb5c66c7 100644 --- a/sw/airborne/modules/sensors/baro_bmp280_i2c.h +++ b/sw/airborne/modules/sensors/baro_bmp280_i2c.h @@ -20,14 +20,14 @@ */ /** - * @file modules/sensors/baro_bmp280.h + * @file modules/sensors/baro_bmp280_i2c.h * Bosch BMP280 I2C sensor interface. * * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C. */ -#ifndef BARO_BMP280_H -#define BARO_BMP280_H +#ifndef BARO_BMP280_I2C_H +#define BARO_BMP280_I2C_H #include "peripherals/bmp280_i2c.h" diff --git a/sw/airborne/modules/uav_recovery/uav_recovery.c b/sw/airborne/modules/uav_recovery/uav_recovery.c new file mode 100644 index 0000000000..0215854ac4 --- /dev/null +++ b/sw/airborne/modules/uav_recovery/uav_recovery.c @@ -0,0 +1,289 @@ +/* + * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com + * + * 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 modules/uav_recovery/uav_recovery.c + * + */ + + +#include "autopilot.h" +#include "state.h" +#include "firmwares/fixedwing/nav.h" +#include "subsystems/navigation/common_nav.h" +#include "generated/flight_plan.h" +#include "generated/airframe.h" +#include "inter_mcu.h" +#include "subsystems/datalink/datalink.h" +#include "modules/multi/traffic_info.h" +#include "uav_recovery.h" + +#if defined(USE_PARACHUTE) && USE_PARACHUTE == 1 + +#ifndef GLUE_DEFINITIONS_STEP2 +#define GLUE_DEFINITIONS_STEP2(a, b) (a##b) +#endif +#ifndef GLUE_DEFINITIONS +#define GLUE_DEFINITIONS(a, b) GLUE_DEFINITIONS_STEP2(a, b) +#endif + +#if defined(PARACHUTE_SERVO_CHANNEL) && PARACHUTE_SERVO_CHANNEL != -1 +#define PARACHUTE_OUTPUT_COMMAND GLUE_DEFINITIONS(COMMAND_, PARACHUTE_SERVO_CHANNEL) +#else +#warning YOU HAVE NOT DEFINED A PARACHUTE RELEASE AUTOPILOT SERVO +#endif + +#endif + +/** TERMINAL VELOCITY OF THE PARACHUTE PLUS PAYLOAD */ +#ifndef PARACHUTE_TRIGGER_DELAY +#define PARACHUTE_TRIGGER_DELAY 2. +#endif + +#ifndef PARACHUTE_DESCENT_RATE +#define PARACHUTE_DESCENT_RATE 3.0 +#endif +#ifndef PARACHUTE_WIND_CORRECTION +#define PARACHUTE_WIND_CORRECTION 1.0 +#endif +#ifndef PARACHUTE_LINE_LENGTH +#define PARACHUTE_LINE_LENGTH 3.0 +#endif + +float parachute_start_qdr; +float parachute_z; +float airborne_wind_dir = 0; +float airborne_wind_speed = 0; +float calculated_wind_dir = 0; +bool wind_measurements_valid = true; +bool wind_info_valid = false; +bool deploy_parachute_var = 0; +bool land_direction = 0; + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_wind_info(struct transport_tx *trans, struct link_device *dev) +{ + + float foo1 = 0, foo2 = 0, foo3 = 0; + pprz_msg_send_WEATHER(trans, dev, AC_ID, &foo1, &foo2, &airborne_wind_speed, &airborne_wind_dir, &foo3); + + return; +} +#endif + +void uav_recovery_init(void) +{ + + deploy_parachute_var = 0; + airborne_wind_dir = 0; + airborne_wind_speed = 0; + wind_measurements_valid = true; + wind_info_valid = true; +#if defined(PARACHUTE_OUTPUT_COMMAND) + ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ; +#endif + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WEATHER, send_wind_info); +#endif + + + return; +} + + +void uav_recovery_periodic(void) +{ +#if defined(PARACHUTE_OUTPUT_COMMAND) + if (autopilot.mode != AP_MODE_MANUAL) { + if (deploy_parachute_var == 1) { + ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MAX_PPRZ; + + } else { ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ; } + } +#else +#warning PARACHUTE COMMAND NOT FOUND +#endif + + return; +} + +uint8_t LockParachute(void) +{ + + deploy_parachute_var = 0; + + return (0); +} + +uint8_t DeployParachute(void) +{ + + deploy_parachute_var = 1; + + return (0); +} + + +uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height) +{ + +//struct EnuCoor_f* pos = stateGetPositionEnu_f(); +//struct UtmCoor_f * pos = stateGetPositionUtm_f(); + + static bool init = false; + static float circle_count = 0; + static bool wind_measurement_started = false; + +//Legacy estimator values + float estimator_hspeed_dir = 0; + float estimator_hspeed_mod = 0; + estimator_hspeed_dir = stateGetHorizontalSpeedDir_f(); + estimator_hspeed_mod = stateGetHorizontalSpeedNorm_f(); + + float speed_max_buf = 0; + float speed_min_buf = 1000.0; + float heading_angle_buf = 0; + + if (init == false) { + airborne_wind_dir = 0; + airborne_wind_speed = 0; + wind_measurements_valid = false; + speed_max_buf = 0; + speed_min_buf = 1000.; + heading_angle_buf = 0; + wind_measurement_started = false; + init = true; + } + + NavVerticalAutoThrottleMode(RadOfDeg(0.000000)); + NavVerticalAltitudeMode(Height(height), 0.); + NavCircleWaypoint(wp, radius); + if (nav_in_circle == false || GetPosAlt() < Height(height - 10)) { + return (true); + } + NavVerticalThrottleMode(MAX_PPRZ * (float)V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE); + // Wait until we are in altitude and in circle. + if (wind_measurement_started == false) { + circle_count = NavCircleCount(); + wind_measurement_started = true; + } + if (estimator_hspeed_mod > speed_max_buf) { + speed_max_buf = estimator_hspeed_mod; + } + // WHEN THE GROUND SPEED IS LOWEST THIS IS WHERE THE WIND IS BLOWING FROM. + if (estimator_hspeed_mod < speed_min_buf) { + speed_min_buf = estimator_hspeed_mod; + heading_angle_buf = estimator_hspeed_dir; //180 to -180 in radians, 0 is to the NORTH! + } + + if ((NavCircleCount() - circle_count) >= 1.1) { + //Update the wind direction and speed in the WEATHER message. + airborne_wind_speed = ((speed_max_buf - speed_min_buf) / 2) * AIRBORNE_WIND_CORRECTION; +#if defined(FIXED_WIND_SPEED_FOR_TESTING) && defined(SITL) +#pragma message "Wind SPEED for UAV recovery is fixed for the simulation" +#pragma message "You can change the value by editing 'uav_recovery.xml' file." + airborne_wind_speed = FIXED_WIND_SPEED_FOR_TESTING; // FOR TESTING IN SIMULATION ONLY! +#endif +#if defined(FIXED_WIND_DIRECTION_FOR_TESTING) && defined(SITL) +#pragma message "Wind direction for UAV recovery is fixed for the simulation" +#pragma message "You can change the value by editing 'uav_recovery.xml' file." + if (FIXED_WIND_DIRECTION_FOR_TESTING > 180.) { + heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING - 360.); + } else { + heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING); + } +#endif + airborne_wind_dir = DegOfRad(heading_angle_buf); //For use with the WEATHER message + //Convert from 180, -180 to 0, 360 in Degrees. 0=NORTH, 90=EAST + if (airborne_wind_dir < 0) { airborne_wind_dir += 360; } + calculated_wind_dir = heading_angle_buf; + wind_measurements_valid = true; + wind_info_valid = true; + + //NavVerticalAutoThrottleMode(RadOfDeg(0.000000)); + // EXIT and continue the flight plan. + init = false; + return (false); + } + + return (true); +} + + +// Compute an approximation for the RELEASE waypoint from expected descent and altitude +unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target) +{ + + float x = 0, y = 0; + float approach_dir = 0; + float calculated_wind_east = 0; + float calculated_wind_north = 0; + + struct { + float x; + float y; + float a; + } start_wp; + + // Calculate a starting waypoint opposite to the wind. + //180 TO -180 in Radians. Convert from 0 = NORTH to 0 = EAST + approach_dir = RadOfDeg(90.) - calculated_wind_dir; + if (approach_dir > M_PI) { approach_dir -= 2 * M_PI; } + approach_dir -= M_PI; // reverse the direction as we must release the parachute against the wind! + + start_wp.x = (cos(approach_dir) * nav_radius) + waypoints[wp_target].x; + start_wp.y = (sin(approach_dir) * nav_radius) + waypoints[wp_target].y; + start_wp.a = waypoints[release].a; + + // We approximate vx and vy, taking into account that RELEASE is close to the TARGET + float x_0 = waypoints[wp_target].x - start_wp.x; + float y_0 = waypoints[wp_target].y - start_wp.y; + + // Unit vector from the calculated starting waypoint to TARGET + float d = sqrt(x_0 * x_0 + y_0 * y_0); + float x1 = x_0 / d; + float y_1 = y_0 / d; + + waypoints[baseleg].x = start_wp.x + y_1 * nav_radius; + waypoints[baseleg].y = start_wp.y - x1 * nav_radius; + waypoints[baseleg].a = start_wp.a; + parachute_start_qdr = M_PI - atan2(-y_1, -x1); + if (nav_radius < 0) { + parachute_start_qdr += M_PI; + } + + parachute_z = waypoints[release].a - ground_alt - PARACHUTE_LINE_LENGTH; + + calculated_wind_north = cosf(calculated_wind_dir) * airborne_wind_speed; + calculated_wind_east = sinf(calculated_wind_dir) * airborne_wind_speed; + x = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_east * PARACHUTE_WIND_CORRECTION; + y = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_north * PARACHUTE_WIND_CORRECTION; + + waypoints[release].x = (waypoints[wp_target].x + x); + waypoints[release].y = waypoints[wp_target].y + y; + + + return 0; +} + diff --git a/sw/airborne/modules/uav_recovery/uav_recovery.h b/sw/airborne/modules/uav_recovery/uav_recovery.h new file mode 100644 index 0000000000..68e12f90a0 --- /dev/null +++ b/sw/airborne/modules/uav_recovery/uav_recovery.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com + * + * 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 modules/uav_recovery/uav_recovery.h + * + */ + +#if !defined(UAV_RECOVERY_H) +#define UAV_RECOVERY_H +#define MY_PARACHUTE_RADIUS DEFAULT_CIRCLE_RADIUS + +extern unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target); +extern float parachute_start_qdr; +extern bool deploy_parachute_var; +extern float airborne_wind_dir; +extern float airborne_wind_speed; +extern bool wind_measurements_valid; +extern bool wind_info_valid; + +extern void uav_recovery_init(void); +extern void uav_recovery_periodic(void); +extern uint8_t DeployParachute(void); +extern uint8_t LockParachute(void); +extern uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height); + + +#define ParachuteComputeApproach(_baseleg, _release, _target) parachute_compute_approach(_baseleg, _release, _target) + +#endif