diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index 736834763..588fa4a9f 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -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); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 75d096de8..98996ee1e 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -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 diff --git a/flight/Modules/Osd/OsdEtStd/OsdEtStd.c b/flight/Modules/Osd/OsdEtStd/OsdEtStd.c index 16820c461..69c4c222d 100644 --- a/flight/Modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/Modules/Osd/OsdEtStd/OsdEtStd.c @@ -34,6 +34,7 @@ #include "flightbatterystate.h" #include "gpsposition.h" #include "attitudeactual.h" +#include "baroaltitude.h" // // Configuration @@ -62,6 +63,8 @@ #define OSD_ADDRESS 0x30 #define OSDMSG_V_LS_IDX 10 +#define OSDMSG_BALT_IDX1 11 +#define OSDMSG_BALT_IDX2 12 #define OSDMSG_A_LS_IDX 17 #define OSDMSG_VA_MS_IDX 18 #define OSDMSG_LAT_IDX 33 @@ -99,14 +102,15 @@ static const char *DumpConfFilePath = "/etosd/dump.ocf"; // Private variables // -// | Header / cmd?| | V | | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| +// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| // 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 uint8_t msg[63] = - { 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + { 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90, 0x00, 0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A }; static volatile bool newPosData = FALSE; static volatile bool newBattData = FALSE; +static volatile bool newBaroData = FALSE; static enum { @@ -167,6 +171,16 @@ static void SetCourse(uint16_t dir) WriteToMsg16(OSDMSG_HOME_IDX, dir); } +static void SetBaroAltitude(int16_t altitudeMeter) +{ + // calculated formula + // ET OSD uses first update as zeropoint and then +- from that + altitudeMeter=(4571-altitudeMeter)/0.37; + msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter&0x00FF); + msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8)&0x3F; +} + + static void SetAltitude(uint32_t altitudeMeter) { WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10); @@ -202,6 +216,11 @@ static void GPSPositionUpdatedCb(UAVObjEvent * ev) newPosData = TRUE; } +static void BaroAltitudeUpdatedCb(UAVObjEvent * ev) +{ + newBaroData = TRUE; +} + static bool Read(uint32_t start, uint8_t length, uint8_t * buffer) { uint8_t cmd[5]; @@ -451,6 +470,14 @@ static void Run(void) } else { msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; } + if (newBaroData) { + BaroAltitudeData baroData; + + BaroAltitudeGet(&baroData); + SetBaroAltitude(baroData.Altitude); + + newBaroData = FALSE; + } DEBUG_MSG("SendMsg %d\n",cnt); { @@ -470,7 +497,6 @@ static void Run(void) } cnt++; - } static void onTimer(UAVObjEvent * ev) @@ -522,6 +548,7 @@ int32_t OsdEtStdInitialize(void) { GPSPositionConnectCallback(GPSPositionUpdatedCb); FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); + BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); memset(&ev,0,sizeof(UAVObjEvent)); EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index f2eb90753..2eff6f933 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -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 diff --git a/flight/OpenPilot/System/inc/pios_config_posix.h b/flight/OpenPilot/System/inc/pios_config_posix.h index 26521928a..136e264c3 100644 --- a/flight/OpenPilot/System/inc/pios_config_posix.h +++ b/flight/OpenPilot/System/inc/pios_config_posix.h @@ -56,4 +56,7 @@ /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION +/* GPS options */ +#define PIOS_GPS_SETS_HOMELOCATION + #endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/PiOS.posix/inc/pios_debug.h b/flight/PiOS.posix/inc/pios_debug.h new file mode 100644 index 000000000..e663d224e --- /dev/null +++ b/flight/PiOS.posix/inc/pios_debug.h @@ -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 */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/pios.h b/flight/PiOS.posix/pios.h index 679296891..4f6266aca 100644 --- a/flight/PiOS.posix/pios.h +++ b/flight/PiOS.posix/pios.h @@ -63,6 +63,7 @@ #include #include #include +#include #define NELEMENTS(x) (sizeof(x) / sizeof(*(x))) diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c new file mode 100644 index 000000000..f79f2e730 --- /dev/null +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -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) ; +} + +/** + * @} + * @} + */