mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Code tidy
This commit is contained in:
Martin Budden
parent
cba9ae7011
commit
83aa2c56b9
5 changed files with 57 additions and 78 deletions
|
|
@ -17,10 +17,10 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
|
|
@ -32,27 +32,22 @@
|
|||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
||||
//#define MIXER_DEBUG
|
||||
|
||||
|
|
@ -326,14 +321,12 @@ void mixerUpdateStateFlags(void)
|
|||
|
||||
void mixerUsePWMIOConfiguration(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
motorCount = 0;
|
||||
|
||||
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
|
||||
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
// load custom mixer into currentMixer
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
// check if done
|
||||
if (customMotorMixer(i)->throttle == 0.0f)
|
||||
break;
|
||||
|
|
@ -344,7 +337,7 @@ void mixerUsePWMIOConfiguration(void)
|
|||
motorCount = MIN(mixers[currentMixerMode].motorCount, pwmGetOutputConfiguration()->motorCount);
|
||||
// copy motor-based mixers
|
||||
if (mixers[currentMixerMode].motor) {
|
||||
for (i = 0; i < motorCount; i++)
|
||||
for (int i = 0; i < motorCount; i++)
|
||||
currentMixer[i] = mixers[currentMixerMode].motor[i];
|
||||
}
|
||||
}
|
||||
|
|
@ -352,7 +345,7 @@ void mixerUsePWMIOConfiguration(void)
|
|||
// in 3D mode, mixer gain has to be halved
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (motorCount > 1) {
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
currentMixer[i].pitch *= 0.5f;
|
||||
currentMixer[i].roll *= 0.5f;
|
||||
currentMixer[i].yaw *= 0.5f;
|
||||
|
|
@ -366,8 +359,7 @@ void mixerUsePWMIOConfiguration(void)
|
|||
void mixerUsePWMIOConfiguration(void)
|
||||
{
|
||||
motorCount = 4;
|
||||
int i;
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
currentMixer[i] = mixerQuadX[i];
|
||||
}
|
||||
mixerResetDisarmedMotors();
|
||||
|
|
@ -378,46 +370,44 @@ void mixerUsePWMIOConfiguration(void)
|
|||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||
{
|
||||
int i;
|
||||
|
||||
// we're 1-based
|
||||
index++;
|
||||
// clear existing
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
customMixers[i].throttle = 0.0f;
|
||||
}
|
||||
|
||||
// do we have anything here to begin with?
|
||||
if (mixers[index].motor != NULL) {
|
||||
for (i = 0; i < mixers[index].motorCount; i++)
|
||||
for (int i = 0; i < mixers[index].motorCount; i++) {
|
||||
customMixers[i] = mixers[index].motor[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
void mixerResetDisarmedMotors(void)
|
||||
{
|
||||
int i;
|
||||
// set disarmed motor values
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
|
||||
}
|
||||
}
|
||||
|
||||
void writeMotors(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < motorCount; i++)
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
pwmWriteMotor(i, motor[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void writeAllMotors(int16_t mc)
|
||||
{
|
||||
int i;
|
||||
|
||||
// Sends commands to all motors
|
||||
for (i = 0; i < motorCount; i++)
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
motor[i] = mc;
|
||||
}
|
||||
writeMotors();
|
||||
}
|
||||
|
||||
|
|
@ -436,8 +426,6 @@ void stopPwmAllMotors()
|
|||
void mixTable(void)
|
||||
{
|
||||
int16_t input[3]; // RPY, range [-500:+500]
|
||||
int i;
|
||||
|
||||
// Allow direct stick input to motors in passthrough mode on airplanes
|
||||
if (STATE(FIXED_WING) && FLIGHT_MODE(PASSTHRU_MODE)) {
|
||||
// Direct passthru from RX
|
||||
|
|
@ -462,7 +450,7 @@ void mixTable(void)
|
|||
int16_t rpyMixMin = 0;
|
||||
|
||||
// motors for non-servo mixes
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
rpyMix[i] =
|
||||
input[PITCH] * currentMixer[i].pitch +
|
||||
input[ROLL] * currentMixer[i].roll +
|
||||
|
|
@ -509,7 +497,7 @@ void mixTable(void)
|
|||
motorLimitReached = true;
|
||||
float mixReduction = (float)throttleRange / rpyMixRange;
|
||||
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
rpyMix[i] = mixReduction * rpyMix[i];
|
||||
}
|
||||
|
||||
|
|
@ -525,7 +513,7 @@ void mixTable(void)
|
|||
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
|
||||
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
|
||||
|
||||
if (failsafeIsActive()) {
|
||||
|
|
@ -556,7 +544,7 @@ void mixTable(void)
|
|||
}
|
||||
}
|
||||
} else {
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
motor[i] = motor_disarmed[i];
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
|
@ -28,37 +27,31 @@
|
|||
#include "build/build_config.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "config/config_reset.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
|
||||
extern const mixer_t mixers[];
|
||||
|
|
@ -90,6 +83,9 @@ void pgResetFn_servoParams(servoParam_t *instance)
|
|||
}
|
||||
}
|
||||
|
||||
// no template required since default is zero
|
||||
PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
|
||||
|
||||
int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
static uint8_t servoRuleCount = 0;
|
||||
|
|
@ -100,7 +96,7 @@ static uint8_t mixerUsesServos;
|
|||
static uint8_t minServoIndex;
|
||||
static uint8_t maxServoIndex;
|
||||
|
||||
static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
|
||||
static bool servoFilterIsSet;
|
||||
|
||||
#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
|
||||
|
|
@ -162,10 +158,8 @@ const mixerRules_t servoMixers[] = {
|
|||
{ 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
};
|
||||
|
||||
// no template required since default is zero
|
||||
PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
|
||||
|
||||
int16_t getFlaperonDirection(uint8_t servoPin) {
|
||||
int16_t getFlaperonDirection(uint8_t servoPin)
|
||||
{
|
||||
if (servoPin == SERVO_FLAPPERON_2) {
|
||||
return -1;
|
||||
} else {
|
||||
|
|
@ -286,9 +280,7 @@ STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
|
|||
{
|
||||
// start forwarding from this channel
|
||||
uint8_t channelOffset = AUX1;
|
||||
|
||||
int servoOffset;
|
||||
for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
for (int servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||
}
|
||||
}
|
||||
|
|
@ -299,14 +291,14 @@ static void filterServos(void)
|
|||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
biquadFilterInitLPF(&servoFitlerState[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime);
|
||||
}
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
// Apply servo lowpass filter and do sanity cheching
|
||||
servo[i] = (int16_t) biquadFilterApply(&servoFitlerState[i], (float)servo[i]);
|
||||
servo[i] = (int16_t) biquadFilterApply(&servoFilter[i], (float)servo[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -23,46 +23,47 @@
|
|||
#include "platform.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/config_reset.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
#include "config/config_reset.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/accgyro/accgyro_adxl345.h"
|
||||
#include "drivers/accgyro/accgyro_bma280.h"
|
||||
#include "drivers/accgyro/accgyro_fake.h"
|
||||
#include "drivers/accgyro/accgyro_l3g4200d.h"
|
||||
#include "drivers/accgyro/accgyro_l3gd20.h"
|
||||
#include "drivers/accgyro/accgyro_lsm303dlhc.h"
|
||||
#include "drivers/accgyro/accgyro_mma845x.h"
|
||||
#include "drivers/accgyro/accgyro_mpu.h"
|
||||
#include "drivers/accgyro/accgyro_mpu3050.h"
|
||||
#include "drivers/accgyro/accgyro_mpu6050.h"
|
||||
#include "drivers/accgyro/accgyro_mpu6500.h"
|
||||
#include "drivers/accgyro/accgyro_l3gd20.h"
|
||||
#include "drivers/accgyro/accgyro_lsm303dlhc.h"
|
||||
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
|
||||
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
|
||||
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
|
||||
#include "drivers/logging.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
||||
|
||||
acc_t acc; // acc access functions
|
||||
|
||||
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
|
@ -62,8 +63,6 @@
|
|||
#include "sensors/gyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "common/axis.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue