Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F79134277
sbcp_ll.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sat, Aug 24, 22:29
Size
3 KB
Mime Type
text/x-c
Expires
Mon, Aug 26, 22:29 (2 d)
Engine
blob
Format
Raw Data
Handle
20147135
Attached To
R6619 Oncilla Motordriver Firmware
sbcp_ll.c
View Options
#include "sbcp_ll.h"
#include "sbcp.h"
#include "error.h"
#include "dma_sbcp.h"
#include "error.h"
#include "command.h"
#include <p33Fxxxx.h>
//Some *PRIVATE* macros ! don't use then at another location please
#define M_BUFFER DMA_PACKET_QUEUE_BUS_LL_TX_BUFFER_CPU
#define LOW_LATENCY_CS_POS (SBCP_POS_PAYLOAD_START + SBCP_LOW_LATENCY_INSTRUCTION_FEEDBACK_PAYLOAD_SIZE)
#define PUT_TABLE_PARAMETER_IN_PLACE(param,index) do{\
M_BUFFER[SBCP_POS_PAYLOAD_START + index ] = control_table[param].c[HIGH_BYTE];\
M_BUFFER[LOW_LATENCY_CS_POS] += control_table[param].c[HIGH_BYTE];\
M_BUFFER[SBCP_POS_PAYLOAD_START + index + 1] = control_table[param].c[LOW_BYTE];\
M_BUFFER[LOW_LATENCY_CS_POS] += control_table[param].c[LOW_BYTE];\
}while(0)
void
sbcp_low_latency_packet_main
(){
if
(
sbcp
.
bus_low_latency_flags
&
SBCP_LL_RX_IS_AVAILABLE
){
//we had a correct check sum and everything
//first parameter is HIP_DESIRED_POSITION
unsigned
char
address_hip
=
(
control_table
[
A_MDRV_HIP_MOTOR_CONTROL_MODE
].
u
&
STATUS_MOTOR_NON_CRITICAL_STATE_MASK
)
==
STATUS_MOTOR_SPEED_CONTROL
?
A_MDRV_HIP_GOAL_SPEED
:
A_MDRV_HIP_GOAL_POSITION
;
unsigned
char
address_knee
=
(
control_table
[
A_MDRV_KNEE_MOTOR_CONTROL_MODE
].
u
&
STATUS_MOTOR_NON_CRITICAL_STATE_MASK
)
==
STATUS_MOTOR_SPEED_CONTROL
?
A_MDRV_KNEE_GOAL_SPEED
:
A_MDRV_KNEE_GOAL_POSITION
;
sbcp
.
bus_low_latency_flags
|=
SBCP_LL_IS_DISABLED
;
control_table
[
address_hip
]
.
c
[
HIGH_BYTE
]
=
sbcp
.
bus_low_latency_payload
[
0
];
control_table
[
address_hip
]
.
c
[
LOW_BYTE
]
=
sbcp
.
bus_low_latency_payload
[
1
];
control_table
[
address_knee
].
c
[
HIGH_BYTE
]
=
sbcp
.
bus_low_latency_payload
[
2
];
control_table
[
address_knee
].
c
[
LOW_BYTE
]
=
sbcp
.
bus_low_latency_payload
[
3
];
sbcp
.
bus_low_latency_flags
&=
~
(
SBCP_LL_RX_IS_AVAILABLE
|
SBCP_LL_IS_DISABLED
);
IEC0bits
.
DMA0IE
=
1
;
//If it has already be interrupted, it will be
//relaunched
sbcp
.
bus_low_latency_previous_error_code
=
check_for_action
(
address_hip
);
if
(
sbcp
.
bus_low_latency_previous_error_code
==
NO_ERROR
)
{
sbcp
.
bus_low_latency_previous_error_code
=
check_for_action
(
address_knee
);
}
//clear the flag !
}
if
(
sbcp
.
bus_low_latency_flags
&
SBCP_LL_TX_IS_AVAILABLE
&&
DMA1CONbits
.
CHEN
==
0
)
{
// Don't modify the data while the bus is tramnsmitting !!!
sbcp
.
bus_low_latency_flags
|=
SBCP_LL_IS_DISABLED
;
M_BUFFER
[
SBCP_POS_PAYLOAD_START
+
SBCP_LOW_LATENCY_INSTRUCTION_FEEDBACK_PAYLOAD_SIZE
]
=
SBCP_MY_CLASS
+
SBCP_MY_ID
+
SBCP_LOW_LATENCY_INSTRUCTION_FEEDBACK_PAYLOAD_SIZE
+
sbcp
.
bus_low_latency_previous_error_code
;
M_BUFFER
[
SBCP_POS_INSTRUCTION
]
=
sbcp
.
bus_low_latency_previous_error_code
;
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_HIP_PRESENT_POSITION
,
0
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_KNEE_PRESENT_POSITION
,
2
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_HIP_PRESENT_TORQUE
,
4
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_KNEE_PRESENT_TORQUE
,
6
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_ME_Q1_POSITION
,
8
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_ME_Q2_POSITION
,
10
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_ME_Q3_POSITION
,
12
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR1
,
14
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR2
,
16
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR3
,
18
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR4
,
20
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR5
,
22
);
PUT_TABLE_PARAMETER_IN_PLACE
(
A_MDRV_TORQUE_SENSOR6
,
24
);
//data is now ready to be send
//don't make this in infinite loop
sbcp
.
bus_low_latency_flags
&=
~
(
SBCP_LL_TX_IS_AVAILABLE
|
SBCP_LL_IS_DISABLED
);
IEC0bits
.
DMA0IE
=
1
;
//will rethrow interrupt if already thrown.
}
}
Event Timeline
Log In to Comment