| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -21,7 +21,7 @@ | |
| */ | ||
|
|
||
| /** | ||
| * @file modules/energy/MPPT.h | ||
| * @brief Solar cells MPTT monitoring | ||
| * | ||
| */ | ||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -21,7 +21,7 @@ | |
| */ | ||
|
|
||
|
|
||
| #include "modules/energy/MPPT.h" | ||
| #include "messages.h" | ||
| #include "subsystems/datalink/downlink.h" | ||
|
|
||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -20,7 +20,7 @@ | |
| * | ||
| */ | ||
|
|
||
| #include "modules/gps/gps_i2c.h" | ||
| #include "mcu_periph/i2c.h" | ||
| #include "subsystems/gps.h" | ||
|
|
||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,375 @@ | ||
| /* | ||
| * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin | ||
| * 2015 NAC-VA, Eduardo Lavratti | ||
| * | ||
| * 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/nav/nav_survey_rectangle_rotorcraft.c | ||
| * | ||
| * Automatic survey of a rectangle for rotorcraft. | ||
| * | ||
| * Rectangle is defined by two points, sweep can be south-north or west-east. | ||
| */ | ||
|
|
||
| #ifndef RECTANGLE_SURVEY_DEFAULT_SWEEP | ||
| #define RECTANGLE_SURVEY_DEFAULT_SWEEP 25 | ||
| #endif | ||
|
|
||
| #ifdef RECTANGLE_SURVEY_USE_INTERLEAVE | ||
| #define USE_INTERLEAVE TRUE | ||
| #else | ||
| #define USE_INTERLEAVE FALSE | ||
| #endif | ||
|
|
||
| #include "mcu_periph/uart.h" | ||
| #include "messages.h" | ||
| #include "subsystems/datalink/downlink.h" | ||
|
|
||
| #if PERIODIC_TELEMETRY | ||
| #include "subsystems/datalink/telemetry.h" | ||
| #endif | ||
|
|
||
| #include "firmwares/rotorcraft/navigation.h" | ||
|
|
||
| #include "modules/nav/nav_survey_rectangle_rotorcraft.h" | ||
| #include "state.h" | ||
|
|
||
| float sweep = RECTANGLE_SURVEY_DEFAULT_SWEEP; | ||
| static bool_t nav_survey_rectangle_active = FALSE; | ||
| uint16_t rectangle_survey_sweep_num; | ||
| bool_t nav_in_segment = FALSE; | ||
| bool_t nav_in_circle = FALSE; | ||
| bool_t interleave = USE_INTERLEAVE; | ||
|
|
||
| static struct EnuCoor_f survey_from, survey_to; | ||
| static struct EnuCoor_i survey_from_i, survey_to_i; | ||
|
|
||
| static bool_t survey_uturn __attribute__((unused)) = FALSE; | ||
| static survey_orientation_t survey_orientation = NS; | ||
|
|
||
| float nav_survey_shift; | ||
| float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south; | ||
|
|
||
| #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y)) | ||
| #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y)) | ||
| #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x)) | ||
| #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x)) | ||
|
|
||
| #include "generated/flight_plan.h" | ||
|
|
||
| #ifndef LINE_START_FUNCTION | ||
| #define LINE_START_FUNCTION {} | ||
| #endif | ||
| #ifndef LINE_STOP_FUNCTION | ||
| #define LINE_STOP_FUNCTION {} | ||
| #endif | ||
|
|
||
| static void send_survey(struct transport_tx *trans, struct link_device *dev) | ||
| { | ||
| if (nav_survey_active) { | ||
| pprz_msg_send_SURVEY(trans, dev, AC_ID, | ||
| &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); | ||
| } | ||
| } | ||
|
|
||
| void nav_survey_rectangle_rotorcraft_init(void) | ||
| { | ||
| #if PERIODIC_TELEMETRY | ||
| register_periodic_telemetry(DefaultPeriodic, "SURVEY", send_survey); | ||
| #endif | ||
| } | ||
|
|
||
| bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) | ||
| { | ||
| rectangle_survey_sweep_num = 0; | ||
| nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); | ||
| nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); | ||
| nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); | ||
| nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); | ||
| survey_orientation = so; | ||
|
|
||
| if (survey_orientation == NS) { | ||
| if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { | ||
| survey_from.x = survey_to.x = nav_survey_west + grid / 4.; | ||
| } else { | ||
| survey_from.x = survey_to.x = nav_survey_east - grid / 4.; | ||
| grid = -grid; | ||
| } | ||
|
|
||
| if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { | ||
| survey_to.y = nav_survey_south; | ||
| survey_from.y = nav_survey_north; | ||
| } else { | ||
| survey_from.y = nav_survey_south; | ||
| survey_to.y = nav_survey_north; | ||
| } | ||
| } else { /* survey_orientation == WE */ | ||
| if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { | ||
| survey_from.y = survey_to.y = nav_survey_south + grid / 4.; | ||
| } else { | ||
| survey_from.y = survey_to.y = nav_survey_north - grid / 4.; | ||
| grid = -grid; | ||
| } | ||
|
|
||
| if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { | ||
| survey_to.x = nav_survey_west; | ||
| survey_from.x = nav_survey_east; | ||
| } else { | ||
| survey_from.x = nav_survey_west; | ||
| survey_to.x = nav_survey_east; | ||
| } | ||
| } | ||
| nav_survey_shift = grid; | ||
| survey_uturn = FALSE; | ||
| nav_survey_rectangle_active = FALSE; | ||
|
|
||
| //go to start position | ||
| ENU_BFP_OF_REAL(survey_from_i, survey_from); | ||
| horizontal_mode = HORIZONTAL_MODE_ROUTE; | ||
| VECT3_COPY(navigation_target, survey_from_i); | ||
| LINE_STOP_FUNCTION; | ||
| NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.); | ||
| if (survey_orientation == NS) { | ||
| nav_set_heading_deg(0); | ||
| } else { | ||
| nav_set_heading_deg(90); | ||
| } | ||
| return FALSE; | ||
| } | ||
|
|
||
|
|
||
| bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2) | ||
| { | ||
| static bool_t is_last_half = FALSE; | ||
| static float survey_radius; | ||
| nav_survey_active = TRUE; | ||
|
|
||
| /* entry scan */ // wait for start position and altitude be reached | ||
| if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0)) | ||
| || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) { | ||
| } else { | ||
| if (!nav_survey_rectangle_active) { | ||
| nav_survey_rectangle_active = TRUE; | ||
| LINE_START_FUNCTION; | ||
| } | ||
|
|
||
| nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); | ||
| nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); | ||
| nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); | ||
| nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); | ||
|
|
||
| /* Update the current segment from corners' coordinates*/ | ||
| if (SurveyGoingNorth()) { | ||
| survey_to.y = nav_survey_north; | ||
| survey_from.y = nav_survey_south; | ||
| } else if (SurveyGoingSouth()) { | ||
| survey_to.y = nav_survey_south; | ||
| survey_from.y = nav_survey_north; | ||
| } else if (SurveyGoingEast()) { | ||
| survey_to.x = nav_survey_east; | ||
| survey_from.x = nav_survey_west; | ||
| } else if (SurveyGoingWest()) { | ||
| survey_to.x = nav_survey_west; | ||
| survey_from.x = nav_survey_east; | ||
| } | ||
|
|
||
| if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */ | ||
| /* if you like to use position croos instead of approaching uncoment this line | ||
| if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) || | ||
| (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) || | ||
| (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) || | ||
| (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) { | ||
| */ | ||
| /* Continue ... */ | ||
| ENU_BFP_OF_REAL(survey_to_i, survey_to); | ||
|
|
||
| if (!nav_approaching_from(&survey_to_i, NULL, 0)) { | ||
| ENU_BFP_OF_REAL(survey_from_i, survey_from); | ||
|
|
||
| horizontal_mode = HORIZONTAL_MODE_ROUTE; | ||
| nav_route(&survey_from_i, &survey_to_i); | ||
|
|
||
| } else { | ||
| if (survey_orientation == NS) { | ||
| /* North or South limit reached, prepare turn and next leg */ | ||
| float x0 = survey_from.x; /* Current longitude */ | ||
| if ((x0 + nav_survey_shift < nav_survey_west) | ||
| || (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep | ||
| if (((x0 + (nav_survey_shift / 2)) < nav_survey_west) | ||
| || ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep | ||
| if (is_last_half) {// was last sweep half? | ||
| nav_survey_shift = -nav_survey_shift; | ||
| if (interleave) { | ||
| survey_radius = nav_survey_shift; | ||
| }else { | ||
| survey_radius = nav_survey_shift /2.; | ||
| } | ||
| is_last_half = FALSE; | ||
| } else { // last sweep not half | ||
| nav_survey_shift = -nav_survey_shift; | ||
| if (interleave) { | ||
| survey_radius = nav_survey_shift /2.; | ||
| }else{ | ||
| survey_radius = nav_survey_shift ; | ||
| } | ||
| } | ||
| rectangle_survey_sweep_num ++; | ||
| } else { //room for half sweep after | ||
| survey_radius = nav_survey_shift / 2.; | ||
| is_last_half = TRUE; | ||
| } | ||
| } else {// room for full sweep after | ||
| survey_radius = nav_survey_shift; | ||
| } | ||
|
|
||
| x0 = x0 + survey_radius; /* Longitude of next leg */ | ||
| survey_from.x = survey_to.x = x0; | ||
|
|
||
| /* Swap South and North extremities */ | ||
| float tmp = survey_from.y; | ||
| survey_from.y = survey_to.y; | ||
| survey_to.y = tmp; | ||
|
|
||
| /** Do half a circle around WP 0 */ | ||
| waypoints[0].enu_f.x = x0; | ||
| waypoints[0].enu_f.y = survey_from.y; | ||
|
|
||
| /* Computes the right direction */ | ||
| if (SurveyGoingEast()) { | ||
| survey_radius = -survey_radius; | ||
| } | ||
| } else { /* (survey_orientation == WE) */ | ||
| /* East or West limit reached, prepare turn and next leg */ | ||
| /* There is a y0 declared in math.h (for ARM) !!! */ | ||
| float my_y0 = survey_from.y; /* Current latitude */ | ||
| if (my_y0 + nav_survey_shift < nav_survey_south | ||
| || my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep | ||
| if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south) | ||
| || ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep | ||
| if (is_last_half) {// was last sweep half? | ||
| nav_survey_shift = -nav_survey_shift; | ||
| if (interleave) { | ||
| survey_radius = nav_survey_shift; | ||
| }else { | ||
| survey_radius = nav_survey_shift /2.; | ||
| } | ||
| is_last_half = FALSE; | ||
| } else { // last sweep not half | ||
| nav_survey_shift = -nav_survey_shift; | ||
| if (interleave) { | ||
| survey_radius = nav_survey_shift /2.; | ||
| }else{ | ||
| survey_radius = nav_survey_shift ; | ||
| } | ||
| } | ||
| rectangle_survey_sweep_num ++; | ||
| } else { //room for half sweep after | ||
| survey_radius = nav_survey_shift / 2.; | ||
| is_last_half = TRUE; | ||
| } | ||
| } else {// room for full sweep after | ||
| survey_radius = nav_survey_shift; | ||
| } | ||
|
|
||
| my_y0 = my_y0 + survey_radius; /* latitude of next leg */ | ||
| survey_from.y = survey_to.y = my_y0; | ||
|
|
||
| /* Swap West and East extremities */ | ||
| float tmp = survey_from.x; | ||
| survey_from.x = survey_to.x; | ||
| survey_to.x = tmp; | ||
|
|
||
| /** Do half a circle around WP 0 */ | ||
| waypoints[0].enu_f.x = survey_from.x; | ||
| waypoints[0].enu_f.y = my_y0; | ||
|
|
||
| /* Computes the right direction */ | ||
| if (SurveyGoingNorth()) { | ||
| survey_radius = -survey_radius; | ||
| } | ||
| } | ||
|
|
||
| nav_in_segment = FALSE; | ||
| survey_uturn = TRUE; | ||
| LINE_STOP_FUNCTION; | ||
| #ifdef DIGITAL_CAM | ||
| float temp; | ||
| if (survey_orientation == NS) { | ||
| temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval; | ||
| } else{ | ||
| temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval; | ||
| } | ||
| double inteiro; | ||
| double fract = modf (temp , &inteiro); | ||
| if (fract > .5) { | ||
| dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep | ||
| } | ||
| #endif | ||
| } | ||
| } else { /* START turn */ | ||
|
|
||
| static struct EnuCoor_f temp_f; | ||
| if (survey_orientation == WE) { | ||
| temp_f.x = waypoints[0].enu_f.x; | ||
| temp_f.y = waypoints[0].enu_f.y - survey_radius; | ||
| } else { | ||
| temp_f.y = waypoints[0].enu_f.y; | ||
| temp_f.x = waypoints[0].enu_f.x - survey_radius; | ||
| } | ||
|
|
||
| //detect when segment has done | ||
| /* if you like to use position croos instead of approaching uncoment this line | ||
| if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )|| | ||
| (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )|| | ||
| (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )|| | ||
| (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) { | ||
| */ | ||
|
|
||
| if (survey_orientation == WE) { | ||
| ENU_BFP_OF_REAL(survey_from_i, temp_f); | ||
| ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); | ||
| } else { | ||
| ENU_BFP_OF_REAL(survey_from_i, temp_f); | ||
| ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); | ||
| } | ||
| if (nav_approaching_from(&survey_to_i, NULL, 0)) { | ||
| survey_uturn = FALSE; | ||
| nav_in_circle = FALSE; | ||
| LINE_START_FUNCTION; | ||
| } else { | ||
|
|
||
| if (survey_orientation == WE) { | ||
| ENU_BFP_OF_REAL(survey_from_i, temp_f); | ||
| ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); | ||
| } else { | ||
| ENU_BFP_OF_REAL(survey_from_i, temp_f); | ||
| ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); | ||
| } | ||
|
|
||
| horizontal_mode = HORIZONTAL_MODE_ROUTE; | ||
| nav_route(&survey_from_i, &survey_to_i); | ||
| } | ||
| } /* END turn */ | ||
|
|
||
| } /* END entry scan */ | ||
| return TRUE; | ||
|
|
||
| }// /* END survey_retangle */ | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,50 @@ | ||
| /* | ||
| * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin | ||
| * 2015 NAC-VA, Eduardo Lavratti | ||
| * | ||
| * 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/nav/nav_survey_rectangle_rotorcraft.h | ||
| * | ||
| * Automatic survey of a rectangle for rotorcraft. | ||
| * | ||
| * Rectangle is defined by two points, sweep can be south-north or west-east. | ||
| */ | ||
|
|
||
| #ifndef NAV_SURVEY_RECTANGLE_ROTORCRAFT_H | ||
| #define NAV_SURVEY_RECTANGLE_ROTORCRAFT_H | ||
|
|
||
| #include "firmwares/rotorcraft/navigation.h" | ||
|
|
||
| typedef enum {NS, WE} survey_orientation_t; | ||
|
|
||
| extern float sweep; | ||
| extern uint16_t rectangle_survey_sweep_num; | ||
| extern bool_t interleave; | ||
|
|
||
|
|
||
| extern void nav_survey_rectangle_rotorcraft_init(void); | ||
| extern bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid1, survey_orientation_t so); | ||
| extern bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2); | ||
|
|
||
| #define NavSurveyRectangleInit(_wp1, _wp2, _grid, _orientation) nav_survey_rectangle_rotorcraft_setup(_wp1, _wp2, _grid, _orientation) | ||
| #define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle_rotorcraft_run(_wp1, _wp2) | ||
|
|
||
| #endif // NAV_SURVEY_RECTANGLE_ROTORCRAFT_H |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,141 @@ | ||
| /* | ||
| * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@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/sonar/sonar_bebop.c | ||
| * @brief Parrot Bebop Sonar driver | ||
| */ | ||
|
|
||
| #include "sonar_bebop.h" | ||
| #include "generated/airframe.h" | ||
| #include "mcu_periph/adc.h" | ||
| #include "mcu_periph/spi.h" | ||
| #include "subsystems/abi.h" | ||
| #include <pthread.h> | ||
| #include "subsystems/datalink/downlink.h" | ||
|
|
||
| #ifdef SITL | ||
| #include "state.h" | ||
| #endif | ||
|
|
||
|
|
||
| struct SonarBebop sonar_bebop; | ||
| static uint8_t sonar_bebop_spi_d[16] = { 0xF0,0xF0,0xF0,0xF0,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }; | ||
| static struct spi_transaction sonar_bebop_spi_t; | ||
| static pthread_t sonar_bebop_thread; | ||
| static void *sonar_bebop_read(void *data); | ||
|
|
||
| void sonar_bebop_init(void) | ||
| { | ||
| sonar_bebop.meas = 0; | ||
| sonar_bebop.offset = 0; | ||
|
|
||
| sonar_bebop_spi_t.status = SPITransDone; | ||
| sonar_bebop_spi_t.select = SPISelectUnselect; | ||
| sonar_bebop_spi_t.dss = SPIDss8bit; | ||
| sonar_bebop_spi_t.output_buf = sonar_bebop_spi_d; | ||
| sonar_bebop_spi_t.output_length = 16; | ||
| sonar_bebop_spi_t.input_buf = NULL; | ||
| sonar_bebop_spi_t.input_length = 0; | ||
|
|
||
| int rc = pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL); | ||
| if (rc < 0) { | ||
| return; | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Read ADC value to update sonar measurement | ||
| */ | ||
| static void *sonar_bebop_read(void *data __attribute__((unused))) | ||
| { | ||
| while(true) { | ||
|
|
||
| #ifndef SITL | ||
| uint16_t i; | ||
| uint16_t adc_buffer[8192]; | ||
|
|
||
|
|
||
| /* Start ADC and send sonar output */ | ||
| adc_enable(&adc0, 1); | ||
| sonar_bebop_spi_t.status = SPITransDone; | ||
| spi_submit(&spi0, &sonar_bebop_spi_t); | ||
| while(sonar_bebop_spi_t.status != SPITransSuccess); | ||
| adc_read(&adc0, adc_buffer, 8192); | ||
| adc_enable(&adc0, 0); | ||
|
|
||
| /* Find the peeks */ | ||
| uint16_t start_send = 0; | ||
| uint16_t stop_send = 0; | ||
| uint16_t first_peek = 0; | ||
| uint16_t lowest_value = 4095; | ||
| for(i = 0; i < 8192; i++){ | ||
| uint16_t adc_val = adc_buffer[i]>>4; | ||
| if(start_send == 0 && adc_val == 4095) | ||
| start_send = i; | ||
| else if(start_send != 0 && stop_send == 0 && adc_val != 4095) | ||
| { | ||
| stop_send = i-1; | ||
| i += 300; | ||
| continue; | ||
| } | ||
|
|
||
| if(start_send != 0 && stop_send != 0 && first_peek == 0 && adc_val < lowest_value) | ||
| lowest_value = adc_val; | ||
| else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100) { | ||
| first_peek = i; | ||
| lowest_value = adc_val - 100; | ||
| } | ||
| else if(start_send != 0 && stop_send != 0 && first_peek != 0 && adc_val+100 < (adc_buffer[first_peek]>>4)) { | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| /* Calculate the distance from the peeks */ | ||
| uint16_t diff = stop_send - start_send; | ||
| int16_t peek_distance = first_peek - (stop_send - diff/2); | ||
| if(first_peek <= stop_send || diff > 250) | ||
| peek_distance = 0; | ||
|
|
||
| sonar_bebop.distance = peek_distance / 1000.0; | ||
| #else // SITL | ||
| sonar_bebop.distance = stateGetPositionEnu_f()->z; | ||
| Bound(sonar_bebop.distance, 0.1f, 7.0f); | ||
| uint16_t peek_distance = 1; | ||
| #endif // SITL | ||
|
|
||
| usleep(10000); | ||
|
|
||
| if(peek_distance > 0) | ||
| { | ||
| // Send ABI message | ||
| AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance); | ||
|
|
||
| #ifdef SENSOR_SYNC_SEND_SONAR | ||
| // Send Telemetry report | ||
| DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance); | ||
| #endif | ||
| } | ||
| } | ||
|
|
||
| return NULL; | ||
| } | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,166 @@ | ||
| /* | ||
| * Copyright (C) 2008-2015 The Paparazzi Team | ||
| * | ||
| * This file is part of Paparazzi. | ||
| * | ||
| * Paparazzi is free software; you can redistribute it and/or modify | ||
| * it under the terms of the GNU General Public License as published by | ||
| * the Free Software Foundation; either version 2, or (at your option) | ||
| * any later version. | ||
| * | ||
| * Paparazzi is distributed in the hope that it will be useful, | ||
| * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| * GNU General Public License for more details. | ||
| * | ||
| * You should have received a copy of the GNU General Public License | ||
| * along with Paparazzi; see the file COPYING. If not, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| */ | ||
|
|
||
| /** @file motor_mixing_types.h | ||
| * Common Motor Mixing configuration types. | ||
| */ | ||
|
|
||
| #ifndef MOTOR_MIXING_TYPES_H | ||
| #define MOTOR_MIXING_TYPES_H | ||
|
|
||
| /* already defined common configurations*/ | ||
| #define QUAD_PLUS 1 | ||
| #define QUAD_X 2 | ||
| #define QUAD_X_CCW 3 | ||
| #define HEXA_X 4 | ||
| #define HEXA_PLUS 5 | ||
| #define OCTO_X 6 | ||
| #define OCTO_PLUS 7 | ||
|
|
||
| #if MOTOR_MIXING_TYPE == QUAD_PLUS | ||
| /* | ||
| * Quadrotor in plus (+) cross configuration with motor order: | ||
| * front (CW), right (CCW), back (CW), left (CCW) | ||
| */ | ||
| #define MOTOR_FRONT 0 | ||
| #define MOTOR_RIGHT 1 | ||
| #define MOTOR_BACK 2 | ||
| #define MOTOR_LEFT 3 | ||
| #define MOTOR_MIXING_NB_MOTOR 4 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 0, -256, 0, 256 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 256, 0, -256, 0 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == QUAD_X | ||
| /* | ||
| * Quadrotor in time cross (X) configuration with motor order: | ||
| * front left (CW), front right (CCW), back right (CW), back left (CCW) | ||
| */ | ||
| #define MOTOR_FRONT_LEFT 0 | ||
| #define MOTOR_FRONT_RIGHT 1 | ||
| #define MOTOR_BACK_RIGHT 2 | ||
| #define MOTOR_BACK_LEFT 3 | ||
| #define MOTOR_MIXING_NB_MOTOR 4 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 181, -181, -181, 181 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 181, 181, -181, -181 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == QUAD_X_CCW | ||
| /* | ||
| * Quadrotor in time cross (X) configuration with motor order (reversed from QUAD_X): | ||
| * front left (CCW), front right (CW), back right (CCW), back left (CW) | ||
| */ | ||
| #define MOTOR_FRONT_LEFT 0 | ||
| #define MOTOR_FRONT_RIGHT 1 | ||
| #define MOTOR_BACK_RIGHT 2 | ||
| #define MOTOR_BACK_LEFT 3 | ||
| #define MOTOR_MIXING_NB_MOTOR 4 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 181, -181, -181, 181 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 181, 181, -181, -181 } | ||
| #define MOTOR_MIXING_YAW_COEF { 128, -128, 128, -128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == HEXA_X | ||
| /* | ||
| * Hexarotor in time cross (X) configuration with motor order: | ||
| * front left (CW), front right (CCW), right (CW), back right (CCW), back left (CW), left (CCW) | ||
| */ | ||
| #define MOTOR_FRONT_LEFT 0 | ||
| #define MOTOR_FRONT_RIGHT 1 | ||
| #define MOTOR_RIGHT 2 | ||
| #define MOTOR_BACK_RIGHT 3 | ||
| #define MOTOR_BACK_LEFT 4 | ||
| #define MOTOR_LEFT 5 | ||
| #define MOTOR_MIXING_NB_MOTOR 6 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 128, -128, -256, -128, 128, 256 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 222, 222, 0, -222, -222, 0 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == HEXA_PLUS | ||
| /* | ||
| * Hexarotor in plus (+) configuration with motor order: | ||
| * front (CW), front right (CCW), back right (CW), back (CCW), back left (CW), front left (CCW) | ||
| */ | ||
| #define MOTOR_FRONT 0 | ||
| #define MOTOR_FRONT_RIGHT 1 | ||
| #define MOTOR_BACK_RIGHT 2 | ||
| #define MOTOR_BACK 3 | ||
| #define MOTOR_BACK_LEFT 4 | ||
| #define MOTOR_FRONT_LEFT 5 | ||
| #define MOTOR_MIXING_NB_MOTOR 6 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 0, -222, -222, 0, 222, 222 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 256, 128, -128, -256, -128, 128 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == OCTO_PLUS | ||
| /* | ||
| * Hexarotor in plus cross (+) configuration with motor order: | ||
| * front (CW), front right (CCW), right (CW), back right (CCW), | ||
| * back (CW), back left (CCW), left (CW), front left (CCW) | ||
| */ | ||
| #define MOTOR_FRONT 0 | ||
| #define MOTOR_FRONT_RIGHT 1 | ||
| #define MOTOR_RIGHT 2 | ||
| #define MOTOR_BACK_RIGHT 3 | ||
| #define MOTOR_BACK 4 | ||
| #define MOTOR_BACK_LEFT 5 | ||
| #define MOTOR_LEFT 6 | ||
| #define MOTOR_FRONT_LEFT 7 | ||
| #define MOTOR_MIXING_NB_MOTOR 8 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 0, -181, -256, -181, 0, 181, 256, 181 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 256, 181, 0, -181, -256, -181, 0, 181 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256, 256, 256 } | ||
|
|
||
| #elif MOTOR_MIXING_TYPE == OCTO_X | ||
| /* | ||
| * Hexarotor in time cross (X) configuration with motor order: | ||
| * front left1 (CW), front right1 (CCW), front right2 (CW), back right1 (CCW), | ||
| * back right2 (CW), back left1 (CCW), back left2 (CW), front left2 (CCW) | ||
| */ | ||
| #define MOTOR_FRONT_LEFT1 0 | ||
| #define MOTOR_FRONT_RIGHT1 1 | ||
| #define MOTOR_FRONT_RIGHT2 2 | ||
| #define MOTOR_BACK_RIGHT1 3 | ||
| #define MOTOR_BACK_RIGHT2 4 | ||
| #define MOTOR_BACK_LEFT1 5 | ||
| #define MOTOR_BACK_LEFT2 6 | ||
| #define MOTOR_FRONT_LEFT2 7 | ||
| #define MOTOR_MIXING_NB_MOTOR 8 | ||
| #define MOTOR_MIXING_SCALE 256 | ||
| #define MOTOR_MIXING_ROLL_COEF { 98, -98, -237, -237, -98, 98, 237, 237 } | ||
| #define MOTOR_MIXING_PITCH_COEF { 237, 237, 98, -98, -237, -237, -98, 98 } | ||
| #define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128, -128, 128 } | ||
| #define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256, 256, 256 } | ||
|
|
||
| #endif | ||
|
|
||
| #endif /* MOTOR_MIXING_TYPES_H */ |