mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_ChibiOS: add scripting heap
This commit is contained in:
parent
44079dcc46
commit
e8373e1fdf
|
@ -34,7 +34,6 @@ extern AP_IOMCU iomcu;
|
|||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace ChibiOS;
|
||||
|
||||
#if CH_CFG_USE_HEAP == TRUE
|
||||
|
||||
/**
|
||||
|
@ -79,6 +78,46 @@ void* Util::try_alloc_from_ccm_ram(size_t size)
|
|||
return ret;
|
||||
}
|
||||
|
||||
#ifdef ENABLE_HEAP
|
||||
|
||||
void *Util::allocate_heap_memory(size_t size)
|
||||
{
|
||||
void *buf = malloc(size);
|
||||
if (buf == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
memory_heap_t *heap = (memory_heap_t *)malloc(sizeof(memory_heap_t));
|
||||
if (heap != nullptr) {
|
||||
chHeapObjectInit(heap, buf, size);
|
||||
}
|
||||
|
||||
return heap;
|
||||
}
|
||||
|
||||
void *Util::heap_realloc(void *heap, void *ptr, size_t new_size)
|
||||
{
|
||||
if (heap == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
if (new_size == 0) {
|
||||
if (ptr != nullptr) {
|
||||
chHeapFree(ptr);
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
if (ptr == nullptr) {
|
||||
return chHeapAlloc((memory_heap_t *)heap, new_size);
|
||||
}
|
||||
void *new_mem = chHeapAlloc((memory_heap_t *)heap, new_size);
|
||||
if (new_mem != nullptr) {
|
||||
memcpy(new_mem, ptr, chHeapGetSize(ptr) > new_size ? new_size : chHeapGetSize(ptr));
|
||||
chHeapFree(ptr);
|
||||
}
|
||||
return new_mem;
|
||||
}
|
||||
#endif // ENABLE_HEAP
|
||||
|
||||
#endif // CH_CFG_USE_HEAP
|
||||
|
||||
/*
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_HAL_ChibiOS_Namespace.h"
|
||||
#include "AP_HAL_ChibiOS.h"
|
||||
#include <ch.h>
|
||||
|
||||
class ChibiOS::Util : public AP_HAL::Util {
|
||||
public:
|
||||
|
@ -33,6 +34,12 @@ public:
|
|||
void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override;
|
||||
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
|
||||
|
||||
#ifdef ENABLE_HEAP
|
||||
// heap functions, note that a heap once alloc'd cannot be dealloc'd
|
||||
virtual void *allocate_heap_memory(size_t size);
|
||||
virtual void *heap_realloc(void *heap, void *ptr, size_t new_size);
|
||||
#endif // ENABLE_HEAP
|
||||
|
||||
/*
|
||||
return state of safety switch, if applicable
|
||||
*/
|
||||
|
@ -93,4 +100,9 @@ private:
|
|||
#ifndef HAL_NO_FLASH_SUPPORT
|
||||
bool flash_bootloader() override;
|
||||
#endif
|
||||
|
||||
#ifdef ENABLE_HEAP
|
||||
static memory_heap_t scripting_heap;
|
||||
#endif // ENABLE_HEAP
|
||||
|
||||
};
|
||||
|
|
|
@ -216,21 +216,4 @@ size_t mem_available(void)
|
|||
return totalp;
|
||||
}
|
||||
|
||||
void *realloc(void *addr, size_t size)
|
||||
{
|
||||
if (size == 0) {
|
||||
free(addr);
|
||||
return NULL;
|
||||
}
|
||||
if (addr == NULL) {
|
||||
return malloc(size);
|
||||
}
|
||||
void *new_mem = malloc(size);
|
||||
if (new_mem != NULL) {
|
||||
memcpy(new_mem, addr, chHeapGetSize(addr) > size ? size : chHeapGetSize(addr));
|
||||
free(addr);
|
||||
}
|
||||
return new_mem;
|
||||
}
|
||||
|
||||
#endif // CH_CFG_USE_HEAP
|
||||
|
|
|
@ -42,7 +42,6 @@ int vsscanf (const char *buf, const char *s, va_list ap);
|
|||
void *malloc(size_t size);
|
||||
void *calloc(size_t nmemb, size_t size);
|
||||
void free(void *ptr);
|
||||
void *realloc(void *ptr, size_t size);
|
||||
|
||||
extern int (*vprintf_console_hook)(const char *fmt, va_list arg);
|
||||
|
||||
|
|
Loading…
Reference in New Issue