Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

mixprofile development

Browse source
This commit is contained in:
haoxiang.qiu 2022-11-14 18:43:16 +09:00
parent cbbef19721
commit 67390e12ed
12 changed files with 116 additions and 153 deletions
Download patch file Download diff file Expand all files Collapse all files

19
src/main/drivers/pwm_mapping.c
View file

@ -47,6 +47,13 @@ enum {
MAP_TO_SERVO_OUTPUT,
};
typedef struct {
int maxTimMotorCount;
int maxTimServoCount;
const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS];
const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS];
} timMotorServoHardware_t;
static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE;
static const char * pwmInitErrorMsg[] = {
@ -220,12 +227,10 @@ static void timerHardwareOverride(timerHardware_t * timer) {
void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos)
{
LOG_INFO(PWM, "pwmBuildTimerOutputList");
timOutputs->maxTimMotorCount = 0;
timOutputs->maxTimServoCount = 0;
uint8_t motorCount = getMotorCount();
LOG_INFO(PWM, "motorCount %d", motorCount);
uint8_t motorIdx = 0;
for (int idx = 0; idx < timerHardwareCount; idx++) {
@ -387,6 +392,7 @@ bool pwmMotorAndServoInit(void)
// Build temporary timer mappings for motor and servo
pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
// At this point we have built tables of timers suitable for motor and servo mappings
// Now we can actually initialize them according to motor/servo count from mixer
pwmInitMotors(&timOutputs);
@ -394,12 +400,3 @@ bool pwmMotorAndServoInit(void)
return (pwmInitError == PWM_INIT_ERROR_NONE);
}
// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs)
// {
// resetAllocatedOutputPortCount();
// pwmInitMotors(timOutputs);
// pwmInitServos(timOutputs);
// return (pwmInitError == PWM_INIT_ERROR_NONE);
// }

10
src/main/drivers/pwm_mapping.h
View file

@ -18,7 +18,6 @@
#pragma once
#include "drivers/io_types.h"
#include "drivers/timer.h"
#include "flight/mixer_profile.h"
#include "flight/servos.h"
@ -62,13 +61,6 @@ typedef enum {
PWM_INIT_ERROR_TIMER_INIT_FAILED,
} pwmInitError_e;
typedef struct {
int maxTimMotorCount;
int maxTimServoCount;
const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS];
const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS];
} timMotorServoHardware_t;
typedef struct rangefinderIOConfig_s {
ioTag_t triggerTag;
ioTag_t echoTag;
@ -79,9 +71,7 @@ typedef struct {
bool isDSHOT;
} motorProtocolProperties_t;
void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos);
bool pwmMotorAndServoInit(void);
// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs);
const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto);
pwmInitError_e getPwmInitError(void);
const char * getPwmInitErrorMessage(void);

4
src/main/drivers/pwm_output.c
View file

@ -113,10 +113,6 @@ static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
static currentExecutingCommand_t currentExecutingCommand;
#endif
void resetAllocatedOutputPortCount(void){
allocatedOutputPortCount = 0;
}
static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
{
p->tch = tch;

1
src/main/drivers/pwm_output.h
View file

@ -34,7 +34,6 @@ void pwmRequestMotorTelemetry(int motorIndex);
ioTag_t pwmGetMotorPinTag(int motorIndex);
void resetAllocatedOutputPortCount(void);
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteMotorUpdate(void);

33
src/main/fc/cli.c
View file

@ -81,7 +81,6 @@ bool cliMode = false;
#include "flight/failsafe.h"
#include "flight/imu.h"
// #include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "flight/pid.h"
#include "flight/servos.h"
@ -283,7 +282,6 @@ typedef enum {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
DUMP_BATTERY_PROFILE = (1 << 2),
// DUMP_RATES = (1 << 3),
DUMP_MIXER_PROFILE = (1 << 3),
DUMP_ALL = (1 << 4),
DO_DIFF = (1 << 5),
@ -1864,7 +1862,6 @@ static void cliServoMix(char *cmdline)
printServoMix(DUMP_MASTER, customServoMixers(0), NULL);
} else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
// pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER);
Reset_servoMixers(customServoMixersMutable(0));
} else {
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT};
@ -3626,17 +3623,6 @@ static void printConfig(const char *cmdline, bool doDiff)
}
cliPrintHashLine("resources");
// printResource(dumpMask, &defaultConfig);
// cliPrintHashLine("mixer");
// cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n");
// printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0));
// print custom servo mixer if exists
// cliPrintHashLine("servo mixer");
// cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n");
// printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0));
// print servo parameters
cliPrintHashLine("servo");
@ -3720,34 +3706,39 @@ static void printConfig(const char *cmdline, bool doDiff)
const int currentProfileIndexSave = getConfigProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
const int currentMixerProfileIndexSave = getConfigMixerProfile();
for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) {
cliDumpMixerProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
cliDumpProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) {
cliDumpBatteryProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) {
cliDumpMixerProfile(ii, dumpMask);
}
setConfigProfile(currentProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
setConfigMixerProfile(currentMixerProfileIndexSave);
cliPrintHashLine("restore original profile selection");
cliPrintLinef("profile %d", currentProfileIndexSave + 1);
cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1);
cliPrintLinef("profile %d", currentProfileIndexSave + 1);
cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);
#ifdef USE_CLI_BATCH
batchModeEnabled = false;
#endif
} else {
// dump just the current profiles
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
cliDumpProfile(getConfigProfile(), dumpMask);
cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
}
}
if (dumpMask & DUMP_MIXER_PROFILE) {
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
}
if (dumpMask & DUMP_PROFILE) {
cliDumpProfile(getConfigProfile(), dumpMask);
}
@ -3756,10 +3747,6 @@ static void printConfig(const char *cmdline, bool doDiff)
cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
}
if (dumpMask & DUMP_MIXER_PROFILE) {
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
}
if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) {
cliPrintHashLine("save configuration\r\nsave");
}

