mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
Last push before summit
This commit is contained in:
Sambas
parent
93ad5d2660
commit
d399469b88
@ -81,7 +81,7 @@ static void osdgenTask(void *parameters);
|
||||
#define LONG_TIME 0xffff
|
||||
xSemaphoreHandle osdSemaphore = NULL;
|
||||
|
||||
#define STACK_SIZE_BYTES 1096
|
||||
#define STACK_SIZE_BYTES 4096
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
|
||||
#define UPDATE_PERIOD 100
|
||||
@ -2183,6 +2183,8 @@ void updateGraphics() {
|
||||
OsdSettingsGet (&OsdSettings);
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
GPSPositionData gpsData;
|
||||
GPSPositionGet(&gpsData);
|
||||
|
||||
/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
|
||||
write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
|
||||
@ -2219,12 +2221,14 @@ void updateGraphics() {
|
||||
|
||||
char temp[50]={0};
|
||||
memset(temp, ' ', 40);
|
||||
sprintf(temp,"Lat:%d",(int)m_gpsLat);
|
||||
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
|
||||
write_string(temp, 5, 5, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Lon:%d",(int)m_gpsLon);
|
||||
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
|
||||
write_string(temp, 5, 15, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Fix:%d",(int)m_gpsStatus);
|
||||
sprintf(temp,"Fix:%d",(int)gpsData.Status);
|
||||
write_string(temp, 5, 25, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
|
||||
write_string(temp, 5, 35, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
|
||||
|
||||
/* Print RTC time */
|
||||
@ -2238,19 +2242,21 @@ void updateGraphics() {
|
||||
write_string(temp, (GRAPHICS_WIDTH_REAL - 2),5, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage */
|
||||
sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
|
||||
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
|
||||
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f/4096));
|
||||
write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print CPU temperature */
|
||||
sprintf(temp,"Temp:%4dC",(int)(PIOS_ADC_PinGet(6)*0.29296875f-279));
|
||||
sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(6)*0.29296875f-279));
|
||||
write_string(temp, (GRAPHICS_WIDTH_REAL - 2),25, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage FLIGHT*/
|
||||
sprintf(temp,"FltV:%4dV",(int)(PIOS_ADC_PinGet(2)*300*61/4096));
|
||||
sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096));
|
||||
write_string(temp, (GRAPHICS_WIDTH_REAL - 2),35, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage VIDEO*/
|
||||
sprintf(temp,"VidV:%4dV",(int)(PIOS_ADC_PinGet(3)*300*61/4096));
|
||||
sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(3)*3.0f*6.1f/4096));
|
||||
write_string(temp, (GRAPHICS_WIDTH_REAL - 2),45, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage RSSI */
|
||||
|
||||
@ -160,7 +160,8 @@ SRC += $(OPUAVSYNTHDIR)/osdsettings.c
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
#SRC += $(PIOSCOMMON)/pios_hmc5843.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
#SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
SRC += $(PIOSCOMMON)/printf2.c
|
||||
SRC += $(PIOSCOMMON)/pios_crc.c
|
||||
SRC += $(PIOSCOMMON)/pios_video.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c
|
||||
|
||||
@ -182,7 +182,7 @@ void processReset(void)
|
||||
}
|
||||
|
||||
// Clear reset flags
|
||||
RCC_ClearFlag();
|
||||
//RCC_ClearFlag();
|
||||
}
|
||||
|
||||
int main()
|
||||
|
||||
453
flight/PiOS/Common/printf2.c
Normal file
453
flight/PiOS/Common/printf2.c
Normal file
@ -0,0 +1,453 @@
|
||||
/*******************************************************************************
|
||||
Copyright 2001, 2002 Georges Menie (<URL snipped>)
|
||||
This program is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2 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 Lesser General Public License for more details.
|
||||
You should have received a copy of the GNU Lesser 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
|
||||
*/
|
||||
|
||||
/*******************************************************************************
|
||||
putchar is the only external dependency for this file,
|
||||
if you have a working putchar, just remove the following
|
||||
define. If the function should be called something else,
|
||||
replace outbyte(c) by your own function call.
|
||||
*/
|
||||
//*******************************************************************************
|
||||
// Updated by Daniel D Miller. Changes to the original Menie code are
|
||||
// Copyright 2009-2012 Daniel D Miller
|
||||
// All such changes are distributed under the same license as the original,
|
||||
// as described above.
|
||||
// 11/06/09 - adding floating-point support
|
||||
// 03/19/10 - pad fractional portion of floating-point number with 0s
|
||||
// 03/30/10 - document the \% bug
|
||||
// 07/20/10 - Fix a round-off bug in floating-point conversions
|
||||
// ( 0.99 with %.1f did not round to 1.0 )
|
||||
// 10/25/11 - Add support for %+ format (always show + on positive numbers)
|
||||
// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0
|
||||
// decimal places, rather than printf's 6.
|
||||
//*******************************************************************************
|
||||
// BUGS
|
||||
// If '%' is included in a format string, in the form \% with the intent
|
||||
// of including the percent sign in the output string, this function will
|
||||
// mis-handle the data entirely!!
|
||||
// Essentially, it will just discard the character following the percent sign.
|
||||
// This bug is not easy to fix in the existing code;
|
||||
// for now, I'll just try to remember to use %% instead of \% ...
|
||||
//*******************************************************************************
|
||||
|
||||
//lint -esym(752, debug_output)
|
||||
//lint -esym(766, stdio.h)
|
||||
|
||||
// #define TEST_PRINTF 1
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
static uint use_leading_plus = 0 ;
|
||||
|
||||
/* based on a example-code from Keil for CS G++ */
|
||||
|
||||
/* for caddr_t (typedef char * caddr_t;) */
|
||||
#include <sys/types.h>
|
||||
|
||||
extern int __HEAP_START;
|
||||
|
||||
caddr_t _sbrk ( int incr )
|
||||
{
|
||||
static unsigned char *heap = NULL;
|
||||
unsigned char *prev_heap;
|
||||
|
||||
if (heap == NULL) {
|
||||
heap = (unsigned char *)&__HEAP_START;
|
||||
}
|
||||
prev_heap = heap;
|
||||
/* check removed to show basic approach */
|
||||
|
||||
heap += incr;
|
||||
|
||||
return (caddr_t) prev_heap;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
static void printchar (char **str, int c)
|
||||
{
|
||||
if (str) {
|
||||
**str = c;
|
||||
++(*str);
|
||||
}
|
||||
#ifdef TEST_PRINTF
|
||||
else {
|
||||
extern int putchar (int c);
|
||||
(void) putchar (c);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
static uint my_strlen(char *str)
|
||||
{
|
||||
if (str == 0)
|
||||
return 0;
|
||||
uint slen = 0 ;
|
||||
while (*str != 0) {
|
||||
slen++ ;
|
||||
str++ ;
|
||||
}
|
||||
return slen;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// This version returns the length of the output string.
|
||||
// It is more useful when implementing a walking-string function.
|
||||
//****************************************************************************
|
||||
static const double round_nums[8] = {
|
||||
0.5,
|
||||
0.05,
|
||||
0.005,
|
||||
0.0005,
|
||||
0.00005,
|
||||
0.000005,
|
||||
0.0000005,
|
||||
0.00000005
|
||||
} ;
|
||||
|
||||
static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
|
||||
{
|
||||
static char local_bfr[128] ;
|
||||
char *output = (outbfr == 0) ? local_bfr : outbfr ;
|
||||
|
||||
//*******************************************
|
||||
// extract negative info
|
||||
//*******************************************
|
||||
if (dbl < 0.0) {
|
||||
*output++ = '-' ;
|
||||
dbl *= -1.0 ;
|
||||
} else {
|
||||
if (use_leading_plus) {
|
||||
*output++ = '+' ;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// handling rounding by adding .5LSB to the floating-point data
|
||||
if (dec_digits < 8) {
|
||||
dbl += round_nums[dec_digits] ;
|
||||
}
|
||||
|
||||
//**************************************************************************
|
||||
// construct fractional multiplier for specified number of digits.
|
||||
//**************************************************************************
|
||||
uint mult = 1 ;
|
||||
uint idx ;
|
||||
for (idx=0; idx < dec_digits; idx++)
|
||||
mult *= 10 ;
|
||||
|
||||
// printf("mult=%u\n", mult) ;
|
||||
uint wholeNum = (uint) dbl ;
|
||||
uint decimalNum = (uint) ((dbl - wholeNum) * mult);
|
||||
|
||||
//*******************************************
|
||||
// convert integer portion
|
||||
//*******************************************
|
||||
char tbfr[40] ;
|
||||
idx = 0 ;
|
||||
while (wholeNum != 0) {
|
||||
tbfr[idx++] = '0' + (wholeNum % 10) ;
|
||||
wholeNum /= 10 ;
|
||||
}
|
||||
// printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ;
|
||||
if (idx == 0) {
|
||||
*output++ = '0' ;
|
||||
} else {
|
||||
while (idx > 0) {
|
||||
*output++ = tbfr[idx-1] ; //lint !e771
|
||||
idx-- ;
|
||||
}
|
||||
}
|
||||
if (dec_digits > 0) {
|
||||
*output++ = '.' ;
|
||||
|
||||
//*******************************************
|
||||
// convert fractional portion
|
||||
//*******************************************
|
||||
idx = 0 ;
|
||||
while (decimalNum != 0) {
|
||||
tbfr[idx++] = '0' + (decimalNum % 10) ;
|
||||
decimalNum /= 10 ;
|
||||
}
|
||||
// pad the decimal portion with 0s as necessary;
|
||||
// We wouldn't want to report 3.093 as 3.93, would we??
|
||||
while (idx < dec_digits) {
|
||||
tbfr[idx++] = '0' ;
|
||||
}
|
||||
// printf("decimal=%s\n", tbfr) ;
|
||||
if (idx == 0) {
|
||||
*output++ = '0' ;
|
||||
} else {
|
||||
while (idx > 0) {
|
||||
*output++ = tbfr[idx-1] ;
|
||||
idx-- ;
|
||||
}
|
||||
}
|
||||
}
|
||||
*output = 0 ;
|
||||
|
||||
// prepare output
|
||||
output = (outbfr == 0) ? local_bfr : outbfr ;
|
||||
return my_strlen(output) ;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
#define PAD_RIGHT 1
|
||||
#define PAD_ZERO 2
|
||||
|
||||
static int prints (char **out, const char *string, int width, int pad)
|
||||
{
|
||||
register int pc = 0, padchar = ' ';
|
||||
if (width > 0) {
|
||||
int len = 0;
|
||||
const char *ptr;
|
||||
for (ptr = string; *ptr; ++ptr)
|
||||
++len;
|
||||
if (len >= width)
|
||||
width = 0;
|
||||
else
|
||||
width -= len;
|
||||
if (pad & PAD_ZERO)
|
||||
padchar = '0';
|
||||
}
|
||||
if (!(pad & PAD_RIGHT)) {
|
||||
for (; width > 0; --width) {
|
||||
printchar (out, padchar);
|
||||
++pc;
|
||||
}
|
||||
}
|
||||
for (; *string; ++string) {
|
||||
printchar (out, *string);
|
||||
++pc;
|
||||
}
|
||||
for (; width > 0; --width) {
|
||||
printchar (out, padchar);
|
||||
++pc;
|
||||
}
|
||||
return pc;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
/* the following should be enough for 32 bit int */
|
||||
#define PRINT_BUF_LEN 12
|
||||
static int printi (char **out, int i, int b, int sg, int width, int pad, int letbase)
|
||||
{
|
||||
char print_buf[PRINT_BUF_LEN];
|
||||
char *s;
|
||||
int t, neg = 0, pc = 0;
|
||||
unsigned u = (unsigned) i;
|
||||
if (i == 0) {
|
||||
print_buf[0] = '0';
|
||||
print_buf[1] = '\0';
|
||||
return prints (out, print_buf, width, pad);
|
||||
}
|
||||
if (sg && b == 10 && i < 0) {
|
||||
neg = 1;
|
||||
u = (unsigned) -i;
|
||||
}
|
||||
// make sure print_buf is NULL-term
|
||||
s = print_buf + PRINT_BUF_LEN - 1;
|
||||
*s = '\0';
|
||||
|
||||
|
||||
while (u) {
|
||||
t = u % b; //lint !e573 Warning 573: Signed-unsigned mix with divide
|
||||
if (t >= 10)
|
||||
t += letbase - '0' - 10;
|
||||
*--s = t + '0';
|
||||
u /= b; //lint !e573 Warning 573: Signed-unsigned mix with divide
|
||||
}
|
||||
if (neg) {
|
||||
if (width && (pad & PAD_ZERO)) {
|
||||
printchar (out, '-');
|
||||
++pc;
|
||||
--width;
|
||||
}
|
||||
else {
|
||||
*--s = '-';
|
||||
}
|
||||
} else {
|
||||
if (use_leading_plus) {
|
||||
*--s = '+';
|
||||
}
|
||||
}
|
||||
return pc + prints (out, s, width, pad);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
static int print (char **out, int *varg)
|
||||
{
|
||||
int post_decimal ;
|
||||
int width, pad ;
|
||||
unsigned dec_width = 6 ;
|
||||
int pc = 0;
|
||||
char *format = (char *) (*varg++);
|
||||
char scr[2];
|
||||
use_leading_plus = 0 ; // start out with this clear
|
||||
for (; *format != 0; ++format) {
|
||||
if (*format == '%') {
|
||||
dec_width = 6 ;
|
||||
++format;
|
||||
width = pad = 0;
|
||||
if (*format == '\0')
|
||||
break;
|
||||
if (*format == '%')
|
||||
goto out_lbl;
|
||||
if (*format == '-') {
|
||||
++format;
|
||||
pad = PAD_RIGHT;
|
||||
}
|
||||
if (*format == '+') {
|
||||
++format;
|
||||
use_leading_plus = 1 ;
|
||||
}
|
||||
while (*format == '0') {
|
||||
++format;
|
||||
pad |= PAD_ZERO;
|
||||
}
|
||||
post_decimal = 0 ;
|
||||
if (*format == '.' ||
|
||||
(*format >= '0' && *format <= '9')) {
|
||||
|
||||
while (1) {
|
||||
if (*format == '.') {
|
||||
post_decimal = 1 ;
|
||||
dec_width = 0 ;
|
||||
format++ ;
|
||||
} else if ((*format >= '0' && *format <= '9')) {
|
||||
if (post_decimal) {
|
||||
dec_width *= 10;
|
||||
dec_width += *format - '0';
|
||||
} else {
|
||||
width *= 10;
|
||||
width += *format - '0';
|
||||
}
|
||||
format++ ;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (*format == 'l')
|
||||
++format;
|
||||
switch (*format) {
|
||||
case 's':
|
||||
{
|
||||
// char *s = *((char **) varg++); //lint !e740
|
||||
char *s = (char *) *varg++ ; //lint !e740 !e826 convert to double pointer
|
||||
pc += prints (out, s ? s : "(null)", width, pad);
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
}
|
||||
break;
|
||||
case 'd':
|
||||
pc += printi (out, *varg++, 10, 1, width, pad, 'a');
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
case 'x':
|
||||
pc += printi (out, *varg++, 16, 0, width, pad, 'a');
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
case 'X':
|
||||
pc += printi (out, *varg++, 16, 0, width, pad, 'A');
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
case 'u':
|
||||
pc += printi (out, *varg++, 10, 0, width, pad, 'a');
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
case 'c':
|
||||
/* char are converted to int then pushed on the stack */
|
||||
scr[0] = *varg++;
|
||||
scr[1] = '\0';
|
||||
pc += prints (out, scr, width, pad);
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
{
|
||||
// http://wiki.debian.org/ArmEabiPort#Structpackingandalignment
|
||||
// Stack alignment
|
||||
//
|
||||
// The ARM EABI requires 8-byte stack alignment at public function entry points,
|
||||
// compared to the previous 4-byte alignment.
|
||||
#ifdef USE_NEWLIB
|
||||
char *cptr = (char *) varg ; //lint !e740 !e826 convert to double pointer
|
||||
uint caddr = (uint) cptr ;
|
||||
if ((caddr & 0xF) != 0) {
|
||||
cptr += 4 ;
|
||||
}
|
||||
double *dblptr = (double *) cptr ; //lint !e740 !e826 convert to double pointer
|
||||
#else
|
||||
double *dblptr = (double *) varg ; //lint !e740 !e826 convert to double pointer
|
||||
#endif
|
||||
double dbl = *dblptr++ ; // increment double pointer
|
||||
varg = (int *) dblptr ; //lint !e740 copy updated pointer back to base pointer
|
||||
char bfr[81] ;
|
||||
// unsigned slen =
|
||||
dbl2stri(bfr, dbl, dec_width) ;
|
||||
// stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ;
|
||||
pc += prints (out, bfr, width, pad);
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
printchar (out, '%');
|
||||
printchar (out, *format);
|
||||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||||
break;
|
||||
}
|
||||
} else
|
||||
// if (*format == '\\') {
|
||||
//
|
||||
// } else
|
||||
{
|
||||
out_lbl:
|
||||
printchar (out, *format);
|
||||
++pc;
|
||||
}
|
||||
} // for each char in format string
|
||||
if (out) //lint !e850
|
||||
**out = '\0';
|
||||
return pc;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
int stringf (char *out, const char *format, ...)
|
||||
{
|
||||
// create a pointer into the stack.
|
||||
// Thematically this should be a void*, since the actual usage of the
|
||||
// pointer will vary. However, int* works fine too.
|
||||
// Either way, the called function will need to re-cast the pointer
|
||||
// for any type which isn't sizeof(int)
|
||||
int *varg = (int *) (char *) (&format);
|
||||
return print (&out, varg);
|
||||
}
|
||||
|
||||
int printf(const char *format, ...)
|
||||
{
|
||||
int *varg = (int *) (char *) (&format);
|
||||
return print (0, varg);
|
||||
}
|
||||
|
||||
int sprintf(char *out, const char *format, ...)
|
||||
{
|
||||
int *varg = (int *) (char *) (&format);
|
||||
return print (&out, varg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -108,6 +108,7 @@ SECTIONS
|
||||
PROVIDE ( _end = _ebss ) ;
|
||||
} > SRAM
|
||||
|
||||
PROVIDE(__HEAP_START = _end );
|
||||
/*
|
||||
* The heap consumes the remainder of the SRAM.
|
||||
*/
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user