Skip to content

Fix embedded binary parameter loading #40

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 13 commits into from
Apr 4, 2019
Merged

Fix embedded binary parameter loading #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
b5ad493
Fix embedded binary parameter struct alignment
at-wat Mar 26, 2019
9f54625
Fix order of PWM init and EEPROM read
at-wat Mar 26, 2019
1f6e6d6
Validate embedded parameter
at-wat Mar 26, 2019
66c712f
Fix EEPROM software reset sequence
at-wat Mar 26, 2019
20e82fb
Fix initialization order to prevent interrupts during EEPROM burst read
at-wat Mar 28, 2019
a146710
Fix velocity control parameter init
at-wat Mar 28, 2019
dfed086
Fix to start timer at boot using default parameters
at-wat Mar 28, 2019
a4d79a9
Merge branch 'master' into fix-embedded-param-alignment
at-wat Mar 28, 2019
8581822
Fix debug msg
at-wat Mar 28, 2019
697a86c
Split DriverState from DriverParam struct
at-wat Mar 29, 2019
7696d51
Check stored parameter version
at-wat Mar 29, 2019
d1d4f36
Fix struct size calculation to ignore trailing padding
at-wat Mar 29, 2019
a5eac50
Merge branch 'master' into fix-embedded-param-alignment
at-wat Apr 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 18 additions & 18 deletions tfrog-motordriver/communication.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -403,7 +403,7 @@ int data_send485(short* cnt, short* pwm, char* en, short* analog, unsigned short

buf[0] = COMMUNICATION_START_BYTE;
buf[1] = saved_param.id485 + 0x40;
if (driver_param.ifmode == 0)
if (driver_state.ifmode == 0)
buf[1] = 0x40;
buf[2] = 0x40 - 1;
buf_len = 3;
Expand Down Expand Up @@ -484,7 +484,7 @@ int int_send485(const char param, const char id, const int value)

buf[0] = COMMUNICATION_INT_BYTE;
buf[1] = saved_param.id485 + 0x40;
if (driver_param.ifmode == 0)
if (driver_state.ifmode == 0)
buf[1] = 0x40;
buf[2] = 0x40 - 1;
buf_len = 3;
Expand Down Expand Up @@ -782,7 +782,7 @@ static inline int data_analyze_(
rawdata[1] = rawdata[1] & 1;

command_analyze(rawdata, data_len);
driver_param.ifmode = fromto;
driver_state.ifmode = fromto;
//printf("for me\n\r");
}
else if (from == -1)
Expand Down Expand Up @@ -1370,7 +1370,7 @@ int extended_command_analyze(char* data)
if (data[i] == '1')
tmp |= 1;
}
driver_param.admask = tmp;
driver_state.admask = tmp;
send(data);
send("\n00P\n\n");
}
Expand All @@ -1386,7 +1386,7 @@ int extended_command_analyze(char* data)
tmp |= 1;
}
set_io_dir(tmp);
driver_param.io_dir = tmp;
driver_state.io_dir = tmp;
send(data);
send("\n00P\n\n");
}
Expand Down Expand Up @@ -1422,20 +1422,20 @@ int extended_command_analyze(char* data)
switch (data[5])
{
case '1':
driver_param.io_mask[0] = 0xFF;
driver_param.io_mask[1] = 0;
driver_state.io_mask[0] = 0xFF;
driver_state.io_mask[1] = 0;
break;
case '2':
driver_param.io_mask[1] = 0xFF;
driver_param.io_mask[0] = 0;
driver_state.io_mask[1] = 0xFF;
driver_state.io_mask[0] = 0;
break;
case '3':
driver_param.io_mask[0] = 0xFF;
driver_param.io_mask[1] = 0xFF;
driver_state.io_mask[0] = 0xFF;
driver_state.io_mask[1] = 0xFF;
break;
default:
driver_param.io_mask[0] = 0;
driver_param.io_mask[1] = 0;
driver_state.io_mask[0] = 0;
driver_state.io_mask[1] = 0;
break;
}
send(data);
Expand Down Expand Up @@ -1534,17 +1534,17 @@ int command_analyze(unsigned char* data, int len)
motor[imotor].servo_level = i.integer;
break;
case PARAM_io_dir:
driver_param.io_dir = i.integer;
set_io_dir(driver_param.io_dir);
driver_state.io_dir = i.integer;
set_io_dir(driver_state.io_dir);
break;
case PARAM_io_data:
set_io_data(i.integer);
break;
case PARAM_ad_mask:
driver_param.admask = i.integer;
driver_state.admask = i.integer;
break;
case PARAM_protocol_version:
driver_param.protocol_version = i.integer;
driver_state.protocol_version = i.integer;
break;
default:
param_set = 1;
Expand Down Expand Up @@ -1670,6 +1670,6 @@ int command_analyze(unsigned char* data, int len)
return 0;
}
}
driver_param.watchdog = 0;
driver_state.watchdog = 0;
return 0;
}
16 changes: 8 additions & 8 deletions tfrog-motordriver/controlPWM.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,7 +295,7 @@ void FIQ_PWMPeriod()