1
src/main/fc/fc_msp.c
View file

@ -3133,7 +3133,6 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src)
break;
}
// If the setting uses a table, send each possible string (null terminated)
if (SETTING_MODE(setting) == MODE_LOOKUP) {
for (int ii = (int)min; ii <= (int)max; ii++) {

29
src/main/flight/mixer.c
View file

@ -83,34 +83,6 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT,
.neutral = SETTING_3D_NEUTRAL_DEFAULT
);
// PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
// void pgResetFn_mixerProfiles(mixerProfile_t *instance)
// {
// for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) {
// RESET_CONFIG(mixerProfile_t, &instance[i],
// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
// .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
// .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
// .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
// .outputMode = SETTING_OUTPUT_MODE_DEFAULT,
// );
// motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0};
// for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) {
// instance->MotorMixer[j] = tmp_mixer;
// }
// }
// }
// PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_PROFILE, 5);
// PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
// .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
// .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
// .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
// .outputMode = SETTING_OUTPUT_MODE_DEFAULT,
// );
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9);
@ -436,6 +408,7 @@ void FAST_CODE writeMotors(void)
// We don't define USE_DSHOT
motorValue = motor[i];
#endif
pwmWriteMotor(i, motorValue);
}
}

20
src/main/flight/mixer.h
View file

