rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

Merge branch 'master' of ssh://git.openpilot.org/OpenPilot into OP-378_CheBuzz_INS

Browse Source
This commit is contained in:
sambas 2011-05-08 08:44:13 +03:00
commit a2c62d9a1b
7 changed files with 337 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/CopterControl/System/coptercontrol.c
View File

@ -42,26 +42,6 @@
/* Global Variables */
/* Local Variables */
#define INCLUDE_TEST_TASKS 0
#if INCLUDE_TEST_TASKS
static uint8_t sdcard_available;
#endif
FILEINFO File;
char Buffer[1024];
uint32_t Cache;
/* Function Prototypes */
#if INCLUDE_TEST_TASKS
static void TaskTick(void *pvParameters);
static void TaskTesting(void *pvParameters);
static void TaskHIDTest(void *pvParameters);
static void TaskServos(void *pvParameters);
static void TaskSDCard(void *pvParameters);
#endif
int32_t CONSOLE_Parse(uint8_t port, char c);
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
/* Prototype of generated InitModules() function */
extern void InitModules(void);

397
flight/Modules/ManualControl/manualcontrol.c
View File

@ -225,239 +225,214 @@ static void manualControlTask(void *parameters)
ManualControlCommandSet(&cmd);
continue;
}
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaledChannel[settings.Accessory1];
else
cmd.Accessory1 = 0;
// decide if we have valid manual input or not
bool valid_input_detected = TRUE;
if (!validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]))
valid_input_detected = FALSE;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaledChannel[settings.Accessory2];
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaledChannel[settings.Accessory3];
else
cmd.Accessory3 = 0;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[2];
else
cmd.FlightMode = settings.FlightModePosition[1];
// Update the ManualControlCommand object
ManualControlCommandSet(&cmd);
// This seems silly to set then get, but the reason is if the GCS is
// the control input, the set command will be blocked by the read only
// setting and the get command will pull the right values from telemetry
} else
ManualControlCommandGet(&cmd); /* Under GCS control */
// decide if we have valid manual input or not
bool valid_input_detected = ManualControlCommandReadOnly(&cmd) >= 0;
if (!validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]))
valid_input_detected = FALSE;
// Implement hysteresis loop on connection status
if (valid_input_detected)
{
if (++connected_count > 10)
// Implement hysteresis loop on connection status
if (valid_input_detected)
{
connection_state = CONNECTED;
connected_count = 0;
disconnected_count = 0;
if (++connected_count > 10)
{
connection_state = CONNECTED;
connected_count = 0;
disconnected_count = 0;
}
}
}
else
{
if (++disconnected_count > 10)
else
{
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
if (++disconnected_count > 10)
{
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
}
}
}
/*
// Implement hysteresis loop on connection status
// Must check both Max and Min in case they reversed
if (!ManualControlCommandReadOnly(&cmd) &&
cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&
cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {
if (disconnected_count++ > 10) {
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
} else
disconnected_count++;
} else {
if (connected_count++ > 10) {
connection_state = CONNECTED;
connected_count = 0;
disconnected_count = 0;
} else
connected_count++;
}
*/
if (connection_state == DISCONNECTED) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
ManualControlCommandSet(&cmd);
}
//
// Arming and Disarming mechanism
//
// Look for state changes and write in newArmState
uint8_t newCmdArmed = cmd.Armed; // By default, keep the arming state the same
if (connection_state == DISCONNECTED) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 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);
ManualControlCommandSet(&cmd);
} else {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
// In all other cases, we will not change the arm state when disconnected
if (connection_state == CONNECTED)
{
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
if (cmd.Throttle < 0) {
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
}
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaledChannel[settings.Accessory1];
else
cmd.Accessory1 = 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaledChannel[settings.Accessory2];
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaledChannel[settings.Accessory3];
else
cmd.Accessory3 = 0;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[2];
else
cmd.FlightMode = settings.FlightModePosition[1];
//
// Arming and Disarming mechanism
//
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
if (cmd.Throttle < 0) {
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// In all other cases, we will not change the arm state when disconnected
if (connection_state == CONNECTED)
{
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
if (cmd.Throttle < 0) {
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
}
} else {
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
if (cmd.Throttle < 0) {
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break;
}
// Calc channel see assumptions7
switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
bool manualArm = false;
bool manualDisarm = false;
if (connection_state == CONNECTED) {
// Should use RC input only if RX is connected
if (armingInputLevel <= -0.50)
manualArm = true;
else if (armingInputLevel >= +0.50)
manualDisarm = true;
}
if (connection_state == CONNECTED) {
// Should use RC input only if RX is connected
if (armingInputLevel <= -0.50)
manualArm = true;
else if (armingInputLevel >= +0.50)
manualDisarm = true;
}
// Swap arm-disarming see assumptions8
if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) {
bool temp = manualArm;
manualArm = manualDisarm;
manualDisarm = temp;
}
// Swap arm-disarming see assumptions8
if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) {
bool temp = manualArm;
manualArm = manualDisarm;
manualDisarm = temp;
}
switch(armState) {
case ARM_STATE_DISARMED:
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
switch(armState) {
case ARM_STATE_DISARMED:
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
if (manualArm)
{
if (okToArm()) // only allow arming if it's OK too
{
if (manualArm)
{
if (okToArm()) // only allow arming if it's OK too
{
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
}
break;
case ARM_STATE_ARMING_MANUAL:
if (manualArm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_ARMED;
}
else
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_ARMING_MANUAL;
armState = ARM_STATE_DISARMING_TIMEOUT;
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
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)
if (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) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_DISARMED;
}
else
armState = ARM_STATE_ARMED;
break;
} // End Switch
} else {
// The throttle is not low, in case we where arming or disarming, abort
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;
}
}
break;
case ARM_STATE_ARMING_MANUAL:
if (manualArm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_ARMED;
}
else
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;
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
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)
if (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) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_DISARMED;
}
else
armState = ARM_STATE_ARMED;
break;
} // End Switch
} else {
// The throttle is not low, in case we where arming or disarming, abort
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;
}
}
}
}
}
// Update cmd object when needed
if (newCmdArmed != cmd.Armed) {
cmd.Armed = newCmdArmed;
ManualControlCommandSet(&cmd);
}
//
// End of arming/disarming
//
//
// End of arming/disarming
//
// Update cmd object
ManualControlCommandSet(&cmd);
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
// Depending on the mode update the Stabilization or Actuator objects