if (driver_param.vsrc_rated != 0)
{
rate = rate * driver_param.vsrc_factor / 32768;
rate = rate * driver_state.vsrc_factor / 32768;
if (rate >= PWM_resolution)
rate = PWM_resolution - 1;
else if (rate <= -PWM_resolution)
Expand Down Expand Up @@ -390,7 +390,7 @@ void FIQ_PWMPeriod()
THEVA.MOTOR[j].PWM[i].L = PWM_resolution;
}
}
else if (_abs(motor[j].ref.torque) < driver_param.zero_torque ||
else if (_abs(motor[j].ref.torque) < driver_state.zero_torque ||
motor[j].servo_level == SERVO_LEVEL_OPENFREE)
{
for (i = 0; i < 3; i++)
Expand Down Expand Up @@ -437,9 +437,9 @@ void FIQ_PWMPeriod()
//if( halldiff == 3 || halldiff >= 5 ) printf( "ENC error: %x->%x\n\r", _hall[i], hall[i] );
// ホール素子信号が全相1、全相0のとき
// ホース素子信号が2ビット以上変化したときはエラー
if (driver_param.error.hall[i] < 128)
driver_param.error.hall[i] += 12;
if (driver_param.error.hall[i] > 12)
if (driver_state.error.hall[i] < 128)
driver_state.error.hall[i] += 12;
if (driver_state.error.hall[i] > 12)
{
// エラー検出後、1周以内に再度エラーがあれば停止
motor[i].error_state |= ERROR_HALL_SEQ;
Expand All @@ -449,8 +449,8 @@ void FIQ_PWMPeriod()
}
else
{
if (driver_param.error.hall[i] > 0)
driver_param.error.hall[i]--;
if (driver_state.error.hall[i] > 0)
driver_state.error.hall[i]--;
}

switch (halldiff)
Expand Down Expand Up @@ -648,7 +648,7 @@ void controlPWM_init()
THEVA.MOTOR[j % 2].PWM[j / 2].L = PWM_resolution;
}

driver_param.PWM_resolution = PWM_resolution;
driver_state.PWM_resolution = PWM_resolution;

PWM_cpms = 256 * 16 * driver_param.control_cycle * 48000 / (PWM_resolution * 2);
driver_param.control_s = 1000 / driver_param.control_cycle;
Expand Down
26 changes: 11 additions & 15 deletions tfrog-motordriver/controlVelocity.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,16 +38,14 @@
MotorState motor[2];
MotorParam motor_param[2];
DriverParam driver_param;
DriverState driver_state;
short com_cnts[COM_MOTORS];
short com_pwms[COM_MOTORS];
char com_en[COM_MOTORS];
int pwm_sum[2] = { 0, 0 };
int pwm_num[2] = { 0, 0 };
short soft_start[2] = { 0, 0 };

extern int watchdog;
extern int velcontrol;

Filter1st accelf[2];
Filter1st accelf0;

Expand Down Expand Up @@ -258,8 +256,8 @@ void timer0_vel_calc()
if (motor[0].servo_level > SERVO_LEVEL_STOP ||
motor[1].servo_level > SERVO_LEVEL_STOP)
{
driver_param.watchdog++;
driver_param.cnt_updated++;
driver_state.watchdog++;
driver_state.cnt_updated++;
}

dummy = AT91C_BASE_TC0->TC_SR;
Expand All @@ -281,7 +279,7 @@ void timer0_vel_calc()
__vel = (int)(enc[i] - __enc[i]);
motor[i].vel1 = __vel;

if (_abs(__vel) > 6 || driver_param.fpga_version == 0)
if (_abs(__vel) > 6 || driver_state.fpga_version == 0)
{
vel = __vel * 16;
_spd_cnt[i] = 0;
Expand Down Expand Up @@ -337,7 +335,7 @@ void timer0_vel_calc()
{
if (motor[i].servo_level >= SERVO_LEVEL_TORQUE)
{
if (driver_param.cnt_updated == 5)
if (driver_state.cnt_updated == 5)
{
motor[i].ref.rate_buf = pwm_sum[i] * 5 / pwm_num[i];
motor[i].enc_buf2 = motor[i].enc_buf;
Expand All @@ -349,7 +347,7 @@ void timer0_vel_calc()

LED_off(1);

velcontrol = 1;
driver_state.velcontrol = 1;
}

// ------------------------------------------------------------------------------
Expand All @@ -363,13 +361,13 @@ void controlVelocity_init()
Filter1st_CreateLPF(&accelf0, ACCEL_FILTER_TIME);
accelf[0] = accelf[1] = accelf0;

driver_param.protocol_version = 0;
driver_param.cnt_updated = 0;
driver_param.watchdog_limit = 600;
driver_param.admask = 0;
driver_param.io_mask[0] = 0;
driver_param.io_mask[1] = 0;
driver_param.control_cycle = 1;
driver_state.protocol_version = 0;
driver_state.cnt_updated = 0;
driver_state.admask = 0;
driver_state.io_mask[0] = 0;
driver_state.io_mask[1] = 0;

for (i = 0; i < 2; i++)
{
Expand All @@ -389,8 +387,6 @@ void controlVelocity_init()
motor_param[i].phase_offset = 0;
motor_param[i].enc_type = 2;
}

controlVelocity_config();
}
void controlVelocity_config()
{
Expand Down
24 changes: 18 additions & 6 deletions tfrog-motordriver/controlVelocity.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

#include <board.h>
#include <stdint.h>
#include <assert.h>
#include "communication.h"

#ifdef __cplusplus
Expand All @@ -33,8 +34,10 @@ typedef enum _ErrorID
ERROR_HALL_SEQ = 0x0002,
ERROR_HALL_ENC = 0x0004,
ERROR_WATCHDOG = 0x0008,
ERROR_EEPROM = 0x0010,
ERROR_INTERNAL = 0x0020,
} ErrorID;
#define ERROR_NUM 4
#define ERROR_NUM 6

typedef struct _MotorState
{
Expand Down Expand Up @@ -111,16 +114,20 @@ typedef struct _MotorParam

typedef struct _DriverParam
{
int PWM_max; // clock
int PWM_min; // clock
int PWM_resolution; // clock
int PWM_max;
int PWM_min;
int control_cycle;
int control_s;
int vsrc_rated;
unsigned short watchdog_limit;
} DriverParam;

typedef struct _DriverState
{
int PWM_resolution;
int vsrc_factor;
int vsrc;
int zero_torque;
unsigned short watchdog_limit;
unsigned short watchdog;
unsigned char cnt_updated;
unsigned short admask;
Expand All @@ -140,11 +147,16 @@ typedef struct _DriverParam
BOARD_R6B
} board_version;
unsigned char protocol_version;
} DriverParam;
int velcontrol;
} DriverState;

static_assert(sizeof(DriverParam) < 0x100, "DriverParam overflows reserved EEPROM capacity");
static_assert(sizeof(MotorParam) < 0x100, "MotorParam overflows reserved EEPROM capacity");

extern MotorState motor[2];
extern MotorParam motor_param[2];
extern DriverParam driver_param;
extern DriverState driver_state;

#define COM_MOTORS 16
extern short com_cnts[COM_MOTORS];
Expand Down
22 changes: 18 additions & 4 deletions tfrog-motordriver/eeprom.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,23 +59,37 @@ void EEPROM_Init()

PIO_Configure(pinsEEPROM_reset, PIO_LISTSIZE(pinsEEPROM_reset));
PIO_Set(&pinsEEPROM_reset[0]);
PIO_Clear(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Set(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[0]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Set(&pinsEEPROM_reset[0]);
msleep(1);

for (i = 0; i < 10; i++)
for (i = 0; i < 9; i++)
{
PIO_Set(&pinsEEPROM_reset[0]);
PIO_Set(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[0]);
PIO_Clear(&pinsEEPROM_reset[1]);
msleep(1);
}

PIO_Set(&pinsEEPROM_reset[0]);
PIO_Set(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[0]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Set(&pinsEEPROM_reset[1]);
msleep(1);
PIO_Set(&pinsEEPROM_reset[0]);
msleep(1);
PIO_Clear(&pinsEEPROM_reset[1]);
msleep(1);

AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_TWI;
PIO_Configure(pinsEEPROM, PIO_LISTSIZE(pinsEEPROM));
Expand Down
Loading