@ -25,7 +25,6 @@
#define MAX_SUPPORTED_MOTORS 12
#endif
// Digital protocol has fixed values
#define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48
@ -63,8 +62,6 @@ typedef struct motorMixer_s {
float yaw;
} motorMixer_t;
// PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
typedef struct mixerConfig_s {
int8_t motorDirectionInverted;
uint8_t platformType;
@ -73,22 +70,6 @@ typedef struct mixerConfig_s {
uint8_t outputMode;
} mixerConfig_t;
// typedef struct mixerProfile_s {
// int8_t motorDirectionInverted;
// uint8_t platformType;
// bool hasFlaps;
// int16_t appliedMixerPreset;
// uint8_t outputMode;
// motorMixer_t MotorMixer[MAX_SUPPORTED_MOTORS];
// } mixerProfile_t;
// PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles);
// #define mixerConfig() mixerProfiles(systemConfig()->current_mixer_profile_index)
// #define mixerConfigMutable() ((mixerProfile_t *)mixerConfig())
// #define primaryMotorMixer(_index) (&((mixerConfig()->MotorMixer)[_index]))
// #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index))
// extern motorMixer_t primaryMotorMixer_CopyArray[12];
typedef struct reversibleMotorsConfig_s {
uint16_t deadband_low; // min 3d value
uint16_t deadband_high; // max 3d value
@ -142,7 +123,6 @@ void processServoAutotrim(const float dT);
void processServoAutotrimMode(void);
void processContinuousServoAutotrim(const float dT);
void stopMotors(void);
void stopAndDisableMotors(void);
void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void);

131
src/main/flight/mixer_profile.c
View file

@ -59,13 +59,54 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
}
}
// PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
static int computeMotorCountByMixerProfileIndex(int index)
{
int motorCount = 0;
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (mixerMotorMixersByIndex(index)[i]->throttle == 0.0f) {
break;
}
motorCount++;
}
return motorCount;
}
static int computeServoCountByMixerProfileIndex(int index)
{
int servoRuleCount = 0;
int minServoIndex = 255;
int maxServoIndex = 0;
for (int i = 0; i < MAX_SERVO_RULES; i++) {
// check if done
if (mixerServoMixersByIndex(index)[i]->rate == 0)
break;
if (mixerServoMixersByIndex(index)[i]->targetChannel < minServoIndex) {
minServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel;
}
if (mixerServoMixersByIndex(index)[i]->targetChannel > maxServoIndex) {
maxServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel;
}
servoRuleCount++;
}
if (servoRuleCount) {
return 1 + maxServoIndex - minServoIndex;
}
else {
return 0;
}
}
bool OutputProfileHotSwitch(int profile_index)
{
// does not work with timerHardwareOverride
LOG_INFO(PWM, "OutputProfileHotSwitch");
if (profile_index >= MAX_MIXER_PROFILE_COUNT) {// sanity check
LOG_INFO(PWM, "invalid profile index");
return false;
}
//do not allow switching between multi rotor and non multi rotor
#ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP
bool MCFW_hotswap_unavailable = false;
@ -88,8 +129,12 @@ bool OutputProfileHotSwitch(int profile_index)
LOG_INFO(PWM, "navModesEnabled");
return false;
}
//TODO add check of each motor/servo is mapped before and after the switch
//do not allow switching if motor or servos counts has changed
if ((getMotorCount() != computeMotorCountByMixerProfileIndex(profile_index)) || (getServoCount() != computeServoCountByMixerProfileIndex(profile_index)))
{
LOG_INFO(PWM, "motor/servo count will change");
return false;
}
if (!setConfigMixerProfile(profile_index)){
LOG_INFO(PWM, "failed to set config");
return false;
@ -110,47 +155,47 @@ bool OutputProfileHotSwitch(int profile_index)
return true;
}
int min_ab(int a,int b)
{
return a > b ? b : a;
}
// static int min_ab(int a,int b)
// {
// return a > b ? b : a;
// }
void checkOutputMapping(int profile_index)//debug purpose
{
timMotorServoHardware_t old_timOutputs;
pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos());
stopMotors();
delay(1000); //check motor stop
if (!setConfigMixerProfile(profile_index)){
LOG_INFO(PWM, "failed to set config");
return;
}
servosInit();
mixerUpdateStateFlags();
mixerInit();
timMotorServoHardware_t timOutputs;
pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount;
bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount;
LOG_INFO(PWM, "maxTimMotorCount:%d,%d",old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount);
for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++)
{
LOG_INFO(PWM, "motor_output_type_not_changed:%d,%d",i,motor_output_type_not_changed);
motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag;
}
LOG_INFO(PWM, "motor_output_type_not_changed:%d",motor_output_type_not_changed);
// void checkOutputMapping(int profile_index)//debug purpose
// {
// timMotorServoHardware_t old_timOutputs;
// pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos());
// stopMotors();
// delay(1000); //check motor stop
// if (!setConfigMixerProfile(profile_index)){
// LOG_INFO(PWM, "failed to set config");
// return;
// }
// servosInit();
// mixerUpdateStateFlags();
// mixerInit();
// timMotorServoHardware_t timOutputs;
// pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
// bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount;
// bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount;
// LOG_INFO(PWM, "maxTimMotorCount:%d,%d",old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount);
// for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++)
// {
// LOG_INFO(PWM, "motor_output_type_not_changed:%d,%d",i,motor_output_type_not_changed);
// motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag;
// }
// LOG_INFO(PWM, "motor_output_type_not_changed:%d",motor_output_type_not_changed);
LOG_INFO(PWM, "maxTimServoCount:%d,%d",old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount);
for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++)
{
LOG_INFO(PWM, "servo_output_type_not_changed:%d,%d",i,servo_output_type_not_changed);
servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag;
}
LOG_INFO(PWM, "servo_output_type_not_changed:%d",servo_output_type_not_changed);
// LOG_INFO(PWM, "maxTimServoCount:%d,%d",old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount);
// for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++)
// {
// LOG_INFO(PWM, "servo_output_type_not_changed:%d,%d",i,servo_output_type_not_changed);
// servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag;
// }
// LOG_INFO(PWM, "servo_output_type_not_changed:%d",servo_output_type_not_changed);
if(!motor_output_type_not_changed || !servo_output_type_not_changed){
LOG_INFO(PWM, "pwm output mapping has changed");
}
}
// if(!motor_output_type_not_changed || !servo_output_type_not_changed){
// LOG_INFO(PWM, "pwm output mapping has changed");
// }
// }

6
src/main/programming/logic_condition.c
View file

@ -46,15 +46,11 @@
#include "sensors/rangefinder.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/mixer_profile.h"
#include "drivers/io_port_expander.h"
#include "io/osd_common.h"
#include "sensors/diagnostics.h"
#include "flight/mixer_profile.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "drivers/pwm_mapping.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"

2
src/main/target/MATEKF405TE/target.c
View file

@ -26,7 +26,7 @@
timerHardware_t timerHardware[] = {
#ifdef MATEKF405TE_SD_VTOL
//using d-shot on motors seems to have problems on s3,maybe dma related,maybe my board problem
//With INAV firmware, DSHOT can not work on S3, S5,S7 because of DMA clash, pls use ONESHOT or MULTISHOT and calibrate ESC PWM range.<-copied from matek website
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217
DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2,6,0) UP256

9
src/main/target/MATEKF405TE/target.h
View file

@ -18,6 +18,11 @@
#pragma once
#define USE_TARGET_CONFIG
#ifdef MATEKF405TE_SD_VTOL
#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP
#define MATEKF405TE_SD
#endif
#if defined(MATEKF405TE_SD)
# define TARGET_BOARD_IDENTIFIER "MF4T"
# define USBD_PRODUCT_STRING "MatekF405TE_SD"
@ -179,7 +184,3 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_DSHOT
#define USE_ESC_SENSOR
#ifdef MATEKF405TE_SD_VTOL
#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP
#endif