274 lines
8.9 KiB
C
274 lines
8.9 KiB
C
/**********************************************************************
|
|
* NAME: failsafe_toggles.c *
|
|
* AUTHOR: Jonas Holmberg *
|
|
* PURPOSE: Give the ability to create boolean flags and failsafe *
|
|
* variables. *
|
|
* INFORMATION: Create any boolean variable that should be able to be *
|
|
* used throughout the system. These can be boolean vales *
|
|
* for standard checks, failsafes etc. These can also be *
|
|
* linked to a RC channel and value range wherein the *
|
|
* value will be set to one. *
|
|
* *
|
|
* GLOBAL VARIABLES: *
|
|
* Variable Type Description *
|
|
* -------- ---- ----------- *
|
|
* systemFlags boolFlags_t Represents a bitfield of all the *
|
|
* boolean values in the system. *
|
|
* flagConfigArr flags_Configuration_t Configuration values for any *
|
|
* flag that should be changeable *
|
|
* from the RC controller. *
|
|
**********************************************************************/
|
|
|
|
#include "drivers/failsafe_toggles.h"
|
|
#include "drivers/sbus.h"
|
|
|
|
/* Stuct containing values for all the flags and failsafe booleans sin the system */
|
|
boolFlags_t systemFlags = {{0}};
|
|
|
|
/* Array of flag configurations. These are values that can be set by RC, should be read from eeprom. */
|
|
flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
|
|
[FLAG_CONFIGURATION_ARM] = {
|
|
.minRange = 1800,
|
|
.maxRange = 2100,
|
|
.channelNumber = 8,
|
|
.flagId = systemFlags_armed_id,
|
|
},
|
|
[FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = {
|
|
.minRange = 1400,
|
|
.maxRange = 2100,
|
|
.channelNumber = 6,
|
|
.flagId = systemFlags_flightmode_acceleromter_id,
|
|
},
|
|
[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
|
|
.minRange = 0,
|
|
.maxRange = 0,
|
|
.channelNumber = 0,
|
|
.flagId = systemFlags_flightmode_barometer_id,
|
|
},
|
|
[FLAG_CONFIGURATION_FLIGHTMODE_COMPASS] = {
|
|
.minRange = 0,
|
|
.maxRange = 0,
|
|
.channelNumber = 0,
|
|
.flagId = systemFlags_flightmode_compass_id,
|
|
},
|
|
[FLAG_CONFIGURATION_FLIGHTMODE_GPS] = {
|
|
.minRange = 0,
|
|
.maxRange = 0,
|
|
.channelNumber = 0,
|
|
.flagId = systemFlags_flightmode_gps_id,
|
|
},
|
|
[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
|
|
.minRange = 1900,
|
|
.maxRange = 2100,
|
|
.channelNumber = 5,
|
|
.flagId = systemFlags_mixerfullscale_id,
|
|
},
|
|
[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
|
|
.minRange = 900,
|
|
.maxRange = 1100,
|
|
.channelNumber = 5,
|
|
.flagId = systemFlags_mixerlowscale_id,
|
|
},
|
|
[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
|
|
.minRange = 0,
|
|
.maxRange = 0,
|
|
.channelNumber = 0,
|
|
.flagId = systemFlags_flightMode_3_id,
|
|
},
|
|
|
|
/*Stick controls*/
|
|
[FLAG_CONFIGURATION_THROTTLEMAX] = {
|
|
.minRange = 1950,
|
|
.maxRange = 2000,
|
|
.channelNumber = 3,
|
|
.flagId = systemFlags_throttleMax_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKLEFT] = { //negative
|
|
.minRange = 1000,
|
|
.maxRange = 1100,
|
|
.channelNumber = 1,
|
|
.flagId = systemFlags_stickLeft_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKRIGHT] = { //positive
|
|
.minRange = 1900,
|
|
.maxRange = 2000,
|
|
.channelNumber = 1,
|
|
.flagId = systemFlags_stickRight_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKUP] = { //negative
|
|
.minRange = 1000,
|
|
.maxRange = 1100,
|
|
.channelNumber = 2,
|
|
.flagId = systemFlags_stickUp_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKDOWN] = { //positive
|
|
.minRange = 1900,
|
|
.maxRange = 2000,
|
|
.channelNumber = 2,
|
|
.flagId = systemFlags_stickDown_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKCENTERH] = {
|
|
.minRange = 1400,
|
|
.maxRange = 1600,
|
|
.channelNumber = 1,
|
|
.flagId = systemFlags_stickCenterH_id,
|
|
},
|
|
[FLAG_CONFIGURATION_STICKCENTERV] = {
|
|
.minRange = 1400,
|
|
.maxRange = 1600,
|
|
.channelNumber = 2,
|
|
.flagId = systemFlags_stickCenterV_id,
|
|
},
|
|
[FLAG_CONFIGURATION_THROTTLELEFT] = {
|
|
.minRange = 2000,
|
|
.maxRange = 1900,
|
|
.channelNumber = 4,
|
|
.flagId = systemFlags_throttleLeft_id,
|
|
}
|
|
|
|
|
|
};
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Could be used to set start values for some values
|
|
* INFORMATION: Possible to set the values for any of the failsafes
|
|
***********************************************************************/
|
|
void initFlags()
|
|
{
|
|
//Could be used to set start values for failsafes.
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Process RC channels that should operate on some flags
|
|
* INFORMATION: Reads the value of the RC channels that and checks if
|
|
* any of the channels should handle some flag value in the
|
|
* system. If it should it will update its state.
|
|
***********************************************************************/
|
|
void flags_ProcessRcChannel(int rcChannel_ID, int value)
|
|
{
|
|
for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++)
|
|
{
|
|
if (flagConfigArr[i].channelNumber == rcChannel_ID)
|
|
{
|
|
if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange)
|
|
{
|
|
flags_Set_ID(flagConfigArr[i].flagId);
|
|
}
|
|
else
|
|
{
|
|
flags_Clear_ID(flagConfigArr[i].flagId);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Process RC channels that should operate on some flags
|
|
* INFORMATION: Reads the value of the RC channels that and checks if
|
|
* any of the channels should handle some flag value in the
|
|
* system. If it should it will update its state.
|
|
***********************************************************************/
|
|
void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
|
|
{
|
|
//Loop through all the active flag configuration values
|
|
for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++)
|
|
{
|
|
int currentChannelNumb = flagConfigArr[i].channelNumber;
|
|
//Check if the current Flag channel is within the set bounds
|
|
if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel)
|
|
{
|
|
//Get the value for the channel that the current flag is linked to
|
|
int value = getChannelValue(sbusChannelData, currentChannelNumb);
|
|
|
|
if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange)
|
|
{
|
|
flags_Set_ID(flagConfigArr[i].flagId);
|
|
}
|
|
else
|
|
{
|
|
flags_Clear_ID(flagConfigArr[i].flagId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Set flag to true(1)
|
|
* INFORMATION: Given a mask set that value to true
|
|
***********************************************************************/
|
|
void flags_Set_MASK(int mask)
|
|
{
|
|
systemFlags.intRepresentation |= mask;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Set flag to false(0)
|
|
* INFORMATION: Given a mask set that value to false
|
|
***********************************************************************/
|
|
void flags_Clear_MASK(int mask)
|
|
{
|
|
systemFlags.intRepresentation &= ~(mask);
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Toggle a flag
|
|
* INFORMATION: Given a mask changes the current value of a flag to its
|
|
* opposite.
|
|
***********************************************************************/
|
|
void flags_Toggle_MASK(int mask)
|
|
{
|
|
systemFlags.intRepresentation ^= mask;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Checks if a flag is set
|
|
* INFORMATION: Given a mask value, check if that is set to true in the
|
|
* bitfield
|
|
***********************************************************************/
|
|
bool flags_IsSet_MASK(int mask)
|
|
{
|
|
return !( (systemFlags.intRepresentation ^ mask) & mask );
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Set flag to true(1)
|
|
* INFORMATION: Given an id set that value to true
|
|
***********************************************************************/
|
|
void flags_Set_ID(int id)
|
|
{
|
|
flags_Set_MASK(getFlagMaskValue(id));
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Set flag to false(0)
|
|
* INFORMATION: Given an id set that value to false
|
|
***********************************************************************/
|
|
void flags_Clear_ID(int id)
|
|
{
|
|
flags_Clear_MASK(getFlagMaskValue(id));
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Toggle a flag
|
|
* INFORMATION: Given an id changes the current value of a flag to its
|
|
* opposite.
|
|
***********************************************************************/
|
|
void flags_Toggle_ID(int id)
|
|
{
|
|
flags_Toggle_MASK(getFlagMaskValue(id));
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Checks if a flag is set
|
|
* INFORMATION: Given an id value, check if that is set to true in the
|
|
* bitfield
|
|
***********************************************************************/
|
|
bool flags_IsSet_ID(int id)
|
|
{
|
|
return flags_IsSet_MASK(getFlagMaskValue(id));
|
|
}
|