diff --git a/.travis.yml b/.travis.yml index 7db4e3f0c..26ade6fb8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,7 @@ env: - TARGET=SPARKY - TARGET=STM32F3DISCOVERY - TARGET=ALIENWIIF1 + - TARGET=ALIENWIIF3 # We use cpp for unit tests, and c for the main project. language: cpp compiler: clang diff --git a/Makefile b/Makefile index 7670f66fc..ac610e7e2 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ SERIAL_DEVICE ?= /dev/ttyUSB0 FORKNAME = cleanflight -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 ALIENWIIF3 # Valid targets for OP BootLoader support OPBL_VALID_TARGETS = CC3D @@ -56,7 +56,7 @@ VPATH := $(SRC_DIR):$(SRC_DIR)/startup USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) -ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY)) +ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3)) STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver @@ -248,7 +248,8 @@ HIGHEND_SRC = flight/autotune.c \ telemetry/smartport.c \ sensors/sonar.c \ sensors/barometer.c \ - blackbox/blackbox.c + blackbox/blackbox.c \ + blackbox/blackbox_io.c VCP_SRC = \ vcp/hw_config.c \ @@ -298,7 +299,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ $(HIGHEND_SRC) \ $(COMMON_SRC) -ALIENWIIF1_SRC = $(NAZE_SRC) +ALIENWIIF1_SRC = $(NAZE_SRC) EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \ drivers/accgyro_adxl345.c \ @@ -397,6 +398,7 @@ CJMCU_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ blackbox/blackbox.c \ + blackbox/blackbox_io.c \ hardware_revision.c \ $(COMMON_SRC) @@ -493,6 +495,8 @@ SPARKY_SRC = \ $(COMMON_SRC) \ $(VCP_SRC) +ALIENWIIF3_SRC = $(SPARKY_SRC) + SPRACINGF3_SRC = \ $(STM32F30x_COMMON_SRC) \ drivers/accgyro_mpu6050.c \ diff --git a/docs/Battery.md b/docs/Battery.md index c233c1194..aac57415d 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -74,8 +74,19 @@ Enable current monitoring using the CLI command feature CURRENT_METER ``` +Configure the current meter type using the `current_meter_type` settings as per the following table. + +| Value | Sensor Type | +| ----- | ---------------------- | +| 0 | None | +| 1 | ADC/hardware sensor | +| 2 | Virtual sensor | + Configure capacity using the `battery_capacity` setting, which takes a value in mAh. +If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1` (this multiplies amperage sent to MSP by 10). + +### ADC Sensor The current meter may need to be configured so that the value read at the ADC input matches actual current draw. Just like you need a voltmeter to correctly calibrate your voltage reading you also need an ammeter to calibrate your current sensor. Use the following settings to adjust calibration. @@ -83,4 +94,23 @@ Use the following settings to adjust calibration. `current_meter_scale` `current_meter_offset` -If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1`. +### Virtual Sensor +The virtual sensor uses the throttle position to calculate as estimated current value. This is useful when a real sensor is not available. The following settings adjust the calibration. + +| Setting | Description | +| ----------------------------- | -------------------------------------------------------- | +| `current_meter_scale` | The throttle scaling factor [centiamps, i.e. 1/100th A] | +| `current_meter_offset` | The current at zero throttle [centiamps, i.e. 1/100th A] | + +If you know your current at zero throttle (Imin) and maximum throttle(Imax) you can calculate the scaling factors using the following formulas where Tmax is maximum throttle offset (i.e. for `max_throttle` = 1850, Tmax = 1850 - 1000 = 850): +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) +current_meter_offset = Imin * 100 +``` +e.g. For a maximum current of 34.2 A and minimum current of 2.8 A with `max_throttle` = 1850 +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) + = (34.2 - 2.8) * 100000 / (850 + (850 * 850 / 50)) + = 205 +current_meter_offset = Imin * 100 = 280 +``` diff --git a/docs/Board - AlienWii32.md b/docs/Board - AlienWii32.md index 912aaaf91..f442edcef 100644 --- a/docs/Board - AlienWii32.md +++ b/docs/Board - AlienWii32.md @@ -1,10 +1,12 @@ -# Board - AlienWii32 +# Board - AlienWii32 (ALIENWIIF1 and ALIENWIIF3 target) -The AlienWii32 is actually in prototype stage and only a few samples exist. There is some more field testing ongoing. The information below is preliminary and will be updated as needed. +The AlienWii32 is actually in prototype stage and only a few samples exist. There are two different variants and some more field testing with some users is ongoing. The information below is preliminary and will be updated as needed. Here are the hardware specifications: -- STM32F103CBT6 MCU +- STM32F103CBT6 MCU (ALIENWIIF1) +- STM32F303CCT6 MCU (ALIENWIIF3) +- optional integrated serial/ppm receiver (ALIENWIIF3 only, future enhancement) - MPU6050 accelerometer/gyro sensor unit - 8x 4.2A brushed ESCs, integrated, to run the strongest micro motors - extra-wide traces on the PCB, for maximum power throughput @@ -21,4 +23,4 @@ Here are the hardware specifications: Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. -The pin layout is very similar as the NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The AlienWii32 firmware will be built with TARGET=NAZE and OPTIONS="AlienWii32". The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. +The pin layout for the ALIENWIIF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENWIIF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The AlienWii32 firmware will be built as target ALIENWIIF1 or ALIENWIIF3. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index 580bd4c35..0f725e533 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -10,9 +10,6 @@ If issues are found with this board please report via the [github issue tracker] The board has a USB port directly connected to the processor. Other boards like the Naze and Flip32 have an on-board USB to uart adapter which connect to the processor's serial port instead. -Currently there is no support for virtual com port functionality on the CC3D which means that cleanflight -does not currently use the USB socket at all. Therefore, the communication with the board is achieved through a USB-UART adaptater connected to the Main port. - The board cannot currently be used for hexacopters/octocopters. Tricopter & Airplane support is untested, please report success or failure if you try it. @@ -71,13 +68,14 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL | Value | Identifier | Board Markings | Notes | | ----- | ------------ | -------------- | -----------------------------------------| -| 1 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS | -| 2 | USART3 | FLEX PORT | | -| 3 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | +| 1 | VCP | USB PORT | | +| 2 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS | +| 3 | USART3 | FLEX PORT | | +| 4 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud. -To connect the GUI to the flight controller you need additional hardware attached to the USART1 serial port (by default). +To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP). # Flex Port @@ -106,6 +104,8 @@ There are two primary ways to get Cleanflight onto a CC3D board. The entire flash ram on the target processor is flashed with a single image. +The image can be flashed by using a USB to UART adapter connected to the main port when the CC3D is put into the STM32 bootloader mode, achieved by powering on the CC3D with the SBL/3.3v pads bridged. + ## OpenPilot Bootloader compatible image mode. The initial section of flash ram on the target process is flashed with a bootloader which can then run the code in the @@ -114,4 +114,3 @@ remaining area of flash ram. The OpenPilot bootloader code also allows the remaining section of flash to be reconfigured and re-flashed by the OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter. -In this mode a USB to uart adapter is still required to connect to via the GUI or CLI. diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 99c770d78..8380bd193 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -104,4 +104,20 @@ only the gyros. The default is 75% For example, at a setting of "100" for "sensitivity_horizon", 100% self-leveling strength will be applied at center stick, 50% self-leveling will be applied at 50% stick, and no self-leveling will be applied at 100% stick. If sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63% -stick, and no self-leveling will be applied at 75% stick and onwards. \ No newline at end of file +stick, and no self-leveling will be applied at 75% stick and onwards. + +### PID controller 3, "MultiWii23" (default for the ALIENWIIF1 and ALIENWIIF3 targets) + +PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later. + +The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one. + +For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors. + +### PID controller 4, "MultiWiiHybrid" + +PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. + +This PID controller was initialy implementd for testing purposes but is also performing quite well. + +For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors. diff --git a/docs/Spektrum bind.md b/docs/Spektrum bind.md index d756f2de9..eb50eb31c 100644 --- a/docs/Spektrum bind.md +++ b/docs/Spektrum bind.md @@ -2,7 +2,7 @@ Spektrum bind with hardware bind plug support. -The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D targets. +The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D, ALIENWIIF1, ALIENWIIF3 targets. ## Configure the bind code @@ -46,4 +46,4 @@ http://wiki.openpilot.org/display/Doc/Spektrum+Satellite ### Supported Hardware -NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets (AlienWii32 with hardware bind plug) +NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets and ALIENWIIF1, ALIENWIIF3 targets with hardware bind plug diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 211e25911..2440af0a5 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" #include "version.h" @@ -37,8 +36,6 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "common/printf.h" - #include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" @@ -75,9 +72,8 @@ #include "config/config_master.h" #include "blackbox.h" +#include "blackbox_io.h" -#define BLACKBOX_BAUDRATE 115200 -#define BLACKBOX_INITIAL_PORT_MODE MODE_TX #define BLACKBOX_I_INTERVAL 32 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) @@ -255,9 +251,6 @@ extern uint8_t motorCount; //From mw.c: extern uint32_t currentTime; -// How many bytes should we transmit per loop iteration? -static uint8_t serialChunkSize = 16; - static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static struct { @@ -281,10 +274,6 @@ STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; -static serialPort_t *blackboxPort; -static portMode_t previousPortMode; -static uint32_t previousBaudRate; - /* * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated. * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs @@ -299,300 +288,6 @@ static blackboxValues_t blackboxHistoryRing[3]; // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old) static blackboxValues_t* blackboxHistory[3]; -static void blackboxWrite(uint8_t value) -{ - serialWrite(blackboxPort, value); -} - -static void _putc(void *p, char c) -{ - (void)p; - serialWrite(blackboxPort, c); -} - -//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) -static void blackboxPrintf(char *fmt, ...) -{ - va_list va; - va_start(va, fmt); - tfp_format(NULL, _putc, fmt, va); - va_end(va); -} - -// Print the null-terminated string 's' to the serial port and return the number of bytes written -static int blackboxPrint(const char *s) -{ - const char *pos = s; - - while (*pos) { - serialWrite(blackboxPort, *pos); - pos++; - } - - return pos - s; -} - -/** - * Write an unsigned integer to the blackbox serial port using variable byte encoding. - */ -static void writeUnsignedVB(uint32_t value) -{ - //While this isn't the final byte (we can only write 7 bits at a time) - while (value > 127) { - blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" - value >>= 7; - } - blackboxWrite(value); -} - -/** - * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. - */ -static void writeSignedVB(int32_t value) -{ - //ZigZag encode to make the value always positive - writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); -} - -/** - * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits - */ -static void writeTag2_3S32(int32_t *values) { - static const int NUM_FIELDS = 3; - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - BITS_2 = 0, - BITS_4 = 1, - BITS_6 = 2, - BITS_32 = 3 - }; - - enum { - BYTES_1 = 0, - BYTES_2 = 1, - BYTES_3 = 2, - BYTES_4 = 3 - }; - - int x; - int selector = BITS_2, selector2; - - /* - * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes - * below: - * - * Selector possibilities - * - * 2 bits per field ss11 2233, - * 4 bits per field ss00 1111 2222 3333 - * 6 bits per field ss11 1111 0022 2222 0033 3333 - * 32 bits per field sstt tttt followed by fields of various byte counts - */ - for (x = 0; x < NUM_FIELDS; x++) { - //Require more than 6 bits? - if (values[x] >= 32 || values[x] < -32) { - selector = BITS_32; - break; - } - - //Require more than 4 bits? - if (values[x] >= 8 || values[x] < -8) { - if (selector < BITS_6) - selector = BITS_6; - } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? - if (selector < BITS_4) - selector = BITS_4; - } - } - - switch (selector) { - case BITS_2: - blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); - break; - case BITS_4: - blackboxWrite((selector << 6) | (values[0] & 0x0F)); - blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); - break; - case BITS_6: - blackboxWrite((selector << 6) | (values[0] & 0x3F)); - blackboxWrite((uint8_t)values[1]); - blackboxWrite((uint8_t)values[2]); - break; - case BITS_32: - /* - * Do another round to compute a selector for each field, assuming that they are at least 8 bits each - * - * Selector2 field possibilities - * 0 - 8 bits - * 1 - 16 bits - * 2 - 24 bits - * 3 - 32 bits - */ - selector2 = 0; - - //Encode in reverse order so the first field is in the low bits: - for (x = NUM_FIELDS - 1; x >= 0; x--) { - selector2 <<= 2; - - if (values[x] < 128 && values[x] >= -128) - selector2 |= BYTES_1; - else if (values[x] < 32768 && values[x] >= -32768) - selector2 |= BYTES_2; - else if (values[x] < 8388608 && values[x] >= -8388608) - selector2 |= BYTES_3; - else - selector2 |= BYTES_4; - } - - //Write the selectors - blackboxWrite((selector << 6) | selector2); - - //And now the values according to the selectors we picked for them - for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { - switch (selector2 & 0x03) { - case BYTES_1: - blackboxWrite(values[x]); - break; - case BYTES_2: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - break; - case BYTES_3: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - break; - case BYTES_4: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - blackboxWrite(values[x] >> 24); - break; - } - } - break; - } -} - -/** - * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. - */ -static void writeTag8_4S16(int32_t *values) { - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - FIELD_ZERO = 0, - FIELD_4BIT = 1, - FIELD_8BIT = 2, - FIELD_16BIT = 3 - }; - - uint8_t selector, buffer; - int nibbleIndex; - int x; - - selector = 0; - //Encode in reverse order so the first field is in the low bits: - for (x = 3; x >= 0; x--) { - selector <<= 2; - - if (values[x] == 0) - selector |= FIELD_ZERO; - else if (values[x] < 8 && values[x] >= -8) - selector |= FIELD_4BIT; - else if (values[x] < 128 && values[x] >= -128) - selector |= FIELD_8BIT; - else - selector |= FIELD_16BIT; - } - - blackboxWrite(selector); - - nibbleIndex = 0; - buffer = 0; - for (x = 0; x < 4; x++, selector >>= 2) { - switch (selector & 0x03) { - case FIELD_ZERO: - //No-op - break; - case FIELD_4BIT: - if (nibbleIndex == 0) { - //We fill high-bits first - buffer = values[x] << 4; - nibbleIndex = 1; - } else { - blackboxWrite(buffer | (values[x] & 0x0F)); - nibbleIndex = 0; - } - break; - case FIELD_8BIT: - if (nibbleIndex == 0) - blackboxWrite(values[x]); - else { - //Write the high bits of the value first (mask to avoid sign extension) - blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); - //Now put the leftover low bits into the top of the next buffer entry - buffer = values[x] << 4; - } - break; - case FIELD_16BIT: - if (nibbleIndex == 0) { - //Write high byte first - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x]); - } else { - //First write the highest 4 bits - blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); - // Then the middle 8 - blackboxWrite(values[x] >> 4); - //Only the smallest 4 bits are still left to write - buffer = values[x] << 4; - } - break; - } - } - //Anything left over to write? - if (nibbleIndex == 1) - blackboxWrite(buffer); -} - -/** - * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is - * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero). - * - * valueCount must be 8 or less. - */ -static void writeTag8_8SVB(int32_t *values, int valueCount) -{ - uint8_t header; - int i; - - if (valueCount > 0) { - //If we're only writing one field then we can skip the header - if (valueCount == 1) { - writeSignedVB(values[0]); - } else { - //First write a one-byte header that marks which fields are non-zero - header = 0; - - // First field should be in low bits of header - for (i = valueCount - 1; i >= 0; i--) { - header <<= 1; - - if (values[i] != 0) - header |= 0x01; - } - - blackboxWrite(header); - - for (i = 0; i < valueCount; i++) - if (values[i] != 0) - writeSignedVB(values[i]); - } - } -} - static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) { switch (condition) { @@ -654,8 +349,9 @@ static void blackboxBuildConditionCache() blackboxConditionCache = 0; for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { - if (testBlackboxConditionUncached(cond)) + if (testBlackboxConditionUncached(cond)) { blackboxConditionCache |= 1 << cond; + } } } @@ -688,6 +384,7 @@ static void blackboxSetState(BlackboxState newState) break; case BLACKBOX_STATE_SHUTTING_DOWN: xmitState.u.startTime = millis(); + blackboxDeviceFlush(); break; default: ; @@ -702,23 +399,28 @@ static void writeIntraframe(void) blackboxWrite('I'); - writeUnsignedVB(blackboxIteration); - writeUnsignedVB(blackboxCurrent->time); + blackboxWriteUnsignedVB(blackboxIteration); + blackboxWriteUnsignedVB(blackboxCurrent->time); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_I[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_I[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) - writeSignedVB(blackboxCurrent->axisPID_D[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); + } + } - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->rcCommand[x]); + for (x = 0; x < 3; x++) { + blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]); + } - writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -727,41 +429,47 @@ static void writeIntraframe(void) * * Write 14 bits even if the number is negative (which would otherwise result in 32 bits) */ - writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); + blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { // 12bit value directly from ADC - writeUnsignedVB(blackboxCurrent->amperageLatest); + blackboxWriteUnsignedVB(blackboxCurrent->amperageLatest); } #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->magADC[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->magADC[x]); + } } #endif #ifdef BARO - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) - writeSignedVB(blackboxCurrent->BaroAlt); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { + blackboxWriteSignedVB(blackboxCurrent->BaroAlt); + } #endif - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->gyroData[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->gyroData[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->accSmooth[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]); + } //Motors can be below minthrottle when disarmed, but that doesn't happen much - writeUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); //Motors tend to be similar to each other - for (x = 1; x < motorCount; x++) - writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + for (x = 1; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxHistory[0]->servo[5] - 1500); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxHistory[0]->servo[5] - 1500); + } //Rotate our history buffers: @@ -789,36 +497,41 @@ static void writeInterframe(void) * Since the difference between the difference between successive times will be nearly zero (due to consistent * looptime spacing), use second-order differences. */ - writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); + blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) + for (x = 0; x < XYZ_AXIS_COUNT; x++) { deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x]; + } /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ - writeTag2_3S32(deltas); + blackboxWriteTag2_3S32(deltas); /* * The PID D term is frequently set to zero for yaw, which makes the result from the calculation * always zero. So don't bother recording D results when PID D terms are zero. */ - for (x = 0; x < XYZ_AXIS_COUNT; x++) - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) - writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); + } + } /* * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * can pack multiple values per byte: */ - for (x = 0; x < 4; x++) + for (x = 0; x < 4; x++) { deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + } - writeTag8_4S16(deltas); + blackboxWriteTag8_4S16(deltas); //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO int optionalFieldCount = 0; @@ -833,29 +546,35 @@ static void writeInterframe(void) #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) + for (x = 0; x < XYZ_AXIS_COUNT; x++) { deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x]; + } } #endif #ifdef BARO - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt; + } #endif - writeTag8_8SVB(deltas, optionalFieldCount); + blackboxWriteTag8_8SVB(deltas, optionalFieldCount); //Since gyros, accs and motors are noisy, base the prediction on the average of the history: - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); + } - for (x = 0; x < motorCount; x++) - writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + for (x = 0; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + } //Rotate our history buffers blackboxHistory[2] = blackboxHistory[1]; @@ -865,8 +584,10 @@ static void writeInterframe(void) static int gcd(int num, int denom) { - if (denom == 0) + if (denom == 0) { return num; + } + return gcd(denom, num % denom); } @@ -886,59 +607,12 @@ static void validateBlackboxConfig() } } -static void configureBlackboxPort(void) -{ - blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - - serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); - serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); - beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - } else { - blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); - - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - } - } - - /* - * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If - * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should - * transmit with each write. - * - * 9 / 1250 = 7200 / 1000000 - */ - serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); -} - -static void releaseBlackboxPort(void) -{ - serialSetMode(blackboxPort, previousPortMode); - serialSetBaudRate(blackboxPort, previousBaudRate); - - endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - - /* - * Normally this would be handled by mw.c, but since we take an unknown amount - * of time to shut down asynchronously, we're the only ones that know when to call it. - */ - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); - } -} - void startBlackbox(void) { if (blackboxState == BLACKBOX_STATE_STOPPED) { validateBlackboxConfig(); - configureBlackboxPort(); - - if (!blackboxPort) { + if (!blackboxDeviceOpen()) { blackboxSetState(BLACKBOX_STATE_DISABLED); return; } @@ -976,7 +650,7 @@ void finishBlackbox(void) * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. * Just give the port back and stop immediately. */ - releaseBlackboxPort(); + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } } @@ -986,8 +660,8 @@ static void writeGPSHomeFrame() { blackboxWrite('H'); - writeSignedVB(GPS_home[0]); - writeSignedVB(GPS_home[1]); + blackboxWriteSignedVB(GPS_home[0]); + blackboxWriteSignedVB(GPS_home[1]); //TODO it'd be great if we could grab the GPS current time and write that too gpsHistory.GPS_home[0] = GPS_home[0]; @@ -1006,15 +680,15 @@ static void writeGPSFrame() */ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { // Predict the time of the last frame in the main log - writeUnsignedVB(currentTime - blackboxHistory[1]->time); + blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time); } - writeUnsignedVB(GPS_numSat); - writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); - writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); - writeUnsignedVB(GPS_altitude); - writeUnsignedVB(GPS_speed); - writeUnsignedVB(GPS_ground_course); + blackboxWriteUnsignedVB(GPS_numSat); + blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); + blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); + blackboxWriteUnsignedVB(GPS_altitude); + blackboxWriteUnsignedVB(GPS_speed); + blackboxWriteUnsignedVB(GPS_ground_course); gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_coord[0] = GPS_coord[0]; @@ -1032,31 +706,39 @@ static void loadBlackboxState(void) blackboxCurrent->time = currentTime; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + } - for (i = 0; i < 4; i++) + for (i = 0; i < 4; i++) { blackboxCurrent->rcCommand[i] = rcCommand[i]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->gyroData[i] = gyroData[i]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->accSmooth[i] = accSmooth[i]; + } - for (i = 0; i < motorCount; i++) + for (i = 0; i < motorCount; i++) { blackboxCurrent->motor[i] = motor[i]; + } blackboxCurrent->vbatLatest = vbatLatestADC; blackboxCurrent->amperageLatest = amperageLatestADC; #ifdef MAG - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->magADC[i] = magADC[i]; + } #endif #ifdef BARO @@ -1097,8 +779,9 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he * the whole header. */ if (xmitState.u.fieldIndex == -1) { - if (xmitState.headerIndex >= headerCount) + if (xmitState.headerIndex >= headerCount) { return false; //Someone probably called us again after we had already completed transmission + } charsWritten = blackboxPrint("H Field "); charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); @@ -1109,15 +792,16 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he } else charsWritten = 0; - for (; xmitState.u.fieldIndex < fieldCount && charsWritten < serialChunkSize; xmitState.u.fieldIndex++) { + for (; xmitState.u.fieldIndex < fieldCount && charsWritten < blackboxWriteChunkSize; xmitState.u.fieldIndex++) { def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex); if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) { if (needComma) { blackboxWrite(','); charsWritten++; - } else + } else { needComma = true; + } // The first header is a field name if (xmitState.headerIndex == 0) { @@ -1163,7 +847,7 @@ static bool blackboxWriteSysinfo() } // How many bytes can we afford to transmit this loop? - xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64); + xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + blackboxWriteChunkSize, 64); // Most headers will consume at least 20 bytes so wait until we've built up that much link budget if (xmitState.u.serialBudget < 20) { @@ -1240,9 +924,10 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H vbatref:%u\n"); break; case 13: - blackboxPrintf("H currentMeter:%i,%i\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); + blackboxPrintf("H currentMeter:%d,%d\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); - xmitState.u.serialBudget -= strlen("H currentMeter:%i,%i\n"); + xmitState.u.serialBudget -= strlen("H currentMeter:%d,%d\n"); + break; default: return true; } @@ -1256,8 +941,9 @@ static bool blackboxWriteSysinfo() */ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) { - if (blackboxState != BLACKBOX_STATE_RUNNING) + if (blackboxState != BLACKBOX_STATE_RUNNING) { return; + } //Shared header for event frames blackboxWrite('E'); @@ -1266,21 +952,28 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) //Now serialize the data for this specific frame type switch (event) { case FLIGHT_LOG_EVENT_SYNC_BEEP: - writeUnsignedVB(data->syncBeep.time); + blackboxWriteUnsignedVB(data->syncBeep.time); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: blackboxWrite(data->autotuneCycleStart.phase); - blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0)); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: - blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleResult.flags); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; + case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: + blackboxWriteS16(data->autotuneTargets.currentAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); + blackboxWriteS16(data->autotuneTargets.firstPeakAngle); + blackboxWriteS16(data->autotuneTargets.secondPeakAngle); + break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); blackboxWrite(0); @@ -1320,11 +1013,13 @@ void handleBlackbox(void) * buffer. */ if (millis() > xmitState.u.startTime + 100) { - for (i = 0; i < serialChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) + for (i = 0; i < blackboxWriteChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { blackboxWrite(blackboxHeader[xmitState.headerIndex]); + } - if (blackboxHeader[xmitState.headerIndex] == '\0') + if (blackboxHeader[xmitState.headerIndex] == '\0') { blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO); + } } break; case BLACKBOX_STATE_SEND_FIELDINFO: @@ -1332,9 +1027,9 @@ void handleBlackbox(void) if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef GPS - if (feature(FEATURE_GPS)) + if (feature(FEATURE_GPS)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS); - else + } else #endif blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } @@ -1359,8 +1054,9 @@ void handleBlackbox(void) //On entry of this state, xmitState.headerIndex is 0 //Keep writing chunks of the system info headers until it returns true to signal completion - if (blackboxWriteSysinfo()) + if (blackboxWriteSysinfo()) { blackboxSetState(BLACKBOX_STATE_PRERUN); + } break; case BLACKBOX_STATE_PRERUN: blackboxSetState(BLACKBOX_STATE_RUNNING); @@ -1412,7 +1108,7 @@ void handleBlackbox(void) } break; case BLACKBOX_STATE_SHUTTING_DOWN: - //On entry of this state, startTime is set + //On entry of this state, startTime is set and a flush is performed /* * Wait for the log we've transmitted to make its way to the logger before we release the serial port, @@ -1420,8 +1116,8 @@ void handleBlackbox(void) * * Don't wait longer than it could possibly take if something funky happens. */ - if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) { - releaseBlackboxPort(); + if (millis() > xmitState.u.startTime + 200 || isBlackboxDeviceIdle()) { + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } break; @@ -1432,16 +1128,14 @@ void handleBlackbox(void) static bool canUseBlackboxWithCurrentConfiguration(void) { - if (!feature(FEATURE_BLACKBOX)) - return false; - - return true; + return feature(FEATURE_BLACKBOX); } void initBlackbox(void) { - if (canUseBlackboxWithCurrentConfiguration()) + if (canUseBlackboxWithCurrentConfiguration()) { blackboxSetState(BLACKBOX_STATE_STOPPED); - else + } else { blackboxSetState(BLACKBOX_STATE_DISABLED); + } } diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 26d862268..d328e71e2 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -101,6 +101,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, + FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -114,21 +115,31 @@ typedef struct flightLogEvent_autotuneCycleStart_t { uint8_t p; uint8_t i; uint8_t d; + uint8_t rising; } flightLogEvent_autotuneCycleStart_t; +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1 +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2 + typedef struct flightLogEvent_autotuneCycleResult_t { - uint8_t overshot; + uint8_t flags; uint8_t p; uint8_t i; uint8_t d; } flightLogEvent_autotuneCycleResult_t; +typedef struct flightLogEvent_autotuneTargets_t { + uint16_t currentAngle; + int8_t targetAngle, targetAngleAtPeak; + uint16_t firstPeakAngle, secondPeakAngle; +} flightLogEvent_autotuneTargets_t; + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_autotuneCycleStart_t autotuneCycleStart; flightLogEvent_autotuneCycleResult_t autotuneCycleResult; - + flightLogEvent_autotuneTargets_t autotuneTargets; } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c new file mode 100644 index 000000000..194c03c40 --- /dev/null +++ b/src/main/blackbox/blackbox_io.c @@ -0,0 +1,437 @@ +#include +#include + +#include "blackbox_io.h" + +#include "platform.h" +#include "version.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/color.h" + +#include "drivers/gpio.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/compass.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/accgyro.h" +#include "drivers/light_led.h" +#include "drivers/sound_beeper.h" + +#include "flight/flight.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "io/beeper.h" +#include "io/display.h" +#include "io/escservo.h" +#include "rx/rx.h" +#include "io/rc_controls.h" +#include "flight/mixer.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/autotune.h" +#include "flight/navigation.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" +#include "io/serial_cli.h" +#include "io/serial_msp.h" +#include "io/statusindicator.h" +#include "rx/msp.h" +#include "telemetry/telemetry.h" +#include "common/printf.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#define BLACKBOX_BAUDRATE 115200 +#define BLACKBOX_INITIAL_PORT_MODE MODE_TX + +// How many bytes should we transmit per loop iteration? +uint8_t blackboxWriteChunkSize = 16; + +static serialPort_t *blackboxPort; +static portMode_t previousPortMode; +static uint32_t previousBaudRate; + +void blackboxWrite(uint8_t value) +{ + serialWrite(blackboxPort, value); +} + +static void _putc(void *p, char c) +{ + (void)p; + blackboxWrite(c); +} + +//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) +void blackboxPrintf(char *fmt, ...) +{ + va_list va; + va_start(va, fmt); + tfp_format(NULL, _putc, fmt, va); + va_end(va); +} + +// Print the null-terminated string 's' to the serial port and return the number of bytes written +int blackboxPrint(const char *s) +{ + const char *pos = s; + + while (*pos) { + blackboxWrite(*pos); + pos++; + } + + return pos - s; +} + +/** + * Write an unsigned integer to the blackbox serial port using variable byte encoding. + */ +void blackboxWriteUnsignedVB(uint32_t value) +{ + //While this isn't the final byte (we can only write 7 bits at a time) + while (value > 127) { + blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" + value >>= 7; + } + blackboxWrite(value); +} + +/** + * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. + */ +void blackboxWriteSignedVB(int32_t value) +{ + //ZigZag encode to make the value always positive + blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); +} + +void blackboxWriteS16(int16_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); +} + +/** + * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits + */ +void blackboxWriteTag2_3S32(int32_t *values) { + static const int NUM_FIELDS = 3; + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + BITS_2 = 0, + BITS_4 = 1, + BITS_6 = 2, + BITS_32 = 3 + }; + + enum { + BYTES_1 = 0, + BYTES_2 = 1, + BYTES_3 = 2, + BYTES_4 = 3 + }; + + int x; + int selector = BITS_2, selector2; + + /* + * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes + * below: + * + * Selector possibilities + * + * 2 bits per field ss11 2233, + * 4 bits per field ss00 1111 2222 3333 + * 6 bits per field ss11 1111 0022 2222 0033 3333 + * 32 bits per field sstt tttt followed by fields of various byte counts + */ + for (x = 0; x < NUM_FIELDS; x++) { + //Require more than 6 bits? + if (values[x] >= 32 || values[x] < -32) { + selector = BITS_32; + break; + } + + //Require more than 4 bits? + if (values[x] >= 8 || values[x] < -8) { + if (selector < BITS_6) { + selector = BITS_6; + } + } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? + if (selector < BITS_4) { + selector = BITS_4; + } + } + } + + switch (selector) { + case BITS_2: + blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); + break; + case BITS_4: + blackboxWrite((selector << 6) | (values[0] & 0x0F)); + blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); + break; + case BITS_6: + blackboxWrite((selector << 6) | (values[0] & 0x3F)); + blackboxWrite((uint8_t)values[1]); + blackboxWrite((uint8_t)values[2]); + break; + case BITS_32: + /* + * Do another round to compute a selector for each field, assuming that they are at least 8 bits each + * + * Selector2 field possibilities + * 0 - 8 bits + * 1 - 16 bits + * 2 - 24 bits + * 3 - 32 bits + */ + selector2 = 0; + + //Encode in reverse order so the first field is in the low bits: + for (x = NUM_FIELDS - 1; x >= 0; x--) { + selector2 <<= 2; + + if (values[x] < 128 && values[x] >= -128) { + selector2 |= BYTES_1; + } else if (values[x] < 32768 && values[x] >= -32768) { + selector2 |= BYTES_2; + } else if (values[x] < 8388608 && values[x] >= -8388608) { + selector2 |= BYTES_3; + } else { + selector2 |= BYTES_4; + } + } + + //Write the selectors + blackboxWrite((selector << 6) | selector2); + + //And now the values according to the selectors we picked for them + for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { + switch (selector2 & 0x03) { + case BYTES_1: + blackboxWrite(values[x]); + break; + case BYTES_2: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + case BYTES_3: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + break; + case BYTES_4: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + blackboxWrite(values[x] >> 24); + break; + } + } + break; + } +} + +/** + * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. + */ +void blackboxWriteTag8_4S16(int32_t *values) { + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + FIELD_ZERO = 0, + FIELD_4BIT = 1, + FIELD_8BIT = 2, + FIELD_16BIT = 3 + }; + + uint8_t selector, buffer; + int nibbleIndex; + int x; + + selector = 0; + //Encode in reverse order so the first field is in the low bits: + for (x = 3; x >= 0; x--) { + selector <<= 2; + + if (values[x] == 0) { + selector |= FIELD_ZERO; + } else if (values[x] < 8 && values[x] >= -8) { + selector |= FIELD_4BIT; + } else if (values[x] < 128 && values[x] >= -128) { + selector |= FIELD_8BIT; + } else { + selector |= FIELD_16BIT; + } + } + + blackboxWrite(selector); + + nibbleIndex = 0; + buffer = 0; + for (x = 0; x < 4; x++, selector >>= 2) { + switch (selector & 0x03) { + case FIELD_ZERO: + //No-op + break; + case FIELD_4BIT: + if (nibbleIndex == 0) { + //We fill high-bits first + buffer = values[x] << 4; + nibbleIndex = 1; + } else { + blackboxWrite(buffer | (values[x] & 0x0F)); + nibbleIndex = 0; + } + break; + case FIELD_8BIT: + if (nibbleIndex == 0) { + blackboxWrite(values[x]); + } else { + //Write the high bits of the value first (mask to avoid sign extension) + blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); + //Now put the leftover low bits into the top of the next buffer entry + buffer = values[x] << 4; + } + break; + case FIELD_16BIT: + if (nibbleIndex == 0) { + //Write high byte first + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x]); + } else { + //First write the highest 4 bits + blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); + // Then the middle 8 + blackboxWrite(values[x] >> 4); + //Only the smallest 4 bits are still left to write + buffer = values[x] << 4; + } + break; + } + } + //Anything left over to write? + if (nibbleIndex == 1) { + blackboxWrite(buffer); + } +} + +/** + * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is + * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero). + * + * valueCount must be 8 or less. + */ +void blackboxWriteTag8_8SVB(int32_t *values, int valueCount) +{ + uint8_t header; + int i; + + if (valueCount > 0) { + //If we're only writing one field then we can skip the header + if (valueCount == 1) { + blackboxWriteSignedVB(values[0]); + } else { + //First write a one-byte header that marks which fields are non-zero + header = 0; + + // First field should be in low bits of header + for (i = valueCount - 1; i >= 0; i--) { + header <<= 1; + + if (values[i] != 0) { + header |= 0x01; + } + } + + blackboxWrite(header); + + for (i = 0; i < valueCount; i++) { + if (values[i] != 0) { + blackboxWriteSignedVB(values[i]); + } + } + } + } +} + +/** + * If there is data waiting to be written to the blackbox device, attempt to write that now. + */ +void blackboxDeviceFlush(void) +{ + //Presently a no-op on serial +} + +/** + * Attempt to open the logging device. Returns true if successful. + */ +bool blackboxDeviceOpen(void) +{ + blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + + serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); + serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); + beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + } else { + blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + } + } + + /* + * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If + * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should + * transmit with each write. + * + * 9 / 1250 = 7200 / 1000000 + */ + blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); + + return blackboxPort != NULL; +} + +void blackboxDeviceClose(void) +{ + serialSetMode(blackboxPort, previousPortMode); + serialSetBaudRate(blackboxPort, previousBaudRate); + + endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } +} + +bool isBlackboxDeviceIdle(void) +{ + return isSerialTransmitBufferEmpty(blackboxPort); +} diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h new file mode 100644 index 000000000..5abacecfa --- /dev/null +++ b/src/main/blackbox/blackbox_io.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +uint8_t blackboxWriteChunkSize; + +void blackboxWrite(uint8_t value); + +void blackboxPrintf(char *fmt, ...); +int blackboxPrint(const char *s); + +void blackboxWriteUnsignedVB(uint32_t value); +void blackboxWriteSignedVB(int32_t value); +void blackboxWriteS16(int16_t value); +void blackboxWriteTag2_3S32(int32_t *values); +void blackboxWriteTag8_4S16(int32_t *values); +void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); + +void blackboxDeviceFlush(void); +bool blackboxDeviceOpen(void); +void blackboxDeviceClose(void); + +bool isBlackboxDeviceIdle(void); diff --git a/src/main/config/config.c b/src/main/config/config.c index d3b7db24c..e99b0eb54 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -110,7 +110,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 89; +static const uint8_t EEPROM_CONF_VERSION = 90; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -197,6 +197,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) escAndServoConfig->minthrottle = 1150; escAndServoConfig->maxthrottle = 1850; escAndServoConfig->mincommand = 1000; + escAndServoConfig->servoCenterPulse = 1500; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) @@ -227,7 +228,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; - + batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; } void resetSerialConfig(serialConfig_t *serialConfig) @@ -453,20 +454,33 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif - // alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target) + // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets #ifdef ALIENWII32 featureSet(FEATURE_RX_SERIAL); featureSet(FEATURE_MOTOR_STOP); featureSet(FEATURE_FAILSAFE); + featureClear(FEATURE_VBAT); +#ifdef ALIENWIIF3 + masterConfig.serialConfig.serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY); +#else masterConfig.serialConfig.serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY); +#endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; + masterConfig.looptime = 2000; + currentProfile->pidController = 3; + currentProfile->pidProfile.P8[ROLL] = 36; + currentProfile->pidProfile.P8[PITCH] = 36; + currentProfile->failsafeConfig.failsafe_delay = 2; + currentProfile->failsafeConfig.failsafe_off_delay = 0; + currentProfile->failsafeConfig.failsafe_throttle = 1000; currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rollPitchRate = 20; currentControlRateProfile->yawRate = 60; + currentControlRateProfile->yawRate = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a09f6ec10..4edd6cbfa 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -235,7 +235,7 @@ static const uint16_t airPWM[] = { }; #endif -#ifdef SPARKY +#if defined(SPARKY) || defined(ALIENWIIF3) static const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 5c10e1674..f17c54f21 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -175,7 +175,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif -#ifdef SPARKY +#if defined(SPARKY) || defined(ALIENWIIF3) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // // 6 x 3 pin headers diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index f74a32c73..5fa40d624 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -175,12 +175,16 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; } +/* +* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination. +* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft +* (my best interpretation of scalar 'tiltAngle') or rename the function. +*/ int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) { return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); } - int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old) { int32_t result = 0; diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index b36168e6b..2770b6824 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -129,7 +129,7 @@ static autotunePhase_e nextPhase = FIRST_TUNE_PHASE; static float targetAngle = 0; static float targetAngleAtPeak; -static float firstPeakAngle, secondPeakAngle; // deci dgrees, 180 deg = 1800 +static float firstPeakAngle, secondPeakAngle; // in degrees typedef struct fp_pid { float p; @@ -161,11 +161,32 @@ static void autotuneLogCycleStart() eventData.p = pid.p * MULTIWII_P_MULTIPLIER; eventData.i = pid.i * MULTIWII_I_MULTIPLIER; eventData.d = pid.d; + eventData.rising = rising ? 1 : 0; blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); } } +static void autotuneLogAngleTargets(float currentAngle) +{ + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneTargets_t eventData; + + // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision: + eventData.targetAngle = (int) targetAngle; + // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement: + eventData.targetAngleAtPeak = (int) targetAngleAtPeak; + + // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again: + eventData.currentAngle = round(currentAngle * 10); + // the peak angles are only ever set to currentAngle, so they get the same treatment: + eventData.firstPeakAngle = round(firstPeakAngle * 10); + eventData.secondPeakAngle = round(secondPeakAngle * 10); + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData); + } +} + #endif static void startNewCycle(void) @@ -199,6 +220,7 @@ static void updateTargetAngle(void) float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle) { float currentAngle; + bool overshot; if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) { return errorAngle; @@ -229,6 +251,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif +#ifdef BLACKBOX + autotuneLogAngleTargets(currentAngle); +#endif + if (secondPeakAngle == 0) { // The peak will be when our angular velocity is negative. To be sure we are in the right place, // we also check to make sure our angle position is greater than zero. @@ -244,20 +270,22 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin switch (cycle) { case CYCLE_TUNE_I: // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } else { + overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2; + + if (overshot) { pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { pid.i = AUTOTUNE_MINIMUM_I_VALUE; } + } else { + pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; } #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1; + eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0; eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; @@ -300,7 +328,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - bool overshot = firstPeakAngle > targetAngleAtPeak; + overshot = firstPeakAngle > targetAngleAtPeak; if (overshot) { #ifdef DEBUG_AUTOTUNE debug[0] = 1; @@ -338,7 +366,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = overshot; + eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0); eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 7b994e5f6..cf2e66c3e 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -42,6 +42,7 @@ #include "io/gps.h" extern uint16_t cycleTime; +extern uint8_t motorCount; int16_t heading, magHold; int16_t axisPID[3]; @@ -287,8 +288,224 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } +#define GYRO_P_MAX 300 #define GYRO_I_MAX 256 +static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + + int axis, prop = 0; + int32_t rc, error, errorAngle; + int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; + static int16_t lastGyro[2] = { 0, 0 }; + static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; + int32_t delta; + + if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); + + // PITCH & ROLL + for (axis = 0; axis < 2; axis++) { + rc = rcCommand[axis] << 1; + error = rc - (gyroData[axis] / 4); + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here + if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; + + ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 + + PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6; + + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC + // 50 degrees max inclination +#ifdef GPS + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); + } +#endif + + errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here + + PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result + + int16_t limit = pidProfile->D8[PIDLEVEL] * 5; + PTermACC = constrain(PTermACC, -limit, +limit); + + ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result + + ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9); + PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); + } + + PTerm -= ((int32_t)(gyroData[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation + + delta = (gyroData[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastGyro[axis] = gyroData[axis]; + DTerm = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + + DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation + + axisPID[axis] = PTerm + ITerm - DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } + + //YAW + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else + error = rc - (gyroData[FD_YAW] / 4); +#endif + errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); + if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; + + PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; + + // Constrain YAW by D value if not servo driven in that case servolimits apply + if(motorCount > 3) { + int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; + PTerm = constrain(PTerm, -limit, +limit); + } + + ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); + + axisPID[FD_YAW] = PTerm + ITerm; + +#ifdef BLACKBOX + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = 0; +#endif +} + +static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + + int axis, prop; + int32_t rc, error, errorAngle; + int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; + static int16_t lastGyro[2] = { 0, 0 }; + static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; + int32_t deltaSum; + int32_t delta; + + UNUSED(controlRateConfig); + + // **** PITCH & ROLL **** + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] + + for (axis = 0; axis < 2; axis++) { + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC + // observe max inclination +#ifdef GPS + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); + } +#endif + + PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result + PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); + + errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp + ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; + } + if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO + error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; + error -= gyroData[axis] / 4; + + PTermGYRO = rcCommand[axis]; + + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp + if (ABS(gyroData[axis]) > (640 * 4)) + errorGyroI[axis] = 0; + + ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; + } + if (FLIGHT_MODE(HORIZON_MODE)) { + PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; + ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; + } else { + if (FLIGHT_MODE(ANGLE_MODE)) { + PTerm = PTermACC; + ITerm = ITermACC; + } else { + PTerm = PTermGYRO; + ITerm = ITermGYRO; + } + } + + PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + delta = (gyroData[axis] - lastGyro[axis]) / 4; + lastGyro[axis] = gyroData[axis]; + deltaSum = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + DTerm = (deltaSum * dynD8[axis]) / 32; + axisPID[axis] = PTerm + ITerm - DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } + //YAW + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else + error = rc - (gyroData[FD_YAW] / 4); +#endif + errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); + if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; + + PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; + + // Constrain YAW by D value if not servo driven in that case servolimits apply + if(motorCount > 3) { + int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; + PTerm = constrain(PTerm, -limit, +limit); + } + + ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); + + axisPID[FD_YAW] = PTerm + ITerm; + + +#ifdef BLACKBOX + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = 0; +#endif +} + static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -390,6 +607,12 @@ void setPIDController(int type) break; case 2: pid_controller = pidBaseflight; + break; + case 3: + pid_controller = pidMultiWii23; + break; + case 4: + pid_controller = pidMultiWiiHybrid; } } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 18c9e380f..f32237474 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -473,14 +473,14 @@ static void airplaneMixer(void) int16_t lFlap = determineServoMiddleOrForwardFromChannel(2); lFlap = constrain(lFlap, servoConf[2].min, servoConf[2].max); - lFlap = rxConfig->midrc - lFlap; + lFlap = escAndServoConfig->servoCenterPulse - lFlap; if (slow_LFlaps < lFlap) slow_LFlaps += airplaneConfig->flaps_speed; else if (slow_LFlaps > lFlap) slow_LFlaps -= airplaneConfig->flaps_speed; servo[2] = ((int32_t)servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += rxConfig->midrc; + servo[2] += escAndServoConfig->servoCenterPulse; } if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index d6ffa4926..66cd7db59 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -18,7 +18,10 @@ #pragma once typedef struct escAndServoConfig_s { + + // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c4e010cc5..f6e2984e0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -240,6 +240,7 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME lower limit should match code in the mixer, 1500 currently, @@ -316,10 +317,10 @@ const clivalue_t valueTable[] = { { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, 10, 50 }, - { "current_meter_scale", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, 1, 10000 }, - { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 }, + { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 }, + { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 3300 }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 }, - + { "current_meter_type", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterType, 0, CURRENT_SENSOR_MAX }, { "align_gyro", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 }, { "align_acc", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 }, @@ -383,7 +384,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 2 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 2b8ca9f64..a9eff4df2 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -200,6 +200,9 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_CF_SERIAL_CONFIG 54 #define MSP_SET_CF_SERIAL_CONFIG 55 +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -1024,16 +1027,26 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_BOARD_ALIGNMENT: - headSerialReply(3); + headSerialReply(6); serialize16(masterConfig.boardAlignment.rollDegrees); serialize16(masterConfig.boardAlignment.pitchDegrees); serialize16(masterConfig.boardAlignment.yawDegrees); break; - case MSP_CURRENT_METER_CONFIG: + case MSP_VOLTAGE_METER_CONFIG: headSerialReply(4); + serialize8(masterConfig.batteryConfig.vbatscale); + serialize8(masterConfig.batteryConfig.vbatmincellvoltage); + serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); + serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + + case MSP_CURRENT_METER_CONFIG: + headSerialReply(7); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); + serialize8(masterConfig.batteryConfig.currentMeterType); + serialize16(masterConfig.batteryConfig.batteryCapacity); break; case MSP_MIXER: @@ -1042,7 +1055,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RX_CONFIG: - headSerialReply(7); + headSerialReply(8); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); @@ -1370,9 +1383,18 @@ static bool processInCommand(void) masterConfig.boardAlignment.yawDegrees = read16(); break; + case MSP_SET_VOLTAGE_METER_CONFIG: + masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert + break; + case MSP_SET_CURRENT_METER_CONFIG: masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); + masterConfig.batteryConfig.currentMeterType = read8(); + masterConfig.batteryConfig.batteryCapacity = read16(); break; #ifndef USE_QUAD_MIXER_ONLY diff --git a/src/main/main.c b/src/main/main.c index a4fcfccb2..e8092aa59 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -291,7 +291,8 @@ void init(void) pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); - pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER); + pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) + && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125); @@ -305,7 +306,7 @@ void init(void) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; + pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwmRxInit(masterConfig.inputFilteringMode); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0fdca6d09..17ce1f7a1 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -172,7 +172,15 @@ void serialRxInit(rxConfig_t *rxConfig) bool isSerialRxFrameComplete(rxConfig_t *rxConfig) { - + /** + * FIXME: Each of the xxxxFrameComplete() methods MUST be able to survive being called without the + * corresponding xxxInit() method having been called first. + * + * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider + * + * A solution is for the ___Init() to configure the serialRxFrameComplete function pointer which + * should be used instead of the switch statement below. + */ switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b88e0c590..2e401969c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -68,11 +68,11 @@ typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers + uint8_t rssi_channel; + uint8_t rssi_scale; uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end - uint8_t rssi_channel; - uint8_t rssi_scale; } rxConfig_t; #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 86539cb3f..a367cb8f8 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -20,9 +20,14 @@ #include "common/maths.h" +#include "config/runtime_config.h" + #include "drivers/adc.h" #include "drivers/system.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "sensors/battery.h" // Battery monitoring stuff @@ -113,13 +118,29 @@ void updateCurrentMeter(int32_t lastUpdateAt) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; + int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + int32_t throttleFactor = 0; - amperageRaw -= amperageRaw / 8; - amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); - amperage = currentSensorToCentiamps(amperageRaw / 8); + switch(batteryConfig->currentMeterType) { + case CURRENT_SENSOR_ADC: + amperageRaw -= amperageRaw / 8; + amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); + amperage = currentSensorToCentiamps(amperageRaw / 8); + break; + case CURRENT_SENSOR_VIRTUAL: + amperage = (int32_t)batteryConfig->currentMeterOffset; + if(ARMING_FLAG(ARMED)) { + throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); + amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + } + break; + case CURRENT_SENSOR_NONE: + amperage = 0; + break; + } - mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhdrawnRaw / (3600 * 100); + mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; + mAhDrawn = mAhdrawnRaw / (3600 * 100); } uint8_t calculateBatteryPercentage(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 23f918c7c..60404b0f5 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -21,14 +21,22 @@ #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 +typedef enum { + CURRENT_SENSOR_NONE = 0, + CURRENT_SENSOR_ADC, + CURRENT_SENSOR_VIRTUAL, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL +} currentSensor_e; + typedef struct batteryConfig_s { uint8_t vbatscale; // adjust this to match battery voltage to reported value uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) - uint16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps + currentSensor_e currentMeterType; // type of current meter used, either ADC or virtual // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp diff --git a/src/main/target/ALIENWIIF3/system_stm32f30x.c b/src/main/target/ALIENWIIF3/system_stm32f30x.c new file mode 100644 index 000000000..fca696982 --- /dev/null +++ b/src/main/target/ALIENWIIF3/system_stm32f30x.c @@ -0,0 +1,372 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/src/main/target/ALIENWIIF3/system_stm32f30x.h b/src/main/target/ALIENWIIF3/system_stm32f30x.h new file mode 100644 index 000000000..4f999d305 --- /dev/null +++ b/src/main/target/ALIENWIIF3/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h new file mode 100644 index 000000000..009782a69 --- /dev/null +++ b/src/main/target/ALIENWIIF3/target.h @@ -0,0 +1,144 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AWF3" // AlienWii32 F3. + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_4 // Blue LEDs - PB4 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB +#define LED1_GPIO GPIOB +#define LED1_PIN Pin_5 // Green LEDs - PB5 +#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define USABLE_TIMER_CHANNEL_COUNT 11 + +// MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. +#define GYRO +#define USE_GYRO_MPU6050 + +#define GYRO_MPU6050_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_MPU6050 + +#define ACC_MPU6050_ALIGN CW270_DEG + +//#define BARO +//#define USE_BARO_MS5611 + +#define MAG +#define USE_MAG_AK8975 + +#define MAG_AK8975_ALIGN CW0_DEG_FLIP + +#define LED0 +#define LED1 + +#define USE_VCP +#define USE_USART1 // Conn 1 - TX (PB6) RX PB7 (AF7) +#define USE_USART2 // Input - RX (PA3) +#define USE_USART3 // Servo out - 10/RX (PB11) 11/TX (PB10) +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN GPIO_Pin_6 // PB6 +#define UART1_RX_PIN GPIO_Pin_7 // PB7 +#define UART1_GPIO GPIOB +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource6 +#define UART1_RX_PINSOURCE GPIO_PinSource7 + +#define UART2_TX_PIN GPIO_Pin_2 // PA2 - Clashes with PWM6 input. +#define UART2_RX_PIN GPIO_Pin_3 // PA3 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +// Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP? + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) + +#define I2C2_SCL_GPIO GPIOA +#define I2C2_SCL_GPIO_AF GPIO_AF_4 +#define I2C2_SCL_PIN GPIO_Pin_9 +#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 +#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA +#define I2C2_SDA_GPIO GPIOA +#define I2C2_SDA_GPIO_AF GPIO_AF_4 +#define I2C2_SDA_PIN GPIO_Pin_10 +#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 +#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA + + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +#define BLACKBOX +#define SERIAL_RX +#define GPS +#define DISPLAY + +#define LED_STRIP +#if 1 +// LED strip configuration using PWM motor output pin 5. +#define LED_STRIP_TIMER TIM16 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#endif + +#if 0 +// Alternate LED strip pin +// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 +#define LED_STRIP_TIMER TIM17 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL7 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource7 +#define WS2811_TIMER TIM17 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 +#define WS2811_DMA_CHANNEL DMA1_Channel7 +#define WS2811_IRQ DMA1_Channel7_IRQn +#endif + + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 + +// Hardware bind plug at PB12 (Pin 25) +#define BINDPLUG_PORT GPIOB +#define BINDPLUG_PIN Pin_12 + +// alternative defaults for AlienWii32 F3 target +#define ALIENWII32 +#define BRUSHED_MOTORS +#define HARDWARE_BIND_PLUG diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 217d14366..9209583ba 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -161,8 +161,10 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 -// alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target) +// alternative defaults for AlienWii32 F1 target #ifdef ALIENWII32 +#undef TARGET_BOARD_IDENTIFIER +#define TARGET_BOARD_IDENTIFIER "AWF1" // AlienWii32 F1. #define BRUSHED_MOTORS #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index b2e1e328c..bc2b7e467 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -54,6 +54,7 @@ extern "C" { extern "C" { bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations); + uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations); } typedef struct inclinationExpectation_s { @@ -90,6 +91,36 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards) } } +typedef struct inclinationAngleExpectations_s { + rollAndPitchInclination_t inclination; + uint16_t expected_angle; +} inclinationAngleExpectations_t; + +TEST(AltitudeHoldTest, TestCalculateTiltAngle) +{ + inclinationAngleExpectations_t inclinationAngleExpectations[] = { + { {0, 0}, 0}, + { {1, 0}, 1}, + { {0, 1}, 1}, + { {0, -1}, 1}, + { {-1, 0}, 1}, + { {-1, -2}, 2}, + { {-2, -1}, 2}, + { {1, 2}, 2}, + { {2, 1}, 2} + }; + + rollAndPitchInclination_t inclination = {0, 0}; + uint16_t tilt_angle = calculateTiltAngle(&inclination); + EXPECT_EQ(tilt_angle, 0); + + for (uint8_t i = 0; i < 9; i++) { + inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i]; + uint16_t result = calculateTiltAngle(&expectation->inclination); + EXPECT_EQ(expectation->expected_angle, result); + } +} + // STUBS extern "C" { diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 0d30723ec..52bd836d1 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -73,6 +73,9 @@ TEST(BatteryTest, BatteryADCToVoltage) extern "C" { +uint8_t armingFlags = 0; +int16_t rcCommand[4] = {0,0,0,0}; + uint16_t adcGetChannel(uint8_t channel) { UNUSED(channel);