3
flight/OpenPilot/Makefile.posix
View File

@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD
USE_THUMB_MODE = YES
# List of modules to include
MODULES = Telemetry Actuator Stabilization Guidance ManualControl FlightPlan
MODULES = Telemetry Actuator Stabilization Guidance ManualControl FlightPlan GPS
#MODULES = Telemetry ManualControl Actuator Attitude Stabilization
#MODULES = Telemetry Example
#MODULES = Telemetry MK/MKSerial
@ -170,6 +170,7 @@ SRC += $(PIOSPOSIX)/pios_udp.c
SRC += $(PIOSPOSIX)/pios_com.c
SRC += $(PIOSPOSIX)/pios_servo.c
SRC += $(PIOSPOSIX)/pios_wdg.c
SRC += $(PIOSPOSIX)/pios_debug.c
## Libraries for flight calculations
#SRC += $(FLIGHTLIB)/fifo_buffer.c

3
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -56,4 +56,7 @@
/* Stabilization options */
#define PIOS_QUATERNION_STABILIZATION
/* GPS options */
#define PIOS_GPS_SETS_HOMELOCATION
#endif /* PIOS_CONFIG_POSIX_H */

56
flight/PiOS.posix/inc/pios_debug.h Normal file
View File

@ -0,0 +1,56 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @defgroup PIOS_DEBUG Debugging Functions
* @brief Debugging functionality
* @{
*
* @file pios_i2c.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Debug helper functions header.
* @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
*/
#ifndef PIOS_DEBUG_H
#define PIOS_DEBUG_H
extern const char *PIOS_DEBUG_AssertMsg;
void PIOS_DEBUG_Init(void);
void PIOS_DEBUG_PinHigh(uint8_t pin);
void PIOS_DEBUG_PinLow(uint8_t pin);
void PIOS_DEBUG_PinValue8Bit(uint8_t value);
void PIOS_DEBUG_PinValue4BitL(uint8_t value);
void PIOS_DEBUG_Panic(const char *msg);
#ifdef DEBUG
#define PIOS_DEBUG_Assert(test) if (!(test)) PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg);
#define PIOS_Assert(test) PIOS_DEBUG_Assert(test)
#else
#define PIOS_DEBUG_Assert(test)
#define PIOS_Assert(test) if (!(test)) while (1);
#endif
#endif /* PIOS_DEBUG_H */
/**
* @}
* @}
*/

1
flight/PiOS.posix/pios.h
View File

@ -63,6 +63,7 @@
#include <pios_com.h>
#include <pios_servo.h>
#include <pios_wdg.h>
#include <pios_debug.h>
#define NELEMENTS(x) (sizeof(x) / sizeof(*(x)))

89
flight/PiOS.posix/posix/pios_debug.c Normal file
View File

@ -0,0 +1,89 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @defgroup PIOS_DEBUG Debugging Functions
* @brief Debugging functionality
* @{
*
* @file pios_debug.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Debugging Functions
* @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
*/
/* Project Includes */
#include "pios.h"
// Global variables
const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED";
/* Private Function Prototypes */
/**
* Initialise Debug-features
*/
void PIOS_DEBUG_Init(void)
{
}
/**
* Set debug-pin high
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinHigh(uint8_t Pin)
{
}
/**
* Set debug-pin low
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinLow(uint8_t Pin)
{
}
void PIOS_DEBUG_PinValue8Bit(uint8_t value)
{
}
void PIOS_DEBUG_PinValue4BitL(uint8_t value)
{
}
/**
* Report a serious error and halt
*/
void PIOS_DEBUG_Panic(const char *msg)
{
#ifdef PIOS_COM_DEBUG
register int *lr asm("lr"); // Link-register holds the PC of the caller
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr);
#endif
// Stay put
while (1) ;
}
/**
* @}
* @}
*/