Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

add sannity check in hot mixprofile switching, and fix cli

Browse source
This commit is contained in:
shota 2022-11-11 04:47:32 +09:00
parent 23968a64d2
commit 97318e4f84
9 changed files with 107 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -395,11 +395,11 @@ bool pwmMotorAndServoInit(void)
return (pwmInitError == PWM_INIT_ERROR_NONE);
}
bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs)
{
resetAllocatedOutputPortCount();
pwmInitMotors(timOutputs);
pwmInitServos(timOutputs);
// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs)
// {
// resetAllocatedOutputPortCount();
// pwmInitMotors(timOutputs);
// pwmInitServos(timOutputs);
return (pwmInitError == PWM_INIT_ERROR_NONE);
}
// return (pwmInitError == PWM_INIT_ERROR_NONE);
// }

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

@ -81,7 +81,7 @@ typedef struct {
void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos);
bool pwmMotorAndServoInit(void);
bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs);
// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs);
const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto);
pwmInitError_e getPwmInitError(void);
const char * getPwmInitErrorMessage(void);

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

@ -3077,12 +3077,8 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask)
cliPrintLinef("mixer_profile %d\r\n", getConfigMixerProfile() + 1);
dumpAllValues(MIXER_CONFIG_VALUE, dumpMask);
cliPrintHashLine("mixer");
cliPrintHashLine("dumpMask");
char buf0[FTOA_BUFFER_SIZE];
itoa(dumpMask,buf0,2);
cliPrintHashLine(buf0);
cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n");
printMotorMix(dumpMask, primaryMotorMixer(0), NULL);
cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n");
printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0));
}
#ifdef USE_CLI_BATCH

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

@ -11,7 +11,8 @@
#include "drivers/pwm_mapping.h"
#include "drivers/time.h"
#include "flight/mixer.h"
// #include "flight/pid.h"
#include "common/axis.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "fc/config.h"
@ -20,6 +21,8 @@
#include "common/log.h"
#include "navigation/navigation.h"
PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
void pgResetFn_mixerProfiles(mixerProfile_t *instance)
@ -43,62 +46,95 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
// PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
bool OutputProfileHotSwitch(int profile_index)
{
// does not work with timerHardwareOverride
LOG_INFO(PWM, "OutputProfileHotSwitch");
//do not allow switching between multi rotor and non multi rotor
#ifdef ENABLE_MCFW_MIXER_PROFILE_HOTSWAP
bool MCFW_hotswap_unavailable = false;
#else
bool MCFW_hotswap_unavailable = true;
#endif
uint8_t old_platform_type = mixerConfig()->platformType;
uint8_t new_platform_type = mixerConfigByIndex(profile_index)->platformType;
bool old_platform_type_mc = old_platform_type == PLATFORM_MULTIROTOR || old_platform_type == PLATFORM_TRICOPTER;
bool new_platform_type_mc = new_platform_type == PLATFORM_MULTIROTOR || new_platform_type == PLATFORM_TRICOPTER;
bool is_mcfw_switching = old_platform_type_mc ^ new_platform_type_mc;
if (MCFW_hotswap_unavailable && is_mcfw_switching)
{
LOG_INFO(PWM, "MCFW_hotswap_unavailable");
return false;
}
//do not allow switching in navigation mode
if (ARMING_FLAG(ARMED) && navigationInAnyMode()){
LOG_INFO(PWM, "navModesEnabled");
return false;
}
if (!setConfigMixerProfile(profile_index)){
LOG_INFO(PWM, "failed to set config");
return false;
}
// stopMotors();
writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors
servosInit();
mixerUpdateStateFlags();
mixerInit();
if(old_platform_type!=mixerConfig()->platformType)
{
pidInit();
pidInitFilters();
schedulePidGainsUpdate();
navigationInit();
}
return true;
}
int min_ab(int a,int b)
{
return a > b ? b : a;
}
bool OutputProfileHotSwitch(int profile_index)
void checkOutputMapping(int profile_index)
{
#ifdef ENABLE_MIXER_PROFILE_HOTSWAP
// does not work with timerHardwareOverride
LOG_INFO(PWM, "OutputProfileHotSwitch");
//store current output for check
uint8_t old_platform_type=mixerConfig()->platformType;
timMotorServoHardware_t old_timOutputs;
pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos());
if (!setConfigMixerProfile(profile_index)){
return false;
}
stopMotors();
// delay(3000); //check motor stop
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, "motor_output_type_not_changed:%d,%d,%d",motor_output_type_not_changed,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, "servo_output_type_not_changed:%d,%d,%d",servo_output_type_not_changed,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, "pwmMotorAndServoHotInit");
// pwmMotorAndServoHotInit(&timOutputs);
// }
if(old_platform_type!=mixerConfig()->platformType)
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);
if(!motor_output_type_not_changed || !servo_output_type_not_changed){
LOG_INFO(PWM, "pwm output mapping has changed");
}
return true;
#else
profile_index=0; //prevent compilier warning: parameter 'profile_index' set but not used
return (bool) profile_index;
#endif
}

5
src/main/flight/mixer_profile.h
View file

@ -22,4 +22,9 @@ PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles);
#define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index])
#define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index))
static inline const mixerProfile_t* mixerProfiles_CopyArray_macro(int _index) { return &mixerProfiles_CopyArray[_index]; }
#define primaryMotorMixer_CopyArray() (mixerProfiles_CopyArray_macro(systemConfig()->current_mixer_profile_index)->MotorMixers)
#define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config))
bool OutputProfileHotSwitch(int profile_index);

7
src/main/navigation/navigation.c
View file

@ -4263,6 +4263,13 @@ bool navigationIsExecutingAnEmergencyLanding(void)
return navGetCurrentStateFlags() & NAV_CTL_EMERG;
}
bool navigationInAnyMode(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return !(stateFlags ==0);
}
bool navigationInAutomaticThrottleMode(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();

1
src/main/navigation/navigation.h
View file

@ -582,6 +582,7 @@ void abortForcedEmergLanding(void);
emergLandState_e getStateOfForcedEmergLanding(void);
/* Getter functions which return data about the state of the navigation system */
bool navigationInAnyMode(void);
bool navigationInAutomaticThrottleMode(void);
bool navigationIsControllingThrottle(void);
bool isFixedWingAutoThrottleManuallyIncreased(void);

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

@ -369,10 +369,7 @@ static int logicConditionCompute(
case LOGIC_CONDITION_SET_MIXER_PROFILE:
operandA--;
if ( getConfigMixerProfile() != operandA && (operandA >= 0 && operandA < MAX_MIXER_PROFILE_COUNT)) {
bool mixerprofileChanged = false;
if (OutputProfileHotSwitch(operandA)) {
mixerprofileChanged = true;
}
bool mixerprofileChanged = OutputProfileHotSwitch(operandA);
return mixerprofileChanged;
} else {
return false;

2
src/main/target/MATEKF405/target.h
View file

@ -197,5 +197,5 @@
#define MAX_PWM_OUTPUT_PORTS 6
#ifdef MATEKF405VTOL
#define ENABLE_MIXER_PROFILE_HOTSWAP
#define ENABLE_MCFW_MIXER_PROFILE_HOTSWAP
#endif