mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +03:00
Merge remote-tracking branch 'upstream/master'
This commit is contained in:
commit
321fc717ab
31 changed files with 1982 additions and 1717 deletions
24
docs/Board - AlienWii32.md
Normal file
24
docs/Board - AlienWii32.md
Normal file
|
@ -0,0 +1,24 @@
|
||||||
|
# Board - AlienWii32
|
||||||
|
|
||||||
|
The AlienWii32 is actually in prototype stage only a few samples exist. There is some more field testing ongoing. The information below is preliminary and will be updated as needed.
|
||||||
|
|
||||||
|
Here are the hardware specifications:
|
||||||
|
|
||||||
|
- STM32F103CBT6 MCU
|
||||||
|
- 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
|
||||||
|
- USB port, integrated
|
||||||
|
- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100 or Lemon RX)
|
||||||
|
- alternatively PPM receiver connection (i.e. Deltang Rx31)
|
||||||
|
- ground and 3.3V for the receiver
|
||||||
|
- hardware bind plug for easy binding
|
||||||
|
- motor connections are at the corners for a clean look with reduced wiring
|
||||||
|
- dimensions: 29x33mm
|
||||||
|
- direct operation from an single cell lipoly battery
|
||||||
|
|
||||||
|
(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset).
|
||||||
|
|
||||||
|
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.
|
|
@ -37,7 +37,7 @@ When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but two
|
||||||
|
|
||||||
| Pin | Identifier | Function | Notes |
|
| Pin | Identifier | Function | Notes |
|
||||||
| --- | ---------- | -------------- | -------------------------------- |
|
| --- | ---------- | -------------- | -------------------------------- |
|
||||||
| 7 | 5 | SOFTSERIAL1 TX | Enable `feature SOFTSERIAL` |
|
| 7 | 5 | SOFTSERIAL1 RX | Enable `feature SOFTSERIAL`|
|
||||||
| 8 | 6 | SOFTSERIAL1 RX | |
|
| 8 | 6 | SOFTSERIAL1 TX | |
|
||||||
| 9 | 7 | SOFTSERIAL2 TX | |
|
| 9 | 7 | SOFTSERIAL2 RX | |
|
||||||
| 10 | 8 | SOFTSERIAL2 RX | |
|
| 10 | 8 | SOFTSERIAL2 TX | |
|
||||||
|
|
|
@ -3,6 +3,7 @@
|
||||||
The Sparky is a very low cost and very powerful board.
|
The Sparky is a very low cost and very powerful board.
|
||||||
|
|
||||||
* 3 hardware serial ports.
|
* 3 hardware serial ports.
|
||||||
|
* Built-in serial port inverters which allows SBUS receivers to be used without external inverters.
|
||||||
* USB (can be used at the same time as the serial ports).
|
* USB (can be used at the same time as the serial ports).
|
||||||
* 10 PWM outputs.
|
* 10 PWM outputs.
|
||||||
* Dedicated PPM/SerialRX input pin.
|
* Dedicated PPM/SerialRX input pin.
|
||||||
|
@ -16,12 +17,13 @@ Flyable!
|
||||||
Tested with revision 1 board.
|
Tested with revision 1 board.
|
||||||
|
|
||||||
## TODO
|
## TODO
|
||||||
* Mag
|
* Baro - detection works but getting bad readings, disabled for now.
|
||||||
* Baro
|
|
||||||
* Led Strip
|
* Led Strip
|
||||||
* ADC
|
* ADC
|
||||||
* Display
|
* Sonar
|
||||||
|
* Display (via Flex port)
|
||||||
* Softserial - though having 3 hardware serial ports makes it a little redundant.
|
* Softserial - though having 3 hardware serial ports makes it a little redundant.
|
||||||
|
* Airplane PWM mappings.
|
||||||
|
|
||||||
# Flashing
|
# Flashing
|
||||||
|
|
||||||
|
@ -46,10 +48,12 @@ Flashing cleanflight will erase the TauLabs bootloader, this is not a problem an
|
||||||
| Value | Identifier | RX | TX | Notes |
|
| Value | Identifier | RX | TX | Notes |
|
||||||
| ----- | ------------ | --------- | ---------- | ------------------------------------------------------------------------------------------- |
|
| ----- | ------------ | --------- | ---------- | ------------------------------------------------------------------------------------------- |
|
||||||
| 1 | USB VCP | RX (USB) | TX (USB) | |
|
| 1 | USB VCP | RX (USB) | TX (USB) | |
|
||||||
| 2 | USART1 | RX / PB7 | TX / PB6 | Conn1 / Flexi Port |
|
| 2 | USART1 | RX / PB7 | TX / PB6 | Conn1 / Flexi Port. |
|
||||||
| 3 | USART2 | RX / PA3 | PWM6 / PA2 | On RX is on INPUT header. Best port for Serial RX input. |
|
| 3 | USART2 | RX / PA3 | PWM6 / PA2 | On RX is on INPUT header. Best port for Serial RX input |
|
||||||
| 4 | USART3 | RX / PB11 | TX / PB10 | RX/TX is on one end of the 6-pin header about the PWM outputs. |
|
| 4 | USART3 | RX / PB11 | TX / PB10 | RX/TX is on one end of the 6-pin header about the PWM outputs. |
|
||||||
|
|
||||||
USB VCP *can* be used at the same time as other serial ports (unlike Naze32).
|
USB VCP *can* be used at the same time as other serial ports (unlike Naze32).
|
||||||
|
|
||||||
|
All USART ports all support automatic hardware inversion which allows direct connection of serial rx receivers like the FrSky X4RSB - no external inverter needed.
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -219,22 +219,22 @@ This is the default so that if you don't want to place LEDs top and bottom in th
|
||||||
### Example 16 LED config
|
### Example 16 LED config
|
||||||
|
|
||||||
```
|
```
|
||||||
15,15:SD:IA
|
led 0 15,15:SD:IA
|
||||||
8,8:E:FW
|
led 1 8,8:E:FW
|
||||||
8,7:E:FW
|
led 2 8,7:E:FW
|
||||||
15,0:ND:IA
|
led 3 15,0:ND:IA
|
||||||
7,7:N:FW
|
led 4 7,7:N:FW
|
||||||
8,7:N:FW
|
led 5 8,7:N:FW
|
||||||
0,0:ND:IA
|
led 6 0,0:ND:IA
|
||||||
7,7:W:FW
|
led 7 7,7:W:FW
|
||||||
7,8:W:FW
|
led 8 7,8:W:FW
|
||||||
0,15:SD:IA
|
led 9 0,15:SD:IA
|
||||||
7,8:S:FW
|
led 10 7,8:S:FW
|
||||||
8,8:S:FW
|
led 11 8,8:S:FW
|
||||||
7,7:D:FW
|
led 12 7,7:D:FW
|
||||||
8,7:D:FW
|
led 13 8,7:D:FW
|
||||||
7,7:U:FW
|
led 14 7,7:U:FW
|
||||||
8,7:U:FW
|
led 15 8,7:U:FW
|
||||||
```
|
```
|
||||||
|
|
||||||
Which translates into the following positions:
|
Which translates into the following positions:
|
||||||
|
|
49
docs/Spektrum bind.md
Normal file
49
docs/Spektrum bind.md
Normal file
|
@ -0,0 +1,49 @@
|
||||||
|
# Spektrum bind support
|
||||||
|
|
||||||
|
Spektrum bind with hardware bind plug support.
|
||||||
|
|
||||||
|
The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D targets.
|
||||||
|
|
||||||
|
## Configure the bind code
|
||||||
|
|
||||||
|
The following parameters can be used to enable and configure this in the related target.h file:
|
||||||
|
|
||||||
|
SPEKTRUM_BIND Enables the Spektrum bind code
|
||||||
|
BIND_PORT GPIOA Defines the port for the bind pin
|
||||||
|
BIND_PIN Pin_3 Defines the bind pin (the satellite receiver is connected to)
|
||||||
|
|
||||||
|
This is to activate the hardware bind plug feature
|
||||||
|
|
||||||
|
HARDWARE_BIND_PLUG Enables the hardware bind plug feature
|
||||||
|
BINDPLUG_PORT GPIOB Defines the port for the hardware bind plug
|
||||||
|
BINDPLUG_PIN Pin_5 Defines the hardware bind plug pin
|
||||||
|
|
||||||
|
## Hardware
|
||||||
|
|
||||||
|
The hardware bind plug will be enabled via defining HARDWARE_BIND_PLUG during building of the firmware. BINDPLUG_PORT and BINDPLUG_PIN also need to be defined (please see above). This is done automatically if the AlienWii32 firmware is build. The hardware bind plug is expected between the defined bind pin and ground.
|
||||||
|
|
||||||
|
## Function
|
||||||
|
|
||||||
|
The bind code will actually work for NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY targets (USART2) and CC3D target (USART3, flex port). The spektrum_sat_bind CLI parameter is defining the number of bind impulses (1-10) send to the satellite receiver. Setting spektrum_sat_bind to zero will disable the bind mode in any case. The bind mode will only be activated after an power on or hard reset. Please refer to the table below for the different possible values.
|
||||||
|
|
||||||
|
If the hardware bind plug is configured the bind mode will only be activated if the plug is set during the firmware start-up. The value of the spektrum_sat_bind parameter will be permanently preserved. The bind plug should be always removed for normal flying.
|
||||||
|
|
||||||
|
If no hardware bind plug is used the spektrum_sat_bind parameter will trigger the bind process during the next hardware reset and will be automatically reset to "0" after this.
|
||||||
|
|
||||||
|
Please refer to the satellite receiver documentation for more details of the specific receiver in bind mode. Usually the bind mode will be indicated with some flashing LEDs.
|
||||||
|
|
||||||
|
## Table with spektrum_sat_bind parameter value
|
||||||
|
|
||||||
|
| Value | Receiver mode | Notes |
|
||||||
|
| ----- | ---------------| -------------------|
|
||||||
|
| 3 | DSM2 1024/22ms | |
|
||||||
|
| 5 | DSM2 2048/11ms | default AlienWii32 |
|
||||||
|
| 7 | DSMX 22ms | |
|
||||||
|
| 9 | DSMX 11ms | |
|
||||||
|
|
||||||
|
More detailed information regarding the satellite binding process can be found here:
|
||||||
|
http://wiki.openpilot.org/display/Doc/Spektrum+Satellite
|
||||||
|
|
||||||
|
### Supported Hardware
|
||||||
|
|
||||||
|
NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets (AlienWii32 with hardware bind plug)
|
|
@ -352,6 +352,7 @@ static void resetConf(void)
|
||||||
|
|
||||||
masterConfig.retarded_arm = 0;
|
masterConfig.retarded_arm = 0;
|
||||||
masterConfig.disarm_kill_switch = 1;
|
masterConfig.disarm_kill_switch = 1;
|
||||||
|
masterConfig.auto_disarm_delay = 5;
|
||||||
masterConfig.small_angle = 25;
|
masterConfig.small_angle = 25;
|
||||||
|
|
||||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||||
|
@ -449,6 +450,71 @@ static void resetConf(void)
|
||||||
masterConfig.blackbox_rate_denom = 1;
|
masterConfig.blackbox_rate_denom = 1;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target)
|
||||||
|
#ifdef ALIENWII32
|
||||||
|
featureSet(FEATURE_RX_SERIAL);
|
||||||
|
featureSet(FEATURE_MOTOR_STOP);
|
||||||
|
featureSet(FEATURE_FAILSAFE);
|
||||||
|
masterConfig.serialConfig.serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY);
|
||||||
|
masterConfig.rxConfig.serialrx_provider = 1;
|
||||||
|
masterConfig.rxConfig.spektrum_sat_bind = 5;
|
||||||
|
masterConfig.escAndServoConfig.minthrottle = 1000;
|
||||||
|
masterConfig.escAndServoConfig.maxthrottle = 2000;
|
||||||
|
masterConfig.motor_pwm_rate = 32000;
|
||||||
|
currentControlRateProfile->rcRate8 = 130;
|
||||||
|
currentControlRateProfile->rollPitchRate = 20;
|
||||||
|
currentControlRateProfile->yawRate = 60;
|
||||||
|
parseRcChannels("TAER1234", &masterConfig.rxConfig);
|
||||||
|
|
||||||
|
// { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
|
||||||
|
masterConfig.customMixer[0].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[0].roll = -0.5f;
|
||||||
|
masterConfig.customMixer[0].pitch = 1.0f;
|
||||||
|
masterConfig.customMixer[0].yaw = -1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R
|
||||||
|
masterConfig.customMixer[1].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[1].roll = -0.5f;
|
||||||
|
masterConfig.customMixer[1].pitch = -1.0f;
|
||||||
|
masterConfig.customMixer[1].yaw = 1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L
|
||||||
|
masterConfig.customMixer[2].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[2].roll = 0.5f;
|
||||||
|
masterConfig.customMixer[2].pitch = 1.0f;
|
||||||
|
masterConfig.customMixer[2].yaw = 1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L
|
||||||
|
masterConfig.customMixer[3].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[3].roll = 0.5f;
|
||||||
|
masterConfig.customMixer[3].pitch = -1.0f;
|
||||||
|
masterConfig.customMixer[3].yaw = -1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R
|
||||||
|
masterConfig.customMixer[4].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[4].roll = -1.0f;
|
||||||
|
masterConfig.customMixer[4].pitch = -0.5f;
|
||||||
|
masterConfig.customMixer[4].yaw = -1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L
|
||||||
|
masterConfig.customMixer[5].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[5].roll = 1.0f;
|
||||||
|
masterConfig.customMixer[5].pitch = -0.5f;
|
||||||
|
masterConfig.customMixer[5].yaw = 1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R
|
||||||
|
masterConfig.customMixer[6].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[6].roll = -1.0f;
|
||||||
|
masterConfig.customMixer[6].pitch = 0.5f;
|
||||||
|
masterConfig.customMixer[6].yaw = 1.0f;
|
||||||
|
|
||||||
|
// { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L
|
||||||
|
masterConfig.customMixer[7].throttle = 1.0f;
|
||||||
|
masterConfig.customMixer[7].roll = 1.0f;
|
||||||
|
masterConfig.customMixer[7].pitch = 0.5f;
|
||||||
|
masterConfig.customMixer[7].yaw = -1.0f;
|
||||||
|
#endif
|
||||||
|
|
||||||
// copy first profile into remaining profile
|
// copy first profile into remaining profile
|
||||||
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
||||||
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
|
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
|
||||||
|
|
|
@ -63,6 +63,7 @@ typedef struct master_t {
|
||||||
|
|
||||||
uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right
|
uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right
|
||||||
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
|
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
|
||||||
|
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
|
||||||
uint8_t small_angle;
|
uint8_t small_angle;
|
||||||
|
|
||||||
airplaneConfig_t airplaneConfig;
|
airplaneConfig_t airplaneConfig;
|
||||||
|
|
|
@ -328,14 +328,15 @@ static void mpu6050GyroInit(void)
|
||||||
delay(100);
|
delay(100);
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
|
|
||||||
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
|
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||||
|
|
||||||
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
|
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
|
||||||
// Accel scale 8g (4096 LSB/g)
|
// Accel scale 8g (4096 LSB/g)
|
||||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||||
|
|
||||||
|
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
|
||||||
|
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu6050GyroRead(int16_t *gyroData)
|
static void mpu6050GyroRead(int16_t *gyroData)
|
||||||
|
|
|
@ -281,10 +281,6 @@ bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (len > 1) {
|
|
||||||
reg |= 0x80;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Send Register address */
|
/* Send Register address */
|
||||||
I2C_SendData(I2Cx, (uint8_t) reg);
|
I2C_SendData(I2Cx, (uint8_t) reg);
|
||||||
|
|
||||||
|
|
|
@ -74,16 +74,30 @@ bool ak8975detect(mag_t *mag)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define AK8975A_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
|
||||||
|
#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
|
||||||
|
#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
|
||||||
|
|
||||||
void ak8975Init()
|
void ak8975Init()
|
||||||
{
|
{
|
||||||
bool ack;
|
bool ack;
|
||||||
|
uint8_t buffer[3];
|
||||||
|
uint8_t status;
|
||||||
|
|
||||||
UNUSED(ack);
|
UNUSED(ack);
|
||||||
|
|
||||||
|
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
|
||||||
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00);
|
|
||||||
delay(20);
|
delay(20);
|
||||||
|
|
||||||
uint8_t status;
|
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
|
||||||
|
delay(10);
|
||||||
|
|
||||||
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
|
||||||
|
delay(10);
|
||||||
|
|
||||||
|
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
|
||||||
|
delay(10);
|
||||||
|
|
||||||
// Clear status registers
|
// Clear status registers
|
||||||
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
||||||
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
||||||
|
@ -109,7 +123,16 @@ void ak8975Read(int16_t *magData)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if 1 // USE_I2C_SINGLE_BYTE_READS
|
||||||
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
||||||
|
#else
|
||||||
|
for (uint8_t i = 0; i < 6; i++) {
|
||||||
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
||||||
|
if (!ack) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
|
@ -124,12 +147,8 @@ void ak8975Read(int16_t *magData)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// align sensors to match MPU6050:
|
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
|
||||||
// x -> y
|
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4;
|
||||||
// y -> x
|
|
||||||
// z-> -z
|
|
||||||
magData[X] = -(int16_t)(buf[3] << 8 | buf[2]) * 4;
|
|
||||||
magData[Y] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
|
|
||||||
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
|
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -297,7 +297,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
|
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
|
||||||
if (init->airplane)
|
if (init->airplane)
|
||||||
i = 2; // switch to air hardware config
|
i = 2; // switch to air hardware config
|
||||||
if (init->usePPM)
|
if (init->usePPM || init->useSerialRx)
|
||||||
i++; // next index is for PPM
|
i++; // next index is for PPM
|
||||||
|
|
||||||
setup = hardwareMaps[i];
|
setup = hardwareMaps[i];
|
||||||
|
|
|
@ -39,6 +39,7 @@
|
||||||
typedef struct drv_pwm_config_t {
|
typedef struct drv_pwm_config_t {
|
||||||
bool useParallelPWM;
|
bool useParallelPWM;
|
||||||
bool usePPM;
|
bool usePPM;
|
||||||
|
bool useSerialRx;
|
||||||
bool useRSSIADC;
|
bool useRSSIADC;
|
||||||
bool useCurrentMeterADC;
|
bool useCurrentMeterADC;
|
||||||
#ifdef STM32F10X
|
#ifdef STM32F10X
|
||||||
|
|
|
@ -39,6 +39,31 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode);
|
||||||
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
|
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
|
||||||
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
|
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
|
||||||
|
|
||||||
|
static void usartConfigurePinInversion(uartPort_t *uartPort) {
|
||||||
|
#ifdef INVERTER
|
||||||
|
if (uartPort->port.inversion == SERIAL_INVERTED && uartPort->USARTx == INVERTER_USART) {
|
||||||
|
// Enable hardware inverter if available.
|
||||||
|
INVERTER_ON;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef STM32F303xC
|
||||||
|
uint32_t inversionPins = 0;
|
||||||
|
|
||||||
|
if (uartPort->port.mode & MODE_TX) {
|
||||||
|
inversionPins |= USART_InvPin_Tx;
|
||||||
|
}
|
||||||
|
if (uartPort->port.mode & MODE_RX) {
|
||||||
|
inversionPins |= USART_InvPin_Rx;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Note: inversion when using MODE_BIDIR not supported yet.
|
||||||
|
|
||||||
|
USART_InvPinCmd(uartPort->USARTx, inversionPins, uartPort->port.inversion == SERIAL_INVERTED ? ENABLE : DISABLE);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
static void uartReconfigure(uartPort_t *uartPort)
|
static void uartReconfigure(uartPort_t *uartPort)
|
||||||
{
|
{
|
||||||
USART_InitTypeDef USART_InitStructure;
|
USART_InitTypeDef USART_InitStructure;
|
||||||
|
@ -63,22 +88,16 @@ static void uartReconfigure(uartPort_t *uartPort)
|
||||||
USART_InitStructure.USART_Mode |= USART_Mode_Tx | USART_Mode_Rx;
|
USART_InitStructure.USART_Mode |= USART_Mode_Tx | USART_Mode_Rx;
|
||||||
|
|
||||||
USART_Init(uartPort->USARTx, &USART_InitStructure);
|
USART_Init(uartPort->USARTx, &USART_InitStructure);
|
||||||
|
|
||||||
|
usartConfigurePinInversion(uartPort);
|
||||||
|
|
||||||
USART_Cmd(uartPort->USARTx, ENABLE);
|
USART_Cmd(uartPort->USARTx, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
|
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
|
||||||
{
|
{
|
||||||
UNUSED(inversion);
|
|
||||||
|
|
||||||
uartPort_t *s = NULL;
|
uartPort_t *s = NULL;
|
||||||
|
|
||||||
#ifdef INVERTER
|
|
||||||
if (inversion == SERIAL_INVERTED && USARTx == INVERTER_USART) {
|
|
||||||
// Enable hardware inverter if available.
|
|
||||||
INVERTER_ON;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (USARTx == USART1) {
|
if (USARTx == USART1) {
|
||||||
s = serialUSART1(baudRate, mode);
|
s = serialUSART1(baudRate, mode);
|
||||||
#ifdef USE_USART2
|
#ifdef USE_USART2
|
||||||
|
@ -101,12 +120,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
s->port.callback = callback;
|
s->port.callback = callback;
|
||||||
s->port.mode = mode;
|
s->port.mode = mode;
|
||||||
s->port.baudRate = baudRate;
|
s->port.baudRate = baudRate;
|
||||||
|
|
||||||
#if 1 // FIXME use inversion on STM32F3
|
|
||||||
s->port.inversion = SERIAL_NOT_INVERTED;
|
|
||||||
#else
|
|
||||||
s->port.inversion = inversion;
|
s->port.inversion = inversion;
|
||||||
#endif
|
|
||||||
|
|
||||||
uartReconfigure(s);
|
uartReconfigure(s);
|
||||||
|
|
||||||
|
|
|
@ -57,6 +57,11 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+
|
||||||
|
|
||||||
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
|
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
|
||||||
|
|
||||||
|
bool isUsingSticksForArming(void)
|
||||||
|
{
|
||||||
|
return isUsingSticksToArm;
|
||||||
|
}
|
||||||
|
|
||||||
bool areSticksInApModePosition(uint16_t ap_mode)
|
bool areSticksInApModePosition(uint16_t ap_mode)
|
||||||
{
|
{
|
||||||
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
|
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
|
||||||
|
@ -519,6 +524,8 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
|
||||||
escAndServoConfig = escAndServoConfigToUse;
|
escAndServoConfig = escAndServoConfigToUse;
|
||||||
pidProfile = pidProfileToUse;
|
pidProfile = pidProfileToUse;
|
||||||
|
|
||||||
|
isUsingSticksToArm = true;
|
||||||
|
|
||||||
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
|
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
|
||||||
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
|
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
|
||||||
if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
|
if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
|
||||||
|
|
|
@ -213,3 +213,4 @@ void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmen
|
||||||
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
|
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
|
||||||
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
|
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
|
||||||
|
|
||||||
|
bool isUsingSticksForArming(void);
|
||||||
|
|
|
@ -237,6 +237,7 @@ const clivalue_t valueTable[] = {
|
||||||
|
|
||||||
{ "retarded_arm", VAR_UINT8 | MASTER_VALUE, &masterConfig.retarded_arm, 0, 1 },
|
{ "retarded_arm", VAR_UINT8 | MASTER_VALUE, &masterConfig.retarded_arm, 0, 1 },
|
||||||
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.disarm_kill_switch, 0, 1 },
|
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.disarm_kill_switch, 0, 1 },
|
||||||
|
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, 0, 60 },
|
||||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 },
|
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 },
|
||||||
|
|
||||||
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
|
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
|
||||||
|
|
|
@ -80,6 +80,8 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
|
||||||
extern uint16_t rssi; // FIXME dependency on mw.c
|
extern uint16_t rssi; // FIXME dependency on mw.c
|
||||||
extern int16_t debug[4]; // FIXME dependency on mw.c
|
extern int16_t debug[4]; // FIXME dependency on mw.c
|
||||||
|
|
||||||
|
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* MSP Guidelines, emphasis is used to clarify.
|
* MSP Guidelines, emphasis is used to clarify.
|
||||||
*
|
*
|
||||||
|
@ -760,7 +762,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
break;
|
break;
|
||||||
case MSP_RAW_IMU:
|
case MSP_RAW_IMU:
|
||||||
headSerialReply(18);
|
headSerialReply(18);
|
||||||
// Retarded hack until multiwiidorks start using real units for sensor data
|
// Hack due to choice of units for sensor data in multiwii
|
||||||
if (acc_1G > 1024) {
|
if (acc_1G > 1024) {
|
||||||
for (i = 0; i < 3; i++)
|
for (i = 0; i < 3; i++)
|
||||||
serialize16(accSmooth[i] / 8);
|
serialize16(accSmooth[i] / 8);
|
||||||
|
@ -1172,6 +1174,8 @@ static bool processInCommand(void)
|
||||||
mac->auxChannelIndex = read8();
|
mac->auxChannelIndex = read8();
|
||||||
mac->range.startStep = read8();
|
mac->range.startStep = read8();
|
||||||
mac->range.endStep = read8();
|
mac->range.endStep = read8();
|
||||||
|
|
||||||
|
useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile);
|
||||||
} else {
|
} else {
|
||||||
headSerialError(0);
|
headSerialError(0);
|
||||||
}
|
}
|
||||||
|
@ -1469,6 +1473,7 @@ static void mspProcessPort(void)
|
||||||
tailSerialReply();
|
tailSerialReply();
|
||||||
}
|
}
|
||||||
currentPort->c_state = IDLE;
|
currentPort->c_state = IDLE;
|
||||||
|
break; // process one command so as not to block.
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -291,6 +291,7 @@ void init(void)
|
||||||
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
|
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
|
||||||
pwm_params.usePPM = feature(FEATURE_RX_PPM);
|
pwm_params.usePPM = feature(FEATURE_RX_PPM);
|
||||||
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
|
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
|
||||||
|
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
|
||||||
pwm_params.useServos = isMixerUsingServos();
|
pwm_params.useServos = isMixerUsingServos();
|
||||||
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
||||||
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
|
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
|
||||||
|
|
|
@ -92,6 +92,7 @@ uint16_t cycleTime = 0; // this is the number in micro second to achieve
|
||||||
int16_t headFreeModeHold;
|
int16_t headFreeModeHold;
|
||||||
|
|
||||||
int16_t telemTemperature1; // gyro sensor temperature
|
int16_t telemTemperature1; // gyro sensor temperature
|
||||||
|
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
||||||
|
|
||||||
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||||
extern failsafe_t *failsafe;
|
extern failsafe_t *failsafe;
|
||||||
|
@ -346,6 +347,8 @@ void mwArm(void)
|
||||||
startBlackbox();
|
startBlackbox();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
|
||||||
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -513,6 +516,21 @@ void processRx(void)
|
||||||
resetErrorAngle();
|
resetErrorAngle();
|
||||||
resetErrorGyro();
|
resetErrorGyro();
|
||||||
}
|
}
|
||||||
|
// When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers.
|
||||||
|
// mixTable constrains motor commands, so checking throttleStatus is enough
|
||||||
|
if (
|
||||||
|
ARMING_FLAG(ARMED)
|
||||||
|
&& feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
|
||||||
|
&& masterConfig.auto_disarm_delay != 0
|
||||||
|
&& isUsingSticksForArming()
|
||||||
|
) {
|
||||||
|
if (throttleStatus == THROTTLE_LOW) {
|
||||||
|
if ((int32_t)(disarmAt - millis()) < 0) // delay is over
|
||||||
|
mwDisarm();
|
||||||
|
} else {
|
||||||
|
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // extend delay
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
|
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
|
||||||
|
|
||||||
|
|
|
@ -137,8 +137,8 @@ bool detectGyro(uint16_t gyroLpf)
|
||||||
|
|
||||||
#ifdef USE_GYRO_MPU6050
|
#ifdef USE_GYRO_MPU6050
|
||||||
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
|
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
|
||||||
#ifdef NAZE
|
#ifdef GYRO_MPU6050_ALIGN
|
||||||
gyroAlign = CW0_DEG;
|
gyroAlign = GYRO_MPU6050_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -146,8 +146,8 @@ bool detectGyro(uint16_t gyroLpf)
|
||||||
|
|
||||||
#ifdef USE_GYRO_L3G4200D
|
#ifdef USE_GYRO_L3G4200D
|
||||||
if (l3g4200dDetect(&gyro, gyroLpf)) {
|
if (l3g4200dDetect(&gyro, gyroLpf)) {
|
||||||
#ifdef NAZE
|
#ifdef GYRO_L3G4200D_ALIGN
|
||||||
gyroAlign = CW0_DEG;
|
gyroAlign = GYRO_L3G4200D_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -155,8 +155,8 @@ bool detectGyro(uint16_t gyroLpf)
|
||||||
|
|
||||||
#ifdef USE_GYRO_MPU3050
|
#ifdef USE_GYRO_MPU3050
|
||||||
if (mpu3050Detect(&gyro, gyroLpf)) {
|
if (mpu3050Detect(&gyro, gyroLpf)) {
|
||||||
#ifdef NAZE
|
#ifdef GYRO_MPU3050_ALIGN
|
||||||
gyroAlign = CW0_DEG;
|
gyroAlign = GYRO_MPU3050_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -164,14 +164,17 @@ bool detectGyro(uint16_t gyroLpf)
|
||||||
|
|
||||||
#ifdef USE_GYRO_L3GD20
|
#ifdef USE_GYRO_L3GD20
|
||||||
if (l3gd20Detect(&gyro, gyroLpf)) {
|
if (l3gd20Detect(&gyro, gyroLpf)) {
|
||||||
|
#ifdef GYRO_GYRO_L3GD20_ALIGN
|
||||||
|
gyroAlign = GYRO_GYRO_L3GD20_ALIGN;
|
||||||
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_GYRO_SPI_MPU6000
|
#ifdef USE_GYRO_SPI_MPU6000
|
||||||
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
|
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
|
||||||
#ifdef CC3D
|
#ifdef GYRO_SPI_MPU6000_ALIGN
|
||||||
gyroAlign = CW270_DEG;
|
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -180,12 +183,16 @@ bool detectGyro(uint16_t gyroLpf)
|
||||||
#ifdef USE_GYRO_SPI_MPU6500
|
#ifdef USE_GYRO_SPI_MPU6500
|
||||||
#ifdef NAZE
|
#ifdef NAZE
|
||||||
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
||||||
gyroAlign = CW0_DEG;
|
#ifdef GYRO_SPI_MPU6500_ALIGN
|
||||||
|
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
|
||||||
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
||||||
gyroAlign = CW0_DEG;
|
#ifdef GYRO_SPI_MPU6500_ALIGN
|
||||||
|
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
|
||||||
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -227,24 +234,37 @@ retry:
|
||||||
acc_params.dataRate = 800; // unused currently
|
acc_params.dataRate = 800; // unused currently
|
||||||
#ifdef NAZE
|
#ifdef NAZE
|
||||||
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
|
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
|
||||||
accAlign = CW270_DEG;
|
|
||||||
#else
|
#else
|
||||||
if (adxl345Detect(&acc_params, &acc)) {
|
if (adxl345Detect(&acc_params, &acc)) {
|
||||||
#endif
|
#endif
|
||||||
accHardware = ACC_ADXL345;
|
#ifdef ACC_ADXL345_ALIGN
|
||||||
|
accAlign = ACC_ADXL345_ALIGN;
|
||||||
|
#endif
|
||||||
accHardware = ACC_ADXL345;
|
accHardware = ACC_ADXL345;
|
||||||
if (accHardwareToUse == ACC_ADXL345)
|
if (accHardwareToUse == ACC_ADXL345)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
; // fallthrough
|
; // fallthrough
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_ACC_LSM303DLHC
|
||||||
|
case ACC_LSM303DLHC:
|
||||||
|
if (lsm303dlhcAccDetect(&acc)) {
|
||||||
|
#ifdef ACC_LSM303DLHC_ALIGN
|
||||||
|
accAlign = ACC_LSM303DLHC_ALIGN;
|
||||||
|
#endif
|
||||||
|
accHardware = ACC_LSM303DLHC;
|
||||||
|
if (accHardwareToUse == ACC_LSM303DLHC)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
; // fallthrough
|
||||||
|
#endif
|
||||||
#ifdef USE_ACC_MPU6050
|
#ifdef USE_ACC_MPU6050
|
||||||
case ACC_MPU6050: // MPU6050
|
case ACC_MPU6050: // MPU6050
|
||||||
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
|
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
|
||||||
accHardware = ACC_MPU6050;
|
#ifdef ACC_MPU6050_ALIGN
|
||||||
#ifdef NAZE
|
accAlign = ACC_MPU6050_ALIGN;
|
||||||
accAlign = CW0_DEG;
|
|
||||||
#endif
|
#endif
|
||||||
|
accHardware = ACC_MPU6050;
|
||||||
if (accHardwareToUse == ACC_MPU6050)
|
if (accHardwareToUse == ACC_MPU6050)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -255,9 +275,11 @@ retry:
|
||||||
#ifdef NAZE
|
#ifdef NAZE
|
||||||
// Not supported with this frequency
|
// Not supported with this frequency
|
||||||
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
|
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
|
||||||
accAlign = CW90_DEG;
|
|
||||||
#else
|
#else
|
||||||
if (mma8452Detect(&acc)) {
|
if (mma8452Detect(&acc)) {
|
||||||
|
#endif
|
||||||
|
#ifdef ACC_MMA8452_ALIGN
|
||||||
|
accAlign = ACC_MMA8452_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
accHardware = ACC_MMA8452;
|
accHardware = ACC_MMA8452;
|
||||||
if (accHardwareToUse == ACC_MMA8452)
|
if (accHardwareToUse == ACC_MMA8452)
|
||||||
|
@ -268,31 +290,22 @@ retry:
|
||||||
#ifdef USE_ACC_BMA280
|
#ifdef USE_ACC_BMA280
|
||||||
case ACC_BMA280: // BMA280
|
case ACC_BMA280: // BMA280
|
||||||
if (bma280Detect(&acc)) {
|
if (bma280Detect(&acc)) {
|
||||||
|
#ifdef ACC_BMA280_ALIGN
|
||||||
|
accAlign = ACC_BMA280_ALIGN;
|
||||||
|
#endif
|
||||||
accHardware = ACC_BMA280;
|
accHardware = ACC_BMA280;
|
||||||
#ifdef NAZE
|
|
||||||
accAlign = CW0_DEG;
|
|
||||||
#endif
|
|
||||||
if (accHardwareToUse == ACC_BMA280)
|
if (accHardwareToUse == ACC_BMA280)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
; // fallthrough
|
; // fallthrough
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_ACC_LSM303DLHC
|
|
||||||
case ACC_LSM303DLHC:
|
|
||||||
if (lsm303dlhcAccDetect(&acc)) {
|
|
||||||
accHardware = ACC_LSM303DLHC;
|
|
||||||
if (accHardwareToUse == ACC_LSM303DLHC)
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
; // fallthrough
|
|
||||||
#endif
|
|
||||||
#ifdef USE_ACC_SPI_MPU6000
|
#ifdef USE_ACC_SPI_MPU6000
|
||||||
case ACC_SPI_MPU6000:
|
case ACC_SPI_MPU6000:
|
||||||
if (mpu6000SpiAccDetect(&acc)) {
|
if (mpu6000SpiAccDetect(&acc)) {
|
||||||
accHardware = ACC_SPI_MPU6000;
|
#ifdef ACC_SPI_MPU6000_ALIGN
|
||||||
#ifdef CC3D
|
accAlign = ACC_SPI_MPU6000_ALIGN;
|
||||||
accAlign = CW270_DEG;
|
|
||||||
#endif
|
#endif
|
||||||
|
accHardware = ACC_SPI_MPU6000;
|
||||||
if (accHardwareToUse == ACC_SPI_MPU6000)
|
if (accHardwareToUse == ACC_SPI_MPU6000)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -305,10 +318,10 @@ retry:
|
||||||
#else
|
#else
|
||||||
if (mpu6500SpiAccDetect(&acc)) {
|
if (mpu6500SpiAccDetect(&acc)) {
|
||||||
#endif
|
#endif
|
||||||
accHardware = ACC_SPI_MPU6500;
|
#ifdef ACC_SPI_MPU6500_ALIGN
|
||||||
#ifdef NAZE
|
accAlign = ACC_SPI_MPU6500_ALIGN;
|
||||||
accAlign = CW0_DEG;
|
|
||||||
#endif
|
#endif
|
||||||
|
accHardware = ACC_SPI_MPU6500;
|
||||||
if (accHardwareToUse == ACC_SPI_MPU6500)
|
if (accHardwareToUse == ACC_SPI_MPU6500)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -408,8 +421,8 @@ retry:
|
||||||
#ifdef USE_MAG_HMC5883
|
#ifdef USE_MAG_HMC5883
|
||||||
case MAG_HMC5883:
|
case MAG_HMC5883:
|
||||||
if (hmc5883lDetect(&mag, hmc5883Config)) {
|
if (hmc5883lDetect(&mag, hmc5883Config)) {
|
||||||
#ifdef NAZE
|
#ifdef MAG_HMC5883_ALIGN
|
||||||
magAlign = CW180_DEG;
|
magAlign = MAG_HMC5883_ALIGN;
|
||||||
#endif
|
#endif
|
||||||
magHardware = MAG_HMC5883;
|
magHardware = MAG_HMC5883;
|
||||||
if (magHardwareToUse == MAG_HMC5883)
|
if (magHardwareToUse == MAG_HMC5883)
|
||||||
|
@ -422,6 +435,10 @@ retry:
|
||||||
#ifdef USE_MAG_AK8975
|
#ifdef USE_MAG_AK8975
|
||||||
case MAG_AK8975:
|
case MAG_AK8975:
|
||||||
if (ak8975detect(&mag)) {
|
if (ak8975detect(&mag)) {
|
||||||
|
|
||||||
|
#ifdef MAG_AK8975_ALIGN
|
||||||
|
magAlign = MAG_AK8975_ALIGN;
|
||||||
|
#endif
|
||||||
magHardware = MAG_AK8975;
|
magHardware = MAG_AK8975;
|
||||||
if (magHardwareToUse == MAG_AK8975)
|
if (magHardwareToUse == MAG_AK8975)
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -35,11 +35,15 @@
|
||||||
#define MPU6000_CS_PIN GPIO_Pin_4
|
#define MPU6000_CS_PIN GPIO_Pin_4
|
||||||
#define MPU6000_SPI_INSTANCE SPI1
|
#define MPU6000_SPI_INSTANCE SPI1
|
||||||
|
|
||||||
|
#define GYRO
|
||||||
|
#define USE_GYRO_SPI_MPU6000
|
||||||
|
|
||||||
|
#define GYRO_SPI_MPU6000_ALIGN CW270_DEG
|
||||||
|
|
||||||
#define ACC
|
#define ACC
|
||||||
#define USE_ACC_SPI_MPU6000
|
#define USE_ACC_SPI_MPU6000
|
||||||
|
|
||||||
#define GYRO
|
#define ACC_SPI_MPU6000_ALIGN CW270_DEG
|
||||||
#define USE_GYRO_SPI_MPU6000
|
|
||||||
|
|
||||||
#define INVERTER
|
#define INVERTER
|
||||||
#define BEEPER
|
#define BEEPER
|
||||||
|
|
|
@ -37,16 +37,22 @@
|
||||||
#define USE_GYRO_L3GD20
|
#define USE_GYRO_L3GD20
|
||||||
#define USE_GYRO_MPU6050
|
#define USE_GYRO_MPU6050
|
||||||
|
|
||||||
|
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||||
|
|
||||||
#define ACC
|
#define ACC
|
||||||
#define USE_ACC_MPU6050
|
#define USE_ACC_MPU6050
|
||||||
#define USE_ACC_LSM303DLHC
|
#define USE_ACC_LSM303DLHC
|
||||||
|
|
||||||
|
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
#define USE_BARO_MS5611
|
#define USE_BARO_MS5611
|
||||||
|
|
||||||
#define MAG
|
#define MAG
|
||||||
#define USE_MAG_AK8975
|
#define USE_MAG_AK8975
|
||||||
|
|
||||||
|
#define MAG_AK8975_ALIGN CW90_DEG_FLIP
|
||||||
|
|
||||||
#define BEEPER
|
#define BEEPER
|
||||||
#define LED0
|
#define LED0
|
||||||
#define LED1
|
#define LED1
|
||||||
|
|
|
@ -39,16 +39,6 @@
|
||||||
#define MPU6500_CS_PIN GPIO_Pin_12
|
#define MPU6500_CS_PIN GPIO_Pin_12
|
||||||
#define MPU6500_SPI_INSTANCE SPI2
|
#define MPU6500_SPI_INSTANCE SPI2
|
||||||
|
|
||||||
#define ACC
|
|
||||||
#define USE_FAKE_ACC
|
|
||||||
#define USE_ACC_ADXL345
|
|
||||||
#define USE_ACC_BMA280
|
|
||||||
#define USE_ACC_MMA8452
|
|
||||||
#define USE_ACC_MPU3050
|
|
||||||
#define USE_ACC_MPU6050
|
|
||||||
//#define USE_ACC_SPI_MPU6000
|
|
||||||
#define USE_ACC_SPI_MPU6500
|
|
||||||
|
|
||||||
#define GYRO
|
#define GYRO
|
||||||
#define USE_FAKE_GYRO
|
#define USE_FAKE_GYRO
|
||||||
#define USE_GYRO_L3G4200D
|
#define USE_GYRO_L3G4200D
|
||||||
|
@ -58,6 +48,19 @@
|
||||||
#define USE_GYRO_SPI_MPU6000
|
#define USE_GYRO_SPI_MPU6000
|
||||||
#define USE_GYRO_SPI_MPU6500
|
#define USE_GYRO_SPI_MPU6500
|
||||||
|
|
||||||
|
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||||
|
|
||||||
|
#define ACC
|
||||||
|
#define USE_FAKE_ACC
|
||||||
|
#define USE_ACC_ADXL345
|
||||||
|
#define USE_ACC_BMA280
|
||||||
|
#define USE_ACC_MMA8452
|
||||||
|
#define USE_ACC_MPU6050
|
||||||
|
//#define USE_ACC_SPI_MPU6000
|
||||||
|
#define USE_ACC_SPI_MPU6500
|
||||||
|
|
||||||
|
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
#define USE_BARO_MS5611
|
#define USE_BARO_MS5611
|
||||||
#define USE_BARO_BMP085
|
#define USE_BARO_BMP085
|
||||||
|
@ -66,6 +69,8 @@
|
||||||
#define USE_MAG_HMC5883
|
#define USE_MAG_HMC5883
|
||||||
#define USE_MAG_AK8975
|
#define USE_MAG_AK8975
|
||||||
|
|
||||||
|
#define MAG_AK8975_ALIGN CW180_DEG_FLIP
|
||||||
|
|
||||||
|
|
||||||
#define SONAR
|
#define SONAR
|
||||||
#define LED0
|
#define LED0
|
||||||
|
|
|
@ -63,11 +63,22 @@
|
||||||
#define USE_GYRO_MPU3050
|
#define USE_GYRO_MPU3050
|
||||||
#define USE_GYRO_MPU6050
|
#define USE_GYRO_MPU6050
|
||||||
|
|
||||||
|
#define GYRO_MPU3050_ALIGN CW0_DEG
|
||||||
|
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||||
|
#define GYRO_SPI_MPU6500_ALIGN CW0_DEG
|
||||||
|
|
||||||
#define ACC
|
#define ACC
|
||||||
#define USE_ACC_ADXL345
|
#define USE_ACC_ADXL345
|
||||||
#define USE_ACC_BMA280
|
#define USE_ACC_BMA280
|
||||||
#define USE_ACC_MMA8452
|
#define USE_ACC_MMA8452
|
||||||
#define USE_ACC_MPU6050
|
#define USE_ACC_MPU6050
|
||||||
|
//#define ACC_SPI_MPU6500
|
||||||
|
|
||||||
|
#define ACC_ADXL345_ALIGN CW270_DEG
|
||||||
|
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||||
|
#define ACC_MMA8452_ALIGN CW90_DEG
|
||||||
|
#define ACC_BMA280_ALIGN CW0_DEG
|
||||||
|
//#define ACC_SPI_MPU6500_ALIGN CW0_DEG
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
#define USE_BARO_MS5611
|
#define USE_BARO_MS5611
|
||||||
|
@ -76,6 +87,8 @@
|
||||||
#define MAG
|
#define MAG
|
||||||
#define USE_MAG_HMC5883
|
#define USE_MAG_HMC5883
|
||||||
|
|
||||||
|
#define MAG_HMC5883_ALIGN CW180_DEG
|
||||||
|
|
||||||
#define SONAR
|
#define SONAR
|
||||||
#define BEEPER
|
#define BEEPER
|
||||||
#define LED0
|
#define LED0
|
||||||
|
@ -120,3 +133,12 @@
|
||||||
// USART2, PA3
|
// USART2, PA3
|
||||||
#define BIND_PORT GPIOA
|
#define BIND_PORT GPIOA
|
||||||
#define BIND_PIN Pin_3
|
#define BIND_PIN Pin_3
|
||||||
|
|
||||||
|
// alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target)
|
||||||
|
#ifdef ALIENWII32
|
||||||
|
#define BRUSHED_MOTORS
|
||||||
|
#define HARDWARE_BIND_PLUG
|
||||||
|
// Hardware bind plug at PB5 (Pin 41)
|
||||||
|
#define BINDPLUG_PORT GPIOB
|
||||||
|
#define BINDPLUG_PIN Pin_5
|
||||||
|
#endif
|
||||||
|
|
|
@ -37,18 +37,6 @@
|
||||||
#define LED1
|
#define LED1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#define ACC
|
|
||||||
#define USE_FAKE_ACC
|
|
||||||
//#define USE_ACC_ADXL345
|
|
||||||
//#define USE_ACC_BMA280
|
|
||||||
//#define USE_ACC_MMA8452
|
|
||||||
//#define USE_ACC_LSM303DLHC
|
|
||||||
//#define USE_ACC_MPU3050
|
|
||||||
#define USE_ACC_MPU6050
|
|
||||||
//#define USE_ACC_SPI_MPU6000
|
|
||||||
//#define USE_ACC_SPI_MPU6500
|
|
||||||
|
|
||||||
#define GYRO
|
#define GYRO
|
||||||
#define USE_FAKE_GYRO
|
#define USE_FAKE_GYRO
|
||||||
//#define USE_GYRO_L3G4200D
|
//#define USE_GYRO_L3G4200D
|
||||||
|
@ -58,6 +46,16 @@
|
||||||
//#define USE_GYRO_SPI_MPU6000
|
//#define USE_GYRO_SPI_MPU6000
|
||||||
//#define USE_GYRO_SPI_MPU6500
|
//#define USE_GYRO_SPI_MPU6500
|
||||||
|
|
||||||
|
#define ACC
|
||||||
|
#define USE_FAKE_ACC
|
||||||
|
//#define USE_ACC_ADXL345
|
||||||
|
//#define USE_ACC_BMA280
|
||||||
|
//#define USE_ACC_MMA8452
|
||||||
|
//#define USE_ACC_LSM303DLHC
|
||||||
|
#define USE_ACC_MPU6050
|
||||||
|
//#define USE_ACC_SPI_MPU6000
|
||||||
|
//#define USE_ACC_SPI_MPU6500
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
//#define USE_BARO_MS5611
|
//#define USE_BARO_MS5611
|
||||||
#define USE_BARO_BMP085
|
#define USE_BARO_BMP085
|
||||||
|
|
|
@ -46,16 +46,6 @@
|
||||||
#define MPU6500_CS_PIN GPIO_Pin_12
|
#define MPU6500_CS_PIN GPIO_Pin_12
|
||||||
#define MPU6500_SPI_INSTANCE SPI2
|
#define MPU6500_SPI_INSTANCE SPI2
|
||||||
|
|
||||||
#define ACC
|
|
||||||
#define USE_FAKE_ACC
|
|
||||||
//#define USE_ACC_ADXL345
|
|
||||||
//#define USE_ACC_BMA280
|
|
||||||
//#define USE_ACC_MMA8452
|
|
||||||
//#define USE_ACC_MPU3050
|
|
||||||
#define USE_ACC_MPU6050
|
|
||||||
//#define USE_ACC_SPI_MPU6000
|
|
||||||
//#define USE_ACC_SPI_MPU6500
|
|
||||||
|
|
||||||
#define GYRO
|
#define GYRO
|
||||||
#define USE_FAKE_GYRO
|
#define USE_FAKE_GYRO
|
||||||
//#define USE_GYRO_L3G4200D
|
//#define USE_GYRO_L3G4200D
|
||||||
|
@ -65,6 +55,15 @@
|
||||||
//#define USE_GYRO_SPI_MPU6000
|
//#define USE_GYRO_SPI_MPU6000
|
||||||
//#define USE_GYRO_SPI_MPU6500
|
//#define USE_GYRO_SPI_MPU6500
|
||||||
|
|
||||||
|
#define ACC
|
||||||
|
#define USE_FAKE_ACC
|
||||||
|
//#define USE_ACC_ADXL345
|
||||||
|
//#define USE_ACC_BMA280
|
||||||
|
//#define USE_ACC_MMA8452
|
||||||
|
#define USE_ACC_MPU6050
|
||||||
|
//#define USE_ACC_SPI_MPU6000
|
||||||
|
//#define USE_ACC_SPI_MPU6500
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
#define USE_BARO_MS5611
|
#define USE_BARO_MS5611
|
||||||
//#define USE_BARO_BMP085
|
//#define USE_BARO_BMP085
|
||||||
|
|
|
@ -30,15 +30,21 @@
|
||||||
#define GYRO
|
#define GYRO
|
||||||
#define USE_GYRO_MPU6050
|
#define USE_GYRO_MPU6050
|
||||||
|
|
||||||
|
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||||
|
|
||||||
#define ACC
|
#define ACC
|
||||||
#define USE_ACC_MPU6050
|
#define USE_ACC_MPU6050
|
||||||
|
|
||||||
|
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||||
|
|
||||||
//#define BARO
|
//#define BARO
|
||||||
//#define USE_BARO_MS5611
|
//#define USE_BARO_MS5611
|
||||||
|
|
||||||
#define MAG
|
#define MAG
|
||||||
#define USE_MAG_AK8975
|
#define USE_MAG_AK8975
|
||||||
|
|
||||||
|
#define MAG_AK8975_ALIGN CW0_DEG_FLIP
|
||||||
|
|
||||||
#define LED0
|
#define LED0
|
||||||
#define LED1
|
#define LED1
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue