diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml index 4185415cb2..3fe419f6c8 100644 --- a/conf/airframes/TUDELFT/tudelft_conf.xml +++ b/conf/airframes/TUDELFT/tudelft_conf.xml @@ -307,6 +307,17 @@ settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/gps_ubx_ucenter.xml" gui_color="blue" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + +
+
+ + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + +
+ + + + + + + +
+
+ + +
+
+ + + + + + + +
+
+ + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + +
+
+ + + + + + + + +
+
+ + +
+
+ + + + + +
+
+ + + + + +
+ diff --git a/conf/modules/glide_wing_lock.xml b/conf/modules/glide_wing_lock.xml new file mode 100644 index 0000000000..86d40d5bae --- /dev/null +++ b/conf/modules/glide_wing_lock.xml @@ -0,0 +1,25 @@ + + + + + Locks the wing of an ornicopter so it can glide. + + + + + + + +
+ +
+ + + + + + + + + + diff --git a/conf/radios/T14SG_SBUS.xml b/conf/radios/T14SG_SBUS.xml new file mode 100644 index 0000000000..59b8278a0c --- /dev/null +++ b/conf/radios/T14SG_SBUS.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_rotorcraft_slow.xml b/conf/telemetry/default_rotorcraft_slow.xml index 935598dcc0..a2ab06a026 100644 --- a/conf/telemetry/default_rotorcraft_slow.xml +++ b/conf/telemetry/default_rotorcraft_slow.xml @@ -28,7 +28,8 @@ - + + diff --git a/sw/airborne/boards/px4fmu_2.4.h b/sw/airborne/boards/px4fmu_2.4.h index 9c96d1fbdb..a0c603fe45 100644 --- a/sw/airborne/boards/px4fmu_2.4.h +++ b/sw/airborne/boards/px4fmu_2.4.h @@ -184,12 +184,36 @@ /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY #define ADC_CHANNEL_VSUPPLY ADC_2 -#endif - -#if USE_ADC_2 #define DefaultVoltageOfAdc(adc) (0.00975*adc) #else -#define DefaultVoltageOfAdc(adc) (0.0021*adc) // scale internal vdd to 5V +#define DefaultVoltageOfAdc(adc) (0.00384*adc) +#endif + +/* External adc (pressure / air speed / 6.6v) */ +#ifndef USE_ADC_4 +#define USE_ADC_4 1 +#endif +#if USE_ADC_4 +#define AD1_4_CHANNEL 15 // ADC12_IN15 +#define ADC_4 AD1_4 +#define ADC_4_GPIO_PORT GPIOC +#define ADC_4_GPIO_PIN GPIO5 +#endif + +/* External adc 3.3v */ +#if USE_ADC_5 +#define AD1_5_CHANNEL 13 // ADC123_IN13 +#define ADC_5 AD1_5 +#define ADC_5_GPIO_PORT GPIOC +#define ADC_5_GPIO_PIN GPIO3 +#endif + +/* External adc 3.3v */ +#if USE_ADC_6 +#define AD1_6_CHANNEL 14 // ADC12_IN14 +#define ADC_6 AD1_6 +#define ADC_6_GPIO_PORT GPIOC +#define ADC_6_GPIO_PIN GPIO4 #endif /* diff --git a/sw/airborne/boards/px4fmu_4.0.h b/sw/airborne/boards/px4fmu_4.0.h index 809192d869..c2b29ac07a 100644 --- a/sw/airborne/boards/px4fmu_4.0.h +++ b/sw/airborne/boards/px4fmu_4.0.h @@ -219,7 +219,7 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi #define USE_AD_TIM3 1 -// Interal ADC for battery +// Internal ADC for battery #ifndef USE_ADC_1 #define USE_ADC_1 1 #endif diff --git a/sw/airborne/boards/px4io_2.4.h b/sw/airborne/boards/px4io_2.4.h index 2e994a2e4d..5dafde5435 100644 --- a/sw/airborne/boards/px4io_2.4.h +++ b/sw/airborne/boards/px4io_2.4.h @@ -60,7 +60,12 @@ #define UART2_GPIO_RX GPIO3 #define UART2_GPIO_PORT_TX GPIOA #define UART2_GPIO_TX GPIO2 - +// SBUS +#define UART3_GPIO_AF 0 +#define UART3_GPIO_PORT_RX GPIOB +#define UART3_GPIO_RX GPIO11 +#define UART3_GPIO_PORT_TX GPIOB +#define UART3_GPIO_TX GPIO10 /* * Spektrum diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c index e24cdc8ac8..f2607c62dd 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c @@ -57,13 +57,13 @@ bool ap_ahrs_is_aligned(void) /** Get autopilot mode as set by a RADIO_MODE 3-way switch and a 2-way switch, which are mixed together * The 2 way switch negates the value, the 3 way switch changes in three steps from 0 - MAX_PPRZ. * E.g. SW_1 has two positions (On/Off), SW_Mode has three positions (M1/M2/M3) - * 1 Mode value - * Off M1 -9500 - * Off M2 -4800 - * Off M3 -1850 - * On M1 2100 - * On M2 4900 - * On M3 9600 + * 1 Mode value + * Off M1 -9500 + * Off M2 -4800 + * Off M3 -1850 + * On M1 2100 + * On M2 4900 + * On M3 9600 * This function filters out the effect of SW_1, such that a normal 3-way switch comes out. **/ uint8_t ap_mode_of_3x2way_switch(void) diff --git a/sw/airborne/modules/glide_wing_lock/glide_wing_lock.c b/sw/airborne/modules/glide_wing_lock/glide_wing_lock.c new file mode 100644 index 0000000000..6d9a5138dc --- /dev/null +++ b/sw/airborne/modules/glide_wing_lock/glide_wing_lock.c @@ -0,0 +1,124 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/glide_wing_lock/glide_wing_lock.c" + * @author Kevin van Hecke + * Locks the wing of an ornicopter so it can glide. + */ + +#include "modules/glide_wing_lock/glide_wing_lock.h" + +#include "pprzlink/messages.h" +#include "mcu_periph/uart.h" +#include "subsystems/datalink/downlink.h" + +#include "mcu_periph/adc.h" +#include "subsystems/commands.h" +#include "subsystems/radio_control.h" +#include "autopilot.h" + +struct adc_buf adcbuf; + +int lock_wings; + +#ifndef WING_POS_DOWN_THRESH +#define WING_POS_DOWN_THRESH 100 +#endif +#ifndef WING_POS_LOCK_MIN_THRESH +#define WING_POS_LOCK_MIN_THRESH 2000 +#endif +#ifndef WING_POS_LOCK_MAX_THRESH +#define WING_POS_LOCK_MAX_THRESH 2100 +#endif +#ifndef WING_POS_NOMINAL_THRUST +#define WING_POS_NOMINAL_THRUST 5000 +#endif +#ifndef WING_POS_LOCK_SWITCH +#define WING_POS_LOCK_SWITCH RADIO_AUX2 +#endif + + +void glide_wing_lock_init(void) +{ + adc_buf_channel(ADC_CHANNEL_MOTORSENSOR, &adcbuf, 1); +} + +void glide_wing_lock_event() +{ + static int lockstate = 0; + if (radio_control.values[WING_POS_LOCK_SWITCH] > (MIN_PPRZ / 2)) { // check glide switch + float wpos = adcbuf.sum / adcbuf.av_nb_sample; + switch (lockstate) { + case 0: + if (wpos < WING_POS_DOWN_THRESH) { //set wings to fixed speed for one rotation starting from when wings are at lowest position + lock_wings = 1; + lockstate++; + } + break; + case 1: + if (wpos > WING_POS_LOCK_MIN_THRESH) { //start wait for a rotation + lockstate++; + } + break; + case 2: + if (wpos < WING_POS_DOWN_THRESH) { //rotation finished + lockstate++; + } + break; + case 3: + if (wpos > WING_POS_LOCK_MIN_THRESH && wpos < WING_POS_LOCK_MAX_THRESH) { // wait for exact wing position + //esc brakes when throttle = 0, which should lock the wing in this position; + lock_wings = 2; + lockstate++; + } + break; + default: + break; + } + } else { + lock_wings = 0; + lockstate = 0; + } +} + +void glide_wing_lock_periodic() +{ + uint16_t wpos = adcbuf.sum / adcbuf.av_nb_sample; + DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &wpos, &wpos); +} + +void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on) +{ + if (!(in_flight)) { cmd_in[COMMAND_YAW] = 0; } + if (!(motors_on)) { cmd_in[COMMAND_THRUST] = 0; } + cmd_out[COMMAND_ROLL] = cmd_in[COMMAND_ROLL]; + cmd_out[COMMAND_PITCH] = cmd_in[COMMAND_PITCH]; + cmd_out[COMMAND_YAW] = cmd_in[COMMAND_YAW]; + cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST]; + + if (lock_wings == 1 && motors_on && cmd_out[COMMAND_THRUST] > 0) { + cmd_out[COMMAND_THRUST] = WING_POS_NOMINAL_THRUST; + } else if (lock_wings == 2) { + cmd_out[COMMAND_THRUST] = 0; + } +} + + + diff --git a/sw/airborne/modules/glide_wing_lock/glide_wing_lock.h b/sw/airborne/modules/glide_wing_lock/glide_wing_lock.h new file mode 100644 index 0000000000..c59bd7f191 --- /dev/null +++ b/sw/airborne/modules/glide_wing_lock/glide_wing_lock.h @@ -0,0 +1,43 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/glide_wing_lock/glide_wing_lock.h" + * @author Kevin van Hecke + * Locks the wing of an ornicopter so it can glide. + */ + +#ifndef GLIDEWINGLOCK_H +#define GLIDEWINGLOCK_H + +#include "std.h" +#include "generated/airframe.h" +#include "paparazzi.h" + +#define ROTORCRAFT_COMMANDS_THROUGH_MODULE + +extern void init(void); +extern void glide_wing_lock_init(void); +extern void glide_wing_lock_event(void); +extern void glide_wing_lock_periodic(void); + +//extern void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on __attribute__((unused))); + +#endif + diff --git a/sw/airborne/subsystems/radio_control/sbus_common.c b/sw/airborne/subsystems/radio_control/sbus_common.c index 89f66048d1..fcfbcfb5a1 100644 --- a/sw/airborne/subsystems/radio_control/sbus_common.c +++ b/sw/airborne/subsystems/radio_control/sbus_common.c @@ -84,26 +84,26 @@ static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst, bool *availabl uint16_t *dstppm __attribute__((unused))) { // decode sbus data, unrolling the loop for efficiency - dst[0] = ((src[0] ) | (src[1]<<8)) & 0x07FF; - dst[1] = ((src[1]>>3 ) | (src[2]<<5)) & 0x07FF; - dst[2] = ((src[2]>>6 ) | (src[3]<<2) | (src[4]<<10)) & 0x07FF; - dst[3] = ((src[4]>>1 ) | (src[5]<<7)) & 0x07FF; - dst[4] = ((src[5]>>4 ) | (src[6]<<4)) & 0x07FF; - dst[5] = ((src[6]>>7 ) | (src[7]<<1 ) | (src[8]<<9)) & 0x07FF; - dst[6] = ((src[8]>>2 ) | (src[9]<<6)) & 0x07FF; - dst[7] = ((src[9]>>5) | (src[10]<<3)) & 0x07FF; - dst[8] = ((src[11] ) | (src[12]<<8)) & 0x07FF; - dst[9] = ((src[12]>>3) | (src[13]<<5)) & 0x07FF; - dst[10] = ((src[13]>>6) | (src[14]<<2) | (src[15]<<10)) & 0x07FF; - dst[11] = ((src[15]>>1) | (src[16]<<7)) & 0x07FF; - dst[12] = ((src[16]>>4) | (src[17]<<4)) & 0x07FF; - dst[13] = ((src[17]>>7) | (src[18]<<1) | (src[19]<<9)) & 0x07FF; - dst[14] = ((src[19]>>2) | (src[20]<<6)) & 0x07FF; - dst[15] = ((src[20]>>5) | (src[21]<<3)) & 0x07FF; + dst[0] = ((src[0]) | (src[1] << 8)) & 0x07FF; + dst[1] = ((src[1] >> 3) | (src[2] << 5)) & 0x07FF; + dst[2] = ((src[2] >> 6) | (src[3] << 2) | (src[4] << 10)) & 0x07FF; + dst[3] = ((src[4] >> 1) | (src[5] << 7)) & 0x07FF; + dst[4] = ((src[5] >> 4) | (src[6] << 4)) & 0x07FF; + dst[5] = ((src[6] >> 7) | (src[7] << 1) | (src[8] << 9)) & 0x07FF; + dst[6] = ((src[8] >> 2) | (src[9] << 6)) & 0x07FF; + dst[7] = ((src[9] >> 5) | (src[10] << 3)) & 0x07FF; + dst[8] = ((src[11]) | (src[12] << 8)) & 0x07FF; + dst[9] = ((src[12] >> 3) | (src[13] << 5)) & 0x07FF; + dst[10] = ((src[13] >> 6) | (src[14] << 2) | (src[15] << 10)) & 0x07FF; + dst[11] = ((src[15] >> 1) | (src[16] << 7)) & 0x07FF; + dst[12] = ((src[16] >> 4) | (src[17] << 4)) & 0x07FF; + dst[13] = ((src[17] >> 7) | (src[18] << 1) | (src[19] << 9)) & 0x07FF; + dst[14] = ((src[19] >> 2) | (src[20] << 6)) & 0x07FF; + dst[15] = ((src[20] >> 5) | (src[21] << 3)) & 0x07FF; // convert sbus to ppm #if PERIODIC_TELEMETRY - for (int channel=0; channel < SBUS_NB_CHANNEL; channel++) { + for (int channel = 0; channel < SBUS_NB_CHANNEL; channel++) { dstppm[channel] = USEC_OF_RC_PPM_TICKS(dst[channel]); } #endif diff --git a/sw/tools/qt_project.py b/sw/tools/qt_project.py index 49d6ade3a4..2d1e0f5e6b 100755 --- a/sw/tools/qt_project.py +++ b/sw/tools/qt_project.py @@ -80,6 +80,14 @@ ac_settings_modules = ac_node.attrib['settings_modules'].split(" ") for ac_settings_module in ac_settings_modules: if ac_settings_module[0] != '[': files_file.write(path.join("conf", ac_settings_module) + "\n") + + +#manually add msg def xml: +files_file.write("sw/ext/pprzlink/message_definitions/v1.0/messages.xml\n" ) +#manually active conf xml: +files_file.write(sys.argv[2] +"\n") + + # Parse the source files srcs_all = ap_srcs_list.readline().split(": ")[1] srcs = re.findall(r'([^ \t\n\r]+)', srcs_all)