From 9e3c499faeb5d6cdca7cec9eb1efd335bc3b5c2b Mon Sep 17 00:00:00 2001 From: CInsights Date: Mon, 9 Apr 2018 17:27:02 +1000 Subject: [PATCH] Fixed camera memory clear bug. Various fixes. Add CRC calc to UBLOX. --- tracker/software/config.c | 10 +-- tracker/software/config.h | 2 +- tracker/software/drivers/ov5640.c | 39 ++++---- tracker/software/drivers/ublox.c | 23 +++++ tracker/software/drivers/ublox.h | 2 +- tracker/software/drivers/usb/debug.h | 4 +- tracker/software/mcuconf.h | 2 +- tracker/software/pkt/devices/si446x.c | 4 +- tracker/software/pkt/managers/pktservice.c | 37 +++++++- tracker/software/pkt/managers/pktservice.h | 1 - tracker/software/pkt/pktconf.h | 11 +-- .../software/pkt/protocols/aprs2/ax25_pad.c | 44 ++++++---- .../software/pkt/protocols/aprs2/ax25_pad.h | 3 +- tracker/software/portab.h | 4 +- tracker/software/protocols/packet/aprs.c | 88 ++++++++++++------- tracker/software/threads/collector.c | 6 +- tracker/software/threads/rxtx/image.c | 65 ++++++++------ tracker/software/threads/rxtx/position.c | 73 +++++++++------ 18 files changed, 270 insertions(+), 148 deletions(-) diff --git a/tracker/software/config.c b/tracker/software/config.c index 1653cb98..1c05f5df 100644 --- a/tracker/software/config.c +++ b/tracker/software/config.c @@ -12,8 +12,8 @@ const conf_t conf_flash_default = { .pos_pri = { .thread_conf = { .active = true, - .cycle = TIME_S2I(1800), - .init_delay = TIME_S2I(60) + .cycle = TIME_S2I(60), + .init_delay = TIME_S2I(10) }, .radio_conf = { .pwr = 0x7F, @@ -56,9 +56,9 @@ const conf_t conf_flash_default = { .img_pri = { .thread_conf = { .active = true, - .cycle = TIME_S2I(60), - .init_delay = TIME_S2I(2*60), - .send_spacing = TIME_S2I(2) + .cycle = TIME_S2I(120), + .init_delay = TIME_S2I(60*2), + .send_spacing = TIME_S2I(3) }, .radio_conf = { .pwr = 0x7F, diff --git a/tracker/software/config.h b/tracker/software/config.h index 7d7b4ab8..22660d11 100644 --- a/tracker/software/config.h +++ b/tracker/software/config.h @@ -8,7 +8,7 @@ * 3V, because USB would not work at 1.8V. Note that the transmission power is increased * too when operating at 3V. This option will also run the STM32 at 48MHz (AHB) permanently * because USB needs that speed, otherwise it is running at 6MHz which saves a lot of power. */ -#define ENABLE_EXTERNAL_I2C FALSE /* The external port can be used for bit bang I2C. */ +#define ENABLE_EXTERNAL_I2C TRUE /* The external port can be used for bit bang I2C. */ #include "types.h" diff --git a/tracker/software/drivers/ov5640.c b/tracker/software/drivers/ov5640.c index 784ef5b1..093a520f 100644 --- a/tracker/software/drivers/ov5640.c +++ b/tracker/software/drivers/ov5640.c @@ -698,6 +698,14 @@ static uint8_t* dma_buffer; static resolution_t last_res = RES_NONE; +/* + * Memory pool integrity checking... will expand to be that + */ +static struct pool_header *pktSystemCheck(void) { + extern guarded_memory_pool_t *ccm_pool; + return ((struct pool_header *)(ccm_pool->pool.next))->next; +} + /** * Captures an image from the camera. * @buffer Buffer in which the image can be sampled @@ -724,25 +732,19 @@ uint32_t OV5640_Snapshot2RAM(uint8_t* buffer, } // Capture image until we get a good image or reach max retries. - - TRACE_INFO("CAM > Capture image"); + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); + TRACE_INFO("CAM > Capture image into buffer @ 0x%08x size 0x%08x", + buffer, size); do { - // Clearing buffer - for(uint32_t i = 0; i < size; i++) - buffer[i] = 0; - //status = false; + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); size_sampled = OV5640_Capture(buffer, size); - //TRACE_INFO("CAM > Capture finished"); - - //size_sampled = size - 1; - while(!buffer[size_sampled - 1] && size_sampled > 0) - size_sampled--; - if(size_sampled > 0) + if(size_sampled > 0) { + TRACE_INFO("CAM > Image size: %d bytes", size_sampled); return size_sampled; - - } while(/*!status && */cntr--); - TRACE_INFO("CAM > Image size: %d bytes", size_sampled); - return size_sampled; + } + } while(cntr--); + TRACE_ERROR("CAM > No image captured"); + return 0; } const stm32_dma_stream_t *dmastp; @@ -992,7 +994,7 @@ void OV5640_unlockResourcesForCapture(void) { } uint32_t OV5640_Capture(uint8_t* buffer, uint32_t size) { - OV5640_setLightIntensity(); + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); /* * Note: @@ -1102,7 +1104,7 @@ uint32_t OV5640_Capture(uint8_t* buffer, uint32_t size) { #endif // Wait for capture to be finished - uint8_t timout = 100; // 1000ms max + uint8_t timout = 50; // 500ms max do { chThdSleep(TIME_MS2I(10)); } while(!capture_finished && !dma_error && --timout); @@ -1141,6 +1143,7 @@ uint32_t OV5640_Capture(uint8_t* buffer, uint32_t size) { TRACE_ERROR("CAM > Timeout error capturing image"); return 0; } + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); TRACE_INFO("CAM > Capture success"); diff --git a/tracker/software/drivers/ublox.c b/tracker/software/drivers/ublox.c index efd95895..98a8c9ea 100644 --- a/tracker/software/drivers/ublox.c +++ b/tracker/software/drivers/ublox.c @@ -296,6 +296,7 @@ uint8_t gps_disable_nmea_output(void) { 0xaa, 0x79 // checksum }; + gps_calc_ubx_csum(nonmea, sizeof(nonmea)); gps_transmit_string(nonmea, sizeof(nonmea)); return gps_receive_ack(0x06, 0x00, 1000); } @@ -335,6 +336,7 @@ uint8_t gps_set_airborne_model(void) { 0x1a, 0x28 // checksum }; + gps_calc_ubx_csum(model6, sizeof(model6)); gps_transmit_string(model6, sizeof(model6)); return gps_receive_ack(0x06, 0x24, 1000); } @@ -445,3 +447,24 @@ void GPS_Deinit(void) palClearLine(LINE_GPS_EN); } + +/* + * Calculate checksum and insert into buffer. + * + */ +bool gps_calc_ubx_csum(uint8_t *mbuf, uint16_t mlen) { + + uint16_t i; + uint8_t ck_a = 0, ck_b = 0; + if(mlen < 5) + /* Excluding sync bytes there must be at at least one byte to checksum. */ + return false; + + for (i = 2; i < mlen - 2; i++) { + ck_b += (ck_a += mbuf[i]); + } + mbuf[mlen - 2] = ck_a; + mbuf[mlen - 1] = ck_b; + +return true; +} diff --git a/tracker/software/drivers/ublox.h b/tracker/software/drivers/ublox.h index ea8aeeed..baef715d 100644 --- a/tracker/software/drivers/ublox.h +++ b/tracker/software/drivers/ublox.h @@ -39,6 +39,6 @@ bool gps_get_fix(gpsFix_t *fix); bool GPS_Init(void); void GPS_Deinit(void); uint32_t GPS_get_mcu_frequency(void); - +bool gps_calc_ubx_csum(uint8_t *mbuf, uint16_t mlen); #endif diff --git a/tracker/software/drivers/usb/debug.h b/tracker/software/drivers/usb/debug.h index 4d2f2f7b..bfc82bd7 100644 --- a/tracker/software/drivers/usb/debug.h +++ b/tracker/software/drivers/usb/debug.h @@ -28,7 +28,7 @@ extern uint8_t usb_trace_level; } \ chprintf((BaseSequentialStream*)&SDU1, "[%s]", type); \ if(TRACE_FILE) { \ - chprintf((BaseSequentialStream*)&SDU1, "[%10s %04d]", __FILENAME__, __LINE__); \ + chprintf((BaseSequentialStream*)&SDU1, "[%12s %04d]", __FILENAME__, __LINE__); \ } \ chprintf((BaseSequentialStream*)&SDU1, " "); \ chprintf((BaseSequentialStream*)&SDU1, (format), ##args); \ @@ -43,7 +43,7 @@ extern uint8_t usb_trace_level; #define TRACE_ERROR(format, args...) if(usb_trace_level > 0) { TRACE_BASE(format, "ERROR", ##args) } #if TRACE_TIME && TRACE_FILE -#define TRACE_TAB " " +#define TRACE_TAB " " #elif TRACE_TIME && !TRACE_FILE #define TRACE_TAB " " #elif !TRACE_TIME && TRACE_FILE diff --git a/tracker/software/mcuconf.h b/tracker/software/mcuconf.h index 490c358d..15c8fb1d 100644 --- a/tracker/software/mcuconf.h +++ b/tracker/software/mcuconf.h @@ -186,7 +186,7 @@ #define STM32_SERIAL_USE_USART2 FALSE #define STM32_SERIAL_USE_USART3 TRUE #define STM32_SERIAL_USE_UART4 FALSE -#define STM32_SERIAL_USE_UART5 FALSE +#define STM32_SERIAL_USE_UART5 TRUE #define STM32_SERIAL_USE_USART6 FALSE #define STM32_SERIAL_USART1_PRIORITY 12 #define STM32_SERIAL_USART2_PRIORITY 12 diff --git a/tracker/software/pkt/devices/si446x.c b/tracker/software/pkt/devices/si446x.c index 170cd7bc..0c0189d7 100644 --- a/tracker/software/pkt/devices/si446x.c +++ b/tracker/software/pkt/devices/si446x.c @@ -982,7 +982,7 @@ THD_FUNCTION(bloc_si_fifo_feeder_afsk, arg) { if(exit_msg == MSG_OK) { /* Send was OK. Release the just completed packet. */ - pktReleaseSendObject(pp); + pktReleaseBufferObject(pp); } else { /* Send failed so release any queue and terminate. */ pktReleaseBufferChain(pp); @@ -1202,7 +1202,7 @@ THD_FUNCTION(bloc_si_fifo_feeder_fsk, arg) { if(exit_msg == MSG_OK) { /* Send was OK. Release the just completed packet. */ - pktReleaseSendObject(pp); + pktReleaseBufferObject(pp); } else { /* Send failed so release any queue and terminate. */ pktReleaseBufferChain(pp); diff --git a/tracker/software/pkt/managers/pktservice.c b/tracker/software/pkt/managers/pktservice.c index c4ad3640..5b1ed54d 100644 --- a/tracker/software/pkt/managers/pktservice.c +++ b/tracker/software/pkt/managers/pktservice.c @@ -18,6 +18,7 @@ /*===========================================================================*/ memory_heap_t *ccm_heap = NULL; +guarded_memory_pool_t *ccm_pool = NULL; /*===========================================================================*/ /* Module local types. */ @@ -29,6 +30,8 @@ memory_heap_t *ccm_heap = NULL; #if USE_CCM_FOR_PKT_TX == TRUE static memory_heap_t _ccm_heap; +#elif USE_CCM_FOR_PKT_POOL == TRUE +static guarded_memory_pool_t _ccm_pool; #endif /*===========================================================================*/ @@ -39,6 +42,7 @@ static memory_heap_t _ccm_heap; /* Module exported functions. */ /*===========================================================================*/ + /** * @brief Initializes the packet system. * @@ -58,13 +62,23 @@ bool pktSystemInit(void) { ccm_heap = &_ccm_heap; chHeapObjectInit(ccm_heap, (void *)0x10000000, 0x10000); } -#endif +#elif USE_CCM_FOR_PKT_POOL == TRUE + if(ccm_pool == NULL) { + ccm_pool = &_ccm_pool; + chGuardedPoolObjectInitAligned(ccm_pool, sizeof(packet_gen_t), 4); + chGuardedPoolLoadArray(ccm_pool, (void *)0x10000000, + NUMBER_COMMON_PKT_BUFFERS); + } + return true; +#else + /* * Create common packet buffer control. */ if(pktInitBufferControl() == NULL) return false; return true; +#endif } /** @@ -871,6 +885,16 @@ dyn_semaphore_t *pktInitBufferControl() { */ msg_t pktGetPacketBuffer(packet_t *pp, sysinterval_t timeout) { +#if USE_CCM_FOR_PKT_POOL == TRUE + (void)timeout; + if(ccm_pool == NULL) + return MSG_TIMEOUT; + *pp = ax25_new(); + if(pp == NULL) + return MSG_TIMEOUT; + return MSG_OK; + +#else /* Check if the packet buffer semaphore already exists. * If so we get a pointer to it and get the semaphore. */ @@ -902,6 +926,7 @@ msg_t pktGetPacketBuffer(packet_t *pp, sysinterval_t timeout) { if(pp == NULL) return MSG_TIMEOUT; return MSG_OK; +#endif } /* @@ -909,6 +934,10 @@ msg_t pktGetPacketBuffer(packet_t *pp, sysinterval_t timeout) { */ void pktReleasePacketBuffer(packet_t pp) { +#if USE_CCM_FOR_PKT_POOL == TRUE + chDbgAssert(pp != NULL, "packet is invalid"); + ax25_delete(pp); +#else /* Check if the packet buffer semaphore exists. * If not this is a system error. */ @@ -925,13 +954,14 @@ void pktReleasePacketBuffer(packet_t pp) { /* Decrease factory ref count. */ chFactoryReleaseSemaphore(dyn_sem); +#endif } /* * Send shares a common pool of buffers. */ void pktReleaseBufferSemaphore(radio_unit_t radio) { - +#if USE_CCM_FOR_PKT_POOL != TRUE packet_svc_t *handler = pktGetServiceObject(radio); chDbgAssert(handler != NULL, "invalid radio ID"); @@ -942,6 +972,9 @@ void pktReleaseBufferSemaphore(radio_unit_t radio) { */ chFactoryReleaseSemaphore(handler->tx_packet_sem); handler->tx_packet_sem = NULL; +#else + (void)radio; +#endif } /* diff --git a/tracker/software/pkt/managers/pktservice.h b/tracker/software/pkt/managers/pktservice.h index 6a49382e..519ce422 100644 --- a/tracker/software/pkt/managers/pktservice.h +++ b/tracker/software/pkt/managers/pktservice.h @@ -537,7 +537,6 @@ static inline bool pktIsTransmitOpen(radio_unit_t radio) { return !(state == PACKET_IDLE || state == PACKET_INVALID); } - #endif /* PKT_CHANNELS_PKTSERVICE_H_ */ /** @} */ diff --git a/tracker/software/pkt/pktconf.h b/tracker/software/pkt/pktconf.h index 8d3c6f0f..4016bb1b 100644 --- a/tracker/software/pkt/pktconf.h +++ b/tracker/software/pkt/pktconf.h @@ -379,14 +379,10 @@ static inline msg_t pktSendRadioCommand(radio_unit_t radio, * * @api */ -static inline void pktReleaseSendObject(packet_t pp) { +static inline void pktReleaseBufferObject(packet_t pp) { #if USE_SPI_ATTACHED_RADIO == TRUE -#if USE_NEW_PKT_TX_ALLOC == TRUE chDbgAssert(pp != NULL, "no packet pointer"); pktReleasePacketBuffer(pp); -#else - ax25_delete (pp); -#endif #else (void)pp; #endif @@ -403,17 +399,14 @@ static inline void pktReleaseSendObject(packet_t pp) { */ static inline void pktReleaseBufferChain(packet_t pp) { #if USE_SPI_ATTACHED_RADIO == TRUE -#if USE_NEW_PKT_TX_ALLOC == TRUE + chDbgAssert(pp != NULL, "no packet pointer"); /* Release all packets in linked list. */ do { packet_t np = pp->nextp; pktReleasePacketBuffer(pp); pp = np; } while(pp != NULL); -#else - ax25_delete (pp); -#endif #else (void)pp; #endif diff --git a/tracker/software/pkt/protocols/aprs2/ax25_pad.c b/tracker/software/pkt/protocols/aprs2/ax25_pad.c index 9c96259e..cf08a4cf 100644 --- a/tracker/software/pkt/protocols/aprs2/ax25_pad.c +++ b/tracker/software/pkt/protocols/aprs2/ax25_pad.c @@ -262,7 +262,11 @@ packet_t ax25_new (void) { } #if USE_NEW_PKT_TX_ALLOC == TRUE -#if USE_CCM_FOR_PKT_TX == TRUE +#if USE_CCM_FOR_PKT_POOL == TRUE + extern guarded_memory_pool_t *ccm_pool; + this_p = chGuardedPoolAllocTimeout(ccm_pool, TIME_INFINITE); + TRACE_DEBUG("PKT > Allocated buffer 0x%x, link 0x%x", this_p, ((struct pool_header *)(this_p))->next); +#elif USE_CCM_FOR_PKT_TX == TRUE extern memory_heap_t *ccm_heap; this_p = chHeapAlloc(ccm_heap, sizeof (struct packet_s)); #else @@ -331,8 +335,13 @@ void ax25_delete (packet_t this_p) this_p->magic1 = 0; this_p->magic1 = 0; - +#if USE_CCM_FOR_PKT_POOL == TRUE + extern guarded_memory_pool_t *ccm_pool; + TRACE_DEBUG("PKT > Returning buffer 0x%x", this_p); + chGuardedPoolFree(ccm_pool, this_p); +#else chHeapFree(this_p); +#endif } @@ -396,9 +405,10 @@ packet_t ax25_from_text (char *monitor, int strict) packet_t this_p; msg_t msg = pktGetPacketBuffer(&this_p, TIME_INFINITE); /* If the semaphore is reset then exit. */ - if(msg == MSG_RESET || this_p == NULL) + if(msg == MSG_RESET || this_p == NULL) { + TRACE_ERROR("PKT > No packet buffer available"); return NULL; - + } #if AX25MEMDEBUG if (ax25memdebug) { TRACE_DEBUG ("PKT > ax25_from_text, seq=%d, called from %s %d", this_p->seq, src_file, src_line); @@ -442,6 +452,7 @@ packet_t ax25_from_text (char *monitor, int strict) pinfo = strchr (stuff, ':'); if (pinfo == NULL) { + TRACE_ERROR("PKT > No address separator"); pktReleasePacketBuffer(this_p); return (NULL); } @@ -512,9 +523,8 @@ packet_t ax25_from_text (char *monitor, int strict) k = this_p->num_addr; if ( ! ax25_parse_addr (k, pa, strict, atemp, &ssid_temp, &heard_temp)) { - TRACE_ERROR("PKT > Bad digipeater address in packet"); - //TRACE_ERROR ("Failed to create packet from text. Bad digipeater address"); - pktReleasePacketBuffer(this_p); + TRACE_ERROR("PKT > Bad digipeater address in packet"); + pktReleasePacketBuffer(this_p); return (NULL); } @@ -525,12 +535,12 @@ packet_t ax25_from_text (char *monitor, int strict) // TODO: Complain if more than one "*". // Could also check for all has been repeated bits are adjacent. - if (heard_temp) { + if (heard_temp) { for ( ; k >= AX25_REPEATER_1; k--) { ax25_set_h (this_p, k); } } - } + } /* @@ -541,7 +551,6 @@ packet_t ax25_from_text (char *monitor, int strict) * We might want to manually generate UTF-8 characters such as degree. */ -//#define DEBUG14H 1 #if DEBUG14H TRACE_DEBUG ("PKT > BEFORE: %s\nSAFE: ", pinfo); @@ -581,8 +590,13 @@ packet_t ax25_from_text (char *monitor, int strict) /* * Append the info part. */ - /* FIXME: Check for buffer overflow here. */ - memcpy ((char*)(this_p->frame_data+this_p->frame_len), info_part, info_len); + /* Check for buffer overflow here. */ + if((this_p->frame_len + info_len) > AX25_MAX_PACKET_LEN) { + TRACE_ERROR ("PKT > frame buffer overrun"); + pktReleasePacketBuffer(this_p); + return (NULL); + } + memcpy ((char*)(this_p->frame_data + this_p->frame_len), info_part, info_len); this_p->frame_len += info_len; return (this_p); @@ -994,10 +1008,10 @@ void ax25_set_addr (packet_t this_p, int n, char *ad) ax25_parse_addr (n, ad, 0, atemp, &ssid_temp, &heard_temp); - memset (this_p->frame_data + n*7, ' ' << 1, 6); + memset (this_p->frame_data + n * 7, ' ' << 1, 6); - for (i=0; i<6 && atemp[i] != '\0'; i++) { - this_p->frame_data[n*7+i] = atemp[i] << 1; + for (i = 0; i < 6 && atemp[i] != '\0'; i++) { + this_p->frame_data[n * 7 + i] = atemp[i] << 1; } ax25_set_ssid (this_p, n, ssid_temp); } diff --git a/tracker/software/pkt/protocols/aprs2/ax25_pad.h b/tracker/software/pkt/protocols/aprs2/ax25_pad.h index 9b840052..d0877e73 100644 --- a/tracker/software/pkt/protocols/aprs2/ax25_pad.h +++ b/tracker/software/pkt/protocols/aprs2/ax25_pad.h @@ -73,6 +73,7 @@ #define USE_NEW_PKT_TX_ALLOC TRUE #define USE_CCM_FOR_PKT_TX FALSE +#define USE_CCM_FOR_PKT_POOL TRUE #include "pkttypes.h" @@ -154,7 +155,7 @@ typedef struct packet_s { /* Will get stomped on if above overflows. */ int magic2; -} packet_tx_t; +} packet_gen_t; /* * packet_t is a pointer to a packet object. diff --git a/tracker/software/portab.h b/tracker/software/portab.h index c62fe4db..3968f78e 100644 --- a/tracker/software/portab.h +++ b/tracker/software/portab.h @@ -91,8 +91,8 @@ /* Number of frame receive buffers. */ #define NUMBER_RX_PKT_BUFFERS 3U -/* Number of frame send buffers. */ -#define NUMBER_COMMON_PKT_BUFFERS 10U +/* Number of general AX25/APRS processing & frame send buffers. */ +#define NUMBER_COMMON_PKT_BUFFERS 20U /*===========================================================================*/ /* Module pre-compile time settings. */ diff --git a/tracker/software/protocols/packet/aprs.c b/tracker/software/protocols/packet/aprs.c index 274157b2..353ff76d 100644 --- a/tracker/software/protocols/packet/aprs.c +++ b/tracker/software/protocols/packet/aprs.c @@ -45,7 +45,7 @@ static bool dedupe_initialized; const conf_command_t command_list[] = { {TYPE_INT, "pos_pri.active", sizeof(conf_sram.pos_pri.thread_conf.active), &conf_sram.pos_pri.thread_conf.active }, {TYPE_TIME, "pos_pri.init_delay", sizeof(conf_sram.pos_pri.thread_conf.init_delay), &conf_sram.pos_pri.thread_conf.init_delay }, - {TYPE_TIME, "pos_pri.send_spacing", sizeof(conf_sram.pos_pri.thread_conf.send_spacing), &conf_sram.pos_pri.thread_conf.send_spacing }, +/* {TYPE_TIME, "pos_pri.send_spacing", sizeof(conf_sram.pos_pri.thread_conf.send_spacing), &conf_sram.pos_pri.thread_conf.send_spacing },*/ {TYPE_INT, "pos_pri.sleep_conf.type", sizeof(conf_sram.pos_pri.thread_conf.sleep_conf.type), &conf_sram.pos_pri.thread_conf.sleep_conf.type }, {TYPE_INT, "pos_pri.sleep_conf.vbat_thres", sizeof(conf_sram.pos_pri.thread_conf.sleep_conf.vbat_thres), &conf_sram.pos_pri.thread_conf.sleep_conf.vbat_thres}, {TYPE_INT, "pos_pri.sleep_conf.vsol_thres", sizeof(conf_sram.pos_pri.thread_conf.sleep_conf.vsol_thres), &conf_sram.pos_pri.thread_conf.sleep_conf.vsol_thres}, @@ -54,8 +54,8 @@ const conf_command_t command_list[] = { {TYPE_INT, "pos_pri.freq", sizeof(conf_sram.pos_pri.radio_conf.freq), &conf_sram.pos_pri.radio_conf.freq }, {TYPE_INT, "pos_pri.mod", sizeof(conf_sram.pos_pri.radio_conf.mod), &conf_sram.pos_pri.radio_conf.mod }, {TYPE_INT, "pos_pri.rssi", sizeof(conf_sram.pos_pri.radio_conf.rssi), &conf_sram.pos_pri.radio_conf.rssi }, - {TYPE_INT, "pos_pri.speed", sizeof(conf_sram.pos_pri.radio_conf.speed), &conf_sram.pos_pri.radio_conf.speed }, - {TYPE_INT, "pos_pri.redundantTx", sizeof(conf_sram.pos_pri.radio_conf.redundantTx), &conf_sram.pos_pri.radio_conf.redundantTx }, +/* {TYPE_INT, "pos_pri.speed", sizeof(conf_sram.pos_pri.radio_conf.speed), &conf_sram.pos_pri.radio_conf.speed },*/ +/* {TYPE_INT, "pos_pri.redundantTx", sizeof(conf_sram.pos_pri.radio_conf.redundantTx), &conf_sram.pos_pri.radio_conf.redundantTx },*/ {TYPE_STR, "pos_pri.call", sizeof(conf_sram.pos_pri.call), &conf_sram.pos_pri.call }, {TYPE_STR, "pos_pri.path", sizeof(conf_sram.pos_pri.path), &conf_sram.pos_pri.path }, {TYPE_INT, "pos_pri.symbol", sizeof(conf_sram.pos_pri.symbol), &conf_sram.pos_pri.symbol }, @@ -64,7 +64,7 @@ const conf_command_t command_list[] = { {TYPE_INT, "pos_sec.active", sizeof(conf_sram.pos_sec.thread_conf.active), &conf_sram.pos_sec.thread_conf.active }, {TYPE_TIME, "pos_sec.init_delay", sizeof(conf_sram.pos_sec.thread_conf.init_delay), &conf_sram.pos_sec.thread_conf.init_delay }, - {TYPE_TIME, "pos_sec.send_spacing", sizeof(conf_sram.pos_sec.thread_conf.send_spacing), &conf_sram.pos_sec.thread_conf.send_spacing }, +/* {TYPE_TIME, "pos_sec.send_spacing", sizeof(conf_sram.pos_sec.thread_conf.send_spacing), &conf_sram.pos_sec.thread_conf.send_spacing },*/ {TYPE_INT, "pos_sec.sleep_conf.type", sizeof(conf_sram.pos_sec.thread_conf.sleep_conf.type), &conf_sram.pos_sec.thread_conf.sleep_conf.type }, {TYPE_INT, "pos_sec.sleep_conf.vbat_thres", sizeof(conf_sram.pos_sec.thread_conf.sleep_conf.vbat_thres), &conf_sram.pos_sec.thread_conf.sleep_conf.vbat_thres}, {TYPE_INT, "pos_sec.sleep_conf.vsol_thres", sizeof(conf_sram.pos_sec.thread_conf.sleep_conf.vsol_thres), &conf_sram.pos_sec.thread_conf.sleep_conf.vsol_thres}, @@ -73,8 +73,8 @@ const conf_command_t command_list[] = { {TYPE_INT, "pos_sec.freq", sizeof(conf_sram.pos_sec.radio_conf.freq), &conf_sram.pos_sec.radio_conf.freq }, {TYPE_INT, "pos_sec.mod", sizeof(conf_sram.pos_sec.radio_conf.mod), &conf_sram.pos_sec.radio_conf.mod }, {TYPE_INT, "pos_sec.rssi", sizeof(conf_sram.pos_sec.radio_conf.rssi), &conf_sram.pos_sec.radio_conf.rssi }, - {TYPE_INT, "pos_sec.speed", sizeof(conf_sram.pos_sec.radio_conf.speed), &conf_sram.pos_sec.radio_conf.speed }, - {TYPE_INT, "pos_sec.redundantTx", sizeof(conf_sram.pos_sec.radio_conf.redundantTx), &conf_sram.pos_sec.radio_conf.redundantTx }, +/* {TYPE_INT, "pos_sec.speed", sizeof(conf_sram.pos_sec.radio_conf.speed), &conf_sram.pos_sec.radio_conf.speed },*/ +/* {TYPE_INT, "pos_sec.redundantTx", sizeof(conf_sram.pos_sec.radio_conf.redundantTx), &conf_sram.pos_sec.radio_conf.redundantTx },*/ {TYPE_STR, "pos_sec.call", sizeof(conf_sram.pos_sec.call), &conf_sram.pos_sec.call }, {TYPE_STR, "pos_sec.path", sizeof(conf_sram.pos_sec.path), &conf_sram.pos_sec.path }, {TYPE_INT, "pos_sec.symbol", sizeof(conf_sram.pos_sec.symbol), &conf_sram.pos_sec.symbol }, @@ -221,12 +221,14 @@ packet_t aprs_encode_position(const char *callsign, const char *path, uint16_t s uint32_t a1 = a / 91; uint32_t a1r = a % 91; - uint8_t gpsFix = dataPoint->gps_state == GPS_LOCKED1 || dataPoint->gps_state == GPS_LOCKED2 ? GSP_FIX_CURRENT : GSP_FIX_OLD; + uint8_t gpsFix = dataPoint->gps_state == GPS_LOCKED1 + || dataPoint->gps_state == GPS_LOCKED2 ? GSP_FIX_CURRENT : GSP_FIX_OLD; uint8_t src = NMEA_SRC_GGA; uint8_t origin = ORIGIN_PICO; char xmit[256]; - uint32_t len = chsnprintf(xmit, sizeof(xmit), "%s>%s,%s:!", callsign, APRS_DEST_CALLSIGN, path); + uint32_t len = chsnprintf(xmit, sizeof(xmit), "%s>%s,%s:!", + callsign, APRS_DEST_CALLSIGN, path); xmit[len+0] = (symbol >> 8) & 0xFF; xmit[len+1] = y3+33; @@ -243,7 +245,9 @@ packet_t aprs_encode_position(const char *callsign, const char *path, uint16_t s xmit[len+12] = ((gpsFix << 5) | (src << 3) | origin) + 33; // Comments - uint32_t len2 = base91_encode((uint8_t*)dataPoint, (uint8_t*)&xmit[len+13], sizeof(dataPoint_t)); + uint32_t len2 = base91_encode((uint8_t*)dataPoint, + (uint8_t*)&xmit[len+13], + sizeof(dataPoint_t)); xmit[len+len2+13] = '|'; @@ -272,10 +276,12 @@ packet_t aprs_encode_position(const char *callsign, const char *path, uint16_t s return ax25_from_text(xmit, true); } -packet_t aprs_encode_data_packet(const char *callsign, const char *path, char packetType, uint8_t *data) +packet_t aprs_encode_data_packet(const char *callsign, const char *path, + char packetType, uint8_t *data) { char xmit[256]; - chsnprintf(xmit, sizeof(xmit), "%s>%s,%s:{{%c%s", callsign, APRS_DEST_CALLSIGN, path, packetType, data); + chsnprintf(xmit, sizeof(xmit), "%s>%s,%s:{{%c%s", callsign, + APRS_DEST_CALLSIGN, path, packetType, data); return ax25_from_text(xmit, true); } @@ -283,13 +289,17 @@ packet_t aprs_encode_data_packet(const char *callsign, const char *path, char pa /** * Transmit message packet */ -packet_t aprs_encode_message(const char *callsign, const char *path, const char *receiver, const char *text, const bool noCounter) +packet_t aprs_encode_message(const char *callsign, const char *path, + const char *receiver, const char *text, + const bool noCounter) { char xmit[256]; if(noCounter) - chsnprintf(xmit, sizeof(xmit), "%s>%s,%s::%-9s:%s", callsign, APRS_DEST_CALLSIGN, path, receiver, text); + chsnprintf(xmit, sizeof(xmit), "%s>%s,%s::%-9s:%s", callsign, + APRS_DEST_CALLSIGN, path, receiver, text); else - chsnprintf(xmit, sizeof(xmit), "%s>%s,%s::%-9s:%s{%d", callsign, APRS_DEST_CALLSIGN, path, receiver, text, ++msg_id); + chsnprintf(xmit, sizeof(xmit), "%s>%s,%s::%-9s:%s{%d", callsign, + APRS_DEST_CALLSIGN, path, receiver, text, ++msg_id); return ax25_from_text(xmit, true); } @@ -299,8 +309,11 @@ packet_t aprs_encode_query_answer_aprsd(const char *callsign, const char *path, char buf[256] = "Directs="; uint32_t out = 8; for(uint8_t i=0; i<20; i++) { - if(heard_list[i].time && heard_list[i].time + TIME_S2I(600) >= chVTGetSystemTime() && heard_list[i].time <= chVTGetSystemTime()) - out += chsnprintf(&buf[out], sizeof(buf)-out, "%s ", heard_list[i].call); + if(heard_list[i].time + && heard_list[i].time + TIME_S2I(600) >= chVTGetSystemTime() + && heard_list[i].time <= chVTGetSystemTime()) + out += chsnprintf(&buf[out], sizeof(buf)-out, "%s ", + heard_list[i].call); } buf[out-1] = 0; // Remove last space @@ -371,7 +384,6 @@ static bool aprs_decode_message(packet_t pp) } } - char *command = strlwr((char*)&pinfo[11]); // Trace @@ -414,7 +426,9 @@ static bool aprs_decode_message(packet_t pp) } else if(!strcmp(command, "?aprsd")) { // Transmit position TRACE_INFO("RX > Message: Directs query"); - packet_t pp = aprs_encode_query_answer_aprsd(conf_sram.aprs.tx.call, conf_sram.aprs.tx.path, src); + packet_t pp = + aprs_encode_query_answer_aprsd(conf_sram.aprs.tx.call, + conf_sram.aprs.tx.path, src); if(pp == NULL) { TRACE_WARN("RX > No free packet objects"); return false; @@ -482,7 +496,9 @@ static bool aprs_decode_message(packet_t pp) packetRepeats[i].packet_id = req & 0xFFFF; packetRepeats[i].n_done = true; - TRACE_INFO("RX > ... Image %3d Packet %3d", packetRepeats[i].image_id, packetRepeats[i].packet_id); + TRACE_INFO("RX > ... Image %3d Packet %3d", + packetRepeats[i].image_id, + packetRepeats[i].packet_id); break; } } @@ -494,22 +510,28 @@ static bool aprs_decode_message(packet_t pp) for(uint8_t i=0; command_list[i].type != TYPE_NULL; i++) { - if(!strncmp(&command[6], command_list[i].name, strlen(command_list[i].name))) { + if(!strncmp(&command[6], command_list[i].name, + strlen(command_list[i].name))) { char *value = &command[strlen(command_list[i].name) + 6]; TRACE_INFO("RX > Message: Configuration Command"); TRACE_INFO("RX > %s => %s", &command[6], value); - if(command_list[i].type == TYPE_INT && command_list[i].size == 1) { + if(command_list[i].type == TYPE_INT + && command_list[i].size == 1) { *((uint8_t*)command_list[i].ptr) = atoi(value); - } else if(command_list[i].type == TYPE_INT && command_list[i].size == 2) { + } else if(command_list[i].type == TYPE_INT + && command_list[i].size == 2) { *((uint16_t*)command_list[i].ptr) = atoi(value); - } else if(command_list[i].type == TYPE_INT && command_list[i].size == 4) { + } else if(command_list[i].type == TYPE_INT + && command_list[i].size == 4) { *((uint32_t*)command_list[i].ptr) = atoi(value); } else if(command_list[i].type == TYPE_TIME) { - *((sysinterval_t*)command_list[i].ptr) = TIME_MS2I(atoi(value)); + *((sysinterval_t*)command_list[i].ptr) = + TIME_MS2I(atoi(value)); } else if(command_list[i].type == TYPE_STR) { - strncpy((char*)command_list[i].ptr, value, sizeof(command_list[i].size)-1); + strncpy((char*)command_list[i].ptr, value, + sizeof(command_list[i].size)-1); } } } @@ -522,7 +544,9 @@ static bool aprs_decode_message(packet_t pp) char buf[16]; chsnprintf(buf, sizeof(buf), "ack%s", msg_id_rx); - packet_t pp = aprs_encode_message(conf_sram.aprs.tx.call, conf_sram.aprs.tx.path, src, buf, true); + packet_t pp = aprs_encode_message(conf_sram.aprs.tx.call, + conf_sram.aprs.tx.path, + src, buf, true); if(pp == NULL) { TRACE_WARN("RX > No free packet objects"); return false; @@ -576,10 +600,14 @@ packet_t aprs_encode_telemetry_configuration(const char *callsign, { switch(type) { - case 0: return aprs_encode_message(callsign, path, callsign, "PARM.Vbat,Vsol,Pbat,Temperature,Airpressure", true); - case 1: return aprs_encode_message(callsign, path, callsign, "UNIT.V,V,W,degC,Pa", true); - case 2: return aprs_encode_message(callsign, path, callsign, "EQNS.0,.001,0,0,.001,0,0,.001,-4.096,0,.1,-100,0,12.5,500", true); - case 3: return aprs_encode_message(callsign, path, callsign, "BITS.11111111,", true); + case 0: return aprs_encode_message(callsign, path, callsign, + "PARM.Vbat,Vsol,Pbat,Temperature,Airpressure", true); + case 1: return aprs_encode_message(callsign, path, callsign, + "UNIT.V,V,W,degC,Pa", true); + case 2: return aprs_encode_message(callsign, path, callsign, + "EQNS.0,.001,0,0,.001,0,0,.001,-4.096,0,.1,-100,0,12.5,500", true); + case 3: return aprs_encode_message(callsign, path, callsign, + "BITS.11111111,", true); default: return NULL; } } diff --git a/tracker/software/threads/collector.c b/tracker/software/threads/collector.c index 7dd5a975..4d3f1054 100644 --- a/tracker/software/threads/collector.c +++ b/tracker/software/threads/collector.c @@ -165,7 +165,7 @@ static void getSensors(dataPoint_t* tp) tp->sen_e1_press = BME280_getPressure(&handle, 32); tp->sen_e1_hum = BME280_getHumidity(&handle); tp->sen_e1_temp = BME280_getTemperature(&handle); - } else { // No internal BME280 found + } else { // No external BME280 found TRACE_ERROR("COLL > External BME280 E1 not found"); tp->sen_e1_press = 0; tp->sen_e1_hum = 0; @@ -179,7 +179,7 @@ static void getSensors(dataPoint_t* tp) tp->sen_e2_press = BME280_getPressure(&handle, 32); tp->sen_e2_hum = BME280_getHumidity(&handle); tp->sen_e2_temp = BME280_getTemperature(&handle); - } else { // No internal BME280 found + } else { // No external BME280 found TRACE_ERROR("COLL > External BME280 E2 not found"); tp->sen_e2_press = 0; tp->sen_e2_hum = 0; @@ -329,7 +329,7 @@ void init_data_collector(void) threadStarted = true; TRACE_INFO("COLL > Startup data collector thread"); - thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(2*1024), "TRA", LOWPRIO, collectorThread, NULL); + thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(10*1024), "TRA", LOWPRIO, collectorThread, NULL); if(!th) { // Print startup error, do not start watchdog for this thread TRACE_ERROR("COLL > Could not startup thread (not enough memory available)"); diff --git a/tracker/software/threads/rxtx/image.c b/tracker/software/threads/rxtx/image.c index 0286af10..8a315796 100644 --- a/tracker/software/threads/rxtx/image.c +++ b/tracker/software/threads/rxtx/image.c @@ -285,6 +285,14 @@ ssdv_packet_t packetRepeats[16]; bool reject_pri; bool reject_sec; +/* + * Memory pool integrity checking... will expand to be that + */ +static struct pool_header *pktSystemCheck(void) { + extern guarded_memory_pool_t *ccm_pool; + return ((struct pool_header *)(ccm_pool->pool.next))->next; +} + static bool transmit_image_packet(const uint8_t *image, uint32_t image_len, thd_img_conf_t* conf, @@ -364,8 +372,7 @@ static bool transmit_image_packets(const uint8_t *image, uint8_t pkt[SSDV_PKT_SIZE]; uint8_t pkt_base91[256] = {0}; - - /* FIXME: This doesn't work with 'en bloc' packet sends. */ + /* FIXME: This doesn't work with burst mode packet sends. */ // Process redundant transmission from last cycle if(strlen((char*)pkt_base91) && conf->radio_conf.redundantTx) { @@ -389,11 +396,7 @@ static bool transmit_image_packets(const uint8_t *image, chThdSleep(TIME_MS2I(10)); // Leave other threads some time } - // Init SSDV (FEC at 2FSK, non FEC at APRS) - //bi = 0; - - - /* Prepare for image encode and send. */ + /* Prepare for new image encode and send. */ ssdv_t ssdv; const uint8_t *b; uint32_t bi = 0; @@ -405,11 +408,19 @@ static bool transmit_image_packets(const uint8_t *image, ssdv_enc_feed(&ssdv, image, 0); while(c != SSDV_EOI) { - // Encode packet(s) - /* Packet burst send is available if redundant TX is not requested. */ + + /* + * Next encode packets. + * Packet burst send is available if redundant TX is not requested. + * Limiting the number of packets in a burst is important. + * Else APRS-IS may drop fast arriving packets. + * Also having a send_spacing > 0 is important. + * This allows APRS-IS time to forward packets from its buffer. + */ + uint8_t buffers = fmin((NUMBER_COMMON_PKT_BUFFERS / 2), 5); uint8_t chain = (conf->radio_conf.mod == MOD_2FSK && !conf->radio_conf.redundantTx) ? - (NUMBER_COMMON_PKT_BUFFERS / 2) : 1; + buffers : 1; TRACE_INFO("IMG > Encode APRS/SSDV packet(s) %i", chain); /* Packet linking control. */ @@ -417,7 +428,6 @@ static bool transmit_image_packets(const uint8_t *image, packet_t previous = NULL; while(chain-- > 0) { - /* TODO: re-implement chunk sized SSDV conversions? */ while((c = ssdv_enc_get_packet(&ssdv)) == SSDV_FEED_ME) { b = &image[bi++]; if(bi > image_len) { @@ -457,12 +467,13 @@ static bool transmit_image_packets(const uint8_t *image, } return false; } - if(head == NULL) - head = packet; if(previous != NULL) /* Link the next packet into the chain. */ previous->nextp = packet; - /* Now set previous as current. */ + else + /* This is the first packet. */ + head = packet; + /* Now set new packet as previous. */ previous = packet; } /* End while(chain-- > 0) */ @@ -534,26 +545,20 @@ static bool analyze_image(uint8_t *image, uint32_t image_len) ssdv_enc_init(&ssdv, SSDV_TYPE_NOFEC, "", 0, 7); ssdv_enc_set_buffer(&ssdv, pkt); - while(true) // FIXME: I get caught in these loops occasionally and never return - { - while((c = ssdv_enc_get_packet(&ssdv)) == SSDV_FEED_ME) - { - b = &image[bi]; - uint8_t r = bi < image_len-128 ? 128 : image_len - bi; - bi += r; - if(r <= 0) - { + while(true) { + while((c = ssdv_enc_get_packet(&ssdv)) == SSDV_FEED_ME) { + b = &image[bi++]; + if(bi > image_len) { TRACE_ERROR("CAM > Error in image (Premature end of file %d)", i); return false; } - ssdv_enc_feed(&ssdv, b, r); + ssdv_enc_feed(&ssdv, b, 1); } if(c == SSDV_EOI) // End of image return true; - if(c != SSDV_OK) // Error in JPEG image - { + if(c != SSDV_OK) { TRACE_ERROR("CAM > Error in image (ssdv_enc_get_packet failed: %d %d)", c, i); return false; } @@ -591,10 +596,12 @@ uint32_t takePicture(uint8_t* buffer, uint32_t size, OV5640_init(); camInitialized = true; } - + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); // Sample data from pseudo DCMI through DMA into RAM size_sampled = OV5640_Snapshot2RAM(buffer, size, res); - + TRACE_DEBUG("CAM > Pool link 0x%x", pktSystemCheck()); + if(size_sampled == 0) + continue; // Switch off camera if(!conf_sram.keep_cam_switched_on) { OV5640_deinit(); @@ -682,7 +689,7 @@ THD_FUNCTION(imgThread, arg) { /* Allow time for output queue to be processed. */ chThdSleep(TIME_S2I(10)); } - } else { // No camera found + } else { // No camera found or capture failed after retries. TRACE_INFO("IMG > Encode/Transmit SSDV (camera error) ID=%d", gimage_id-1); if(!transmit_image_packets(noCameraFound, sizeof(noCameraFound), diff --git a/tracker/software/threads/rxtx/position.c b/tracker/software/threads/rxtx/position.c index c4e15957..9334baa1 100644 --- a/tracker/software/threads/rxtx/position.c +++ b/tracker/software/threads/rxtx/position.c @@ -12,6 +12,9 @@ #include #include "watchdog.h" +/* + * + */ THD_FUNCTION(posThread, arg) { thd_pos_conf_t* conf = (thd_pos_conf_t*)arg; @@ -26,74 +29,89 @@ THD_FUNCTION(posThread, arg) TRACE_INFO("POS > Startup position thread"); // Set telemetry configuration transmission variables - sysinterval_t last_conf_transmission = chVTGetSystemTime() - conf->tel_enc_cycle; + sysinterval_t last_conf_transmission = + chVTGetSystemTime() - conf->tel_enc_cycle; sysinterval_t time = chVTGetSystemTime(); - while(true) - { + while(true) { TRACE_INFO("POS > Do module POSITION cycle"); TRACE_INFO("POS > Get last data point"); dataPoint_t* dataPoint = getLastDataPoint(); - if(!p_sleep(&conf->thread_conf.sleep_conf)) - { + if(!p_sleep(&conf->thread_conf.sleep_conf)) { TRACE_INFO("POS > Transmit position"); // Encode/Transmit position packet - packet_t packet = aprs_encode_position(conf->call, conf->path, conf->symbol, dataPoint); + packet_t packet = aprs_encode_position(conf->call, conf->path, + conf->symbol, dataPoint); if(packet == NULL) { - TRACE_WARN("POS > No free packet objects for position transmission"); + TRACE_WARN("POS > No free packet objects" + " for position transmission"); } else { - transmitOnRadio(packet, + if(!transmitOnRadio(packet, conf->radio_conf.freq, 0, 0, conf->radio_conf.pwr, conf->radio_conf.mod, - conf->radio_conf.rssi); + conf->radio_conf.rssi)) { + TRACE_ERROR("POS > failed to transmit position data"); + } + chThdSleep(TIME_S2I(5)); } - chThdSleep(TIME_S2I(5)); // Encode/Transmit APRSD packet + /* + * FIXME: When sending out an unsolicited APRSD who to send it to? + * Should there be a callsign set in config that APRSD is sent to? + * For now just send to self... doesn't make sense though. + */ bool rx = conf->aprs_msg; - packet_t pp = aprs_encode_query_answer_aprsd(conf->call, + packet = aprs_encode_query_answer_aprsd(conf->call, conf->path, rx ? conf->call - : conf_sram.aprs.rx.call); + : conf_sram.aprs.tx.call); if(packet == NULL) { - TRACE_WARN("POS > No free packet objects for APRSD transmission"); + TRACE_WARN("POS > No free packet objects for " + "APRSD transmission"); } else { - transmitOnRadio(pp, + if(!transmitOnRadio(packet, conf->radio_conf.freq, 0, 0, conf->radio_conf.pwr, conf->radio_conf.mod, - conf->radio_conf.rssi); + conf->radio_conf.rssi)) { + TRACE_ERROR("POS > Failed to transmit APRSD data"); + } + chThdSleep(TIME_S2I(5)); } // Telemetry encoding parameter transmission - if(conf->tel_enc_cycle != 0 && last_conf_transmission + conf->tel_enc_cycle < chVTGetSystemTime()) - { - chThdSleep(TIME_S2I(5)); // Take a little break between the packet transmissions + if(conf->tel_enc_cycle != 0 && last_conf_transmission + + conf->tel_enc_cycle < chVTGetSystemTime()) { + TRACE_INFO("POS > Transmit telemetry configuration"); // Encode and transmit telemetry config packet - for(uint8_t type=0; type<4; type++) - { - packet = aprs_encode_telemetry_configuration(conf->call, conf->path, type); + for(uint8_t type = 0; type < 4; type++) { + packet = aprs_encode_telemetry_configuration(conf->call, + conf->path, type); if(packet == NULL) { - TRACE_WARN("POS > No free packet objects for telemetry transmission"); + TRACE_WARN("POS > No free packet objects for" + " telemetry transmission"); } else { - transmitOnRadio(packet, + if(!transmitOnRadio(packet, conf->radio_conf.freq, 0, 0, conf->radio_conf.pwr, conf->radio_conf.mod, - conf->radio_conf.rssi); + conf->radio_conf.rssi)) { + TRACE_ERROR("POS > Failed to transmit telemetry data"); + } chThdSleep(TIME_S2I(5)); } } @@ -101,14 +119,17 @@ THD_FUNCTION(posThread, arg) last_conf_transmission += conf->tel_enc_cycle; } } - time = waitForTrigger(time, conf->thread_conf.cycle); } } +/* + * + */ void start_position_thread(thd_pos_conf_t *conf) { - thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(10*1024), "POS", LOWPRIO, posThread, conf); + thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(20*1024), + "POS", LOWPRIO, posThread, conf); if(!th) { // Print startup error, do not start watchdog for this thread TRACE_ERROR("POS > Could not startup thread (not enough memory available)");