Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-12 19:10:32 +03:00
Code Issues Wiki Activity

trashed old eeprom config struct and retarded eeprom code. replaced with config_t stuff to allow more dynamic changes

Browse source 
implemented cli (press # in serial to enter it). no commands except exit/version yet.
added uartPrint
fixed bug in task switching with missing breaks - was failing baro and perhaps mag readings
dynamic yaw direction, camtilt feature, camtrig feature.

ported some of 2.0-pre1 features:
* gyro smoothing
* baro/althold cleanup


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@102 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-03-05 02:01:26 +00:00
parent 14e12ce0bf
commit 9b48d45bca
14 changed files with 680 additions and 536 deletions
Download patch file Download diff file Expand all files Collapse all files

158
baseflight.uvgui.timecop
View file

@ -90,8 +90,8 @@
<MDIClientArea>
<RegID>0</RegID>
<MDITabState>
<Len>1325</Len>
<Data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ata>
<Len>1214</Len>
<Data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ata>
</MDITabState>
</MDIClientArea>
<ViewEx>
@ -8040,10 +8040,16 @@
<AlienFiles>
<Doc>
<Name>D:\fly_122\projects\baseflight\drv_system.h</Name>
<Name>D:\fly_122\projects\baseflight\drv_uart.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>1</TopLine>
<CurrentLine>15</CurrentLine>
<CurrentLine>9</CurrentLine>
</Doc>
<Doc>
<Name>C:\Keil\ARM\RV31\Inc\stdlib.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>477</TopLine>
<CurrentLine>477</CurrentLine>
</Doc>
</AlienFiles>
@ -8052,102 +8058,102 @@
<ActiveMDIGroup>0</ActiveMDIGroup>
<MDIGroup>
<Size>100</Size>
<ActiveTab>13</ActiveTab>
<ActiveTab>11</ActiveTab>
<Doc>
<Name>.\drv_pwm.c</Name>
<ColumnNumber>8</ColumnNumber>
<TopLine>258</TopLine>
<CurrentLine>258</CurrentLine>
</Doc>
<Doc>
<Name>.\config.c</Name>
<ColumnNumber>27</ColumnNumber>
<TopLine>103</TopLine>
<CurrentLine>115</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_system.c</Name>
<Name>.\cli.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
</Doc>
<Doc>
<Name>mw.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>225</TopLine>
<CurrentLine>253</CurrentLine>
</Doc>
<Doc>
<Name>.\main.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>10</TopLine>
<CurrentLine>24</CurrentLine>
</Doc>
<Doc>
<Name>.\mixer.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>92</TopLine>
<CurrentLine>108</CurrentLine>
</Doc>
<Doc>
<Name>.\serial.c</Name>
<ColumnNumber>12</ColumnNumber>
<TopLine>190</TopLine>
<CurrentLine>208</CurrentLine>
<ColumnNumber>0</ColumnNumber>
<TopLine>220</TopLine>
<CurrentLine>233</CurrentLine>
</Doc>
<Doc>
<Name>drv_system.h</Name>
<Name>.\drv_uart.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>110</TopLine>
<CurrentLine>144</CurrentLine>
</Doc>
<Doc>
<Name>drv_uart.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>1</TopLine>
<CurrentLine>15</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_bmp085.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>246</TopLine>
<CurrentLine>264</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_i2c.c</Name>
<ColumnNumber>1</ColumnNumber>
<TopLine>122</TopLine>
<CurrentLine>143</CurrentLine>
</Doc>
<Doc>
<Name>.\imu.c</Name>
<ColumnNumber>4</ColumnNumber>
<TopLine>55</TopLine>
<CurrentLine>73</CurrentLine>
<CurrentLine>9</CurrentLine>
</Doc>
<Doc>
<Name>.\startup_stm32f10x_md.s</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>147</TopLine>
<CurrentLine>180</CurrentLine>
<TopLine>133</TopLine>
<CurrentLine>133</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_mpu3050.c</Name>
<ColumnNumber>39</ColumnNumber>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<Name>.\mw.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>660</TopLine>
<CurrentLine>679</CurrentLine>
</Doc>
<Doc>
<Name>.\imu.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>67</TopLine>
<CurrentLine>89</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_i2c.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>122</TopLine>
<CurrentLine>131</CurrentLine>
</Doc>
<Doc>
<Name>board.h</Name>
<ColumnNumber>26</ColumnNumber>
<TopLine>14</TopLine>
<CurrentLine>38</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_system.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>34</TopLine>
<CurrentLine>43</CurrentLine>
</Doc>
<Doc>
<Name>C:\Keil\ARM\RV31\Inc\stdlib.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>477</TopLine>
<CurrentLine>477</CurrentLine>
</Doc>
<Doc>
<Name>.\sensors.c</Name>
<ColumnNumber>21</ColumnNumber>
<TopLine>183</TopLine>
<CurrentLine>214</CurrentLine>
<ColumnNumber>0</ColumnNumber>
<TopLine>168</TopLine>
<CurrentLine>203</CurrentLine>
</Doc>
<Doc>
<Name>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_flash.c</Name>
<ColumnNumber>34</ColumnNumber>
<TopLine>852</TopLine>
<CurrentLine>858</CurrentLine>
<Name>mw.h</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>187</TopLine>
<CurrentLine>200</CurrentLine>
</Doc>
<Doc>
<Name>.\drv_hmc5883l.c</Name>
<ColumnNumber>22</ColumnNumber>
<TopLine>13</TopLine>
<CurrentLine>27</CurrentLine>
<Name>.\config.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>92</TopLine>
<CurrentLine>103</CurrentLine>
</Doc>
<Doc>
<Name>.\mixer.c</Name>
<ColumnNumber>0</ColumnNumber>
<TopLine>235</TopLine>
<CurrentLine>248</CurrentLine>
</Doc>
<Doc>
<Name>.\main.c</Name>
<ColumnNumber>8</ColumnNumber>
<TopLine>16</TopLine>
<CurrentLine>25</CurrentLine>
</Doc>
</MDIGroup>
</MDIGroups>

130
baseflight.uvopt
View file

@ -157,38 +157,6 @@
<Name>-UV0168AVR -O238 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>114</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134231132</Address>
<ByteObject>0</ByteObject>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename></Filename>
<ExecCommand></ExecCommand>
<Expression>\\baseflight\config.c\114</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>77</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134230968</Address>
<ByteObject>0</ByteObject>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename></Filename>
<ExecCommand></ExecCommand>
<Expression>\\baseflight\config.c\77</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
@ -510,10 +478,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>8</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>10</TopLine>
<CurrentLine>24</CurrentLine>
<TopLine>16</TopLine>
<CurrentLine>25</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\main.c</PathWithFileName>
<FilenameWithoutPath>main.c</FilenameWithoutPath>
@ -524,10 +492,10 @@
<FileType>5</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>25</ColumnNumber>
<ColumnNumber>26</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<TopLine>14</TopLine>
<CurrentLine>38</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\board.h</PathWithFileName>
<FilenameWithoutPath>board.h</FilenameWithoutPath>
@ -538,10 +506,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>4</ColumnNumber>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>55</TopLine>
<CurrentLine>73</CurrentLine>
<TopLine>67</TopLine>
<CurrentLine>89</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\imu.c</PathWithFileName>
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
@ -554,8 +522,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<TopLine>602</TopLine>
<CurrentLine>634</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\mw.c</PathWithFileName>
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
@ -568,8 +536,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>225</TopLine>
<CurrentLine>253</CurrentLine>
<TopLine>187</TopLine>
<CurrentLine>200</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\mw.h</PathWithFileName>
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
@ -580,10 +548,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>21</ColumnNumber>
<ColumnNumber>18</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>183</TopLine>
<CurrentLine>214</CurrentLine>
<TopLine>124</TopLine>
<CurrentLine>148</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\sensors.c</PathWithFileName>
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
@ -596,8 +564,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>92</TopLine>
<CurrentLine>108</CurrentLine>
<TopLine>235</TopLine>
<CurrentLine>248</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\mixer.c</PathWithFileName>
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
@ -608,10 +576,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>12</ColumnNumber>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>190</TopLine>
<CurrentLine>208</CurrentLine>
<TopLine>220</TopLine>
<CurrentLine>233</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\serial.c</PathWithFileName>
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
@ -622,14 +590,28 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>27</ColumnNumber>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>103</TopLine>
<CurrentLine>115</CurrentLine>
<TopLine>92</TopLine>
<CurrentLine>103</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\config.c</PathWithFileName>
<FilenameWithoutPath>config.c</FilenameWithoutPath>
</File>
<File>
<GroupNumber>1</GroupNumber>
<FileNumber>0</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\cli.c</PathWithFileName>
<FilenameWithoutPath>cli.c</FilenameWithoutPath>
</File>
</Group>
<Group>
@ -643,10 +625,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>1</ColumnNumber>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>122</TopLine>
<CurrentLine>143</CurrentLine>
<CurrentLine>131</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_i2c.c</PathWithFileName>
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
@ -659,8 +641,8 @@
<Focus>0</Focus>
<ColumnNumber>8</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>258</TopLine>
<CurrentLine>258</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_pwm.c</PathWithFileName>
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
@ -687,8 +669,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>246</TopLine>
<CurrentLine>264</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_bmp085.c</PathWithFileName>
<FilenameWithoutPath>drv_bmp085.c</FilenameWithoutPath>
@ -715,8 +697,8 @@
<Focus>0</Focus>
<ColumnNumber>39</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_mpu3050.c</PathWithFileName>
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
@ -729,8 +711,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<TopLine>110</TopLine>
<CurrentLine>144</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_uart.c</PathWithFileName>
<FilenameWithoutPath>drv_uart.c</FilenameWithoutPath>
@ -743,8 +725,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<TopLine>34</TopLine>
<CurrentLine>43</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_system.c</PathWithFileName>
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
@ -757,8 +739,8 @@
<Focus>0</Focus>
<ColumnNumber>22</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>13</TopLine>
<CurrentLine>27</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\drv_hmc5883l.c</PathWithFileName>
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
@ -932,8 +914,8 @@
<Focus>0</Focus>
<ColumnNumber>34</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>852</TopLine>
<CurrentLine>858</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_flash.c</PathWithFileName>
<FilenameWithoutPath>stm32f10x_flash.c</FilenameWithoutPath>
@ -946,8 +928,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>147</TopLine>
<CurrentLine>180</CurrentLine>
<TopLine>133</TopLine>
<CurrentLine>133</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\startup_stm32f10x_md.s</PathWithFileName>
<FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>

10
baseflight.uvproj
View file

@ -431,6 +431,11 @@
<FileType>1</FileType>
<FilePath>.\config.c</FilePath>
</File>
<File>
<FileName>cli.c</FileName>
<FileType>1</FileType>
<FilePath>.\cli.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -980,6 +985,11 @@
<FileType>1</FileType>
<FilePath>.\config.c</FilePath>
</File>
<File>
<FileName>cli.c</FileName>
<FileType>1</FileType>
<FilePath>.\cli.c</FilePath>
</File>
</Files>
</Group>
<Group>

6
board.h
View file

@ -1,8 +1,10 @@
#pragma once
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "stm32f10x_conf.h"
#include "core_cm3.h"
@ -30,6 +32,10 @@ typedef enum {
FEATURE_VBAT = 1 << 1,
FEATURE_SERVO = 1 << 2,
FEATURE_DIGITAL_SERVO = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_CAMTRIG = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7,
} AvailableFeatures;
#define digitalHi(p, i) { p->BSRR = i; }

117
cli.c Normal file
View file

@ -0,0 +1,117 @@
#include "board.h"
#include "mw.h"
// we unset this on 'exit'
extern uint8_t cliMode;
static void cliExit(char *cmdline);
static void cliHelp(char *cmdline);
static void cliVersion(char *cmdline);
// buffer
char cliBuffer[32];
uint8_t bufferIndex = 0;
typedef struct {
char *name;
char *param;
void (*func)(char *cmdline);
} cliCmd;
// should be sorted a..z for bsearch()
const cliCmd cmdTable[] = {
{ "exit", "", cliExit },
{ "help", "", cliHelp },
{ "version", "", cliVersion },
};
#define CMD_COUNT (sizeof cmdTable / sizeof cmdTable[0])
static void cliPrompt(void)
{
uartPrint("\r\n# ");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
}
static int cliCompare(const void *a, const void *b)
{
const cliCmd *ca = a, *cb = b;
return strncasecmp(ca->name, cb->name, strlen(cb->name));
}
static void cliExit(char *cmdline)
{
uartPrint("Leaving CLI mode...\r\n");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
cliMode = 0;
}
static void cliHelp(char *cmdline)
{
uint8_t i = 0;
uartPrint("Available commands:\r\n");
for (i = 0; i < CMD_COUNT; i++) {
uartPrint((uint8_t *)cmdTable[i].name);
uartWrite(' ');
uartPrint((uint8_t *)cmdTable[i].param);
uartPrint("\r\n");
}
}
static void cliVersion(char *cmdline)
{
uartPrint("Afro32 CLI version 2.0-pre1");
}
void cliProcess(void)
{
while (uartAvailable()) {
uint8_t c = uartRead();
cliBuffer[bufferIndex++] = c;
if (bufferIndex == sizeof(cliBuffer)) {
bufferIndex--;
c = '\n';
}
if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed
cliCmd *cmd = NULL;
cliCmd target;
uartPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name));
else
uartPrint("ERR: Unknown command, try 'HELP'");
// 'exit' will reset this flag, so we don't need to print prompt again
if (cliMode)
cliPrompt();
} else if (c == 127) {
// backspace
if (bufferIndex > 1) {
cliBuffer[bufferIndex - 2] = 0;
uartPrint("\r# ");
uartPrint((uint8_t *)cliBuffer);
uartWrite(' ');
uartPrint("\r# ");
uartPrint((uint8_t *)cliBuffer);
bufferIndex -= 2;
}
} else if (c < 32 || c > 126) {
// non-printable ascii
bufferIndex--;
} else {
uartWrite(c);
}
}
}

