Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix INRM301 failsafe BUG #3298

Merged
merged 3 commits into from
Mar 22, 2023
Merged

fix INRM301 failsafe BUG #3298

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
14b2b83
fix: Automatic power on and INRM301 failsafe BUG
Mar 4, 2023
ccdee09
fix EL18 Automatic power on
Mar 7, 2023
8312b9a
Merge branch 'main' into INRM301-FailSafe
Xy2019 Mar 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 42 additions & 29 deletions radio/src/pulses/afhds3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@
#define FAILSAFE_HOLD 1
#define FAILSAFE_CUSTOM 2

#define FAILSAFE_KEEP_LAST 0x8000

#define FAILSAFE_HOLD_VALUE 0x8000
#define FAILSAFE_NOPULSES_VALUE 0x8001
//get channel value outside of afhds3 namespace
int32_t getChannelValue(uint8_t channel);

Expand Down Expand Up @@ -225,7 +225,7 @@ class ProtoState

void requestInfoAndRun(bool send = false);

uint8_t setFailSafe(int16_t* target);
uint8_t setFailSafe(int16_t* target, uint8_t rfchannelcount=AFHDS3_MAX_CHANNELS);

inline int16_t convert(int channelValue);

Expand Down Expand Up @@ -276,7 +276,7 @@ class ProtoState
*/
ModuleVersion version;
};

static const char* const moduleStateText[] =
{
"Not ready",
Expand Down Expand Up @@ -399,7 +399,7 @@ void ProtoState::setupFrame()
if (this->state != ModuleState::STATE_STANDBY) {
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
return;
} else {
modelIDSet = true;
modelID = g_model.header.modelId[module_index];
Expand All @@ -416,13 +416,13 @@ void ProtoState::setupFrame()
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
}

if (this->state == ModuleState::STATE_STANDBY) {
cmdCount = 0;
requestInfoAndRun(true);
return;
}

// exit bind
if (this->state == STATE_BINDING) {
TRACE("AFHDS3 [EXIT BIND]");
Expand All @@ -441,16 +441,26 @@ void ProtoState::setupFrame()
COMMAND cmd = periodicRequestCommands[cmdIndex++];

if (cmd == COMMAND::VIRTUAL_FAILSAFE) {
if (isConnected() && !hasTelemetry()) {
TRACE("AFHDS ONE WAY FAILSAFE");
Config_u* cfg = this->getConfig();
uint8_t len =_phyMode_channels[cfg->v0.PhyMode];
if (!hasTelemetry()) {
uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
setFailSafe((int16_t*)(&failSafe[1]));
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
setFailSafe((int16_t*)(&failSafe[1]), len);
TRACE("AFHDS ONE WAY FAILSAFE");
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
AFHDS3_MAX_CHANNELS * 2 + 2);
return;
}
else if( isConnected() ){
uint8_t data[AFHDS3_MAX_CHANNELS*2 + 3] = { (uint8_t)(RX_CMD_FAILSAFE_VALUE&0xFF), (uint8_t)((RX_CMD_FAILSAFE_VALUE>>8)&0xFF), (uint8_t)(2*len)};
int16_t failSafe[18];
setFailSafe(&failSafe[0], len);
std::memcpy( &data[3], failSafe, len );
trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, 2*len+3);
return;
}
} else {
trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
return;
Expand Down Expand Up @@ -794,34 +804,37 @@ void ProtoState::applyConfigFromModel()

inline int16_t ProtoState::convert(int channelValue)
{
//pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
//988 - 750 = 238
//238 * 20 = 4760
//2250 - 2012 = 238
//238 * 20 = 4760
// 988 ---- 2012
//-10240 ---- 10240
//-1024 ---- 1024
return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 10, AFHDS3_FAILSAFE_MAX);
return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 14.65, AFHDS3_FAILSAFE_MAX);
}

