mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
0116e6a007
- both CC serial ports are now disabled by default (no telemetry); - serial ports now have DSM2, DSMX (10bit) and DSMX (11bit) options; - ReceiverGroups now have DSM (MainPort) and DSM (FlexiPort) options. For DSM2 protocol there is an explicit resolution bit in the stream, so the DSM2 should be selected. For DSMX there is no such bit, and user should choose the resolution from the list configuring the spektrum port. ReceiverGroups have single DSM option which is handled by the same driver. Downside: this implementation saves received frame first, unrolls by the end of frame. This should be ok, but may be improved by unrolling channels on the fly in the rx callback. Another minor difference is that a ChannelGroup is now bound to port: DSM (MainPort) or DSM (FlexiPort). This was considered as acceptable solution in order to not have 6 DSM options for each ChannelGroup and even more in case of new DSM protocol variations. Known problem: it is not possible to choose same protocols like DSM2/DSM2 for two ports. It can be enabled by adding an exception to common rule, though. The DSMX throttle channel misbehavior (zero value) is not treated specially yet. It should trigger the failsafe being out of bounds. More info and data dumps are required to handle this properly.
828 lines
30 KiB
C
828 lines
30 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup ManualControlModule Manual Control Module
|
|
* @brief Provide manual control or allow it alter flight mode.
|
|
* @{
|
|
*
|
|
* Reads in the ManualControlCommand FlightMode setting from receiver then either
|
|
* pass the settings straght to ActuatorDesired object (manual mode) or to
|
|
* AttitudeDesired object (stabilized mode)
|
|
*
|
|
* @file manualcontrol.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief ManualControl module. Handles safety R/C link and flight mode.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "manualcontrol.h"
|
|
#include "manualcontrolsettings.h"
|
|
#include "stabilizationsettings.h"
|
|
#include "manualcontrolcommand.h"
|
|
#include "actuatordesired.h"
|
|
#include "stabilizationdesired.h"
|
|
#include "flighttelemetrystats.h"
|
|
#include "flightstatus.h"
|
|
#include "accessorydesired.h"
|
|
#include "receiveractivity.h"
|
|
|
|
// Private constants
|
|
#if defined(PIOS_MANUAL_STACK_SIZE)
|
|
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
|
|
#else
|
|
#define STACK_SIZE_BYTES 824
|
|
#endif
|
|
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
|
#define UPDATE_PERIOD_MS 20
|
|
#define THROTTLE_FAILSAFE -0.1
|
|
#define FLIGHT_MODE_LIMIT 1.0/3.0
|
|
#define ARMED_TIME_MS 1000
|
|
#define ARMED_THRESHOLD 0.50
|
|
//safe band to allow a bit of calibration error or trim offset (in microseconds)
|
|
#define CONNECTION_OFFSET 150
|
|
|
|
// Private types
|
|
typedef enum
|
|
{
|
|
ARM_STATE_DISARMED,
|
|
ARM_STATE_ARMING_MANUAL,
|
|
ARM_STATE_ARMED,
|
|
ARM_STATE_DISARMING_MANUAL,
|
|
ARM_STATE_DISARMING_TIMEOUT
|
|
} ArmState_t;
|
|
|
|
// Private variables
|
|
static xTaskHandle taskHandle;
|
|
static ArmState_t armState;
|
|
static portTickType lastSysTime;
|
|
|
|
// Private functions
|
|
static void updateActuatorDesired(ManualControlCommandData * cmd);
|
|
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
|
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
|
|
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
|
static void setArmedIfChanged(uint8_t val);
|
|
|
|
static void manualControlTask(void *parameters);
|
|
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
|
|
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
|
|
static bool okToArm(void);
|
|
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
|
|
|
|
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
|
|
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
|
|
struct rcvr_activity_fsm {
|
|
ManualControlSettingsChannelGroupsOptions group;
|
|
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
|
|
uint8_t sample_count;
|
|
};
|
|
static struct rcvr_activity_fsm activity_fsm;
|
|
|
|
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm);
|
|
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
|
|
|
|
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount)
|
|
|
|
/**
|
|
* Module starting
|
|
*/
|
|
int32_t ManualControlStart()
|
|
{
|
|
// Start main task
|
|
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
|
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
|
|
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Module initialization
|
|
*/
|
|
int32_t ManualControlInitialize()
|
|
{
|
|
|
|
/* Check the assumptions about uavobject enum's are correct */
|
|
if(!assumptions)
|
|
return -1;
|
|
|
|
AccessoryDesiredInitialize();
|
|
ManualControlCommandInitialize();
|
|
FlightStatusInitialize();
|
|
StabilizationDesiredInitialize();
|
|
ReceiverActivityInitialize();
|
|
ManualControlSettingsInitialize();
|
|
|
|
return 0;
|
|
}
|
|
MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
|
|
|
|
/**
|
|
* Module task
|
|
*/
|
|
static void manualControlTask(void *parameters)
|
|
{
|
|
ManualControlSettingsData settings;
|
|
ManualControlCommandData cmd;
|
|
FlightStatusData flightStatus;
|
|
float flightMode = 0;
|
|
|
|
uint8_t disconnected_count = 0;
|
|
uint8_t connected_count = 0;
|
|
|
|
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
|
|
// this includes not even registering it if not used
|
|
AccessoryDesiredCreateInstance();
|
|
AccessoryDesiredCreateInstance();
|
|
|
|
// Make sure unarmed on power up
|
|
ManualControlCommandGet(&cmd);
|
|
FlightStatusGet(&flightStatus);
|
|
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
|
|
armState = ARM_STATE_DISARMED;
|
|
|
|
/* Initialize the RcvrActivty FSM */
|
|
portTickType lastActivityTime = xTaskGetTickCount();
|
|
resetRcvrActivity(&activity_fsm);
|
|
|
|
// Main task loop
|
|
lastSysTime = xTaskGetTickCount();
|
|
while (1) {
|
|
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
|
|
|
|
// Wait until next update
|
|
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
|
|
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
|
|
|
|
// Read settings
|
|
ManualControlSettingsGet(&settings);
|
|
|
|
/* Update channel activity monitor */
|
|
if (flightStatus.Armed == ARM_STATE_DISARMED) {
|
|
if (updateRcvrActivity(&activity_fsm)) {
|
|
/* Reset the aging timer because activity was detected */
|
|
lastActivityTime = lastSysTime;
|
|
}
|
|
}
|
|
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
|
|
resetRcvrActivity(&activity_fsm);
|
|
lastActivityTime = lastSysTime;
|
|
}
|
|
|
|
if (ManualControlCommandReadOnly(&cmd)) {
|
|
FlightTelemetryStatsData flightTelemStats;
|
|
FlightTelemetryStatsGet(&flightTelemStats);
|
|
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
|
|
/* trying to fly via GCS and lost connection. fall back to transmitter */
|
|
UAVObjMetadata metadata;
|
|
UAVObjGetMetadata(&cmd, &metadata);
|
|
metadata.access = ACCESS_READWRITE;
|
|
UAVObjSetMetadata(&cmd, &metadata);
|
|
}
|
|
}
|
|
|
|
if (!ManualControlCommandReadOnly(&cmd)) {
|
|
|
|
bool valid_input_detected = true;
|
|
|
|
// Read channel values in us
|
|
for (uint8_t n = 0;
|
|
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
|
|
++n) {
|
|
extern uint32_t pios_rcvr_group_map[];
|
|
|
|
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
cmd.Channel[n] = PIOS_RCVR_INVALID;
|
|
} else {
|
|
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
|
|
settings.ChannelNumber[n]);
|
|
}
|
|
|
|
// If a channel has timed out this is not valid data and we shouldn't update anything
|
|
// until we decide to go to failsafe
|
|
if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
|
|
valid_input_detected = false;
|
|
else
|
|
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
|
|
}
|
|
|
|
// Check settings, if error raise alarm
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
|
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
|
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
|
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
|
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
|
// Check all channel mappings are valid
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
|
|
// Check the driver is exists
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER) {
|
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
|
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
|
ManualControlCommandSet(&cmd);
|
|
|
|
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
|
|
// immediately disarm
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
|
|
|
continue;
|
|
}
|
|
|
|
// decide if we have valid manual input or not
|
|
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
|
|
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
|
|
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
|
|
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
|
|
|
|
// Implement hysteresis loop on connection status
|
|
if (valid_input_detected && (++connected_count > 10)) {
|
|
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
|
|
connected_count = 0;
|
|
disconnected_count = 0;
|
|
} else if (!valid_input_detected && (++disconnected_count > 10)) {
|
|
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
|
connected_count = 0;
|
|
disconnected_count = 0;
|
|
}
|
|
|
|
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
|
|
cmd.Throttle = -1; // Shut down engine with no control
|
|
cmd.Roll = 0;
|
|
cmd.Yaw = 0;
|
|
cmd.Pitch = 0;
|
|
cmd.Collective = 0;
|
|
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
|
|
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
|
|
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
|
|
AccessoryDesiredData accessory;
|
|
// Set Accessory 0
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = 0;
|
|
if(AccessoryDesiredInstSet(0, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
// Set Accessory 1
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = 0;
|
|
if(AccessoryDesiredInstSet(1, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
// Set Accessory 2
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = 0;
|
|
if(AccessoryDesiredInstSet(2, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
|
|
|
|
// Scale channels to -1 -> +1 range
|
|
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
|
|
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
|
|
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
|
|
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
|
|
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
|
|
|
|
if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_INVALID &&
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER &&
|
|
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT)
|
|
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
|
|
|
|
AccessoryDesiredData accessory;
|
|
// Set Accessory 0
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
|
|
if(AccessoryDesiredInstSet(0, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
// Set Accessory 1
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
|
|
if(AccessoryDesiredInstSet(1, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
// Set Accessory 2
|
|
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
|
|
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
|
|
if(AccessoryDesiredInstSet(2, &accessory) != 0)
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
|
|
|
|
processFlightMode(&settings, flightMode);
|
|
|
|
}
|
|
|
|
// Process arming outside conditional so system will disarm when disconnected
|
|
processArm(&cmd, &settings);
|
|
|
|
// Update cmd object
|
|
ManualControlCommandSet(&cmd);
|
|
|
|
} else {
|
|
ManualControlCommandGet(&cmd); /* Under GCS control */
|
|
}
|
|
|
|
FlightStatusGet(&flightStatus);
|
|
|
|
// Depending on the mode update the Stabilization or Actuator objects
|
|
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
|
|
case FLIGHTMODE_UNDEFINED:
|
|
// This reflects a bug in the code architecture!
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
|
break;
|
|
case FLIGHTMODE_MANUAL:
|
|
updateActuatorDesired(&cmd);
|
|
break;
|
|
case FLIGHTMODE_STABILIZED:
|
|
updateStabilizationDesired(&cmd, &settings);
|
|
break;
|
|
case FLIGHTMODE_GUIDANCE:
|
|
// TODO: Implement
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
|
|
{
|
|
ReceiverActivityData data;
|
|
bool updated = false;
|
|
|
|
/* Clear all channel activity flags */
|
|
ReceiverActivityGet(&data);
|
|
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE &&
|
|
data.ActiveChannel != 255) {
|
|
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
|
|
data.ActiveChannel = 255;
|
|
updated = true;
|
|
}
|
|
if (updated) {
|
|
ReceiverActivitySet(&data);
|
|
}
|
|
|
|
/* Reset the FSM state */
|
|
fsm->group = 0;
|
|
fsm->sample_count = 0;
|
|
}
|
|
|
|
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) {
|
|
for (uint8_t channel = 1; channel <= max_channels; channel++) {
|
|
// Subtract 1 because channels are 1 indexed
|
|
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
|
|
}
|
|
}
|
|
|
|
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm)
|
|
{
|
|
bool activity_updated = false;
|
|
|
|
/* Compare the current value to the previous sampled value */
|
|
for (uint8_t channel = 1;
|
|
channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP;
|
|
channel++) {
|
|
uint16_t delta;
|
|
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
|
|
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
|
|
if (curr > prev) {
|
|
delta = curr - prev;
|
|
} else {
|
|
delta = prev - curr;
|
|
}
|
|
|
|
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
|
|
/* Mark this channel as active */
|
|
ReceiverActivityActiveGroupOptions group;
|
|
|
|
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
|
|
switch (fsm->group) {
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
|
|
break;
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
|
|
break;
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
|
|
break;
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
|
|
break;
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
|
|
break;
|
|
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
|
|
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
|
|
break;
|
|
default:
|
|
PIOS_Assert(0);
|
|
break;
|
|
}
|
|
|
|
ReceiverActivityActiveGroupSet((uint8_t*)&group);
|
|
ReceiverActivityActiveChannelSet(&channel);
|
|
activity_updated = true;
|
|
}
|
|
}
|
|
return (activity_updated);
|
|
}
|
|
|
|
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
|
|
{
|
|
bool activity_updated = false;
|
|
|
|
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
/* We're out of range, reset things */
|
|
resetRcvrActivity(fsm);
|
|
}
|
|
|
|
extern uint32_t pios_rcvr_group_map[];
|
|
if (!pios_rcvr_group_map[fsm->group]) {
|
|
/* Unbound group, skip it */
|
|
goto group_completed;
|
|
}
|
|
|
|
if (fsm->sample_count == 0) {
|
|
/* Take a sample of each channel in this group */
|
|
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
|
|
fsm->prev,
|
|
NELEMENTS(fsm->prev));
|
|
fsm->sample_count++;
|
|
return (false);
|
|
}
|
|
|
|
/* Compare with previous sample */
|
|
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
|
|
|
|
group_completed:
|
|
/* Reset the sample counter */
|
|
fsm->sample_count = 0;
|
|
|
|
/* Find the next active group, but limit search so we can't loop forever here */
|
|
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
|
|
/* Move to the next group */
|
|
fsm->group++;
|
|
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
|
/* Wrap back to the first group */
|
|
fsm->group = 0;
|
|
}
|
|
if (pios_rcvr_group_map[fsm->group]) {
|
|
/*
|
|
* Found an active group, take a sample here to avoid an
|
|
* extra 20ms delay in the main thread so we can speed up
|
|
* this algorithm.
|
|
*/
|
|
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
|
|
fsm->prev,
|
|
NELEMENTS(fsm->prev));
|
|
fsm->sample_count++;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return (activity_updated);
|
|
}
|
|
|
|
static void updateActuatorDesired(ManualControlCommandData * cmd)
|
|
{
|
|
ActuatorDesiredData actuator;
|
|
ActuatorDesiredGet(&actuator);
|
|
actuator.Roll = cmd->Roll;
|
|
actuator.Pitch = cmd->Pitch;
|
|
actuator.Yaw = cmd->Yaw;
|
|
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
|
ActuatorDesiredSet(&actuator);
|
|
}
|
|
|
|
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
|
|
{
|
|
StabilizationDesiredData stabilization;
|
|
StabilizationDesiredGet(&stabilization);
|
|
|
|
StabilizationSettingsData stabSettings;
|
|
StabilizationSettingsGet(&stabSettings);
|
|
|
|
uint8_t * stab_settings;
|
|
FlightStatusData flightStatus;
|
|
FlightStatusGet(&flightStatus);
|
|
switch(flightStatus.FlightMode) {
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
|
stab_settings = settings->Stabilization1Settings;
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
|
stab_settings = settings->Stabilization2Settings;
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
|
stab_settings = settings->Stabilization3Settings;
|
|
break;
|
|
default:
|
|
// Major error, this should not occur because only enter this block when one of these is true
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
|
return;
|
|
}
|
|
|
|
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
|
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
|
|
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
|
|
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
|
|
|
|
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
|
0; // this is an invalid mode
|
|
;
|
|
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
|
0; // this is an invalid mode
|
|
|
|
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
|
0; // this is an invalid mode
|
|
|
|
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
|
StabilizationDesiredSet(&stabilization);
|
|
}
|
|
|
|
/**
|
|
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
|
|
*/
|
|
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
|
|
{
|
|
float valueScaled;
|
|
|
|
// Scale
|
|
if ((max > min && value >= neutral) || (min > max && value <= neutral))
|
|
{
|
|
if (max != neutral)
|
|
valueScaled = (float)(value - neutral) / (float)(max - neutral);
|
|
else
|
|
valueScaled = 0;
|
|
}
|
|
else
|
|
{
|
|
if (min != neutral)
|
|
valueScaled = (float)(value - neutral) / (float)(neutral - min);
|
|
else
|
|
valueScaled = 0;
|
|
}
|
|
|
|
// Bound
|
|
if (valueScaled > 1.0) valueScaled = 1.0;
|
|
else
|
|
if (valueScaled < -1.0) valueScaled = -1.0;
|
|
|
|
return valueScaled;
|
|
}
|
|
|
|
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
|
|
if(end_time > start_time)
|
|
return (end_time - start_time) * portTICK_RATE_MS;
|
|
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
|
|
}
|
|
|
|
/**
|
|
* @brief Determine if the aircraft is safe to arm
|
|
* @returns True if safe to arm, false otherwise
|
|
*/
|
|
static bool okToArm(void)
|
|
{
|
|
// read alarms
|
|
SystemAlarmsData alarms;
|
|
SystemAlarmsGet(&alarms);
|
|
|
|
|
|
// Check each alarm
|
|
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
|
|
{
|
|
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
|
|
{ // found an alarm thats set
|
|
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
|
|
continue;
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Update the flightStatus object only if value changed. Reduces callbacks
|
|
* @param[in] val The new value
|
|
*/
|
|
static void setArmedIfChanged(uint8_t val) {
|
|
FlightStatusData flightStatus;
|
|
FlightStatusGet(&flightStatus);
|
|
|
|
if(flightStatus.Armed != val) {
|
|
flightStatus.Armed = val;
|
|
FlightStatusSet(&flightStatus);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Process the inputs and determine whether to arm or not
|
|
* @param[out] cmd The structure to set the armed in
|
|
* @param[in] settings Settings indicating the necessary position
|
|
*/
|
|
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
|
|
{
|
|
|
|
bool lowThrottle = cmd->Throttle <= 0;
|
|
|
|
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
|
|
// In this configuration we always disarm
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
|
} else {
|
|
// Not really needed since this function not called when disconnected
|
|
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
|
|
lowThrottle = true;
|
|
|
|
// The throttle is not low, in case we where arming or disarming, abort
|
|
if (!lowThrottle) {
|
|
switch(armState) {
|
|
case ARM_STATE_DISARMING_MANUAL:
|
|
case ARM_STATE_DISARMING_TIMEOUT:
|
|
armState = ARM_STATE_ARMED;
|
|
break;
|
|
case ARM_STATE_ARMING_MANUAL:
|
|
armState = ARM_STATE_DISARMED;
|
|
break;
|
|
default:
|
|
// Nothing needs to be done in the other states
|
|
break;
|
|
}
|
|
return;
|
|
}
|
|
|
|
// The rest of these cases throttle is low
|
|
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
|
|
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
|
|
return;
|
|
}
|
|
|
|
|
|
// When the configuration is not "Always armed" and no "Always disarmed",
|
|
// the state will not be changed when the throttle is not low
|
|
static portTickType armedDisarmStart;
|
|
float armingInputLevel = 0;
|
|
|
|
// Calc channel see assumptions7
|
|
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
|
|
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
|
|
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
|
|
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
|
|
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
|
|
}
|
|
|
|
bool manualArm = false;
|
|
bool manualDisarm = false;
|
|
|
|
if (armingInputLevel <= -ARMED_THRESHOLD)
|
|
manualArm = true;
|
|
else if (armingInputLevel >= +ARMED_THRESHOLD)
|
|
manualDisarm = true;
|
|
|
|
switch(armState) {
|
|
case ARM_STATE_DISARMED:
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
|
|
|
// only allow arming if it's OK too
|
|
if (manualArm && okToArm()) {
|
|
armedDisarmStart = lastSysTime;
|
|
armState = ARM_STATE_ARMING_MANUAL;
|
|
}
|
|
break;
|
|
|
|
case ARM_STATE_ARMING_MANUAL:
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
|
|
|
|
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
|
armState = ARM_STATE_ARMED;
|
|
else if (!manualArm)
|
|
armState = ARM_STATE_DISARMED;
|
|
break;
|
|
|
|
case ARM_STATE_ARMED:
|
|
// When we get here, the throttle is low,
|
|
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
|
|
armedDisarmStart = lastSysTime;
|
|
armState = ARM_STATE_DISARMING_TIMEOUT;
|
|
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
|
|
break;
|
|
|
|
case ARM_STATE_DISARMING_TIMEOUT:
|
|
// We get here when armed while throttle low, even when the arming timeout is not enabled
|
|
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
|
|
armState = ARM_STATE_DISARMED;
|
|
|
|
// Switch to disarming due to manual control when needed
|
|
if (manualDisarm) {
|
|
armedDisarmStart = lastSysTime;
|
|
armState = ARM_STATE_DISARMING_MANUAL;
|
|
}
|
|
break;
|
|
|
|
case ARM_STATE_DISARMING_MANUAL:
|
|
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
|
armState = ARM_STATE_DISARMED;
|
|
else if (!manualDisarm)
|
|
armState = ARM_STATE_ARMED;
|
|
break;
|
|
} // End Switch
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
|
|
* @param[out] cmd Pointer to the command structure to set the flight mode in
|
|
* @param[in] settings The settings which indicate which position is which mode
|
|
* @param[in] flightMode the value of the switch position
|
|
*/
|
|
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
|
|
{
|
|
FlightStatusData flightStatus;
|
|
FlightStatusGet(&flightStatus);
|
|
|
|
uint8_t newMode;
|
|
// Note here the code is ass
|
|
if (flightMode < -FLIGHT_MODE_LIMIT)
|
|
newMode = settings->FlightModePosition[0];
|
|
else if (flightMode > FLIGHT_MODE_LIMIT)
|
|
newMode = settings->FlightModePosition[2];
|
|
else
|
|
newMode = settings->FlightModePosition[1];
|
|
|
|
if(flightStatus.FlightMode != newMode) {
|
|
flightStatus.FlightMode = newMode;
|
|
FlightStatusSet(&flightStatus);
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* @brief Determine if the manual input value is within acceptable limits
|
|
* @returns return TRUE if so, otherwise return FALSE
|
|
*/
|
|
bool validInputRange(int16_t min, int16_t max, uint16_t value)
|
|
{
|
|
if (min > max)
|
|
{
|
|
int16_t tmp = min;
|
|
min = max;
|
|
max = tmp;
|
|
}
|
|
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|