Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-09-26 10:04:42 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

cleanup

Browse Source
This commit is contained in:
Rene Hopf
2016-01-13 23:07:13 +01:00
parent f76066c268
commit 4bf0dee4cf
10 changed files with 0 additions and 4303 deletions
Download Patch File Download Diff File Expand all files Collapse all files
memtemp/data.txt
-1992
View File
File diff suppressed because it is too large Load Diff
memtemp/memtemp.c
-464
View File
@@ -1,464 +0,0 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define MEM_SIZE 1024
//process data
#define RECORD_TYPE_PROCESS_DATA_RECORD 0xA0
#define DATA_TYPE_PAD 0x00
#define DATA_TYPE_BITS 0x01
#define DATA_TYPE_UNSIGNED 0x02
#define DATA_TYPE_SIGNED 0x03
#define DATA_TYPE_NONVOL_UNSIGNED 0x04
#define DATA_TYPE_NONVOL_SIGNED 0x05
#define DATA_TYPE_NONVOL_STREAM 0x06
#define DATA_TYPE_NONVOL_BOOLEAN 0x07
#define DATA_DIRECTION_INPUT 0x00
#define DATA_DIRECTION_BI_DIRECTIONAL 0x40
#define DATA_DIRECTION_OUTPUT 0x80
//modes
#define RECORD_TYPE_MODE_DATA_RECORD 0xB0
#define NO_MODES 2//gtoc modes
#define NO_GPD 1//gtoc process data
#define NO_PD 4//process data
#define IS_INPUT(pdr) (pdr->data_direction != 0x80)
#define IS_OUTPUT(pdr) (pdr->data_direction != 0x00)
#define MAX_PD_STRLEN 32 // this is the max space for both the unit and name strings in the PD descriptors
#define MAX_PD_VAL_BYTES 64 // this should be >= the sum of the data sizes of all pd vars
#define MEMPTR(p) ((uint32_t)&p-(uint32_t)&memory)
#define MEMU8(ptr) (memory.bytes[ptr])
#define MEMU16(ptr) (memory.bytes[ptr] | memory.bytes[ptr+1]<<8)
#define MEMU32(ptr) (memory.bytes[ptr] | memory.bytes[ptr+1]<<8 | memory.bytes[ptr+2]<<16 | memory.bytes[ptr+3]<<24)
#define NUM_BYTES(bits) (bits / 8 + (bits % 8 > 0 ? 1 : 0))
#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define SIGNED(pd) (pd->data_type == DATA_TYPE_SIGNED || pd->data_type == DATA_TYPE_NONVOL_SIGNED)
#define UNSIGNED(pd) (pd->data_type == DATA_TYPE_UNSIGNED || pd->data_type == DATA_TYPE_NONVOL_UNSIGNED)
typedef struct{
uint8_t record_type;//0xa0
uint8_t data_size;
uint8_t data_type;
uint8_t data_direction;
float param_min;
float param_max;
uint16_t data_addr;
char names; // we can pick up the address of that member for a memory location to write strings to.
} process_data_descriptor_t;
typedef struct{
uint8_t record_type;//0xb0
uint8_t index;
uint8_t type;
uint8_t unused;
char names;
} mode_descriptor_t;
typedef struct{
uint8_t input; // these are in BITS now. I'll convert to bytes when I respond to the rpc.
uint8_t output;
uint16_t ptocp;//pointer to process data table
uint16_t gtocp;//pointer to mode data table
} discovery_rpc_t;
typedef union {
struct {
discovery_rpc_t discovery;
uint8_t heap[MEM_SIZE - sizeof(discovery_rpc_t)];
};
uint8_t bytes[MEM_SIZE];
} memory_t;
typedef struct {
process_data_descriptor_t *ptr;
float range;
uint32_t bitmax;
} pd_metadata_t;
static memory_t memory;
static uint8_t *heap_ptr;
process_data_descriptor_t create_pdr(uint8_t data_size_in_bits, uint8_t data_type, uint8_t data_dir, float param_min, float param_max) {
process_data_descriptor_t pd;
pd.record_type = RECORD_TYPE_PROCESS_DATA_RECORD;
pd.data_size = data_size_in_bits;
pd.data_type = data_type;
pd.data_direction = data_dir;
pd.param_min = param_min;
pd.param_max = param_max;
// strcpy(pd.names, unit_string);
// strcpy(pd.names + strlen(unit_string) + 1, name_string);
return pd;
}
mode_descriptor_t create_mdr(uint8_t index, uint8_t type) {
mode_descriptor_t md;
md.record_type = RECORD_TYPE_MODE_DATA_RECORD;
md.index = index;
md.type = type;
md.unused = 0x00;
//strcpy(md.name_string, name_string);
return md;
}
#define BITSLEFT(ptr) (8-ptr)
void process_data_rpc(uint8_t *input, uint8_t *output) {
uint16_t *ptocp = (uint16_t *)(memory.bytes + memory.discovery.ptocp);
*(input++) = 0x00; // fault byte, just for easy recognition
*input = 0x00; // clear the next byte
// data needs to be packed and unpacked based on its type and size
// input is a pointer to the data that gets sent back to the host
// need a bit pointer to keep track of partials
uint8_t output_bit_ptr = 0;
uint8_t input_bit_ptr = 0;
while(*ptocp != 0x0000) {
process_data_descriptor_t *pd = (process_data_descriptor_t *)(memory.bytes + *ptocp++);
if (IS_INPUT(pd)) {
//printf("input pd data size is %d\n", pd->data_size);
uint16_t data_addr = pd->data_addr;
uint8_t data_size = pd->data_size;
uint8_t data_bit_ptr = 0;
//*(input++) = MEMU8(pd->data_addr);
while(data_size > 0) {
uint8_t bits_to_pack = data_size < BITSLEFT(input_bit_ptr) ? data_size : BITSLEFT(input_bit_ptr);
if (BITSLEFT(data_bit_ptr) < bits_to_pack) { bits_to_pack = BITSLEFT(data_bit_ptr); }
//printf("encoding partial byte, need to read %d bits from the data_val 0x%02x at bit position %d\n", bits_to_pack, MEMU8(data_addr), data_bit_ptr);
uint8_t mask = ((1<<bits_to_pack) - 1) << (data_bit_ptr);
//printf("mask is 0x%02x\n", mask);
*input |= ((MEMU8(data_addr) & mask) >> data_bit_ptr) << input_bit_ptr;
//printf("*input is 0x%02x\n", *input);
input_bit_ptr += bits_to_pack;
data_bit_ptr += bits_to_pack;
data_size -= bits_to_pack;
if((input_bit_ptr %= 8) == 0) *++input = 0x00; // clear the next byte
if((data_bit_ptr %= 8) == 0) data_addr++;
}
}
if (IS_OUTPUT(pd)) {
// printf("output pd data size is %d\n", pd->data_size);
uint16_t data_addr = pd->data_addr;
uint8_t data_size = pd->data_size;
uint8_t val_bits_remaining = 8;
uint8_t val = 0x00;
while(data_size > 0) {
// the number of bits to unpack this iteration is the number of bits remaining in the pd, or the number of bits remaining in the output byte,
// whichever is smaller. Then, it can be even smaller if we have less room in the current val.
uint8_t bits_to_unpack = data_size < BITSLEFT(output_bit_ptr) ? data_size : BITSLEFT(output_bit_ptr);
if (val_bits_remaining < bits_to_unpack) { bits_to_unpack = val_bits_remaining; }
// printf("decoding partial byte, need to read %d bits from current output byte 0x%02x at bit position %d\n", bits_to_unpack, *output, output_bit_ptr);
// create a bitmask the width of the bits to read, shifted to the position in the output byte that we're pointing to
uint8_t mask = ((1<<bits_to_unpack) - 1) << (output_bit_ptr);
// printf("mask is 0x%02x\n", mask);
// val is what we get when we mask off output and then shift it to the proper place.
val = val | ((*output & mask) >> (output_bit_ptr)) << (8-val_bits_remaining);
// printf("val is 0x%02x\n", val);
val_bits_remaining -= bits_to_unpack;
data_size -= bits_to_unpack;
output_bit_ptr += bits_to_unpack;
if((output_bit_ptr %= 8) == 0) output++;
if(val_bits_remaining == 0 || data_size == 0) {
MEMU8(data_addr++) = val;
// printf("adding 0x%02x to data\n", val);
val_bits_remaining = 8;
val = 0x00;
}
}
// now we've finished unpacking it and storing it in memory, but we have to fix up the high bits if it wasn't a byte-aligned datasize.
// for instance, if we receive 0xFFF in a 12 bit field, that is a negative number, but we stored it as 0x0FFF in memory.
// strategy is to set the most significant n bits of the MSB to the most significant bit of the output value, iff the pd is defined as signed.
if (SIGNED(pd) && pd->data_size % 8 != 0) {
uint8_t msb_addr = pd->data_addr + NUM_BYTES(pd->data_size) - 1;
//printf("in output fixup. MSB: 0x%02x\n", MEMU8(msb_addr));
if(MEMU8(msb_addr) & 1<<(pd->data_size%8 - 1)) {
uint8_t mask = 0xFF ^ ((1<<pd->data_size%8) - 1);
//printf("applying mask: 0x%02x\n", mask);
MEMU8(msb_addr) |= mask;
}
//printf("fixed up val: 0x%02x\n", MEMU8(msb_addr));
}
}
}
}
void dump_memory_range(uint16_t start, uint16_t end) {
start -= (start % 16);
end -= ((end+1) % 16);
printf("dump memory from 0x%04x to 0x%04x\n\n", start, end);
printf(" ");
for(int i = 0; i < 16; i++) {
printf(" %x ", i);
}
printf("\n---------------------------------------------------\n");
for(int j = start >> 4; j <= end >> 4; j++) {
printf("%03x ", j);
for (int i = 0; i < 16; i++) {
uint16_t addr = (j<<4) | i;
printf("%02x ", memory.bytes[addr]);
}
printf("\n");
}
printf("\n");
}
void dump_memory() {
dump_memory_range(0, MEM_SIZE);
}
void write_memory_to_file() {
FILE *fileptr;
uint8_t *buffer;
long filelen;
fileptr = fopen("test.hex", "wb");
fwrite(memory.bytes, sizeof(uint8_t), MEM_SIZE, fileptr);
fclose(fileptr);
printf("Wrote memory out to test.hex\n");
}
uint16_t add_pd(char *name_string, char *unit_string, uint8_t data_size_in_bits, uint8_t data_type, uint8_t data_dir, float param_min, float param_max) {
process_data_descriptor_t pdr = create_pdr(data_size_in_bits, data_type, data_dir, param_min, param_max);
pdr.data_addr = MEMPTR(*heap_ptr);
heap_ptr += NUM_BYTES(data_size_in_bits);
memcpy(heap_ptr, &pdr, sizeof(process_data_descriptor_t));
// note that we don't store the names in the struct anymore. The fixed-length struct is copied into memory, and then the nmaes go in directly behind it, so they'll read out properly
uint16_t pd_ptr = MEMPTR(*heap_ptr); // save off the ptr to return, before we modify the heap ptr
heap_ptr = (uint8_t *)&(((process_data_descriptor_t *)heap_ptr)->names);
// copy the strings in after the pd
strcpy((char *)heap_ptr, unit_string);
heap_ptr += strlen(unit_string)+1;
strcpy((char *)heap_ptr, name_string);
heap_ptr += strlen(name_string)+1;
return pd_ptr;
}
uint16_t add_mode(char *name_string, uint8_t index, uint8_t type) {
mode_descriptor_t mdr = create_mdr(index, type);
memcpy(heap_ptr, &mdr, sizeof(mode_descriptor_t));
uint16_t md_ptr = MEMPTR(*heap_ptr);
heap_ptr = (uint8_t *)&(((mode_descriptor_t *)heap_ptr)->names);
strcpy((char *)heap_ptr, name_string);
heap_ptr += strlen(name_string)+1;
return md_ptr;
}
void metadata(pd_metadata_t *pdm, process_data_descriptor_t *ptr) {
pdm->ptr = ptr;
pdm->range = ptr->data_type == DATA_TYPE_SIGNED ? MAX(ptr->param_min, ptr->param_max)*2 : ptr->param_max;
pdm->bitmax = (1<<ptr->data_size)-1;
}
#define INDIRECT_PD(pd_ptr) ((process_data_descriptor_t *)(memory.bytes + *pd_ptr))
#define DATA_DIR(pd_ptr) INDIRECT_PD(pd_ptr)->data_direction
#define DATA_SIZE(pd_ptr) INDIRECT_PD(pd_ptr)->data_size
#define ADD_PROCESS_VAR(args) *ptocp = add_pd args; input_bits += IS_INPUT(INDIRECT_PD(ptocp)) ? DATA_SIZE(ptocp) : 0; output_bits += IS_OUTPUT(INDIRECT_PD(ptocp)) ? DATA_SIZE(ptocp) : 0; last_pd = INDIRECT_PD(ptocp++)
#define ADD_GLOBAL_VAR(args) *gtocp++ = add_pd args
#define ADD_MODE(args) *gtocp++ = add_mode args
//#define SCALE_OUT(pd, val) (((float)val / (float)((1<<pd->data_size)-1) * (pd->param_max - pd->param_min)) + pd->param_min)
//#define SCALE_IN(pd, val) ((val - pd->param_min)/(pd->param_max - pd->param_min) * ((1<<pd->data_size)-1))
//#define SCALE_IN(pd, val) (val * (pd->param_max - pd->param_min) / ((1<<pd->data_size)-1) + pd->param_min)
float scale_out(pd_metadata_t pd, int32_t val) {
return val * pd.range / (float)pd.bitmax;
}
int32_t scale_in(pd_metadata_t pd, float val) {
return CLAMP(val, pd.ptr->param_min, pd.ptr->param_max) * pd.bitmax / pd.range;
}
int main(void) {
heap_ptr = memory.heap;
uint16_t input_bits = 8; // this starts at 8 bits = 1 byte for the fault byte
uint16_t output_bits = 0;
// these are temp toc arrays that the macros will write pointers into. the tocs get copied to main memory after everything else is written in
uint16_t ptoc[32];
uint16_t gtoc[32];
uint16_t *ptocp = ptoc; uint16_t *gtocp = gtoc;
process_data_descriptor_t *last_pd;
pd_metadata_t cmd_vel;
pd_metadata_t fb_vel;
printf("sizeof pdr: %ld\n", sizeof(process_data_descriptor_t));
ADD_PROCESS_VAR(("output_pins", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_OUTPUT, 0, 1));
ADD_PROCESS_VAR(("cmd_vel", "rps", 12, DATA_TYPE_SIGNED, DATA_DIRECTION_OUTPUT, -1.0, 1.0)); metadata(&cmd_vel, last_pd);
ADD_PROCESS_VAR(("input_pins", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_INPUT, 0, 1));
ADD_PROCESS_VAR(("fb_vel", "rps", 12, DATA_TYPE_SIGNED, DATA_DIRECTION_INPUT, -1.0, 1.0)); metadata(&fb_vel, last_pd);
printf("cmd_vel->data_addr: 0x%04x\n", cmd_vel.ptr->data_addr);
ADD_GLOBAL_VAR(("swr", "non", 8, DATA_TYPE_UNSIGNED, DATA_DIRECTION_OUTPUT, 0, 0));
ADD_MODE(("foo", 0, 0));
ADD_MODE(("io_", 1, 1));
printf("input bits %d output bits %d\n", input_bits, output_bits);
if (input_bits % 8) ADD_PROCESS_VAR(("", "", 8 - (input_bits%8), DATA_TYPE_PAD, DATA_DIRECTION_INPUT, 0, 0));
if (output_bits % 8) ADD_PROCESS_VAR(("", "", 8 - (output_bits%8), DATA_TYPE_PAD, DATA_DIRECTION_OUTPUT, 0, 0));
// todo: automatically create padding pds based on the mod remainder of input/output bits
// now that all the toc entries have been added, write out the tocs to memory and set up the toc pointers
memory.discovery.input = input_bits >> 3;
memory.discovery.output = output_bits >> 3;
memory.discovery.ptocp = MEMPTR(*heap_ptr);
for(uint8_t i = 0; i < ptocp - ptoc; i++) {
*heap_ptr++ = ptoc[i] & 0x00FF;
*heap_ptr++ = (ptoc[i] & 0xFF00) >> 8;
}
*heap_ptr++ = 0x00; *heap_ptr++ = 0x00; // this is the ptoc end marker
memory.discovery.gtocp = MEMPTR(*heap_ptr);
for(uint8_t i = 0; i < gtocp - gtoc; i++) {
*heap_ptr++ = gtoc[i] & 0x00FF;
*heap_ptr++ = (gtoc[i] & 0xFF00) >> 8;
}
*heap_ptr++ = 0x00; *heap_ptr++ = 0x00; // this is the gtoc end marker
dump_memory_range(0, 0x100);
write_memory_to_file();
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0x7ff));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0x3ff));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0x1ff));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0x000));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0xe01));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0xc01));
printf("Scale out test: %f\n", scale_out(cmd_vel, (int16_t)0x801));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, 1), (int16_t)scale_in(fb_vel, 1));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, 0.5), (int16_t)scale_in(fb_vel, 0.5));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, 0.25), (int16_t)scale_in(fb_vel, 0.5));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, 0), (int16_t)scale_in(fb_vel, 0));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, -0.25), (int16_t)scale_in(fb_vel, 0.5));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, -0.5), (int16_t)scale_in(fb_vel, -0.5));
printf("Scale in test: 0x%04x (%d)\n", (int16_t)scale_in(fb_vel, -1), (int16_t)scale_in(fb_vel, -1));
MEMU8(fb_vel.ptr->data_addr) = 0xFF;
MEMU8(fb_vel.ptr->data_addr + 1) = 0x07;
uint8_t output_buf[memory.discovery.output];
uint8_t input_buf[memory.discovery.input];
output_buf[0] = 0x1a;
output_buf[1] = 0xe0;
printf("incoming output bytes: "); for (uint8_t i = 0; i < memory.discovery.output; i++) { printf("0x%02x ", output_buf[i]); } printf("\n");
process_data_rpc(input_buf, output_buf);
printf("outgoing input bytes: "); for (uint8_t i = 0; i < memory.discovery.input; i++) { printf("0x%02x ", input_buf[i]); } printf("\n");
float cmd_vel_val = scale_out(cmd_vel, (int16_t)MEMU16(cmd_vel.ptr->data_addr));
printf("cmd_vel's value is 0x%04x (%f)\n", MEMU16(cmd_vel.ptr->data_addr), cmd_vel_val);
*((uint16_t *)&(memory.bytes[fb_vel.ptr->data_addr])) = scale_in(fb_vel, cmd_vel_val);
printf("fb_vel's value is 0x%04x (%f)\n", MEMU16(fb_vel.ptr->data_addr), scale_out(fb_vel, (int16_t)MEMU16(fb_vel.ptr->data_addr)));
output_buf[0] = 0xfa;
output_buf[1] = 0x7f;
printf("incoming output bytes: "); for (uint8_t i = 0; i < memory.discovery.output; i++) { printf("0x%02x ", output_buf[i]); } printf("\n");
process_data_rpc(input_buf, output_buf);
printf("outgoing input bytes: "); for (uint8_t i = 0; i < memory.discovery.input; i++) { printf("0x%02x ", input_buf[i]); } printf("\n");
/*
printf("setting fb and bidir\n");
*fb_vel = 0xCA;
output_buf[0] = 0x30;
output_buf[1] = 0x40;
printf("updated vals: cmd_vel 0x%02x fb_vel 0x%02x bidir 0x%02x\n", *cmd_vel, *fb_vel, *bidir);
printf("incoming output bytes: "); for (uint8_t i = 0; i < discovery_rpc.output; i++) { printf("0x%02x ", output_buf[i]); } printf("\n");
process_data_rpc(input_buf, output_buf);
printf("outgoing input bytes: "); for (uint8_t i = 0; i < discovery_rpc.input; i++) { printf("0x%02x ", input_buf[i]); } printf("\n");
printf("updated vals: cmd_vel 0x%02x fb_vel 0x%02x bidir 0x%02x\n", *cmd_vel, *fb_vel, *bidir);
*/
exit(0);
}
memtemp/sserial.sr
BIN
View File
Binary file not shown.
memtemp/tocdump.c
-95
View File
@@ -1,95 +0,0 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//#define MEMPTR(p) ((uint32_t)&p-(uint32_t)&memory)
#define RECORD_TYPE_PROCESS_DATA_RECORD 0xA0
#define RECORD_TYPE_MODE_DATA_RECORD 0xB0
#define MEMU8(m, ptr) (m[ptr])
#define MEMU16(m, ptr) (m[ptr] | m[ptr+1]<<8)
#define MEMU32(m, ptr) (m[ptr] | m[ptr+1]<<8 | m[ptr+2]<<16 | m[ptr+3]<<24)
void print_tocs(uint8_t *m, uint16_t ptocp, uint16_t gtocp) {
uint16_t pd_ptr;
printf("\nptoc: (at 0x%04x)\n", ptocp);
while(MEMU16(m, ptocp) != 0x0000) {
pd_ptr = MEMU16(m, ptocp);
printf("pd ptr: 0x%04x\n", pd_ptr);
printf("\trectype: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tdatasize: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tdatatype: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tdatadir: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tparmmin: 0x%08x\n", MEMU32(m, pd_ptr)); pd_ptr += 4;
printf("\tparmmax: 0x%08x\n", MEMU32(m, pd_ptr)); pd_ptr += 4;
printf("\tdataaddr: 0x%04x\n", MEMU16(m, pd_ptr)); pd_ptr += 2;
char *unitstr = (char *)(m + pd_ptr);
printf("\tunitstr: %s\n", unitstr);
pd_ptr += strlen(unitstr) + 1;
char *namestr = (char *)(m + pd_ptr);
printf("\tnamestr: %s\n", namestr);
pd_ptr += strlen(namestr) + 1;
ptocp += 2;
}
printf("\ngtoc: (at 0x%04x)\n", gtocp);
while(MEMU16(m, gtocp) != 0x0000) {
pd_ptr = MEMU16(m, gtocp);
printf("pd ptr: 0x%04x\n", pd_ptr);
uint8_t rectype = MEMU8(m, pd_ptr++);
printf("\trectype: 0x%02x\n", rectype);
switch(rectype) {
case RECORD_TYPE_PROCESS_DATA_RECORD:
printf("\tdatasize: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tdatatype: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tdatadir: 0x%02x\n", MEMU8(m, pd_ptr++));
printf("\tparmmin: 0x%08x\n", MEMU32(m, pd_ptr)); pd_ptr += 4;
printf("\tparmmax: 0x%08x\n", MEMU32(m, pd_ptr)); pd_ptr += 4;
printf("\tdataaddr: 0x%04x\n", MEMU16(m, pd_ptr)); pd_ptr += 2;
char *unitstr = (char *)(m + pd_ptr);
printf("\tunitstr: %s\n", unitstr);
pd_ptr += strlen(unitstr) + 1;
char *namestr = (char *)(m + pd_ptr);
printf("\tnamestr: %s\n", namestr);
pd_ptr += strlen(namestr) + 1;
break;
case RECORD_TYPE_MODE_DATA_RECORD:
printf("\tindex: 0x%02x\n", pd_ptr++);
printf("\ttype: 0x%02x\n", pd_ptr++);
pd_ptr++; // skip 'unused' byte
namestr = (char *)(m + pd_ptr);
printf("\tnamestr: %s\n", namestr);
pd_ptr += strlen(namestr);
break;
}
gtocp += 2;
}
}
int main(void) {
FILE *fileptr;
uint8_t *buffer;
long filelen;
fileptr = fopen("test.hex", "rb"); // open in binary read mode
fseek(fileptr, 0, SEEK_END); // jump to the end of the file
filelen = ftell(fileptr); // get the byte offset of the end of the file
rewind(fileptr); // jump back to the beginning to start reading
buffer = (uint8_t *)malloc((filelen)*sizeof(uint8_t));
fread(buffer, filelen, 1, fileptr);
fclose(fileptr);
printf("read mem.hex\n\n");
// MY memory dumps have the discovery_rpc struct at the beginning, so byte 0 and 1 are input and output byte counts, bytes 2/3 and 4/5 are the ptocp and gtocp.
uint16_t ptocp = buffer[2] | buffer[3]<<8;
uint16_t gtocp = buffer[4] | buffer[5]<<8;
print_tocs(buffer, ptocp, gtocp);
}
memtemp/tocdump.txt
-306
View File
@@ -1,306 +0,0 @@
read mem.hex
ptoc: (at 0xa53e)
pd ptr: 0xa290
rectype: 0xb0
datasize: 0x00
datatype: 0x00
datadir: 0x00
parmmin: 0x6e617453
parmmax: 0x64726164
dataaddr: 0x0000
unitstr: °
namestr: 
pd ptr: 0xa29e
rectype: 0xb0
datasize: 0x00
datatype: 0x01
datadir: 0x00
parmmin: 0x535f4f49
parmmax: 0x006e6970
dataaddr: 0x01b0
unitstr: 
namestr: IO_Ana_Spin
pd ptr: 0xa2d2
rectype: 0xa0
datasize: 0x20
datatype: 0x01
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0808
unitstr: None
namestr: Input
pd ptr: 0xa2ec
rectype: 0xa0
datasize: 0x10
datatype: 0x01
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0800
unitstr: None
namestr: Output
pd ptr: 0xa306
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x42c80000
dataaddr: 0x0840
unitstr: Percent
namestr: SpinOut
pd ptr: 0xa324
rectype: 0xa0
datasize: 0x08
datatype: 0x07
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0842
unitstr: None
namestr: SpinEna
pd ptr: 0xa340
rectype: 0xa0
datasize: 0x08
datatype: 0x07
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0843
unitstr: None
namestr: SpinDir
gtoc: (at 0xa584)
pd ptr: 0xa290
rectype: 0xb0
index: 0xa291
type: 0xa292
namestr: Standard
pd ptr: 0xa29e
rectype: 0xb0
index: 0xa29f
type: 0xa2a0
namestr: IO_Spin
pd ptr: 0xa2aa
rectype: 0xb0
index: 0xa2ab
type: 0xa2ac
namestr: IO_Ana_Spin
pd ptr: 0xa2ba
rectype: 0xb0
index: 0xa2bb
type: 0xa2bc
namestr: IO_Enc_Ana_Spin_FV
pd ptr: 0xa2d2
rectype: 0xa0
datasize: 0x20
datatype: 0x01
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0808
unitstr: None
namestr: Input
pd ptr: 0xa2ec
rectype: 0xa0
datasize: 0x10
datatype: 0x01
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0800
unitstr: None
namestr: Output
pd ptr: 0xa306
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x42c80000
dataaddr: 0x0840
unitstr: Percent
namestr: SpinOut
pd ptr: 0xa324
rectype: 0xa0
datasize: 0x08
datatype: 0x07
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0842
unitstr: None
namestr: SpinEna
pd ptr: 0xa340
rectype: 0xa0
datasize: 0x08
datatype: 0x07
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0843
unitstr: None
namestr: SpinDir
pd ptr: 0xa35c
rectype: 0xa0
datasize: 0x08
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x42113333
dataaddr: 0x0810
unitstr: Volts
namestr: AnalogIn0
pd ptr: 0xa37a
rectype: 0xa0
datasize: 0x08
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x42113333
dataaddr: 0x0811
unitstr: Volts
namestr: AnalogIn1
pd ptr: 0xa398
rectype: 0xa0
datasize: 0x08
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x42113333
dataaddr: 0x0812
unitstr: Volts
namestr: AnalogIn2
pd ptr: 0xa3b6
rectype: 0xa0
datasize: 0x08
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x42113333
dataaddr: 0x0813
unitstr: Volts
namestr: AnalogIn3
pd ptr: 0xa3d4
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x42113333
dataaddr: 0x0a18
unitstr: Volts
namestr: FieldVoltage
pd ptr: 0xa3f6
rectype: 0xa0
datasize: 0x08
datatype: 0x08
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x09a0
unitstr: Counts
namestr: Enc0
pd ptr: 0xa410
rectype: 0xa0
datasize: 0x08
datatype: 0x08
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x09a2
unitstr: Counts
namestr: Enc1
pd ptr: 0xa42a
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x095c
unitstr: None
namestr: SwRevision
pd ptr: 0xa448
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x80
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0974
unitstr: None
namestr: HwRevision
pd ptr: 0xa466
rectype: 0xa0
datasize: 0x10
datatype: 0x04
datadir: 0x40
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0000
unitstr: None
namestr: NVBaudRate
pd ptr: 0xa484
rectype: 0xa0
datasize: 0x20
datatype: 0x04
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0078
unitstr: None
namestr: NVUnitNumber
pd ptr: 0xa4a4
rectype: 0xa0
datasize: 0x10
datatype: 0x04
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0038
unitstr: None
namestr: NVWatchDogTimeout
pd ptr: 0xa4ca
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x0998
unitstr: None
namestr: EncMode0
pd ptr: 0xa4e6
rectype: 0xa0
datasize: 0x10
datatype: 0x02
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x099a
unitstr: None
namestr: EncMode1
pd ptr: 0xa502
rectype: 0xa0
datasize: 0x10
datatype: 0x04
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x003a
unitstr: None
namestr: NVEncMode0
pd ptr: 0xa520
rectype: 0xa0
datasize: 0x10
datatype: 0x04
datadir: 0x00
parmmin: 0x00000000
parmmax: 0x00000000
dataaddr: 0x003c
unitstr: None
namestr: NVEncMode1
sim/gp
-5
View File
@@ -1,5 +0,0 @@
set term x1
set key autotitle columnheader
#set yrange [-3:400]
#set xrange [0.245:0.4]
plot "plot" every 1 using 1:2 w l, "plot" every 1 using 1:3 w l, "plot" every 1 using 1:4 w l, "plot" every 1 using 1:5 w l, "plot" every 1 using 1:6 w l, "plot" every 1 using 1:7 w l, "plot" every 1 using 1:8 w l, "plot" every 1 using 1:9 w l, "plot" every 1 using 1:10 w l
sim/pwm.html
-403
View File
@@ -1,403 +0,0 @@
<html>
<meta charset="utf-8">
<head><title>pwm</title>
<script type="text/javascript">
var isMouseDown = false;
document.onmousedown = function() { isMouseDown = true };
document.onmouseup = function() { isMouseDown = false };
function round(x, a){
return(Math.round(x * a) / a)
}
function clamp(x, a){
return((-a < x) ? ((a < x) ? (a) : (x)) : (-a));
}
function svm(u, v, w, mode, center){
switch(mode){
case "std":
u += 0.5;
v += 0.5;
w += 0.5;
break;
case "svm":
var uv = v - u;
var vw = w - v;
var wu = u - w;
if(Math.abs(uv) > Math.abs(vw)){
if(Math.abs(uv) > Math.abs(wu)){ // uv
u = -uv / 2.0;
v = uv / 2.0;
w = v + vw;
}
else{ // wu
u = wu / 2.0;
w = -wu / 2.0;
v = u + uv;
}
}
else{
if(Math.abs(vw) > Math.abs(wu)){ // vw
v = -vw / 2.0;
w = vw / 2.0;
u = w + wu;
}
else{ // wu
u = wu / 2.0;
w = -wu / 2.0;
v = u + uv;
}
}
u += 0.5;
v += 0.5;
w += 0.5;
break;
case "flat bottom svm":
if(u < v){
if(u < w){
v -= u;
w -= u;
u = 0.0;
}
else{
u -= w;
v -= w;
w = 0.0;
}
}
else{
if(v < w){
u -= v;
w -= v;
v = 0.0;
}
else{
u -= w;
v -= w;
w = 0.0;
}
}
}
var radio = document.getElementById('df');
if(radio){
radio.innerHTML += "<br>PWM_U = " + round(u, 100) + "<br>PWM_V = " + round(v, 100) + "<br>PWM_W = " + round(w, 100);
}
// var canvas = document.getElementById('canvas_plot');
// if (canvas.getContext){
// var ctx = canvas.getContext('2d');
//
//
// ctx.save();
// ctx.translate(canvas.width / 2, canvas.height / 2);
//
// ctx.moveTo(0, 0);
// ctx.lineTo(Math.cos(0 / 3 * 2 * Math.PI) * (u - 0.5) * canvas.width, Math.sin(0 / 3 * 2 * Math.PI) * (u - 0.5) * canvas.height);
// ctx.moveTo(0, 0);
// ctx.lineTo(Math.cos(1 / 3 * 2 * Math.PI) * (v - 0.5) * canvas.width, Math.sin(1 / 3 * 2 * Math.PI) * (v - 0.5) * canvas.height);
// ctx.moveTo(0, 0);
// ctx.lineTo(Math.cos(2 / 3 * 2 * Math.PI) * (w - 0.5) * canvas.width, Math.sin(2 / 3 * 2 * Math.PI) * (w - 0.5) * canvas.height);
//
//
// ctx.stroke();
// ctx.restore();
// }
draw(u, v, w, center);
}
function draw_vec(x, y){
y *= -1;
var canvas = document.getElementById('canvas_plot');
if (canvas.getContext){
var ctx = canvas.getContext('2d');
var w = canvas.width;
var h = canvas.height;
//clear
ctx.clearRect(0, 0, w, h);
ctx.beginPath();
ctx.save();
ctx.translate(w / 2, h / 2);
// limit circle
ctx.arc(0, 0, h / 2, 0, 2 * Math.PI);
ctx.arc(0, 0, h / 2 * 2 / Math.sqrt(3), 0, 2 * Math.PI);
// labels
ctx.font = "12px serif";
ctx.fillText("A", w / 2 - 24, 4);
ctx.fillText("B", -4, 10 - h / 2);
//ctx.fillText("V", Math.cos(1 / 3 * 2 * Math.PI) * w / 2, Math.sin(120 / 180 * Math.PI) * h / 2);
//ctx.fillText("W", Math.cos(2 / 3 * 2 *Math.PI) * w / 2, Math.sin(240 / 180 * Math.PI) * h / 2 + 10);
//ctx.fillText("W", -4, 10 - h / 2);
// vector
// voltage
ctx.moveTo(0, 0);
ctx.lineTo(x * w, y * h);
// a, b
ctx.moveTo(0, 0);
ctx.lineTo(x * w, 0);
ctx.moveTo(0, 0);
ctx.lineTo(0, y * h);
ctx.stroke();
ctx.restore();
}
}
function draw(u, v, w, align) {
u = round(1-u, 250) * 250;
v = round(1-v, 250) * 250;
w = round(1-w, 250) * 250;
var canvas = document.getElementById('canvas');
if (canvas.getContext){
var ctx = canvas.getContext('2d');
//clear
ctx.clearRect(0, 0, canvas.width, canvas.height);
ctx.beginPath();
// axis
ctx.moveTo(15,0);
ctx.lineTo(15, 100);
ctx.lineTo(515, 100);
// labels
ctx.font = "12px serif";
ctx.fillText("U", 0, 30);
ctx.fillText("V", 0, 60);
ctx.fillText("W", 0, 90);
// UVW waves
switch(align){
case "center":
ctx.moveTo(15,35);
ctx.lineTo(u + 15, 35);
ctx.lineTo(u + 15, 25);
ctx.lineTo(515 - u, 25);
ctx.lineTo(515 - u, 35);
ctx.lineTo(515, 35);
ctx.moveTo(15,65);
ctx.lineTo(v + 15, 65);
ctx.lineTo(v + 15, 55);
ctx.lineTo(515 - v, 55);
ctx.lineTo(515 - v, 65);
ctx.lineTo(515, 65);
ctx.moveTo(15,95);
ctx.lineTo(w + 15, 95);
ctx.lineTo(w + 15, 85);
ctx.lineTo(515 - w, 85);
ctx.lineTo(515 - w, 95);
ctx.lineTo(515, 95);
break;
case "right":
ctx.moveTo(15,35);
ctx.lineTo(u + 15, 35);
ctx.lineTo(u + 15, 25);
ctx.lineTo(265, 25);
ctx.lineTo(265, 35);
ctx.lineTo(u + 265, 35);
ctx.lineTo(u + 265, 25);
ctx.lineTo(515, 25);
ctx.moveTo(15,65);
ctx.lineTo(v + 15, 65);
ctx.lineTo(v + 15, 55);
ctx.lineTo(265, 55);
ctx.lineTo(265, 65);
ctx.lineTo(v + 265, 65);
ctx.lineTo(v + 265, 55);
ctx.lineTo(515, 55);
ctx.moveTo(15,95);
ctx.lineTo(w + 15, 95);
ctx.lineTo(w + 15, 85);
ctx.lineTo(265, 85);
ctx.lineTo(265, 95);
ctx.lineTo(w + 265, 95);
ctx.lineTo(w + 265, 85);
ctx.lineTo(515, 85);
break;
case "left":
ctx.moveTo(15, 25);
ctx.lineTo(265 - u, 25);
ctx.lineTo(265 - u, 35);
ctx.lineTo(265, 35);
ctx.lineTo(265, 25);
ctx.lineTo(515 - u, 25);
ctx.lineTo(515 - u, 35);
ctx.lineTo(515, 35);
ctx.moveTo(15, 55);
ctx.lineTo(265 - v, 55);
ctx.lineTo(265 - v, 65);
ctx.lineTo(265, 65);
ctx.lineTo(265, 55);
ctx.lineTo(515 - v, 55);
ctx.lineTo(515 - v, 65);
ctx.lineTo(515, 65);
ctx.moveTo(15, 85);
ctx.lineTo(265 - w, 85);
ctx.lineTo(265 - w, 95);
ctx.lineTo(265, 95);
ctx.lineTo(265, 85);
ctx.lineTo(515 - w, 85);
ctx.lineTo(515 - w, 95);
ctx.lineTo(515, 95);
}
ctx.stroke();
}
}
function update(){
var mode = "std";
var align = "left";
var a = 0.0;
var b = 0.0;
var u = 0.0;
var v = 0.0;
var w = 0.0;
var radio = document.getElementById('radio_std');
if(radio){
if(radio.checked){
mode = "std";
}
}
radio = document.getElementById('radio_svm');
if(radio){
if(radio.checked){
mode = "svm";
}
}
radio = document.getElementById('radio_fbsvm');
if(radio){
if(radio.checked){
mode = "flat bottom svm";
}
}
radio = document.getElementById('radio_left');
if(radio){
if(radio.checked){
align = "left";
}
}
radio = document.getElementById('radio_right');
if(radio){
if(radio.checked){
align = "right";
}
}
radio = document.getElementById('radio_center');
if(radio){
if(radio.checked){
align = "center";
}
}
radio = document.getElementById('range_a');
if(radio){
a = radio.value;
}
radio = document.getElementById('range_b');
if(radio){
b = radio.value;
}
u = a * 1.0; // inverse clarke
v = - a / 2.0 + b / 2.0 * Math.sqrt(3);
w = - a / 2.0 - b / 2.0 * Math.sqrt(3);
u = clamp(u, 0.5);
v = clamp(v, 0.5);
w = clamp(w, 0.5);
radio = document.getElementById('df');
if(radio){
radio.innerHTML = "A = " + round(a, 100) + "<br>B = " + round(b, 100) + "<br>U = " + round(u, 100) + "<br>V = " + round(v, 100) + "<br>W = " + round(w, 100);
}
draw_vec(a, b);
svm(u, v, w, mode, align);
}
function set_ab(e){
if(isMouseDown){
var mouseX, mouseY;
if(e.offsetX) {
mouseX = e.offsetX;
mouseY = e.offsetY;
}
else if(e.layerX) {
mouseX = e.layerX;
mouseY = e.layerY;
}
var canvas = document.getElementById('canvas_plot');
if (canvas.getContext){
var radio = document.getElementById('range_a');
if(radio){
radio.value = (mouseX - canvas.width / 2) / canvas.width;
}
radio = document.getElementById('range_b');
if(radio){
radio.value = -(mouseY - canvas.height / 2) / canvas.height;
}
}
update();
}
}
</script>
</head>
<style type="text/css">
input:button {width : 500px}
</style>
<body onload="update();">
<canvas id="canvas" width="550" height="100">
not supported
</canvas>
<canvas id="canvas_plot" width="200" height="200" onmousedown="isMouseDown = true; set_ab(event);" onmousemove="set_ab(event);">
not supported
</canvas><br>
<input type="radio" id="radio_std" name="mode" onclick="update();" checked="checked">std<br>
<input type="radio" id="radio_svm" name="mode" onclick="update();">SVM<br>
<input type="radio" id="radio_fbsvm" name="mode" onclick="update();">flat bottom SVM<br>
<input type="radio" id="radio_left" name="align" onclick="update();">left pwm<br>
<input type="radio" id="radio_right" name="align" onclick="update();">right pwm<br>
<input type="radio" id="radio_center" name="align" onclick="update();"" checked="checked">center pwm<br>
<input type="range" id="range_a" onchange="update();" oninput="update();" min="-0.5" max="0.5" step="0.01" value="0.0">A<br>
<input type="range" id="range_b" onchange="update();" oninput="update();" min="-0.5" max="0.5" step="0.01" value="0.0">B<br>
<a id="df"></a>
</body>
</html>
sim/sim.cpp
-410
View File
@@ -1,410 +0,0 @@
#include "sim.h"
// Rcpp implementation of the 1D TVD algorithm in:
// Condat, L (2012) A Direct Algorithm for 1D Total Variation Denoising
// http://hal.inria.fr/docs/00/67/50/43/PDF/condat_killer_tv.pdf
// Accessed 11 August 2014.
// [[Rcpp::export]]
void tvd_1d_condat(std::vector<float> &y, std::vector<float> &x, int N, float lambda)
{
int k, k0, kn, kp, i;
float vmin, vmax, umin, umax;
k = k0 = kn = kp = 1;
vmin = y[0] - lambda;
vmax = y[0] + lambda;
umin = lambda;
umax = -lambda;
while (1)
{
// b
if (k == N)
{
x[k-1] = vmin + umin;
break;
}
if (y[k] + umin < vmin - lambda)
{
// b1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kp = kn;
vmin = y[k-1];
vmax = y[k-1] + 2*lambda;
umin = lambda;
umax = -lambda;
}
else if (y[k] + umax > vmax + lambda)
{
// b2
for (i = k0-1; i < kp; i++)
x[i] = vmax;
kp++;
k = k0 = kn = kp;
vmin = y[k-1] - 2*lambda;
vmax = y[k-1];
umin = lambda;
umax = -lambda;
}
else
{
// b3
k++;
umin = umin + y[k-1] - vmin;
umax = umax + y[k-1] - vmax;
if (umin >= lambda)
{
// b31
vmin = vmin + (umin - lambda)/((float)(k - k0 + 1));
umin = lambda;
kn = k;
}
if (umax <= -lambda)
{
// b32
vmax = vmax + (umax + lambda)/((float)(k - k0 + 1));
umax = -lambda;
kp = k;
}
}
// c
if (k == N)
{
if (umin < 0)
{
// c1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kn;
vmin = y[k-1];
umin = lambda;
umax = y[k-1] + lambda - vmax;
}
else if (umax > 0)
{
// c2
for (i = k0-1; i < kn; i++)
x[i] = vmax;
kp++;
k = k0 = kp;
vmax = y[k-1];
umax = -lambda;
umin = y[k-1] - lambda - vmin;
}
else
{
// c3
for (i = k0-1; i < N; i++)
x[i] = vmin + umin/((float)(k - k0 + 1));
break;
}
}
}
}
// Rcpp implementation of the 1D TVD algorithm in:
// Condat, L (2012) A Direct Algorithm for 1D Total Variation Denoising
// http://hal.inria.fr/docs/00/67/50/43/PDF/condat_killer_tv.pdf
// Accessed 11 August 2014.
// [[Rcpp::export]]
std::vector<double> tvd_1d_condat_worker(std::vector<double>& y, double lambda)
{
// NOTE: in wrapping R code, verify that y.size() <= 2^32-1. Although
// newer versions of R get around this index limit, Rcpp is still
// limited to int indices.
int N = y.size();
int k, k0, kn, kp, i;
double vmin, vmax, umin, umax;
std::vector<double> x(N);
k = k0 = kn = kp = 1;
vmin = y[0] - lambda;
vmax = y[0] + lambda;
umin = lambda;
umax = -lambda;
while (true)
{
// b
if (k == N)
{
x[k-1] = vmin + umin;
break;
}
if (y[k] + umin < vmin - lambda)
{
// b1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kp = kn;
vmin = y[k-1];
vmax = y[k-1] + 2*lambda;
umin = lambda;
umax = -lambda;
}
else if (y[k] + umax > vmax + lambda)
{
// b2
for (i = k0-1; i < kp; i++)
x[i] = vmax;
kp++;
k = k0 = kn = kp;
vmin = y[k-1] - 2*lambda;
vmax = y[k-1];
umin = lambda;
umax = -lambda;
}
else
{
// b3
k++;
umin = umin + y[k-1] - vmin;
umax = umax + y[k-1] - vmax;
if (umin >= lambda)
{
// b31
vmin = vmin + (umin - lambda)/(static_cast<double>(k - k0 + 1));
umin = lambda;
kn = k;
}
if (umax <= -lambda)
{
// b32
vmax = vmax + (umax + lambda)/(static_cast<double>(k - k0 + 1));
umax = -lambda;
kp = k;
}
}
// c
if (k == N)
{
if (umin < 0)
{
// c1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kn;
vmin = y[k-1];
umin = lambda;
umax = y[k-1] + lambda - vmax;
}
else if (umax > 0)
{
// c2
for (i = k0-1; i < kn; i++)
x[i] = vmax;
kp++;
k = k0 = kp;
vmax = y[k-1];
umax = -lambda;
umin = y[k-1] - lambda - vmin;
}
else
{
// c3
for (i = k0-1; i < N; i++)
x[i] = vmin + umin/(static_cast<double>(k - k0 + 1));
break;
}
}
}
return x;
}
int main(){
float sim_time = 0.0;
float sim_step = 0.0001;
float sim_end_time = 0.1;
srand(14235);
// e240
mot_c mot;
mot.reset();
mot.mech_spec.max_rps = 83.3;
mot.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot.mech_spec.pole_count = 1;
mot.mech_spec.friction = 0.021;//0.021;
mot.mech_spec.damping = 0.0000426;//0.0000426;
mot.mech_spec.inertia = 0.0000268;//0.0000268;
mot.elec_spec.max_i = 13.9;
mot.elec_spec.i = 1.9;
mot.elec_spec.nm_a = 0.135;
mot.elec_spec.r = 5.4;
mot.elec_spec.l = 0.0082;
mot.elec_spec.v_rps = 0.852;
mot.elec_spec.slip = 0;
mot.feedback.type = mot_c::feedback_s::RES;
mot.feedback.count = 1;
mot.feedback.res_offset = 0.0;
mot.noise.sin_scale = 1.0;
mot.noise.cos_scale = 1.0;
mot.noise.sin_offset = 0.0;
mot.noise.cos_offset = 0.0;
mot.noise.var = 0.0;
mot.load.friction = 0.0;
mot.load.load = 0.0;
mot.load.damping = 0.0;
mot.load.inertia = 0.0;
// bautz e728
mot_c mot2;
mot2.reset();
mot2.mech_spec.max_rps = 50;
mot2.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot2.mech_spec.pole_count = 1;
mot2.mech_spec.friction = 0.18;
mot2.mech_spec.damping = 0.004236;
mot2.mech_spec.inertia = 0.0012;
mot2.elec_spec.max_i = 60;
mot2.elec_spec.i = 10;
mot2.elec_spec.nm_a = 0.36;
mot2.elec_spec.r = 0.67;
mot2.elec_spec.l = 0.0011;
mot2.elec_spec.v_rps = 2.28;
mot2.elec_spec.slip = 0;
mot2.feedback.type = mot_c::feedback_s::RES;
mot2.feedback.count = 1;
mot2.feedback.res_offset = 0.0;
mot2.noise.sin_scale = 1.0;
mot2.noise.cos_scale = 1.0;
mot2.noise.sin_offset = 0.0;
mot2.noise.cos_offset = 0.0;
mot2.noise.var = 0.0;
mot2.load.friction = 0.0;
mot2.load.load = 0.0;
mot2.load.damping = 0.0;
mot2.load.inertia = 0.0;
// bosch
mot_c mot3;
mot3.reset();
mot3.mech_spec.max_rps = 16.6;
mot3.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot3.mech_spec.pole_count = 4;
mot3.mech_spec.friction = 0.065;//0.0;
mot3.mech_spec.damping = 0.0000826;//0.0;
mot3.mech_spec.inertia = 0.000141;
mot3.elec_spec.max_i = 4;
mot3.elec_spec.i = 2.2;
mot3.elec_spec.nm_a = 0.2727;
mot3.elec_spec.r = 15.2;
mot3.elec_spec.l = 0.0082;//0.030;
mot3.elec_spec.v_rps = 3.4;//5;
mot3.elec_spec.slip = 0;
mot3.feedback.type = mot_c::feedback_s::RES;
mot3.feedback.count = 1;
mot3.feedback.res_offset = 0.0;
mot3.noise.sin_scale = 1.0;
mot3.noise.cos_scale = 1.0;
mot3.noise.sin_offset = 0.0;
mot3.noise.cos_offset = 0.0;
mot3.noise.var = 0.01;
mot3.load.friction = 0.0;
mot3.load.load = 0.0;
mot3.load.damping = 0.0;
mot3.load.inertia = 0.0;
cmd_c cmd;
cmd.reset();
cmd.periode = 2;
cmd.amplitude = 1;
cmd.wave = cmd_c::CONST;
cmd.type = cmd_c::POS;
cmd.pos_res = 0.01;
cmd.vel_res = 0.01;
cmd.acc_res = 0.01;
drive_c drive;
drive.dc = 50;
drive.pwm_scale = 0.9;
drive.pwm_res = 8400;
drive.pid_periode = 0.001;
drive.mot = &mot3;
drive.in = &cmd;
drive.input_cmd = input_cmd;
drive.input_feedback = input_feedback_real;
drive.pid = pid;
drive.output = output;
drive.reset();
cout << flush << "time pos pos2 pos3" << endl;
int count = 0;
int pid_count = drive.pid_periode / sim_step;
double vel0 = MIN(drive.dc * (2 * drive.pwm_scale - 1) / drive.mot->elec_spec.v_rps, drive.mot->mech_spec.max_rps) * 0.7;
double ind0 = vel0 * drive.mot->elec_spec.v_rps;
double volt0 = drive.dc * (2 * drive.pwm_scale - 1);
double cur0 = (volt0 - ind0) / drive.mot->elec_spec.r;
double torq0 = cur0 * drive.mot->elec_spec.nm_a;
double acc0 = torq0 / drive.mot->mech_spec.inertia;
double vel1 = MIN(drive.dc * (2 * drive.pwm_scale - 1) / drive.mot->elec_spec.v_rps, drive.mot->mech_spec.max_rps) * 0.3;
double ind1 = vel1 * drive.mot->elec_spec.v_rps;
double volt1 = drive.dc * (2 * drive.pwm_scale - 1);
double cur1 = (volt1 - ind1) / drive.mot->elec_spec.r;
double torq1 = cur1 * drive.mot->elec_spec.nm_a;
double acc1 = torq1 / drive.mot->mech_spec.inertia;
double a, b, y;
a = (acc1 - acc0) / (vel1 - vel0);
b = acc0 - a * vel0;
std::vector<float> in,out,foo;
int i = 0;
for(sim_time = 0.0; sim_time < sim_end_time; sim_time += sim_step){
drive.in->step(sim_step);
if(count == 0){
drive.step(sim_step * pid_count);
}
count++;
count %= pid_count;
//drive.state.ctr = 1;
drive.output(&drive, sim_step);
drive.mot->step(sim_step);
in.push_back(drive.mot->get_sin()*drive.mot->get_polarity());
out.push_back(drive.mot->get_sin()*drive.mot->get_polarity());
foo.push_back(sin((drive.mot->state.pos + drive.mot->feedback.res_offset) * drive.mot->feedback.count));
//e_pos = (torq - drive.est.load) / drive.est.inertia * exp(-sim_time * mot.elec_spec.v_rps / drive.est.inertia * mot.elec_spec.nm_a / mot.elec_spec.r);
//y = a * drive.mot->state.vel + b;
//cout << sim_time << ", " << drive.in->get_pos() << ", " << drive.mot->state.pos << ", " << drive.est.pos << ", " << drive.est.sin_avg << ", " << drive.est.sin_scale << ", " << drive.est.cos_avg << ", " << drive.est.cos_scale << endl;
//cout << sim_time << ", " << drive.mot->state.pos << ", " << drive.mot->state.vel << ", " << drive.mot->state.acc << ", " << drive.est.p << ", " << drive.est.v << ", " << drive.est.a << endl;//", " << drive.state.ctr << ", " << minus_(drive.in->get_pos(), drive.est.pos) << endl;
//cout << sim_time << ", " << "-15, " << drive.in->get_pos() * 5 - 15<< ", " << drive.state.ctr * 10 << ", " << drive.mot->state.cur << ", " << drive.mot->state.vel / 5 << ", " << drive.mot->state.pos * 5 - 15 << ", " << minus_(drive.in->state.pos, drive.mot->state.pos) * (-100) - 15 << endl;//", " << drive.state.ctr << ", " << minus_(drive.in->get_pos(), drive.est.pos) << endl;
//cout << sim_time/*drive.mot->state.vel*/ << ", " << drive.est.pos << ", " << drive.mot->state.pos << endl;
}
//out = tvd_1d_condat_worker(in, 0.05);
tvd_1d_condat(in, out, in.size(),0.05);
for(sim_time = 0.0; sim_time < sim_end_time; sim_time += sim_step){
cout << sim_time/*drive.mot->state.vel*/ << ", " << in[i] << ", " << out[i] << ", " << foo[i] << endl;
i++;
}
system("gnuplot --persist gp");
return(0);
}
sim/sim.h
-616
View File
File diff suppressed because it is too large Load Diff
tools/checkcrc.py
-12
View File
@@ -1,12 +0,0 @@
#!/usr/bin/env python
import binascii
import sys
buf = open(sys.argv[1],'rb').read()
buf = (binascii.crc32(buf) & 0xFFFFFFFF)
if buf == 0:
print "crc ok!"
sys.exit(0)
else:
print "This should not happen. Please report a bug."
sys.exit(-1)