From 074b391cc042c3aadff481ec4cd5300d0bf552a8 Mon Sep 17 00:00:00 2001 From: sambas Date: Sat, 20 Apr 2013 18:07:19 +0300 Subject: [PATCH] spring cleaning --- flight/Modules/Osd/OsdHk/OsdHk.c | 315 ---------- flight/Modules/Osd/WavPlayer/inc/wavplayer.h | 38 +- flight/Modules/Osd/WavPlayer/wavplayer.c | 65 +-- flight/Modules/Osd/osdgen/inc/osdgen.h | 44 +- flight/Modules/Osd/osdgen/osdgen.c | 37 +- flight/Modules/Osd/osdinput/inc/osdinput.h | 39 +- flight/Modules/Osd/osdinput/osdinput.c | 296 +++------- .../inc/OsdHk.h => osdoutput/inc/osdoutput.h} | 23 +- flight/Modules/Osd/osdoutput/osdoutput.c | 304 ++++++++++ flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 24 +- flight/PiOS/Boards/STM32F4xx_OSD.h | 50 +- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 2 +- flight/PiOS/Boards/STM32F4xx_Revolution.h | 30 +- flight/PiOS/Common/pios_video.c | 552 +++++++++--------- flight/targets/CopterControl/Makefile | 1 + flight/targets/RevoMini/Makefile | 2 +- 16 files changed, 874 insertions(+), 948 deletions(-) delete mode 100644 flight/Modules/Osd/OsdHk/OsdHk.c rename flight/Modules/Osd/{OsdHk/inc/OsdHk.h => osdoutput/inc/osdoutput.h} (78%) create mode 100644 flight/Modules/Osd/osdoutput/osdoutput.c diff --git a/flight/Modules/Osd/OsdHk/OsdHk.c b/flight/Modules/Osd/OsdHk/OsdHk.c deleted file mode 100644 index e81268d68..000000000 --- a/flight/Modules/Osd/OsdHk/OsdHk.c +++ /dev/null @@ -1,315 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup HKOSDModule HK OSD Module - * @brief On screen display support - * @{ - * - * @file OsdHk.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Interfacing with HobbyKing OSD module - * @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" - -#if FLIGHTBATTERYSTATE_SUPPORTED -#include "flightbatterystate.h" -#endif - -#if POSITIONACTUAL_SUPPORTED -#include "positionactual.h" -#endif - -#include "systemalarms.h" -#include "attitudeactual.h" -#include "hwsettings.h" - -static bool osdhkEnabled; - - -enum osd_hk_sync { - OSD_HK_SYNC_A = 0xCB, - OSD_HK_SYNC_B = 0x34, -}; - -enum osd_hk_pkt_type { - OSD_HK_PKT_TYPE_MISC = 0, - OSD_HK_PKT_TYPE_NAV = 1, - OSD_HK_PKT_TYPE_MAINT = 2, - OSD_HK_PKT_TYPE_ATT = 3, -}; - -enum osd_hk_control_mode { - OSD_HK_CONTROL_MODE_MANUAL = 0, - OSD_HK_CONTROL_MODE_STABILIZED = 1, - OSD_HK_CONTROL_MODE_AUTO = 2, -}; - -struct osd_hk_blob_misc { - uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ - int16_t roll; - int16_t pitch; - //uint16_t home; /* Big Endian */ - enum osd_hk_control_mode control_mode; - uint8_t low_battery; - uint16_t current; /* Big Endian */ -} __attribute__((packed)); - -struct osd_hk_blob_att { - uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */ - int16_t roll; - int16_t pitch; - int16_t yaw; - int16_t speed; /* Big Endian */ -} __attribute__((packed)); - -struct osd_hk_blob_nav { - uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ - uint32_t gps_lat; /* Big Endian */ - uint32_t gps_lon; /* Big Endian */ -} __attribute__((packed)); - -struct osd_hk_blob_maint { - uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ - uint8_t gps_speed; - uint16_t gps_alt; /* Big Endian */ - uint16_t gps_dis; /* Big Endian */ - uint8_t status; - uint8_t config; - uint8_t emerg; -} __attribute__((packed)); - -union osd_hk_pkt_blobs -{ - struct osd_hk_blob_misc misc; - struct osd_hk_blob_nav nav; - struct osd_hk_blob_maint maint; - struct osd_hk_blob_att att; -} __attribute__((packed)); - -struct osd_hk_msg { - enum osd_hk_sync sync; - enum osd_hk_pkt_type t; - union osd_hk_pkt_blobs v; -} __attribute__((packed)); - -#if 0 -#if sizeof(struct osd_hk_msg) != 11 -#error struct osd_hk_msg is the wrong size, something is wrong with the definition -#endif -#endif - -static struct osd_hk_msg osd_hk_msg_buf; - -static volatile bool newPositionActualData = false; -static volatile bool newBattData = false; -static volatile bool newAttitudeData = false; -static volatile bool newAlarmData = false; - -static uint32_t osd_hk_com_id; -static uint8_t osd_hk_msg_dropped; - -static void send_update(UAVObjEvent * ev) -{ - static enum osd_hk_sync sync = OSD_HK_SYNC_A; - - struct osd_hk_msg * msg = &osd_hk_msg_buf; - union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v); - - /* Make sure we have a COM port bound */ - if (!osd_hk_com_id) { - return; - } - - /* - * Set up the message - * - * NOTE: Only packet type 0 (MISC) is supported so far - */ - msg->sync = sync; - msg->t = OSD_HK_PKT_TYPE_ATT; - - if (newAttitudeData) { - float roll; - AttitudeActualRollGet(&roll); - //blob->misc.roll = (int16_t) roll; - blob->att.roll = (int16_t) roll; - - float pitch; - AttitudeActualPitchGet(&pitch); - //blob->misc.pitch = (int16_t) pitch; - blob->att.pitch = (int16_t) pitch; - - float yaw; - AttitudeActualYawGet(&yaw); - blob->att.yaw = (int16_t) yaw; - } - - /* Field not supported yet */ - //blob->misc.control_mode = 0; - - /*if (newAlarmData) { - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); - - switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { - case SYSTEMALARMS_ALARM_UNINITIALISED: - case SYSTEMALARMS_ALARM_OK: - blob->misc.low_battery = 0; - break; - case SYSTEMALARMS_ALARM_WARNING: - case SYSTEMALARMS_ALARM_ERROR: - case SYSTEMALARMS_ALARM_CRITICAL: - default: - blob->misc.low_battery = 1; - break; - } - - newAlarmData = false; - }*/ - -#if FLIGHTBATTERYSUPPORTED - if (newBattData) { - float consumed_energy; - FlightBatteryStateConsumedEnergyGet(&consumed_energy); - - uint16_t current = (uint16_t)(consumed_energy * 10); - - /* convert to big endian */ - blob->misc.current = ( - (current & 0xFF00 >> 8) | - (current & 0x00FF << 8)); - - newBattData = false; - } -#else - //blob->misc.current = 0; -#endif - -#if POSITIONACTUAL_SUPPORTED - if (newPositionActualData) { - PositionActualData position; - PositionActualGet(&position); - - /* compute 3D distance */ - float d = sqrt( - pow(position.North,2) + - pow(position.East,2) + - pow(position.Down,2)); - /* convert from cm to dm (10ths of m) */ - uint16_t home = (uint16_t)(d / 10); - - /* convert to big endian */ - blob->misc.home = ( - (home & 0xFF00 >> 8) | - (home & 0x00FF << 8)); - - newPositionActualData = false; - } -#else - //blob->misc.home = 0; -#endif - - if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *)&osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { - /* Sent a packet, flip to the opposite sync */ - if (sync == OSD_HK_SYNC_A) { - sync = OSD_HK_SYNC_B; - } else { - sync = OSD_HK_SYNC_A; - } - } else { - /* Failed to send this update */ - osd_hk_msg_dropped++; - } -} - -#if FLIGHTBATTERYSTATE_SUPPORTED -static void FlightBatteryStateUpdatedCb(UAVObjEvent * ev) -{ - newBattData = true; -} -#endif - -#if POSITIONACTUAL_SUPPORTED -static void PositionActualUpdatedCb(UAVObjEvent * ev) -{ - newPositionActualData = true; -} -#endif - -static void AttitudeActualUpdatedCb(UAVObjEvent * ev) -{ - newAttitudeData = true; -} - -static void SystemAlarmsUpdatedCb(UAVObjEvent * ev) -{ - newAlarmData = true; -} - -static UAVObjEvent ev; - -static int32_t OsdHkStart(void) -{ - if (osdhkEnabled) { - // Start main task - AttitudeActualConnectCallback(AttitudeActualUpdatedCb); - - SystemAlarmsConnectCallback(SystemAlarmsUpdatedCb); - -#if FLIGHTBATTERYSTATE_SUPPORTED - FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); -#endif - -#if POSITIONACTUAL_SUPPORTED - PositionActualConnectCallback(PositionActualUpdatedCb); -#endif - - /* Start a periodic timer to kick sending of an update */ - EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS); - return 0; - } - return -1; -} - -static int32_t OsdHkInitialize(void) -{ - osd_hk_com_id = PIOS_COM_OSDHK; -#ifdef MODULE_OsdHk_BUILTIN - osdhkEnabled = 1; -#else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - osdhkEnabled = 1; - } else { - osdhkEnabled = 0; - } -#endif - return 0; -} -MODULE_INITCALL(OsdHkInitialize, OsdHkStart) - -/** - * @} - * @} - */ diff --git a/flight/Modules/Osd/WavPlayer/inc/wavplayer.h b/flight/Modules/Osd/WavPlayer/inc/wavplayer.h index c2d1f7c65..719e32c3a 100644 --- a/flight/Modules/Osd/WavPlayer/inc/wavplayer.h +++ b/flight/Modules/Osd/WavPlayer/inc/wavplayer.h @@ -1,15 +1,37 @@ -/* - * WavPlayer.h +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup WavPlayerModule WavPlayer Module + * @brief Process WavPlayer information + * @{ * - * Created on: 15.07.2012 - * Author: Samba + * @file wavplayer.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief WavPlayer module + * @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 WavPlayer_H_ -#define WavPlayer_H_ +#ifndef WAVPLAYER_H_ +#define WAVPLAYER_H_ #include "openpilot.h" int32_t WavPlayerInitialize(void); -#endif /* WavPlayer_H_ */ +#endif /* WAVPLAYER_H_ */ diff --git a/flight/Modules/Osd/WavPlayer/wavplayer.c b/flight/Modules/Osd/WavPlayer/wavplayer.c index f0fd97a7a..1788f0a97 100644 --- a/flight/Modules/Osd/WavPlayer/wavplayer.c +++ b/flight/Modules/Osd/WavPlayer/wavplayer.c @@ -6,7 +6,7 @@ * @brief Process WavPlayer information * @{ * - * @file WavPlayer.c + * @file wavplayer.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief WavPlayer module * @see The GNU Public License (GPL) Version 3 @@ -29,10 +29,8 @@ */ // **************** - #include "openpilot.h" - // **************** // Private functions @@ -56,10 +54,10 @@ static uint32_t timeOfLastUpdateMs; // **************** int32_t WavPlayerStart(void) { - // Start WavPlayer task - xTaskCreate(WavPlayerTask, (signed char *)"WavPlayer", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); + // Start WavPlayer task + xTaskCreate(WavPlayerTask, (signed char *) "WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); - return 0; + return 0; } /** * Initialise the WavPlayer module @@ -69,55 +67,38 @@ int32_t WavPlayerStart(void) int32_t WavPlayerInitialize(void) { - return 0; + return 0; } -MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart) +MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart) // **************** /** - * Main gps task. It does not return. + * Main WavPlayer task. It does not return. */ static void WavPlayerTask(void *parameters) { - portTickType xDelay = 100 / portTICK_RATE_MS; - portTickType lastSysTime; - // Loop forever - lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; + portTickType xDelay = 100 / portTICK_RATE_MS; + portTickType lastSysTime; + // Loop forever + lastSysTime = xTaskGetTickCount(); + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; #if defined(PIOS_INCLUDE_WAVE) - WavePlayer_Start(); + WavePlayer_Start(); #endif - // Loop forever - while (1) - { - - vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); - // Check for GPS timeout - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - /*if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) - { // we have not received any valid GPS sentences for a while. - // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - - - } - else - { // we appear to be receiving GPS sentences OK, we've had an update - - - }*/ - - } + // Loop forever + while (1) { + vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + } } - // **************** /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/Modules/Osd/osdgen/inc/osdgen.h b/flight/Modules/Osd/osdgen/inc/osdgen.h index 75b341bd3..92f8d6cd3 100644 --- a/flight/Modules/Osd/osdgen/inc/osdgen.h +++ b/flight/Modules/Osd/osdgen/inc/osdgen.h @@ -1,8 +1,31 @@ -/* - * osdgen.h +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup OSDgenModule osdgen Module + * @brief Process OSD information + * @{ * - * Created on: 2.10.2011 - * Author: Samba + * @file osdgen.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief OSD gen module, handles OSD draw. Parts from CL-OSD and SUPEROSD projects + * @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 OSDGEN_H_ @@ -13,7 +36,6 @@ int32_t osdgenInitialize(void); - // Size of an array (num items.) #define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0])) @@ -79,12 +101,9 @@ int32_t osdgenInitialize(void); write_pixel(buff, (cx) + (x), (cy) - (y), mode); \ write_pixel(buff, (cx) - (x), (cy) - (y), mode); - - // Font flags. #define FONT_BOLD 1 // bold text (no outline) #define FONT_INVERT 2 // invert: border white, inside black - // Text alignments. #define TEXT_VA_TOP 0 #define TEXT_VA_MIDDLE 1 @@ -96,10 +115,9 @@ int32_t osdgenInitialize(void); // Text dimension structures. struct FontDimensions { - int width, height; + int width, height; }; - // Max/Min macros. #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MIN(a, b) ((a) < (b) ? (a) : (b)) @@ -124,7 +142,6 @@ struct FontDimensions // Macro to swap two variables using XOR swap. #define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } - // Line triggering #define LAST_LINE 312 //625/2 //PAL //#define LAST_LINE 525/2 //NTSC @@ -142,8 +159,6 @@ struct FontDimensions #define DELAY_9_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") #define DELAY_10_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") - - uint8_t getCharData(uint16_t charPos); void introText(); @@ -190,7 +205,4 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned void updateOnceEveryFrame(); - - - #endif /* OSDGEN_H_ */ diff --git a/flight/Modules/Osd/osdgen/osdgen.c b/flight/Modules/Osd/osdgen/osdgen.c index fad4e25c7..9172cc006 100644 --- a/flight/Modules/Osd/osdgen/osdgen.c +++ b/flight/Modules/Osd/osdgen/osdgen.c @@ -2,7 +2,7 @@ ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup OSDGENModule osdgen Module + * @addtogroup OSDgenModule osdgen Module * @brief Process OSD information * @{ * @@ -38,6 +38,7 @@ #include "gpssatellites.h" #include "osdsettings.h" #include "baroaltitude.h" +#include "flightstatus.h" #include "fonts.h" #include "font12x18.h" @@ -2096,6 +2097,8 @@ void updateGraphics() HomeLocationGet(&home); BaroAltitudeData baro; BaroAltitudeGet(&baro); + FlightStatusData status; + FlightStatusGet(&status); PIOS_Servo_Set(0, OsdSettings.White); PIOS_Servo_Set(1, OsdSettings.Black); @@ -2234,7 +2237,6 @@ void updateGraphics() }*/ //drawAltitude(200,50,m_alt,dir); - //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); // Draw airspeed (left side.) if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { @@ -2272,6 +2274,36 @@ void updateGraphics() 0); } + char temp[50] = + { 0 }; + memset(temp, ' ', 50); + switch (status.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + sprintf(temp, "Man"); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + sprintf(temp, "Stab1"); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + sprintf(temp, "Stab2"); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + sprintf(temp, "Stab3"); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + sprintf(temp, "PH"); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + sprintf(temp, "RTB"); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + sprintf(temp, "PATH"); + break; + default: + sprintf(temp, "Mode: %d", status.FlightMode); + break; + } + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); } break; case 3: @@ -2348,6 +2380,7 @@ int32_t osdgenInitialize(void) #endif OsdSettingsInitialize(); BaroAltitudeInitialize(); + FlightStatusInitialize(); return 0; } diff --git a/flight/Modules/Osd/osdinput/inc/osdinput.h b/flight/Modules/Osd/osdinput/inc/osdinput.h index 58e506ae1..e98386beb 100644 --- a/flight/Modules/Osd/osdinput/inc/osdinput.h +++ b/flight/Modules/Osd/osdinput/inc/osdinput.h @@ -1,15 +1,38 @@ -/* - * OpOsd.h +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup osdinputModule osdinput Module + * @brief Process osdinput information + * @{ * - * Created on: 2.10.2011 - * Author: Samba + * @file osdinput.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief osdinput module, handles osdinput stream + * @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 OPOSD_H_ -#define OPOSD_H_ +#ifndef OSDINPUT_H_ +#define OSDINPUT_H_ #include "openpilot.h" -int32_t OpOsdInitialize(void); +int32_t osdinputInitialize(void); -#endif /* OPOSD_H_ */ +#endif /* OSDINPUT_H_ */ diff --git a/flight/Modules/Osd/osdinput/osdinput.c b/flight/Modules/Osd/osdinput/osdinput.c index b0a6acd89..abde6bd22 100644 --- a/flight/Modules/Osd/osdinput/osdinput.c +++ b/flight/Modules/Osd/osdinput/osdinput.c @@ -29,264 +29,150 @@ */ // **************** - #include "openpilot.h" #include "osdinput.h" #include "attitudeactual.h" +#include "flightstatus.h" #include "fifo_buffer.h" - // **************** // Private functions -static void OpOsdTask(void *parameters); +static void osdinputTask(void *parameters); // **************** // Private constants -#define GPS_TIMEOUT_MS 500 -#define NMEA_MAX_PACKET_LENGTH 33 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed -// same as in COM buffer - - -#ifdef PIOS_GPS_SETS_HOMELOCATION -// Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 800 -#else - #define STACK_SIZE_BYTES 1024 -#endif - +#define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) - +#define MAX_PACKET_LENGTH 33 // **************** // Private variables static uint32_t oposdPort; -static xTaskHandle OpOsdTaskHandle; +static xTaskHandle osdinputTaskHandle; static char* oposd_rx_buffer; t_fifo_buffer rx; -static uint32_t timeOfLastCommandMs; -static uint32_t timeOfLastUpdateMs; -static uint32_t numUpdates; -static uint32_t numChecksumErrors; -static uint32_t numParsingErrors; +enum osd_pkt_type +{ + OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4, +}; // **************** /** - * Initialise the gps module + * Initialise the osdinput module * \return -1 if initialisation failed * \return 0 on success */ -int32_t OpOsdStart(void) +int32_t osdinputStart(void) { - // Start gps task - xTaskCreate(OpOsdTask, (signed char *)"OSD", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &OpOsdTaskHandle); - //TaskMonitorAdd(TASKINFO_RUNNING_GPS, OpOsdTaskHandle); + // Start osdinput task + xTaskCreate(osdinputTask, (signed char *) "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle); - return 0; + return 0; } /** * Initialise the gps module * \return -1 if initialisation failed * \return 0 on success */ -int32_t OpOsdInitialize(void) +int32_t osdinputInitialize(void) { - AttitudeActualInitialize(); - // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = 0; - attitude.Pitch = 0; - attitude.Yaw = 0; - AttitudeActualSet(&attitude); + AttitudeActualInitialize(); + FlightStatusInitialize(); + // Initialize quaternion + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = 0; + attitude.Pitch = 0; + attitude.Yaw = 0; + AttitudeActualSet(&attitude); + oposdPort = PIOS_COM_OSD; - // TODO: Get gps settings object - oposdPort = PIOS_COM_OSD; + oposd_rx_buffer = pvPortMalloc(MAX_PACKET_LENGTH); + PIOS_Assert(oposd_rx_buffer); - oposd_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); - PIOS_Assert(oposd_rx_buffer); - - return 0; + return 0; } -MODULE_INITCALL(OpOsdInitialize, OpOsdStart) +MODULE_INITCALL( osdinputInitialize, osdinputStart) // **************** /** - * Main gps task. It does not return. + * Main osdinput task. It does not return. */ -static void OpOsdTask(void *parameters) +static void osdinputTask(void *parameters) { - portTickType xDelay = 100 / portTICK_RATE_MS; - portTickType lastSysTime; - // Loop forever - lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; - //GPSPositionData GpsData; + portTickType xDelay = 100 / portTICK_RATE_MS; + portTickType lastSysTime; + lastSysTime = xTaskGetTickCount(); - //uint8_t rx_count = 0; - //bool start_flag = false; - //bool found_cr = false; - //int32_t gpsRxOverflow = 0; + uint8_t rx_count = 0; + bool start_flag = false; + int32_t osdRxOverflow = 0; + uint8_t c = 0xAA; + // Loop forever + while (1) { + // This blocks the task until there is something on the buffer + while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) { - numUpdates = 0; - numChecksumErrors = 0; - numParsingErrors = 0; + // detect start while acquiring stream + if (!start_flag && ((c == 0xCB) || (c == 0x34))) { + start_flag = true; + rx_count = 0; + } else if (!start_flag) { + continue; + } - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - uint8_t rx_count = 0; - bool start_flag = false; - //bool found_cr = false; - int32_t gpsRxOverflow = 0; - uint8_t c=0xAA; - // Loop forever - while (1) - { - /*//DMA_Cmd(DMA1_Stream2, DISABLE); //prohibit channel3 for a little time - uint16_t cnt = DMA_GetCurrDataCounter(DMA1_Stream2); - rx.wr = rx.buf_size-cnt; - if(rx.wr) - { - //PIOS_LED_Toggle(LED2); - while ( fifoBuf_getData(&rx, &c, 1) > 0) - { - - // detect start while acquiring stream - if (!start_flag && ((c == 0xCB) || (c == 0x34))) - { - start_flag = true; - rx_count = 0; - } - else - if (!start_flag) - continue; - - if (rx_count >= 11) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - start_flag = false; - rx_count = 0; - } - else - { - oposd_rx_buffer[rx_count] = c; - rx_count++; - } - if (start_flag && rx_count == 11) - { - //PIOS_LED_Toggle(LED3); - if(oposd_rx_buffer[1]==3) - { - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8); - attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8); - attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8); - AttitudeActualSet(&attitude); - //setAttitudeOsd((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8), //pitch - // (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8), //roll - // (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8)); //yaw - - } - //frame completed - start_flag = false; - rx_count = 0; - } - } - } - //DMA_Cmd(DMA1_Stream2, ENABLE); - - */ - - //PIOS_COM_SendBufferNonBlocking(oposdPort, &c, 1); - - // This blocks the task until there is something on the buffer - while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) - { - - // detect start while acquiring stream - if (!start_flag && ((c == 0xCB) || (c == 0x34))) - { - start_flag = true; - rx_count = 0; - } - else - if (!start_flag) - continue; - - if (rx_count >= 11) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - start_flag = false; - rx_count = 0; - } - else - { - oposd_rx_buffer[rx_count] = c; - rx_count++; - } - if (rx_count == 11) - { - if(oposd_rx_buffer[1]==3) - { - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8); - attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8); - attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8); - AttitudeActualSet(&attitude); - } - //frame completed - start_flag = false; - rx_count = 0; - - } - } - vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); - // Check for GPS timeout - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) - { // we have not received any valid GPS sentences for a while. - // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - - - } - else - { // we appear to be receiving GPS sentences OK, we've had an update - - - } - - } + if (rx_count >= 11) { + // Flush the buffer and note the overflow event. + osdRxOverflow++; + start_flag = false; + rx_count = 0; + } else { + oposd_rx_buffer[rx_count] = c; + rx_count++; + } + if (rx_count == 11) { + if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) { + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; + attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; + attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; + AttitudeActualSet(&attitude); + } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) { + FlightStatusData status; + FlightStatusGet(&status); + status.Armed = oposd_rx_buffer[8]; + status.FlightMode = oposd_rx_buffer[3]; + FlightStatusSet(&status); + } + //frame completed + start_flag = false; + rx_count = 0; + } + } + vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); + } } - // **************** /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/Modules/Osd/OsdHk/inc/OsdHk.h b/flight/Modules/Osd/osdoutput/inc/osdoutput.h similarity index 78% rename from flight/Modules/Osd/OsdHk/inc/OsdHk.h rename to flight/Modules/Osd/osdoutput/inc/osdoutput.h index ab0b16608..a6f2e80bd 100644 --- a/flight/Modules/Osd/OsdHk/inc/OsdHk.h +++ b/flight/Modules/Osd/osdoutput/inc/osdoutput.h @@ -1,15 +1,14 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup HKOSDModule HK OSD Module - * @brief On screen display support - * @{ - * - * @file OsdHk.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Interfacing with HobbyKing OSD module + * @{ + * @addtogroup osdoutputModule osdoutput Module + * @brief Process osdoutput information + * @{ * + * @file osdoutput.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief osdoutput module, handles osdoutput stream * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -28,12 +27,12 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef OSDHK_H -#define OSDHK_H +#ifndef OSDOUTPUT_H +#define OSDOUTPUT_H -int32_t OsdHkInitialize(void); +int32_t osdoutputInitialize(void); -#endif /* OSDHK_H */ +#endif /* OSDOUTPUT_H */ /** * @} diff --git a/flight/Modules/Osd/osdoutput/osdoutput.c b/flight/Modules/Osd/osdoutput/osdoutput.c new file mode 100644 index 000000000..5285a88a8 --- /dev/null +++ b/flight/Modules/Osd/osdoutput/osdoutput.c @@ -0,0 +1,304 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup OSDOUTPUTModule OSDOutput Module + * @brief On screen display support + * @{ + * + * @file osdoutput.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Interfacing with OSD module + * @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" + +#if FLIGHTBATTERYSTATE_SUPPORTED +#include "flightbatterystate.h" +#endif + +#if POSITIONACTUAL_SUPPORTED +#include "positionactual.h" +#endif + +#include "systemalarms.h" +#include "attitudeactual.h" +#include "hwsettings.h" +#include "flightstatus.h" + +static bool osdoutputEnabled; + +enum osd_hk_sync +{ + OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34, +}; + +enum osd_hk_pkt_type +{ + OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4, +}; + +enum osd_hk_control_mode +{ + OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2, +}; + +struct osd_hk_blob_misc +{ + uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ + int16_t roll; + int16_t pitch; + //uint16_t home; /* Big Endian */ + enum osd_hk_control_mode control_mode; + uint8_t low_battery; + uint16_t current; /* Big Endian */ +}__attribute__((packed)); + +struct osd_hk_blob_att +{ + uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */ + int16_t roll; + int16_t pitch; + int16_t yaw; + int16_t speed; /* Big Endian */ +}__attribute__((packed)); + +struct osd_hk_blob_nav +{ + uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ + uint32_t gps_lat; /* Big Endian */ + uint32_t gps_lon; /* Big Endian */ +}__attribute__((packed)); + +struct osd_hk_blob_maint +{ + uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ + uint8_t gps_speed; + uint16_t gps_alt; /* Big Endian */ + uint16_t gps_dis; /* Big Endian */ + uint8_t status; + uint8_t config; + uint8_t emerg; +}__attribute__((packed)); + +struct osd_hk_blob_mode +{ + uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */ + uint8_t fltmode; + uint16_t gps_alt; /* Big Endian */ + uint16_t gps_dis; /* Big Endian */ + uint8_t armed; + uint8_t config; + uint8_t emerg; +}__attribute__((packed)); + +union osd_hk_pkt_blobs +{ + struct osd_hk_blob_misc misc; + struct osd_hk_blob_nav nav; + struct osd_hk_blob_maint maint; + struct osd_hk_blob_att att; + struct osd_hk_blob_mode mode; +}__attribute__((packed)); + +struct osd_hk_msg +{ + enum osd_hk_sync sync; + enum osd_hk_pkt_type t; + union osd_hk_pkt_blobs v; +}__attribute__((packed)); + +static struct osd_hk_msg osd_hk_msg_buf; + +static volatile bool newPositionActualData = false; +static volatile bool newBattData = false; +static volatile bool newAttitudeData = false; +static volatile bool newAlarmData = false; + +static uint32_t osd_hk_com_id; +static uint8_t osd_hk_msg_dropped; +static uint8_t osd_packet; + +static void send_update(UAVObjEvent * ev) +{ + static enum osd_hk_sync sync = OSD_HK_SYNC_A; + + struct osd_hk_msg * msg = &osd_hk_msg_buf; + union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v); + + /* Make sure we have a COM port bound */ + if (!osd_hk_com_id) { + return; + } + + FlightStatusData flightStatus; + + /* + * Set up the message + */ + msg->sync = sync; + + switch (osd_packet) { + case OSD_HK_PKT_TYPE_MISC: + break; + case OSD_HK_PKT_TYPE_NAV: + break; + case OSD_HK_PKT_TYPE_MAINT: + break; + case OSD_HK_PKT_TYPE_ATT: + msg->t = OSD_HK_PKT_TYPE_ATT; + float roll; + AttitudeActualRollGet(&roll); + blob->att.roll = (int16_t)(roll * 10); + + float pitch; + AttitudeActualPitchGet(&pitch); + blob->att.pitch = (int16_t)(pitch * 10); + + float yaw; + AttitudeActualYawGet(&yaw); + blob->att.yaw = (int16_t)(yaw * 10); + break; + case OSD_HK_PKT_TYPE_MODE: + msg->t = OSD_HK_PKT_TYPE_MODE; + FlightStatusGet(&flightStatus); + blob->mode.fltmode = flightStatus.FlightMode; + blob->mode.armed = flightStatus.Armed; + break; + default: + break; + } + + /* Field not supported yet */ +//blob->misc.control_mode = 0; + /*if (newAlarmData) { + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); + + switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { + case SYSTEMALARMS_ALARM_UNINITIALISED: + case SYSTEMALARMS_ALARM_OK: + blob->misc.low_battery = 0; + break; + case SYSTEMALARMS_ALARM_WARNING: + case SYSTEMALARMS_ALARM_ERROR: + case SYSTEMALARMS_ALARM_CRITICAL: + default: + blob->misc.low_battery = 1; + break; + } + + newAlarmData = false; + }*/ + +#if FLIGHTBATTERYSUPPORTED + if (newBattData) { + float consumed_energy; + FlightBatteryStateConsumedEnergyGet(&consumed_energy); + + uint16_t current = (uint16_t)(consumed_energy * 10); + + /* convert to big endian */ + blob->misc.current = ( + (current & 0xFF00 >> 8) | + (current & 0x00FF << 8)); + + newBattData = false; + } +#else +//blob->misc.current = 0; +#endif + +#if POSITIONACTUAL_SUPPORTED + if (newPositionActualData) { + PositionActualData position; + PositionActualGet(&position); + + /* compute 3D distance */ + float d = sqrt( + pow(position.North,2) + + pow(position.East,2) + + pow(position.Down,2)); + /* convert from cm to dm (10ths of m) */ + uint16_t home = (uint16_t)(d / 10); + + /* convert to big endian */ + blob->misc.home = ( + (home & 0xFF00 >> 8) | + (home & 0x00FF << 8)); + + newPositionActualData = false; + } +#else +//blob->misc.home = 0; +#endif + + if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { + /* Sent a packet, flip to the opposite sync */ + if (sync == OSD_HK_SYNC_A) { + sync = OSD_HK_SYNC_B; + } else { + sync = OSD_HK_SYNC_A; + } + } else { + /* Failed to send this update */ + osd_hk_msg_dropped++; + } + osd_packet++; + if (osd_packet > OSD_HK_PKT_TYPE_MODE) { + osd_packet = OSD_HK_PKT_TYPE_MISC; + } +} + +static UAVObjEvent ev; + +static int32_t osdoutputStart(void) +{ + if (osdoutputEnabled) { + /* Start a periodic timer to kick sending of an update */ + EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS); + return 0; + } + return -1; +} + +static int32_t osdoutputInitialize(void) +{ + osd_hk_com_id = PIOS_COM_OSDHK; +#ifdef MODULE_OSDOUTPUT_BUILTIN + osdoutputEnabled = 1; +#else + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + osdoutputEnabled = 1; + } else { + osdoutputEnabled = 0; + } +#endif + return 0; +} +MODULE_INITCALL(osdoutputInitialize, osdoutputStart) + +/** + * @} + * @} + */ diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index ac4be841c..d0a38b7c3 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * * @file pios_board.h @@ -23,7 +23,6 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef STM32103CB_CC_H_ #define STM32103CB_CC_H_ @@ -31,14 +30,14 @@ // Timers and Channels Used //------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | Servo 4 | | | -TIM2 | RC In 5 | RC In 6 | Servo 6 | -TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 -TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ //------------------------ // DMA Channels Used @@ -56,7 +55,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 /* Channel 11 - */ /* Channel 12 - */ - //------------------------ // BOOTLOADER_SETTINGS //------------------------ @@ -64,7 +62,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define BOARD_WRITABLE TRUE #define MAX_DEL_RETRYS 3 - //------------------------ // WATCHDOG_SETTINGS //------------------------ @@ -99,7 +96,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_IRQ_PRIO_MID 8 // higher than RTOS #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... - //------------------------ // PIOS_I2C // See also pios_board.c @@ -159,7 +155,6 @@ extern uint32_t pios_com_debug_id; extern uint32_t pios_com_hkosd_id; #define PIOS_COM_OSDHK (pios_com_hkosd_id) - //------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z @@ -269,7 +264,6 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_GPIO_CLKS { } #define PIOS_GPIO_NUM 0 - //------------------------- // USB //------------------------- diff --git a/flight/PiOS/Boards/STM32F4xx_OSD.h b/flight/PiOS/Boards/STM32F4xx_OSD.h index a49c3220d..c4d1e34b5 100644 --- a/flight/PiOS/Boards/STM32F4xx_OSD.h +++ b/flight/PiOS/Boards/STM32F4xx_OSD.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * * @file pios_board.h @@ -26,21 +26,20 @@ #ifndef PIOS_BOARD_H #define PIOS_BOARD_H - #include // ***************************************************************** // Timers and Channels Used /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+------------+------------+------------+------------ -TIM1 | DELAY | -TIM2 | | PPM Output | PPM Input | -TIM3 | TIMER INTERRUPT | -TIM4 | STOPWATCH | -------+------------+------------+------------+------------ -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+------------+------------+------------+------------ + TIM1 | DELAY | + TIM2 | | PPM Output | PPM Input | + TIM3 | TIMER INTERRUPT | + TIM4 | STOPWATCH | + ------+------------+------------+------------+------------ + */ //------------------------ // DMA Channels Used @@ -99,7 +98,6 @@ TIM4 | STOPWATCH | // #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK - //------------------------ // TELEMETRY //------------------------ @@ -114,7 +112,6 @@ TIM4 | STOPWATCH | #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... - //------------------------ // WATCHDOG_SETTINGS //------------------------ @@ -126,7 +123,6 @@ TIM4 | STOPWATCH | #define PIOS_WDG_MANUAL 0x0008 #define PIOS_WDG_OSDGEN 0x0010 - // ***************************************************************** // PIOS_LED #define PIOS_LED_HEARTBEAT 0 @@ -144,13 +140,13 @@ TIM4 | STOPWATCH | // Timer interrupt /*#define TIMER_INT_TIMER TIM3 -#define TIMER_INT_FUNC TIM3_IRQHandler -#define TIMER_INT_PRIORITY 2 + #define TIMER_INT_FUNC TIM3_IRQHandler + #define TIMER_INT_PRIORITY 2 -// ***************************************************************** -// Stop watch timer + // ***************************************************************** + // Stop watch timer -#define STOPWATCH_TIMER TIM4*/ + #define STOPWATCH_TIMER TIM4*/ //------------------------ // PIOS_SPI @@ -191,7 +187,6 @@ extern uint32_t pios_com_telem_usb_id; //extern uint32_t pios_com_gps_id; //#define PIOS_COM_GPS (pios_com_gps_id) - #if defined(PIOS_INCLUDE_USB_HID) extern uint32_t pios_com_telem_usb_id; #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) @@ -234,16 +229,15 @@ extern uint32_t pios_com_telem_usb_id; // USB #if defined(PIOS_INCLUDE_USB_HID) - #define PIOS_USB_ENABLED 1 - #define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT - #define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN - #define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 - #define PIOS_IRQ_USB_PRIORITY 8 - #define PIOS_USB_RX_BUFFER_SIZE 512 - #define PIOS_USB_TX_BUFFER_SIZE 512 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT +#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN +#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 +#define PIOS_IRQ_USB_PRIORITY 8 +#define PIOS_USB_RX_BUFFER_SIZE 512 +#define PIOS_USB_TX_BUFFER_SIZE 512 #endif - // ***************************************************************** //-------------------------- // Timer controller settings @@ -272,7 +266,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; //------------------------ #define PIOS_BMP085_OVERSAMPLING 3 - /** * glue macros for file IO * STM32 uses DOSFS for file IO @@ -289,5 +282,4 @@ extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector) - #endif /* PIOS_BOARD_H */ diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index f36d6c6da..546ebfb6a 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -291,7 +291,7 @@ extern uint32_t pios_packet_handler; #define PIOS_ADC_NUM_CHANNELS 4 #define PIOS_ADC_MAX_OVERSAMPLING 2 #define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f #define PIOS_ADC_USE_TEMP_SENSOR 1 //------------------------- diff --git a/flight/PiOS/Boards/STM32F4xx_Revolution.h b/flight/PiOS/Boards/STM32F4xx_Revolution.h index 32f8ab451..adbb7e3ea 100644 --- a/flight/PiOS/Boards/STM32F4xx_Revolution.h +++ b/flight/PiOS/Boards/STM32F4xx_Revolution.h @@ -26,7 +26,6 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef STM3210E_INS_H_ #define STM3210E_INS_H_ @@ -36,18 +35,18 @@ // Timers and Channels Used //------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | | | | -TIM2 | --------------- PIOS_DELAY ----------------- -TIM3 | | | | -TIM4 | | | | -TIM5 | | | | -TIM6 | | | | -TIM7 | | | | -TIM8 | | | | -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- + */ //------------------------ // DMA Channels Used @@ -72,7 +71,6 @@ TIM8 | | | | #define BOARD_WRITABLE true #define MAX_DEL_RETRYS 3 - //------------------------ // PIOS_LED //------------------------ @@ -177,7 +175,6 @@ extern uint32_t pios_com_hkosd_id; // #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK - //------------------------- // Interrupt Priorities //------------------------- @@ -185,7 +182,6 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_IRQ_PRIO_MID 8 // higher than RTOS #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... - //------------------------ // PIOS_RCVR // See also pios_board.c @@ -258,7 +254,7 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_ADC_NUM_CHANNELS 4 #define PIOS_ADC_MAX_OVERSAMPLING 2 #define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f #define PIOS_ADC_USE_TEMP_SENSOR 1 //------------------------- diff --git a/flight/PiOS/Common/pios_video.c b/flight/PiOS/Common/pios_video.c index 679da1084..8e211d68c 100644 --- a/flight/PiOS/Common/pios_video.c +++ b/flight/PiOS/Common/pios_video.c @@ -44,7 +44,6 @@ static void flush_spi(); extern xSemaphoreHandle osdSemaphore; static const struct pios_video_cfg * dev_cfg; - // Define the buffers. // For 256x192 pixel mode: // buffer0_level/buffer0_mask becomes buffer_level; and @@ -54,11 +53,11 @@ static const struct pios_video_cfg * dev_cfg; // Must be allocated in one block, so it is in a struct. struct _buffers { - uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; -} buffers; + uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; + uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; + uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; + uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; +}buffers; // Remove the struct definition (makes it easier to write for.) #define buffer0_level (buffers.buffer0_level) @@ -87,58 +86,58 @@ static int16_t m_osdLines=0; */ void swap_buffers() { - // While we could use XOR swap this is more reliable and - // dependable and it's only called a few times per second. - // Many compliers should optimise these to EXCH instructions. - uint8_t *tmp; - SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask); - SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level); + // While we could use XOR swap this is more reliable and + // dependable and it's only called a few times per second. + // Many compliers should optimise these to EXCH instructions. + uint8_t *tmp; + SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask); + SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level); } bool PIOS_Hsync_ISR() { - // On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1 - if(Hsync_update==GRAPHICS_LINE) - { - prepare_line(0); - gActiveLine = 1; - } - Hsync_update++; + // On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1 + if(Hsync_update==GRAPHICS_LINE) { + prepare_line(0); + gActiveLine = 1; + } + Hsync_update++; + return true; } bool PIOS_Vsync_ISR() { - static portBASE_TYPE xHigherPriorityTaskWoken; - + static portBASE_TYPE xHigherPriorityTaskWoken; + xHigherPriorityTaskWoken = pdFALSE; - m_osdLines = gActiveLine; + m_osdLines = gActiveLine; - stop_hsync_timers(); - - // Wait for previous word to clock out of each - TIM_Cmd(dev_cfg->pixel_timer.timer, ENABLE); - flush_spi(); - TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); + stop_hsync_timers(); - gActiveLine = 0; - Hsync_update = 0; - Vsync_update++; - if(Vsync_update>=2) - { - // load second image buffer - swap_buffers(); - Vsync_update=0; + // Wait for previous word to clock out of each + TIM_Cmd(dev_cfg->pixel_timer.timer, ENABLE); + flush_spi(); + TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - // trigger redraw every second field - xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken); - } + gActiveLine = 0; + Hsync_update = 0; + Vsync_update++; + if(Vsync_update>=2) + { + // load second image buffer + swap_buffers(); + Vsync_update=0; - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + // trigger redraw every second field + xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken); + } - return xHigherPriorityTaskWoken == pdTRUE; + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + + return xHigherPriorityTaskWoken == pdTRUE; } uint16_t PIOS_Video_GetOSDLines(void) { - return m_osdLines; + return m_osdLines; } /** @@ -147,14 +146,14 @@ uint16_t PIOS_Video_GetOSDLines(void) { */ static void stop_hsync_timers() { - // This removes the slave mode configuration - TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - TIM_InternalClockConfig(dev_cfg->pixel_timer.timer); + // This removes the slave mode configuration + TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); + TIM_InternalClockConfig(dev_cfg->pixel_timer.timer); } const struct pios_tim_callbacks px_callback = { - .overflow = NULL, - .edge = NULL, + .overflow = NULL, + .edge = NULL, }; #ifdef PAL @@ -171,104 +170,103 @@ const uint32_t dc = (11 / 2); uint32_t failcount = 0; static void reset_hsync_timers() { - // Stop both timers - TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); + // Stop both timers + TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - uint32_t tim_id; - const struct pios_tim_channel *channels = &dev_cfg->hsync_capture; + uint32_t tim_id; + const struct pios_tim_channel *channels = &dev_cfg->hsync_capture; - //BUG: This is nuts this line is needed. It simply results in allocating - //all the memory but somehow leaving it out breaks the timer functionality. - // I do not see how these can be related - if (failcount == 0) { - if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) - failcount++; - } + //BUG: This is nuts this line is needed. It simply results in allocating + //all the memory but somehow leaving it out breaks the timer functionality. + // I do not see how these can be related + if (failcount == 0) { + if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) + failcount++; + } - dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc; + dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc; - // Listen to Channel1 (HSYNC) - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: - TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1); - break; - case TIM_Channel_2: - TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2); - break; - default: - PIOS_Assert(0); - } - TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger); + // Listen to Channel1 (HSYNC) + switch(dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: + TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1); + break; + case TIM_Channel_2: + TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2); + break; + default: + PIOS_Assert(0); + } + TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger); } - static void configure_hsync_timers() { - // Stop both timers - TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); + // Stop both timers + TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - // This is overkill but used for consistency. No interrupts used for pixel clock - // but this function calls the GPIO_Remap - uint32_t tim_id; - const struct pios_tim_channel *channels; + // This is overkill but used for consistency. No interrupts used for pixel clock + // but this function calls the GPIO_Remap + uint32_t tim_id; + const struct pios_tim_channel *channels; - // Init the channel to output the pixel clock - channels = &dev_cfg->pixel_timer; - PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0); + // Init the channel to output the pixel clock + channels = &dev_cfg->pixel_timer; + PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0); - // Init the channel to capture the pulse - channels = &dev_cfg->hsync_capture; - PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0); + // Init the channel to capture the pulse + channels = &dev_cfg->hsync_capture; + PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0); - // Configure the input capture channel - TIM_ICInitTypeDef TIM_ICInitStructure; - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: - TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; - break; - case TIM_Channel_2: - TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; - break; - default: - PIOS_Assert(0); - } - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - //TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0; - TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure); + // Configure the input capture channel + TIM_ICInitTypeDef TIM_ICInitStructure; + switch(dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: + TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; + break; + case TIM_Channel_2: + TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; + break; + default: + PIOS_Assert(0); + } + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + //TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0; + TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure); - // Set up the channel to output the pixel clock - switch(dev_cfg->pixel_timer.timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); - TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); - TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc); - break; - case TIM_Channel_2: - TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); - TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); - TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc); - break; - case TIM_Channel_3: - TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); - TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); - TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc); - break; - case TIM_Channel_4: - TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); - TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); - TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc); - break; - } - TIM_ARRPreloadConfig(dev_cfg->pixel_timer.timer, ENABLE); - TIM_CtrlPWMOutputs(dev_cfg->pixel_timer.timer, ENABLE); + // Set up the channel to output the pixel clock + switch(dev_cfg->pixel_timer.timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); + TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); + TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc); + break; + case TIM_Channel_2: + TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); + TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); + TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc); + break; + case TIM_Channel_3: + TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); + TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); + TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc); + break; + case TIM_Channel_4: + TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); + TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); + TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc); + break; + } + TIM_ARRPreloadConfig(dev_cfg->pixel_timer.timer, ENABLE); + TIM_CtrlPWMOutputs(dev_cfg->pixel_timer.timer, ENABLE); - // This shouldn't be needed as it should come from the config struture. Something - // is clobbering that - TIM_PrescalerConfig(dev_cfg->pixel_timer.timer, 0, TIM_PSCReloadMode_Immediate); - TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period); + // This shouldn't be needed as it should come from the config struture. Something + // is clobbering that + TIM_PrescalerConfig(dev_cfg->pixel_timer.timer, 0, TIM_PSCReloadMode_Immediate); + TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period); } DMA_TypeDef * main_dma; @@ -277,94 +275,94 @@ DMA_Stream_TypeDef * main_stream; DMA_Stream_TypeDef * mask_stream; void PIOS_Video_Init(const struct pios_video_cfg * cfg) { - dev_cfg = cfg; // store config before enabling interrupt + dev_cfg = cfg; // store config before enabling interrupt - configure_hsync_timers(); + configure_hsync_timers(); - /* needed for HW hack */ - const GPIO_InitTypeDef initStruct = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN , - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }; - GPIO_Init(GPIOC, &initStruct); - - /* SPI3 - MASKBUFFER */ - GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init)); - GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init)); + /* needed for HW hack */ + const GPIO_InitTypeDef initStruct = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN , + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }; + GPIO_Init(GPIOC, &initStruct); - /* SPI1 SLAVE FRAMEBUFFER */ - GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init)); - GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init)); + /* SPI3 - MASKBUFFER */ + GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init)); + GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init)); - if (cfg->mask.remap) { - GPIO_PinAFConfig(cfg->mask.sclk.gpio, - __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), - cfg->mask.remap); - GPIO_PinAFConfig(cfg->mask.miso.gpio, - __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), - cfg->mask.remap); - } - if (cfg->level.remap) - { - GPIO_PinAFConfig(cfg->level.sclk.gpio, - __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), - cfg->level.remap); - GPIO_PinAFConfig(cfg->level.miso.gpio, - __builtin_ctz(cfg->level.miso.init.GPIO_Pin), - cfg->level.remap); - } + /* SPI1 SLAVE FRAMEBUFFER */ + GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init)); + GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init)); - /* Initialize the SPI block */ - SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init)); - SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init)); - - /* Enable SPI */ - SPI_Cmd(cfg->level.regs, ENABLE); - SPI_Cmd(cfg->mask.regs, ENABLE); + if (cfg->mask.remap) { + GPIO_PinAFConfig(cfg->mask.sclk.gpio, + __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), + cfg->mask.remap); + GPIO_PinAFConfig(cfg->mask.miso.gpio, + __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), + cfg->mask.remap); + } + if (cfg->level.remap) + { + GPIO_PinAFConfig(cfg->level.sclk.gpio, + __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), + cfg->level.remap); + GPIO_PinAFConfig(cfg->level.miso.gpio, + __builtin_ctz(cfg->level.miso.init.GPIO_Pin), + cfg->level.remap); + } - /* Configure DMA for SPI Tx SLAVE Maskbuffer */ - DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); - DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init)); + /* Initialize the SPI block */ + SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init)); + SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init)); - /* Configure DMA for SPI Tx SLAVE Framebuffer*/ - DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); - DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init)); + /* Enable SPI */ + SPI_Cmd(cfg->level.regs, ENABLE); + SPI_Cmd(cfg->mask.regs, ENABLE); - /* Trigger interrupt when for half conversions too to indicate double buffer */ - DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); + /* Configure DMA for SPI Tx SLAVE Maskbuffer */ + DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); + DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init)); - /* Configure and clear buffers */ + /* Configure DMA for SPI Tx SLAVE Framebuffer*/ + DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); + DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init)); + + /* Trigger interrupt when for half conversions too to indicate double buffer */ + DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); + + /* Configure and clear buffers */ draw_buffer_level = buffer0_level; draw_buffer_mask = buffer0_mask; disp_buffer_level = buffer1_level; disp_buffer_mask = buffer1_mask; - memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - /* Configure DMA interrupt */ - - NVIC_Init(&cfg->level.dma.irq.init); + /* Configure DMA interrupt */ - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); - SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); + NVIC_Init(&cfg->level.dma.irq.init); - mask_dma = DMA1; - main_dma = DMA2; - main_stream = cfg->level.dma.tx.channel; - mask_stream = cfg->mask.dma.tx.channel; - /* Configure the Video Line interrupt */ - PIOS_EXTI_Init(cfg->hsync); - PIOS_EXTI_Init(cfg->vsync); + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); - //set levels to zero - PIOS_Servo_Set(0,0); - PIOS_Servo_Set(1,0); + mask_dma = DMA1; + main_dma = DMA2; + main_stream = cfg->level.dma.tx.channel; + mask_stream = cfg->mask.dma.tx.channel; + /* Configure the Video Line interrupt */ + PIOS_EXTI_Init(cfg->hsync); + PIOS_EXTI_Init(cfg->vsync); + + //set levels to zero + PIOS_Servo_Set(0,0); + PIOS_Servo_Set(1,0); } /** @@ -373,34 +371,34 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) */ static void prepare_line(uint32_t line_num) { - if(line_numpixel_timer.timer->CNT = dc; + dev_cfg->pixel_timer.timer->CNT = dc; - DMA_ClearFlag(dev_cfg->mask.dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7); - DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); + DMA_ClearFlag(dev_cfg->mask.dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7); + DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); - // Load new line - DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0); - DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0); + // Load new line + DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0); - // Enable DMA, Slave first - DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH); - DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel, BUFFER_LINE_LENGTH); + // Enable DMA, Slave first + DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH); + DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel, BUFFER_LINE_LENGTH); - SPI_Cmd(dev_cfg->level.regs, ENABLE); - SPI_Cmd(dev_cfg->mask.regs, ENABLE); - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(dev_cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); - SPI_I2S_DMACmd(dev_cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); - - DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE); - DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE); - } - reset_hsync_timers(); + SPI_Cmd(dev_cfg->level.regs, ENABLE); + SPI_Cmd(dev_cfg->mask.regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(dev_cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(dev_cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); + + DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE); + DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE); + } + reset_hsync_timers(); } void PIOS_VIDEO_DMA_Handler(void); @@ -412,35 +410,35 @@ void DMA2_Stream5_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler */ static void flush_spi() { - bool level_empty = false; - bool mask_empty = false; - bool level_stopped = false; - bool mask_stopped = false; - - // Can't flush if clock not running - while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) { + bool level_empty = false; + bool mask_empty = false; + bool level_stopped = false; + bool mask_stopped = false; - level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET; - mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET; + // Can't flush if clock not running + while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) { - if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) { - SPI_Cmd(dev_cfg->level.regs, DISABLE); - level_stopped = true; - } + level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET; + mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET; - if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == RESET) { - SPI_Cmd(dev_cfg->mask.regs, DISABLE); - mask_stopped = true; - } - } - /* - uint32_t i = 0; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ - SPI_Cmd(dev_cfg->mask.regs, DISABLE); - SPI_Cmd(dev_cfg->level.regs, DISABLE); + if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) { + SPI_Cmd(dev_cfg->level.regs, DISABLE); + level_stopped = true; + } + + if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == RESET) { + SPI_Cmd(dev_cfg->mask.regs, DISABLE); + mask_stopped = true; + } + } + /* + uint32_t i = 0; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ + SPI_Cmd(dev_cfg->mask.regs, DISABLE); + SPI_Cmd(dev_cfg->level.regs, DISABLE); } /** @@ -448,35 +446,35 @@ static void flush_spi() */ void PIOS_VIDEO_DMA_Handler(void) { - // Handle flags from stream channel - if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); - if(gActiveLine < GRAPHICS_HEIGHT) - { - flush_spi(); - stop_hsync_timers(); + // Handle flags from stream channel + if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled + DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); + if(gActiveLine < GRAPHICS_HEIGHT) + { + flush_spi(); + stop_hsync_timers(); - dev_cfg->pixel_timer.timer->CNT = dc; + dev_cfg->pixel_timer.timer->CNT = dc; - prepare_line(gActiveLine); - } - else if(gActiveLine >= GRAPHICS_HEIGHT) - { - //last line completed - flush_spi(); - stop_hsync_timers(); + prepare_line(gActiveLine); + } + else if(gActiveLine >= GRAPHICS_HEIGHT) + { + //last line completed + flush_spi(); + stop_hsync_timers(); - // STOP DMA, master first - DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE); - DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE); - } - gActiveLine++; - } - else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5); - } - else { - } + // STOP DMA, master first + DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE); + DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE); + } + gActiveLine++; + } + else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5); + } + else { + } } #endif /* PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/CopterControl/Makefile b/flight/targets/CopterControl/Makefile index c2c69871d..770a7afdf 100644 --- a/flight/targets/CopterControl/Makefile +++ b/flight/targets/CopterControl/Makefile @@ -43,6 +43,7 @@ OPTMODULES += CameraStab OPTMODULES += ComUsbBridge OPTMODULES += GPS OPTMODULES += TxPID +OPTMODULES += Osd/osdoutput #OPTMODULES += Altitude #OPTMODULES += Fault diff --git a/flight/targets/RevoMini/Makefile b/flight/targets/RevoMini/Makefile index 2f1bb04c5..6fba8f589 100644 --- a/flight/targets/RevoMini/Makefile +++ b/flight/targets/RevoMini/Makefile @@ -45,7 +45,7 @@ MODULES += FirmwareIAP MODULES += Radio MODULES += PathPlanner MODULES += FixedWingPathFollower -MODULES += Osd/OsdHk +MODULES += Osd/osdoutout MODULES += Telemetry OPTMODULES =