178
config.c
View file

@ -5,93 +5,45 @@
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage
uint32_t enabledSensors = 0;
uint32_t enabledFeatures = 0;
config_t cfg;
static uint8_t checkNewConf = 152;
typedef struct eep_entry_t {
void *var;
uint8_t size;
} eep_entry_t;
// ************************************************************************************************************
// EEPROM Layout definition
// ************************************************************************************************************
volatile eep_entry_t eep_entry[] = {
{&checkNewConf, sizeof(checkNewConf)}
, {&enabledFeatures, sizeof(enabledFeatures)}
, {&mixerConfiguration, sizeof(mixerConfiguration)}
, {&P8, sizeof(P8)}
, {&I8, sizeof(I8)}
, {&D8, sizeof(D8)}
, {&rcRate8, sizeof(rcRate8)}
, {&rcExpo8, sizeof(rcExpo8)}
, {&rollPitchRate, sizeof(rollPitchRate)}
, {&yawRate, sizeof(yawRate)}
, {&dynThrPID, sizeof(dynThrPID)}
, {&accZero, sizeof(accZero)}
, {&magZero, sizeof(magZero)}
, {&accTrim, sizeof(accTrim)}
, {&activate1, sizeof(activate1)}
, {&activate2, sizeof(activate2)}
, {&powerTrigger1, sizeof(powerTrigger1)}
, {&wing_left_mid, sizeof(wing_left_mid)}
, {&wing_right_mid, sizeof(wing_right_mid)}
, {&tri_yaw_middle, sizeof(tri_yaw_middle)}
};
#define EEBLOCK_SIZE sizeof(eep_entry) / sizeof(eep_entry_t)
static uint32_t enabledSensors = 0;
static uint8_t checkNewConf = 2;
void readEEPROM(void)
{
uint8_t i, _address = eep_entry[0].size;
uint8_t i;
// Read flash
for (i = 1; i < EEBLOCK_SIZE; i++) {
memcpy(eep_entry[i].var, (char *)FLASH_WRITE_ADDR + _address, eep_entry[i].size);
_address += eep_entry[i].size;
}
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t));
#if defined(POWERMETER)
pAlarm = (uint32_t) powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
pAlarm = (uint32_t) cfg.powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
#endif
for (i = 0; i < 7; i++)
lookupRX[i] = (2500 + rcExpo8 * (i * i - 25)) * i * (int32_t) rcRate8 / 1250;
lookupRX[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 1250;
wing_left_mid = constrain(wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
wing_right_mid = constrain(wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
tri_yaw_middle = constrain(tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
cfg.wing_left_mid = constrain(cfg.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
cfg.wing_right_mid = constrain(cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
}
void writeParams(void)
{
FLASH_Status FLASHStatus;
uint32_t address;
uint8_t conf[256];
uint8_t *p = conf;
uint8_t i;
// TODO this is garbage. do it properly later using FLASH_ProgramHalfWord without caching shit.
for (i = 0; i < EEBLOCK_SIZE; i++) {
memcpy(p, eep_entry[i].var, eep_entry[i].size);
p += eep_entry[i].size;
}
p = conf;
FLASH_Status status;
uint32_t i;
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
if ((FLASHStatus = FLASH_ErasePage(FLASH_WRITE_ADDR)) == FLASH_COMPLETE) {
address = 0;
while (FLASHStatus == FLASH_COMPLETE && address < sizeof(eep_entry)) {
if ((FLASHStatus = FLASH_ProgramWord(FLASH_WRITE_ADDR + address, *(uint32_t *)((char *)p + address))) != FLASH_COMPLETE)
break;
address += 4;
}
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(config_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&cfg + i));
if (status != FLASH_COMPLETE)
break; // TODO: fail
}
}
FLASH_Lock();
@ -110,48 +62,62 @@ void checkFirstTime(void)
return;
// Default settings
mixerConfiguration = MULTITYPE_QUADX;
cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT | FEATURE_PPM);
P8[ROLL] = 40;
I8[ROLL] = 30;
D8[ROLL] = 23;
P8[PITCH] = 40;
I8[PITCH] = 30;
D8[PITCH] = 23;
P8[YAW] = 85;
I8[YAW] = 0;
D8[YAW] = 0;
P8[PIDALT] = 16;
I8[PIDALT] = 15;
D8[PIDALT] = 7;
P8[PIDGPS] = 10;
I8[PIDGPS] = 0;
D8[PIDGPS] = 0;
P8[PIDVEL] = 0;
I8[PIDVEL] = 0;
D8[PIDVEL] = 0;
P8[PIDLEVEL] = 90;
I8[PIDLEVEL] = 45;
D8[PIDLEVEL] = 100;
P8[PIDMAG] = 40;
rcRate8 = 45; // = 0.9 in GUI
rcExpo8 = 65;
rollPitchRate = 0;
yawRate = 0;
dynThrPID = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
cfg.P8[PITCH] = 40;
cfg.I8[PITCH] = 30;
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 0;
cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDGPS] = 50;
cfg.I8[PIDGPS] = 0;
cfg.D8[PIDGPS] = 15;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.P8[PIDLEVEL] = 90;
cfg.I8[PIDLEVEL] = 45;
cfg.D8[PIDLEVEL] = 100;
cfg.P8[PIDMAG] = 40;
cfg.rcRate8 = 45; // = 0.9 in GUI
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
for (i = 0; i < CHECKBOXITEMS; i++) {
activate1[i] = 0;
activate2[i] = 0;
cfg.activate1[i] = 0;
cfg.activate2[i] = 0;
}
accTrim[0] = 0;
accTrim[1] = 0;
powerTrigger1 = 0;
cfg.accTrim[0] = 0;
cfg.accTrim[1] = 0;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.powerTrigger1 = 0;
wing_left_mid = WING_LEFT_MID;
wing_right_mid = WING_RIGHT_MID;
tri_yaw_middle = TRI_YAW_MIDDLE;
// Radio/ESC
cfg.midrc = 1500;
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
// servos
cfg.yaw_direction = 1;
cfg.wing_left_mid = 1500;
cfg.wing_right_mid = 1500;
cfg.tri_yaw_middle = 1500;
// gimbal
cfg.tilt_pitch_prop = 10;
cfg.tilt_roll_prop = 10;
writeParams();
}
@ -173,20 +139,20 @@ void sensorsClear(uint32_t mask)
bool feature(uint32_t mask)
{
return enabledFeatures & mask;
return cfg.enabledFeatures & mask;
}
void featureSet(uint32_t mask)
{
enabledFeatures |= mask;
cfg.enabledFeatures |= mask;
}
void featureClear(uint32_t mask)
{
enabledFeatures &= ~(mask);
cfg.enabledFeatures &= ~(mask);
}
void featureClearAll()
{
enabledFeatures = 0;
cfg.enabledFeatures = 0;
}

6
drv_uart.c
View file

@ -139,3 +139,9 @@ void uartWrite(uint8_t ch)
if (!(DMA1_Channel4->CCR & 1))
uartTxDMA();
}
void uartPrint(uint8_t *str)
{
while (*str)
uartWrite(*(str++));
}