uint8_t ProtoState::setFailSafe(int16_t* target)
uint8_t ProtoState::setFailSafe(int16_t* target, uint8_t rfchannelsCount )
{
int16_t pulseValue = 0;
uint8_t channels_start = moduleData->channelsStart;
uint8_t channels_last = channels_start + 8 + moduleData->channelsCount;

for (uint8_t channel = channels_start; channel < channels_last; channel++) {
uint8_t channelsCount = 8 + moduleData->channelsCount;
uint8_t channels_last = channels_start + channelsCount;
std::memset(target, 0, 2*rfchannelsCount );
for (uint8_t channel = channels_start, i=0; i<rfchannelsCount && channel < channels_last; channel++, i++) {
if (moduleData->failsafeMode == FAILSAFE_CUSTOM) {
pulseValue = convert(g_model.failsafeChannels[channel]);
if(FAILSAFE_CHANNEL_HOLD==g_model.failsafeChannels[channel]){
pulseValue = FAILSAFE_HOLD_VALUE;
}else if(FAILSAFE_CHANNEL_NOPULSE==g_model.failsafeChannels[channel]){
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
else{
pulseValue = convert(g_model.failsafeChannels[channel]);
}
}
else if (moduleData->failsafeMode == FAILSAFE_HOLD) {
pulseValue = FAILSAFE_KEEP_LAST;
pulseValue = FAILSAFE_HOLD_VALUE;
}
else if (moduleData->failsafeMode == FAILSAFE_NOPULSES) {
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
else {
pulseValue = convert(::getChannelValue(channel));
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
target[channel - channels_start] = pulseValue;
target[i] = pulseValue;
}
//return max channels because channel count can not be change after bind
return (uint8_t) (AFHDS3_MAX_CHANNELS);
Expand Down Expand Up @@ -871,7 +884,7 @@ static void* initModule(uint8_t module)
}

if (!mod_st) return nullptr;

auto p_state = &protoState[module];
p_state->init(module, pulsesGetModuleBuffer(module), mod_st, fAddr);
mod_st->user_data = (void*)p_state;
Expand Down
16 changes: 16 additions & 0 deletions radio/src/pulses/afhds3_transport.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,22 @@ enum COMMAND : uint8_t {
UNDEFINED = 0xFF
};

#define RX_CMD_RANGE ( 0x7013 )
#define RX_CMD_FAILSAFE_VALUE ( 0x6011 )
#define RX_CMD_FAILSAFE_TIME ( 0x6012 )
#define RX_CMD_RSSI_CHANNEL_SETUP ( 0x602B )
#define RX_CMD_GET_CAPABILITIES ( 0x7015 )
#define RX_CMD_OUT_PWM_PPM_MODE ( 0x7016 ) //PWM or PPM
#define RX_CMD_FREQUENCY_V0 ( 0x7017 )
#define RX_CMD_PORT_TYPE_V1 ( 0x7027 )
#define RX_CMD_FREQUENCY_V1 ( 0x7028 )
#define RX_CMD_FREQUENCY_V1_2 ( 0x7028 )
#define RX_CMD_BUS_TYPE_V0 ( 0x7018 ) //I-BUS/S-BUS
#define RX_CMD_IBUS_SETUP ( 0x7019 )
#define RX_CMD_IBUS_DIRECTION ( 0x7020 ) //IBUS INPUT or OUTPUT
#define RX_CMD_BUS_FAILSAFE ( 0x702A )
#define RX_CMD_GET_VERSION ( 0x701F )

// one byte frames for request queue
struct Frame {
enum COMMAND command;
Expand Down
22 changes: 12 additions & 10 deletions radio/src/targets/nv14/board.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/

#include "board.h"
#include "boards/generic_stm32/module_ports.h"

Expand Down Expand Up @@ -166,7 +166,7 @@ void boardInit()

// detect NV14 vs EL18
hardwareOptions.pcbrev = boardGetPcbRev();

#if defined(DEBUG)
serialInit(SP_AUX1, UART_MODE_DEBUG);
#endif
Expand Down Expand Up @@ -197,9 +197,9 @@ void boardInit()
} else {

// prime debounce state...
usbPlugged();

while (usbPlugged()) {
uint8_t usb_state = usbPlugged();
usb_state |= usbPlugged();
while (usb_state) {
uint32_t now = get_tmr10ms();
if (pwrPressed()) {
press_end = now;
Expand All @@ -208,6 +208,8 @@ void boardInit()
pwrOn();
break;
}
} else if(!usbPlugged()){
boardOff();
} else {
uint32_t press_end_touch = press_end;
if (touchPanelEventOccured()) {
Expand Down Expand Up @@ -235,8 +237,8 @@ void boardInit()
#if defined(RTCLOCK)
rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif


#if defined(DEBUG)
DBGMCU_APB1PeriphConfig(
DBGMCU_IWDG_STOP | DBGMCU_TIM1_STOP | DBGMCU_TIM2_STOP |
Expand All @@ -249,7 +251,7 @@ void boardInit()
}

void boardOff()
{
{
lcdOff();

while (pwrPressed()) {
Expand Down Expand Up @@ -289,7 +291,7 @@ void boardOff()
}


// We reach here only in forced power situations, such as hw-debugging with external power
// We reach here only in forced power situations, such as hw-debugging with external power
// Enter STM32 stop mode / deep-sleep
// Code snippet from ST Nucleo PWR_EnterStopMode example
#define PDMode 0x00000000U
Expand All @@ -303,7 +305,7 @@ void boardOff()

/* Set SLEEPDEEP bit of Cortex System Control Register */
SET_BIT(SCB->SCR, ((uint32_t)SCB_SCR_SLEEPDEEP_Msk));

// To avoid HardFault at return address, end in an endless loop
while (1) {

Expand Down