rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

spring cleaning

Browse Source
This commit is contained in:
sambas 2013-04-20 18:07:19 +03:00
parent 3512e7a657
commit 074b391cc0
16 changed files with 874 additions and 948 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

315
flight/Modules/Osd/OsdHk/OsdHk.c
View File

@ -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)
/**
* @}
* @}
*/

38
flight/Modules/Osd/WavPlayer/inc/wavplayer.h
View File

@ -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_ */

65
flight/Modules/Osd/WavPlayer/wavplayer.c
View File

@ -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;
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

44
flight/Modules/Osd/osdgen/inc/osdgen.h
View File

@ -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_ */

37
flight/Modules/Osd/osdgen/osdgen.c
View File

@ -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;
}

39
flight/Modules/Osd/osdinput/inc/osdinput.h
View File

@ -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_ */

296
flight/Modules/Osd/osdinput/osdinput.c
View File

@ -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);
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

23
flight/Modules/Osd/OsdHk/inc/OsdHk.h → flight/Modules/Osd/osdoutput/inc/osdoutput.h
View File

@ -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 */
/**
* @}

304
flight/Modules/Osd/osdoutput/osdoutput.c Normal file
View File

@ -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)
/**
* @}
* @}
*/

24
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -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
//-------------------------

50
flight/PiOS/Boards/STM32F4xx_OSD.h
View File

@ -1,4 +1,4 @@
/**
/**
******************************************************************************
*
* @file pios_board.h
@ -26,21 +26,20 @@
#ifndef PIOS_BOARD_H
#define PIOS_BOARD_H
#include <stdbool.h>
// *****************************************************************
// 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 */

2
flight/PiOS/Boards/STM32F4xx_RevoMini.h
View File

@ -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
//-------------------------

30
flight/PiOS/Boards/STM32F4xx_Revolution.h
View File

@ -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
//-------------------------

552
flight/PiOS/Common/pios_video.c
View File

@ -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_num<GRAPHICS_HEIGHT)
{
uint32_t buf_offset = line_num * GRAPHICS_WIDTH;
if(line_num<GRAPHICS_HEIGHT)
{
uint32_t buf_offset = line_num * GRAPHICS_WIDTH;
dev_cfg->pixel_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 */

1
flight/targets/CopterControl/Makefile
View File

@ -43,6 +43,7 @@ OPTMODULES += CameraStab
OPTMODULES += ComUsbBridge
OPTMODULES += GPS
OPTMODULES += TxPID
OPTMODULES += Osd/osdoutput
#OPTMODULES += Altitude
#OPTMODULES += Fault

2
flight/targets/RevoMini/Makefile
View File

@ -45,7 +45,7 @@ MODULES += FirmwareIAP
MODULES += Radio
MODULES += PathPlanner
MODULES += FixedWingPathFollower
MODULES += Osd/OsdHk
MODULES += Osd/osdoutout
MODULES += Telemetry
OPTMODULES =