1
drv_uart.h
View file

@ -5,3 +5,4 @@ uint16_t uartAvailable(void);
uint8_t uartRead(void);
uint8_t uartReadPoll(void);
void uartWrite(uint8_t ch);
void uartPrint(uint8_t *str);

77
imu.c
View file

@ -5,21 +5,17 @@
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int16_t acc_25deg = 0;
int32_t pressure = 0;
int32_t BaroAlt;
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int32_t EstVelocity;
// **************
// gyro+acc IMU
// **************
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t accZero[3] = { 0, 0, 0 };
int16_t magZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int8_t smallAngle25 = 1;
@ -71,7 +67,21 @@ void computeIMU(void)
accADC[axis] = 0;
}
if (mixerConfiguration == MULTITYPE_TRI) {
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
static int16_t gyroSmooth[3] = { 0, 0, 0 };
if (Smoothing[0] == 0) {
// initialize
Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
Smoothing[axis] = constrain(Smoothing[axis], 1, 100); // Avoid to divide with Zero
gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis];
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
gyroYawSmooth = gyroData[YAW];
}
@ -287,76 +297,53 @@ static void getEstimatedAttitude(void)
}
}
float InvSqrt(float x)
{
union {
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x)
{
return x * x;
}
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
#define Kp1 5.5f // PI observer velocity gain
#define Kp2 10.0f // PI observer position gain
#define Ki 0.01f // PI observer integral gain (bias cancellation)
#define dt (UPDATE_INTERVAL / 1000000.0f)
void getEstimatedAltitude(void)
{
static uint8_t inited = 0;
uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx = 0;
static int8_t BaroHistIdx;
int32_t BaroHigh, BaroLow;
int32_t temp32;
int16_t last;
if (currentTime < deadLine)
return;
deadLine = currentTime + UPDATE_INTERVAL;
if (!inited) {
inited = 1;
EstAlt = BaroAlt;
}
//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
BaroHistTab[BaroHistIdx] = BaroAlt;
BaroHigh = 0;
BaroLow = 0;
BaroPID = 0;
for (temp32 = 0; temp32 < BARO_TAB_SIZE / 2; temp32++) {
BaroHigh += BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum last half samples
BaroLow += BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum older samples
}
last = BaroHistTab[BaroHistIdx];
BaroHistTab[BaroHistIdx] = BaroAlt / 10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
BaroHistIdx++;
if (BaroHistIdx >= BARO_TAB_SIZE)
BaroHistIdx = 0;
temp32 = D8[PIDALT] * (BaroHigh - BaroLow) / 400;
BaroPID = 0;
//D
temp32 = cfg.D8[PIDALT] * (BaroHigh - BaroLow) / 40;
BaroPID -= temp32;
EstAlt = BaroHigh / (BARO_TAB_SIZE / 2);
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
temp32 = constrain(AltHold - EstAlt, -1000, 1000);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10)
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
// P
BaroPID += P8[PIDALT] * constrain(temp32, (-2) * P8[PIDALT], 2 * P8[PIDALT]) / 100;
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
// I
errorAltitudeI += temp32 * I8[PIDALT] / 50;
errorAltitudeI += temp32 * cfg.I8[PIDALT] / 50;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
temp32 = errorAltitudeI / 500; // I in range +/-60
BaroPID += temp32;

121
mixer.c
View file

@ -4,17 +4,17 @@
static uint8_t numberMotor = 4;
int16_t motor[8];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
uint8_t mixerConfiguration = MULTITYPE_TRI;
uint16_t wing_left_mid = WING_LEFT_MID;
uint16_t wing_right_mid = WING_RIGHT_MID;
uint16_t tri_yaw_middle = TRI_YAW_MIDDLE;
void mixerInit(void)
{
if (mixerConfiguration == MULTITYPE_BI || mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_GIMBAL || mixerConfiguration == MULTITYPE_FLYING_WING)
// enable servos for mixes that require them. note, this shifts motor counts.
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
featureSet(FEATURE_SERVO);
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CAMTRIG))
featureSet(FEATURE_SERVO);
switch (mixerConfiguration) {
switch (cfg.mixerConfiguration) {
case MULTITYPE_GIMBAL:
numberMotor = 0;
break;
@ -54,10 +54,10 @@ void writeServos(void)
if (!feature(FEATURE_SERVO))
return;
if (mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_BI) {
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) {
/* One servo on Motor #4 */
pwmWrite(0, servo[4]);
if (mixerConfiguration == MULTITYPE_BI)
if (cfg.mixerConfiguration == MULTITYPE_BI)
pwmWrite(1, servo[5]);
} else {
/* Two servos for camstab or FLYING_WING */
@ -89,7 +89,7 @@ void writeAllMotors(int16_t mc)
writeMotors();
}
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + cfg.yaw_direction * axisPID[YAW] * Z
void mixTable(void)
{
@ -104,20 +104,20 @@ void mixTable(void)
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
switch (mixerConfiguration) {
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI:
motor[0] = PIDMIX(+1, 0, 0); //LEFT
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
servo[4] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
servo[5] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
servo[4] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
servo[5] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[4] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
break;
case MULTITYPE_QUADP:
@ -209,75 +209,78 @@ void mixTable(void)
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
servo[0] = constrain(TILT_PITCH_MIDDLE + cfg.tilt_pitch_prop * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(TILT_ROLL_MIDDLE + cfg.tilt_roll_prop * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC);
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
break;
}
#ifdef SERVO_TILT
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500;
if (rcOptions[BOXCAMSTAB]) {
servo[0] += TILT_PITCH_PROP * angle[PITCH] / 16;
servo[1] += TILT_ROLL_PROP * angle[ROLL] / 16;
}
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
#endif
#if defined(CAMTRIG)
if (camCycle == 1) {
if (camState == 0) {
servo[2] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
} else if (camState == 1) {
if ((millis() - camTime) > CAM_TIME_HIGH) {
servo[2] = CAM_SERVO_LOW;
camState = 2;
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500;
if (rcOptions[BOXCAMSTAB]) {
servo[0] += cfg.tilt_pitch_prop * angle[PITCH] / 16;
servo[1] += cfg.tilt_roll_prop * angle[ROLL] / 16;
}
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
}
// do camtrig (this doesn't actually work)
if (feature(FEATURE_CAMTRIG)) {
if (camCycle == 1) {
if (camState == 0) {
servo[2] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
}
} else { //camState ==2
if ((millis() - camTime) > CAM_TIME_LOW) {
camState = 0;
camCycle = 0;
} else if (camState == 1) {
if ((millis() - camTime) > CAM_TIME_HIGH) {
servo[2] = CAM_SERVO_LOW;
camState = 2;
camTime = millis();
}
} else { //camState ==2
if ((millis() - camTime) > CAM_TIME_LOW) {
camState = 0;
camCycle = 0;
}
}
}
if (rcOptions[BOXCAMTRIG])
camCycle = 1;
}
if (rcOptions[BOXCAMTRIG])
camCycle = 1;
#endif
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - MAXTHROTTLE;
motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);
if ((rcData[THROTTLE]) < MINCHECK)
#ifndef MOTOR_STOP
motor[i] = MINTHROTTLE;
#else
motor[i] = MINCOMMAND;
#endif
if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - cfg.maxthrottle;
motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle);
if ((rcData[THROTTLE]) < MINCHECK) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = cfg.minthrottle;
else
motor[i] = cfg.mincommand;
}
if (armed == 0)
motor[i] = MINCOMMAND;
motor[i] = cfg.mincommand;
}
#if (LOG_VALUES == 2) || defined(POWERMETER_SOFT)

139
mw.c
View file

@ -1,7 +1,7 @@
#include "board.h"
#include "mw.h"
// February 2012 V1.dev
// March 2012 V2.0_pre_version_1
#define CHECKBOXITEMS 11
#define PIDITEMS 8
@ -24,17 +24,17 @@ volatile int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
int16_t rcData[8]; // interval [1000;2000]
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint8_t rcRate8;
uint8_t rcExpo8;
//uint8_t rcRate8;
//uint8_t rcExpo8;
int16_t lookupRX[7]; // lookup table for expo & RC rate
uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t rollPitchRate;
uint8_t yawRate;
uint8_t dynThrPID;
uint8_t activate1[CHECKBOXITEMS];
uint8_t activate2[CHECKBOXITEMS];
// uint8_t rollPitchRate;
// uint8_t yawRate;
// uint8_t dynThrPID;
// uint8_t activate1[CHECKBOXITEMS];
// uint8_t activate2[CHECKBOXITEMS];
uint8_t rcOptions[CHECKBOXITEMS];
uint8_t okToArm = 0;
uint8_t accMode = 0; // if level mode is a activated
@ -54,6 +54,8 @@ uint8_t GPS_fix, GPS_fix_home = 0;
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // in meters
int16_t GPS_directionToHome = 0; // in degrees
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
@ -73,7 +75,7 @@ uint16_t AccInflightCalibrationActive = 0;
uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
uint8_t powerTrigger1 = 0; // trigger for alarm based on power consumption
// uint8_t powerTrigger1 = 0;
uint16_t powerValue = 0; // last known current
uint16_t intPowerMeterSum, intPowerTrigger1;
@ -117,13 +119,13 @@ void annexCode(void)
if (rcData[THROTTLE] < 1500) {
prop2 = 100;
} else if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t) dynThrPID *(rcData[THROTTLE] - 1500) / 500;
prop2 = 100 - (uint16_t) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500;
} else {
prop2 = 100 - dynThrPID;
prop2 = 100 - cfg.dynThrPID;
}
for (axis = 0; axis < 3; axis++) {
uint16_t tmp = min(abs(rcData[axis] - MIDRC), 500);
uint16_t tmp = min(abs(rcData[axis] - cfg.midrc), 500);
#if defined(DEADBAND)
if (tmp > DEADBAND) {
tmp -= DEADBAND;
@ -134,18 +136,18 @@ void annexCode(void)
if (axis != 2) { //ROLL & PITCH
uint16_t tmp2 = tmp / 100;
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
prop1 = 100 - (uint16_t) rollPitchRate *tmp / 500;
prop1 = 100 - (uint16_t) cfg.rollPitchRate *tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { //YAW
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t) yawRate * tmp / 500;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) D8[axis] * prop1 / 100;
if (rcData[axis] < MIDRC)
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
rcCommand[THROTTLE] = MINTHROTTLE + (int32_t) (MAXTHROTTLE - MINTHROTTLE) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK);
rcCommand[THROTTLE] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK);
if (headFreeMode) {
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
@ -251,7 +253,7 @@ void annexCode(void)
#if defined(POWERMETER)
intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV);
intPowerTrigger1 = powerTrigger1 * PLEVELSCALE;
intPowerTrigger1 = cfg.powerTrigger1 * PLEVELSCALE;
#endif
#ifdef LCD_TELEMETRY_AUTO
@ -392,7 +394,7 @@ void loop(void)
}
}
#endif
else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) {
else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
if (rcOptions[BOXARM] && okToArm) {
armed = 1;
headFreeModeHold = heading;
@ -431,25 +433,25 @@ void loop(void)
calibratingM = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
accTrim[PITCH] += 2;
cfg.accTrim[PITCH] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[PITCH] < MINCHECK) {
accTrim[PITCH] -= 2;
cfg.accTrim[PITCH] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] > MAXCHECK) {
accTrim[ROLL] += 2;
cfg.accTrim[ROLL] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] < MINCHECK) {
accTrim[ROLL] -= 2;
cfg.accTrim[ROLL] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
@ -483,7 +485,7 @@ void loop(void)
#endif
for(i = 0; i < CHECKBOXITEMS; i++) {
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & activate2[i]);
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
}
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
@ -512,7 +514,6 @@ void loop(void)
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
EstVelocity = 0;
BaroPID = 0;
}
} else
@ -539,16 +540,21 @@ void loop(void)
} else
GPSModeHome = 0;
if (rcOptions[BOXGPSHOLD]) {
GPSModeHold = 1;
} else
if (GPSModeHold == 0) {
GPSModeHold = 1;
GPS_latitude_hold = GPS_latitude;
GPS_longitude_hold = GPS_longitude;
}
} else {
GPSModeHold = 0;
}
#endif
if (rcOptions[BOXPASSTHRU]) {
passThruMode = 1;
} else
passThruMode = 0;
} else { // not in rc loop
static int8_t taskOrder = 0; //never call all function in the same loop
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
taskOrder++;
@ -559,10 +565,19 @@ void loop(void)
taskOrder++;
if (sensors(SENSOR_BARO))
Baro_update();
break;
case 2:
taskOrder++;
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
break;
case 3:
taskOrder++;
#if GPS
GPS_NewData();
#endif
break;
default:
taskOrder = 0;
break;
@ -584,7 +599,7 @@ void loop(void)
if (dif >= +180)
dif -= 360;
if (smallAngle25)
rcCommand[YAW] -= dif * P8[PIDMAG] / 30; //18 deg
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; //18 deg
} else
magHold = heading;
}
@ -592,23 +607,30 @@ void loop(void)
if (sensors(SENSOR_BARO)) {
if (baroMode) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
EstVelocity = 0;
BaroPID = 0;
baroMode = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
#if GPS
if ((GPSModeHome == 1)) {
float radDiff = (GPS_directionToHome - heading) * 0.0174533f;
GPS_angle[ROLL] = constrain(P8[PIDGPS] * sinf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination
GPS_angle[PITCH] = constrain(P8[PIDGPS] * cosf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // max inclination = D deg
} else {
GPS_angle[ROLL] = 0;
uint16_t GPS_dist;
int16_t GPS_dir;
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
if (GPSModeHome == 1) {
GPS_dist = GPS_distanceToHome;
GPS_dir = GPS_directionToHome;
}
if (GPSModeHold == 1) {
GPS_dist = GPS_distanceToHold;
GPS_dir = GPS_directionToHold;
}
float radDiff = (GPS_dir - heading) * 0.0174533f;
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
}
#endif
@ -616,18 +638,18 @@ void loop(void)
for (axis = 0; axis < 3; axis++) {
if (accMode == 1 && axis < 2) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + accTrim[axis]; //16 bits is ok here
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t) angle[axis] * P8[PIDLEVEL] / 100;
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTerm = (int32_t) errorAngle * P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm, -D8[PIDLEVEL] * 5, +D8[PIDLEVEL] * 5);
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
ITerm = ((int32_t) errorAngleI[axis] * I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { //ACRO MODE or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
@ -635,7 +657,7 @@ void loop(void)
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] / 125 * I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
@ -653,23 +675,4 @@ void loop(void)
mixTable();
writeServos();
writeMotors();
#if GPS
while (SerialAvailable(GPS_SERIAL)) {
if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
if (GPS_update == 1)
GPS_update = 0;
else
GPS_update = 1;
if (GPS_fix == 1 && GPS_numSat == 4) {
if (GPS_fix_home == 0) {
GPS_fix_home = 1;
GPS_latitude_home = GPS_latitude;
GPS_longitude_home = GPS_longitude;
}
GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome);
}
}
}
#endif
}

