Homec4science
Diffusion Oncilla Motordriver Firmware (load-bad-commit) Browse

oncilla-motordriver-fw/src/sbcp_ll.c
df700679f13cload-bad-commit

Last Change
Raw File

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.
}
}
c4science · Help