diff --git a/flight/Modules/Osd/osdgen/osdgen.c b/flight/Modules/Osd/osdgen/osdgen.c index 9f9a725ad..26fe6712e 100644 --- a/flight/Modules/Osd/osdgen/osdgen.c +++ b/flight/Modules/Osd/osdgen/osdgen.c @@ -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 */ diff --git a/flight/OSD/Makefile b/flight/OSD/Makefile index fc94c4d2f..7c95ca3b7 100644 --- a/flight/OSD/Makefile +++ b/flight/OSD/Makefile @@ -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 diff --git a/flight/OSD/System/osd.c b/flight/OSD/System/osd.c index 6d536925b..5615925ca 100644 --- a/flight/OSD/System/osd.c +++ b/flight/OSD/System/osd.c @@ -182,7 +182,7 @@ void processReset(void) } // Clear reset flags - RCC_ClearFlag(); + //RCC_ClearFlag(); } int main() diff --git a/flight/PiOS/Common/printf2.c b/flight/PiOS/Common/printf2.c new file mode 100644 index 000000000..48f722566 --- /dev/null +++ b/flight/PiOS/Common/printf2.c @@ -0,0 +1,453 @@ +/******************************************************************************* + Copyright 2001, 2002 Georges Menie () + 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 + +static uint use_leading_plus = 0 ; + +/* based on a example-code from Keil for CS G++ */ + +/* for caddr_t (typedef char * caddr_t;) */ +#include + +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); +} + +/** + * @} + */ diff --git a/flight/PiOS/STM32F4xx/link_STM32F4xx_sections.ld b/flight/PiOS/STM32F4xx/link_STM32F4xx_sections.ld index bc75294d5..1c903c5c4 100644 --- a/flight/PiOS/STM32F4xx/link_STM32F4xx_sections.ld +++ b/flight/PiOS/STM32F4xx/link_STM32F4xx_sections.ld @@ -108,6 +108,7 @@ SECTIONS PROVIDE ( _end = _ebss ) ; } > SRAM + PROVIDE(__HEAP_START = _end ); /* * The heap consumes the remainder of the SRAM. */