135
mw.h
View file

@ -3,23 +3,9 @@
#define MINCHECK 1100
#define MAXCHECK 1900
#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction
//#define YAW_DIRECTION -1
/* 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 */
#define MINCOMMAND 1000
/* 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 */
//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
//#define MINTHROTTLE 1220
#define MINTHROTTLE 1150
/* this is the maximum value for the ESCs at full power this value can be increased up to 2000 */
#define MAXTHROTTLE 1850
/* some radios have not a neutral point centered on 1500. can be changed here */
#define MIDRC 1500
// #define MINCOMMAND 1000
/* This option should be uncommented if ACC Z is accurate enough when motors are running*/
/* should now be ok with BMA020 and BMA180 ACC */
@ -41,7 +27,6 @@
/* you can change the tricopter servo travel here */
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000
#define TRI_YAW_MIDDLE 1500 // tail servo center pos. - use this for initial trim; later trim midpoint via LCD
/* Flying Wing: you can change change servo orientation and servo min/max values here */
/* valid for all flight modes, even passThrough mode */
@ -50,8 +35,6 @@
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation)
#define ROLL_DIRECTION_L 1 // left servo - roll orientation
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
#define WING_LEFT_MID 1500 // left servo center pos. - use this for initial trim; later trim midpoint via LCD
#define WING_RIGHT_MID 1500 // right servo center pos. - use this for initial trim; later trim midpoint via LCD
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000]
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000]
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]
@ -60,28 +43,39 @@
/* The following lines apply only for a pitch/roll tilt stabilization system
On promini board, it is not compatible with config with 6 motors or more
Uncomment the first line to activate it */
//#define SERVO_TILT
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000
#define TILT_PITCH_MIDDLE 1500 //servo neutral value
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement
#define TILT_ROLL_MIN 1020
#define TILT_ROLL_MAX 2000
#define TILT_ROLL_MIDDLE 1500
#define TILT_ROLL_PROP 10
/* experimental
camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
/* for V BAT monitoring
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
with R1=33k and R2=51k
vbat = [0;1023]*16/VBATSCALE */
#define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage
#define VBATSCALE 151 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99 // 9.9V
#define NO_VBAT 16 // Avoid beeping without any battery
#define VERSION 19
// Moving Average Gyros by Magnetron1 (Michele Ardito) ########## beta
//#define MMGYRO // Active Moving Average Function for Gyros
//#define MMGYROVECTORLENGHT 10 // Lenght of Moving Average Vector
// Moving Average ServoGimbal Signal Output
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector
#define VERSION 201
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
@ -99,9 +93,11 @@ typedef enum MultiType
MULTITYPE_OCTOX8 = 11, // XK
MULTITYPE_OCTOFLATP = 12, // XL the GUI is the same for all 8 motor configs
MULTITYPE_OCTOFLATX = 13, // XM the GUI is the same for all 8 motor configs
// XN missing for some reason??
MULTITYPE_VTAIL4 = 15, // XO
MULTITYPE_LAST = 16
MULTITYPE_AIRPLANE = 14, // XN
MULTITYPE_HELI_120_CCPM = 15, // XO simple model
MULTITYPE_HELI_90_DEG = 16, // XP simple model
MULTITYPE_VTAIL4 = 17, // XO
MULTITYPE_LAST = 18
} MultiType;
/*********** RC alias *****************/
@ -120,16 +116,16 @@ typedef enum MultiType
#define PIDLEVEL 6
#define PIDMAG 7
#define BOXACC 0
#define BOXBARO 1
#define BOXMAG 2
#define BOXCAMSTAB 3
#define BOXCAMTRIG 4
#define BOXARM 5
#define BOXGPSHOME 6
#define BOXGPSHOLD 7
#define BOXPASSTHRU 8
#define BOXHEADFREE 9
#define BOXACC 0
#define BOXBARO 1
#define BOXMAG 2
#define BOXCAMSTAB 3
#define BOXCAMTRIG 4
#define BOXARM 5
#define BOXGPSHOME 6
#define BOXGPSHOLD 7
#define BOXPASSTHRU 8
#define BOXHEADFREE 9
#define BOXBEEPERON 10
#define CHECKBOXITEMS 11
@ -137,21 +133,60 @@ typedef enum MultiType
#define ACC_ORIENTATION(X, Y, Z) { accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z; }
#define GYRO_ORIENTATION(X, Y, Z) { gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z; }
#define MAG_ORIENTATION(X, Y, Z) { magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z; }
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define abs(x) ((x)>0?(x):-(x))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
extern int16_t accZero[3];
extern int16_t accTrim[2];
typedef struct config_t {
uint8_t version;
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint8_t P8[8];
uint8_t I8[8];
uint8_t D8[8];
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t rollPitchRate;
uint8_t yawRate;
uint8_t dynThrPID;
int16_t accZero[3];
int16_t magZero[3];
int16_t accTrim[2];
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t activate1[CHECKBOXITEMS];
uint8_t activate2[CHECKBOXITEMS];
uint8_t powerTrigger1; // trigger for alarm based on power consumption
// Radio/ESC-related configuration
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
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
// mixer-related configuration
int8_t yaw_direction;
uint16_t wing_left_mid; // left servo center pos. - use this for initial trim
uint16_t wing_right_mid; // right servo center pos. - use this for initial trim
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
// gimbal-related configuration
int8_t tilt_pitch_prop; // servo proportional (tied to angle) ; can be negative to invert movement
int8_t tilt_roll_prop; // servo proportional (tied to angle) ; can be negative to invert movement
} config_t;
extern int16_t gyroZero[3];
extern int16_t magZero[3];
extern int16_t gyroData[3];
extern int16_t angle[2];
extern int16_t axisPID[3];
extern int16_t rcCommand[4];
extern uint8_t rcOptions[CHECKBOXITEMS];
extern int16_t debug1, debug2, debug3, debug4;
extern uint8_t armed;
@ -171,7 +206,6 @@ extern int32_t BaroAlt;
extern int32_t EstAlt;
extern int32_t AltHold;
extern int16_t errorAltitudeI;
extern int32_t EstVelocity;
extern int16_t BaroPID;
extern uint8_t headFreeMode;
extern int16_t headFreeModeHold;
@ -185,16 +219,7 @@ extern int16_t rcData[8];
extern uint8_t accMode;
extern uint8_t magMode;
extern uint8_t baroMode;
extern uint8_t P8[8], I8[8], D8[8];
extern uint8_t dynThrPID;
extern uint8_t activate1[CHECKBOXITEMS];
extern uint8_t activate2[CHECKBOXITEMS];
extern uint16_t intPowerMeterSum, intPowerTrigger1;
extern uint8_t rollPitchRate;
extern uint8_t yawRate;
extern uint8_t dynThrPID;
extern uint8_t rcRate8;
extern uint8_t rcExpo8;
extern int32_t GPS_latitude, GPS_longitude;
extern int32_t GPS_latitude_home, GPS_longitude_home;
extern uint8_t GPS_fix, GPS_fix_home;
@ -205,12 +230,9 @@ extern uint8_t GPS_update;
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
extern uint8_t vbat;
extern uint8_t powerTrigger1;
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
extern uint8_t mixerConfiguration;
extern uint16_t wing_left_mid;
extern uint16_t wing_right_mid;
extern uint16_t tri_yaw_middle;
extern config_t cfg;
// main
void loop(void);
@ -250,3 +272,6 @@ bool feature(uint32_t mask);
void featureSet(uint32_t mask);
void featureClear(uint32_t mask);
void featureClearAll(void);
// cli
void cliProcess(void);

60
sensors.c
View file

@ -6,7 +6,6 @@ uint16_t calibratingA = 0; // the calibration is done is the main loop. Ca
uint16_t calibratingG = 0;
uint8_t calibratingM = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration
int16_t accTrim[2] = { 0, 0 };
int16_t heading, magHold;
void sensorsAutodetect(void)
@ -33,15 +32,15 @@ static void ACC_Common(void)
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accZero[axis] = 0;
cfg.accZero[axis] = 0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
accZero[ROLL] = a[ROLL] / 400;
accZero[PITCH] = a[PITCH] / 400;
accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
calibratingA--;
@ -94,9 +93,9 @@ static void ACC_Common(void)
writeParams(); // write accZero in EEPROM
}
#endif
accADC[ROLL] -= accZero[ROLL];
accADC[PITCH] -= accZero[PITCH];
accADC[YAW] -= accZero[YAW];
accADC[ROLL] -= cfg.accZero[ROLL];
accADC[PITCH] -= cfg.accZero[PITCH];
accADC[YAW] -= cfg.accZero[YAW];
}
@ -118,6 +117,8 @@ static int16_t baroTemp = 0;
void Baro_update(void)
{
int32_t pressure;
if (currentTime < baroDeadline)
return;
@ -156,6 +157,15 @@ static void GYRO_Common(void)
static int32_t g[3];
uint8_t axis;
#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
@ -174,12 +184,25 @@ static void GYRO_Common(void)
calibratingG--;
}
#ifdef MMGYRO
mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGTH;
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
//anti gyro glitch, limit the variation between two consecutive readings
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGTH;
previousGyroADC[axis] = gyroADC[axis];
}
#else
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
previousGyroADC[axis] = gyroADC[axis];
}
#endif
}
void Gyro_getADC(void)
@ -205,13 +228,8 @@ static void Mag_getRawADC(void)
static int16_t rawADC[3];
hmc5883lRead(rawADC);
// Hearty FUCK-YOU goes to all teh breakout sensor faggots who make a new orientation for each shitty board they make
// sensor order: X Z Y
// magADC[ROLL] = rawADC[0]; // X or negative? who knows mag stuff in multiwii is broken hardcore
// magADC[PITCH] = rawADC[2]; // Y
// no way? is this finally the proper orientation??
magADC[ROLL] = -rawADC[2]; // X or negative? who knows mag stuff in multiwii is broken hardcore
magADC[ROLL] = -rawADC[2]; // X
magADC[PITCH] = rawADC[0]; // Y
magADC[YAW] = rawADC[1]; // Z
}
@ -249,7 +267,7 @@ void Mag_getADC(void)
if (calibratingM == 1) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
magZero[axis] = 0;
cfg.magZero[axis] = 0;
magZeroTempMin[axis] = 0;
magZeroTempMax[axis] = 0;
}
@ -259,9 +277,9 @@ void Mag_getADC(void)
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= magZero[ROLL];
magADC[PITCH] -= magZero[PITCH];
magADC[YAW] -= magZero[YAW];
magADC[ROLL] -= cfg.magZero[ROLL];
magADC[PITCH] -= cfg.magZero[PITCH];
magADC[YAW] -= cfg.magZero[YAW];
}
if (tCal != 0) {
@ -276,7 +294,7 @@ void Mag_getADC(void)
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2;
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2;
writeParams();
}
}

64
serial.c
View file

@ -1,6 +1,9 @@
#include "board.h"
#include "mw.h"
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialize16(int16_t a)
{
uartWrite(a);
@ -19,11 +22,21 @@ void UartSendData()
void serialCom(void)
{
int16_t a;
uint8_t i;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
if (uartAvailable()) {
switch (uartRead()) {
case '#':
uartPrint("\r\nEntering CLI Mode, type 'exit' to return\r\n");
cliMode = 1;
break;
#ifdef BTSERIAL
case 'K': //receive RC data from Bluetooth Serial adapter as a remote
rcData[THROTTLE] = (SerialRead(0) * 4) + 1000;
@ -155,20 +168,21 @@ void serialCom(void)
serialize16(i2cGetErrorCounter());
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize8(mixerConfiguration);
serialize8(cfg.mixerConfiguration);
for (i = 0; i < PIDITEMS; i++) {
serialize8(P8[i]);
serialize8(I8[i]);
serialize8(D8[i]);
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
serialize8(rcRate8);
serialize8(rcExpo8);
serialize8(rollPitchRate);
serialize8(yawRate);
serialize8(dynThrPID);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
for (i = 0; i < CHECKBOXITEMS; i++) {
serialize8(activate1[i]);
serialize8(activate2[i]);
serialize8(cfg.activate1[i]);
serialize8(cfg.activate2[i] | (rcOptions[i] << 7)); // use highest bit to transport state in mwc
}
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
@ -179,7 +193,7 @@ void serialCom(void)
serialize16(intPowerTrigger1);
serialize8(vbat);
serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose
serialize16(0); // debug2
serialize16(debug2); // debug2
serialize16(debug3); // debug3
serialize16(debug4); // debug4
serialize8('M');
@ -216,7 +230,7 @@ void serialCom(void)
if (i > 64 && i < 64 + MULTITYPE_LAST) {
serialize8('O');
serialize8('K');
mixerConfiguration = i - '@'; // A..B..C.. index
cfg.mixerConfiguration = i - '@'; // A..B..C.. index
writeParams();
systemReset(false);
break;
@ -228,21 +242,21 @@ void serialCom(void)
case 'W': //GUI write params to eeprom @ arduino
// while (uartAvailable() < (7 + 3 * PIDITEMS + 2 * CHECKBOXITEMS)) { }
for (i = 0; i < PIDITEMS; i++) {
P8[i] = uartReadPoll();
I8[i] = uartReadPoll();
D8[i] = uartReadPoll();
cfg.P8[i] = uartReadPoll();
cfg.I8[i] = uartReadPoll();
cfg.D8[i] = uartReadPoll();
}
rcRate8 = uartReadPoll();
rcExpo8 = uartReadPoll(); //2
rollPitchRate = uartReadPoll();
yawRate = uartReadPoll(); //4
dynThrPID = uartReadPoll(); //5
cfg.rcRate8 = uartReadPoll();
cfg.rcExpo8 = uartReadPoll(); //2
cfg.rollPitchRate = uartReadPoll();
cfg.yawRate = uartReadPoll(); //4
cfg.dynThrPID = uartReadPoll(); //5
for (i = 0; i < CHECKBOXITEMS; i++) {
activate1[i] = uartReadPoll();
activate2[i] = uartReadPoll();
cfg.activate1[i] = uartReadPoll();
cfg.activate2[i] = uartReadPoll();
}
#if defined(POWERMETER)
powerTrigger1 = (uartReadPoll() + 256 * uartReadPoll()) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value
cfg.powerTrigger1 = (uartReadPoll() + 256 * uartReadPoll()) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value
#else
uartReadPoll();
uartReadPoll(); //7 so we unload the two bytes