/*检测是否有按键按下*/
void GetKey(void)
{
if(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_13))
{
delay(1000000);//去抖动//去抖动
if(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_13))
{
while(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_13)){ ; }//等待按键释放
can_tx(0X55,0X77);
LED1(1);LED2(1);
}
}
if(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_12))
{
delay(1000000);//去抖动//去抖动
if(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_12))
{
while(Bit_RESET == GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_12)){ ; }//等待按键释放
can_tx(0X99,0Xbb);
LED1(1);LED2(1);
}
}
}
/********************
Callback function for handling the incoming init CAN message
***********************/
void incoming_init(CAN_packet* p, unsigned char mob){
cli();
if (p->data[0] == 0x01 /*&& init_status == no_init*/) {
init_status = init_1;
while(!can_tx(2, &init_ack_sync_packet));
} else if (p->data[0] == 0x02 && init_status == init_1) {
init_status = init_2;
}
sei();
}
//
// power_control()
//
// Turn track on or off.
//
void power_control(void) {
if (rx_ptr->d0 == OPC_RTOF) {
// Turn off main track power
op_flags.op_pwr_m = 0;
// Acknowledge it
Tx1[d0] = OPC_TOF;
} else {
// Turn on main track power
op_flags.op_pwr_m = 1;
// Acknowledge it
Tx1[d0] = OPC_TON;
}
can_tx(1);
}
/********************
Callback function for handling the incoming DA_out_value CAN message
***********************/
void incoming_DA_out_value(CAN_packet* p, unsigned char mob){
U16 MSB;
U16 LSB;
U16 DA_val[3];
int i;
red_LED_toggle();
for (i = 0; i < 3; i ++) {
MSB = p->data[i*2];
LSB = p->data[i*2+1];
DA_val[i]= LSB + (MSB << 8);
}
update_DA_A(DA_val[0]);
update_DA_B(DA_val[1]);
update_DA_C(DA_val[2]);
DA_LDAC_high();
DA_LDAC_low();
prepare_incremental_in_value_packet();
while(!can_tx(5, &incremental_in_value_packet));
}
/*! \brief CAN Prepare Data to Send
* - Allocate one MOB in transmission
* - Fill the MOB with the correct DATA
* - Start the transmission
*/
void can_example_prepare_data_to_send(void)
{
// Initialize channel 1
can_init(1,
((U32)&mob_ram_ch1[0]),
CANIF_CHANNEL_MODE_NORMAL,
can_out_callback_channel1);
// Allocate one mob for TX
pCANMOB_message0[0].handle = can_mob_alloc(1);
// Check return if no mob are available
if (pCANMOB_message0[0].handle==CAN_CMD_REFUSED) {
while(true);
}
pCANMOB_message0[0].can_msg->data.u8[0] =
((adc_current_conversion&0xFF00)>>8);
pCANMOB_message0[0].can_msg->data.u8[1] =
(adc_current_conversion&0xFF);
can_tx(1,
pCANMOB_message0[0].handle,
pCANMOB_message0[0].dlc,
pCANMOB_message0[0].req_type,
pCANMOB_message0[0].can_msg);
}
static void __attribute__((noreturn)) node_run(
uint8_t node_id,
Configuration& configuration,
const struct motor_params_t& motor_params,
const struct control_params_t& control_params
) {
size_t length;
uint32_t message_id, current_time, node_status_time, esc_status_time,
foc_status_time, node_status_interval, esc_status_interval,
foc_status_interval, last_setpoint_update, enumeration_deadline;
uint8_t filter_id, foc_status_transfer_id, node_status_transfer_id,
esc_status_transfer_id, enumeration_indication_transfer_id,
esc_index, message[8], broadcast_filter_id, service_filter_id;
enum controller_mode_t mode;
float setpoint, value, power_w, v_dq_v[2], to_rpm;
bool got_setpoint, param_valid, wants_bootloader_restart;
struct param_t param;
struct motor_state_t motor_state;
uint16_t foc_status_dtid;
UAVCANTransferManager broadcast_manager(node_id);
UAVCANTransferManager service_manager(node_id);
uavcan::equipment::esc::RawCommand raw_cmd;
uavcan::equipment::indication::BeepCommand beep_cmd;
uavcan::protocol::param::ExecuteOpcode::Request xo_req;
uavcan::protocol::param::GetSet::Request gs_req;
uavcan::protocol::RestartNode::Request rn_req;
uavcan::protocol::enumeration::Begin::Request enum_req;
foc_status_transfer_id = node_status_transfer_id =
esc_status_transfer_id = enumeration_indication_transfer_id = 0u;
broadcast_filter_id = service_filter_id = 0xFFu;
foc_status_time = node_status_time = esc_status_time =
last_setpoint_update = enumeration_deadline = 0u;
wants_bootloader_restart = false;
node_status_interval = 900u;
to_rpm = float(60.0 / M_PI) / float(motor_params.num_poles);
while (true) {
current_time = g_controller_state.time;
got_setpoint = false;
setpoint = 0.0f;
/* Allow UAVCAN parameters to be changed without restarting */
esc_index = (uint8_t)
configuration.get_param_value_by_index(PARAM_UAVCAN_ESC_INDEX);
foc_status_dtid = (uint16_t)
configuration.get_param_value_by_index(PARAM_THIEMAR_STATUS_ID);
esc_status_interval = (uint32_t)(
configuration.get_param_value_by_index(
PARAM_UAVCAN_ESCSTATUS_INTERVAL) * 0.001f);
foc_status_interval = (uint32_t)(
configuration.get_param_value_by_index(
PARAM_THIEMAR_STATUS_INTERVAL) * 0.001f);
/*
Check for UAVCAN commands (FIFO 0) -- these are all broadcasts
*/
while (can_rx(0u, &filter_id, &message_id, &length, message)) {
uavcan::TransferCRC crc;
/* Filter IDs are per-FIFO, so convert this back to the bank index */
filter_id = (uint8_t)(filter_id + UAVCAN_EQUIPMENT_ESC_RAWCOMMAND);
switch (filter_id) {
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND:
crc = uavcan::equipment::esc::RawCommand::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND:
crc = uavcan::equipment::indication::BeepCommand::getDataTypeSignature().toTransferCRC();
break;
default:
break;
}
broadcast_manager.receive_frame(current_time, message_id, crc,
length, message);
if (broadcast_manager.is_rx_done()) {
broadcast_filter_id = filter_id;
break;
}
}
if (broadcast_manager.is_rx_done()) {
if (broadcast_filter_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND &&
broadcast_manager.decode(raw_cmd) &&
esc_index < raw_cmd.cmd.size() &&
raw_cmd.cmd[esc_index] != 0) {
got_setpoint = true;
mode = CONTROLLER_VOLTAGE;
/* Scale 0-8191 to represent 0 to maximum voltage. */
value = g_vbus_v;
setpoint = /* std::min(motor_params.max_voltage_v, value) */
motor_params.max_voltage_v *
float(raw_cmd.cmd[esc_index]) *
float(1.0 / 8192.0);
if (setpoint > 0.0f &&
setpoint < motor_params.accel_voltage_v) {
setpoint = motor_params.accel_voltage_v;
}
if (setpoint < 0.0f &&
setpoint > -motor_params.accel_voltage_v) {
setpoint = -motor_params.accel_voltage_v;
}
} else if (broadcast_filter_id ==
UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND &&
broadcast_manager.decode(beep_cmd)) {
/* Set up audio generation */
g_audio_state.off_time = uint32_t(beep_cmd.duration *
(1.0f / hal_control_t_s));
g_audio_state.angular_velocity_rad_per_u =
float(2.0 * M_PI) * hal_control_t_s *
std::max(100.0f, beep_cmd.frequency);
g_audio_state.volume_v = motor_params.accel_voltage_v;
}
broadcast_manager.receive_acknowledge();
}
/*
Check for UAVCAN service requests (FIFO 1) -- only process if the
first byte of the data is the local node ID
*/
while (can_rx(1u, &filter_id, &message_id, &length, message)) {
uavcan::TransferCRC crc;
switch (filter_id) {
case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE:
crc = uavcan::protocol::param::ExecuteOpcode::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_PROTOCOL_PARAM_GETSET:
crc = uavcan::protocol::param::GetSet::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE:
crc = uavcan::protocol::file::BeginFirmwareUpdate::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_PROTOCOL_GETNODEINFO:
crc = uavcan::protocol::GetNodeInfo::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_PROTOCOL_RESTARTNODE:
crc = uavcan::protocol::RestartNode::getDataTypeSignature().toTransferCRC();
break;
case UAVCAN_PROTOCOL_ENUMERATION_BEGIN:
crc = uavcan::protocol::enumeration::Begin::getDataTypeSignature().toTransferCRC();
break;
default:
break;
}
service_manager.receive_frame(current_time, message_id, crc,
length, message);
if (service_manager.is_rx_done()) {
service_filter_id = filter_id;
break;
}
}
/*
Don't process service requests until the last service response is
completely sent, to avoid overwriting the TX buffer.
*/
if (service_manager.is_rx_done() && service_manager.is_tx_done()) {
if (service_filter_id == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE &&
service_manager.decode(xo_req)) {
/*
Return OK if the opcode is understood and the controller is
stopped, otherwise reject.
*/
uavcan::protocol::param::ExecuteOpcode::Response xo_resp;
xo_resp.ok = false;
if (g_controller_state.mode == CONTROLLER_STOPPED &&
xo_req.opcode == xo_req.OPCODE_SAVE) {
configuration.write_params();
xo_resp.ok = true;
} else if (g_controller_state.mode == CONTROLLER_STOPPED &&
xo_req.opcode == xo_req.OPCODE_ERASE) {
/*
Set all parameters to default values, then erase the flash
*/
configuration.reset_params();
configuration.write_params();
xo_resp.ok = true;
}
service_manager.encode_response<uavcan::protocol::param::ExecuteOpcode>(xo_resp);
} else if (service_filter_id == UAVCAN_PROTOCOL_PARAM_GETSET &&
service_manager.decode(gs_req)) {
uavcan::protocol::param::GetSet::Response resp;
if (!gs_req.name.empty()) {
param_valid = configuration.get_param_by_name(
param, gs_req.name.c_str());
} else {
param_valid = configuration.get_param_by_index(
param, (uint8_t)gs_req.index);
}
if (param_valid) {
if (param.public_type == PARAM_TYPE_FLOAT && !gs_req.name.empty() &&
gs_req.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
value = gs_req.value.to<uavcan::protocol::param::Value::Tag::real_value>();
configuration.set_param_value_by_index(param.index,
value);
} else if (param.public_type == PARAM_TYPE_INT && !gs_req.name.empty() &&
gs_req.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
value = (float)((int32_t)gs_req.value.to<uavcan::protocol::param::Value::Tag::integer_value>());
configuration.set_param_value_by_index(param.index,
value);
}
value = configuration.get_param_value_by_index(
param.index);
resp.name = (const char*)param.name;
if (param.public_type == PARAM_TYPE_FLOAT) {
resp.value.to<uavcan::protocol::param::Value::Tag::real_value>() = value;
resp.default_value.to<uavcan::protocol::param::Value::Tag::real_value>() = param.default_value;
resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = param.min_value;
resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = param.max_value;
} else if (param.public_type == PARAM_TYPE_INT) {
resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = (int32_t)value;
resp.default_value.to<uavcan::protocol::param::Value::Tag::integer_value>() = (int32_t)param.default_value;
resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = (int32_t)param.min_value;
resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = (int32_t)param.max_value;
}
}
service_manager.encode_response<uavcan::protocol::param::GetSet>(resp);
} else if (service_filter_id == UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE) {
uavcan::protocol::file::BeginFirmwareUpdate::Response resp;
/*
Don't actually need to decode since we don't care about the
request data
*/
if (g_controller_state.mode == CONTROLLER_STOPPED) {
resp.error = resp.ERROR_OK;
wants_bootloader_restart = true;
} else {
resp.error = resp.ERROR_INVALID_MODE;
}
service_manager.encode_response<uavcan::protocol::file::BeginFirmwareUpdate>(resp);
} else if (service_filter_id == UAVCAN_PROTOCOL_GETNODEINFO) {
uavcan::protocol::GetNodeInfo::Response resp;
/* Empty request so don't need to decode */
resp.status.uptime_sec = current_time / 1000u;
resp.status.health = g_controller_state.fault ?
resp.status.HEALTH_CRITICAL :
resp.status.HEALTH_OK;
resp.status.mode = resp.status.MODE_OPERATIONAL;
resp.status.sub_mode = 0u;
resp.status.vendor_specific_status_code =
(uint16_t)g_controller_state.mode;
resp.software_version.major =
flash_app_descriptor.major_version;
resp.software_version.minor =
flash_app_descriptor.minor_version;
resp.software_version.optional_field_flags =
resp.software_version.OPTIONAL_FIELD_FLAG_VCS_COMMIT |
resp.software_version.OPTIONAL_FIELD_FLAG_IMAGE_CRC;
resp.software_version.vcs_commit =
flash_app_descriptor.vcs_commit;
resp.software_version.image_crc =
flash_app_descriptor.image_crc;
resp.hardware_version.major = HW_VERSION_MAJOR;
resp.hardware_version.minor = HW_VERSION_MINOR;
/* Set the unique ID */
memset(resp.hardware_version.unique_id.begin(), 0u,
resp.hardware_version.unique_id.size());
memcpy(resp.hardware_version.unique_id.begin(),
(uint8_t*)0x1ffff7ac, 12u);
/* Set the hardware name */
resp.name = HW_UAVCAN_NAME;
service_manager.encode_response<uavcan::protocol::GetNodeInfo>(resp);
} else if (service_filter_id == UAVCAN_PROTOCOL_RESTARTNODE &&
service_manager.decode(rn_req)) {
uavcan::protocol::RestartNode::Response resp;
/*
Restart if the magic number is correct and the controller is
currently stopped, otherwise reject.
*/
if (g_controller_state.mode == CONTROLLER_STOPPED &&
rn_req.magic_number == rn_req.MAGIC_NUMBER) {
resp.ok = true;
wants_bootloader_restart = true;
} else {
resp.ok = false;
}
service_manager.encode_response<uavcan::protocol::RestartNode>(resp);
} else if (service_filter_id == UAVCAN_PROTOCOL_ENUMERATION_BEGIN &&
service_manager.decode(enum_req)) {
uavcan::protocol::enumeration::Begin::Response resp;
/*
We only support enumeration for the esc_index property, and
only while the controller is stopped.
*/
if (g_controller_state.mode != CONTROLLER_STOPPED) {
resp.error = resp.ERROR_INVALID_MODE;
} else if (enum_req.parameter_name != "esc_index") {
resp.error = resp.ERROR_INVALID_PARAMETER;
} else {
/* All fine, start enumeration */
resp.error = resp.ERROR_OK;
enumeration_deadline =
current_time + 1000 * enum_req.timeout_sec;
g_enumeration_active = true;
}
service_manager.encode_response<uavcan::protocol::enumeration::Begin>(resp);
}
service_manager.receive_acknowledge();
}
/* Transmit service responses if available */
if (can_is_ready(1u) &&
service_manager.transmit_frame(message_id, length, message)) {
can_tx(1u, message_id, length, message);
}
if (broadcast_manager.is_tx_done() && service_manager.is_tx_done() &&
!service_manager.is_rx_in_progress(current_time)) {
motor_state = const_cast<struct motor_state_t&>(g_motor_state);
v_dq_v[0] = g_v_dq_v[0];
v_dq_v[1] = g_v_dq_v[1];
if (foc_status_interval && current_time - foc_status_time >=
foc_status_interval) {
thiemar::equipment::esc::Status msg;
msg.i_dq[0] = motor_state.i_dq_a[0];
msg.i_dq[1] = motor_state.i_dq_a[1];
msg.i_setpoint = g_controller_state.current_setpoint;
msg.v_dq[0] = v_dq_v[0];
msg.v_dq[1] = v_dq_v[1];
msg.esc_index = esc_index;
broadcast_manager.encode_message(
foc_status_transfer_id++, foc_status_dtid, msg);
foc_status_time = current_time;
} else if (esc_status_interval &&
current_time - esc_status_time >= esc_status_interval) {
uavcan::equipment::esc::Status msg;
power_w = __VSQRTF(
(motor_state.i_dq_a[0] * motor_state.i_dq_a[0] +
motor_state.i_dq_a[1] * motor_state.i_dq_a[1]) *
(v_dq_v[0] * v_dq_v[0] + v_dq_v[1] * v_dq_v[1])
);
msg.voltage = g_vbus_v;
msg.current = power_w / g_vbus_v;
/*
If Q current has opposite sign to Q voltage, the flow is
reversed due to regenerative braking.
*/
if (motor_state.i_dq_a[1] * v_dq_v[1] < 0.0f) {
msg.current = -msg.current;
}
msg.temperature = 273.15f + hal_get_temperature_degc();
msg.rpm = int32_t(g_motor_state.angular_velocity_rad_per_s *
to_rpm);
msg.power_rating_pct = uint8_t(100.0f * power_w /
(motor_params.max_current_a *
motor_params.max_voltage_v));
msg.esc_index = esc_index;
broadcast_manager.encode_message(
esc_status_transfer_id++, msg);
esc_status_time = current_time;
} else if (current_time - node_status_time >=
node_status_interval) {
uavcan::protocol::NodeStatus msg;
msg.uptime_sec = current_time / 1000u;
msg.health = g_controller_state.fault ?
msg.HEALTH_CRITICAL :
msg.HEALTH_OK;
msg.mode = msg.MODE_OPERATIONAL;
msg.sub_mode = 0u;
msg.vendor_specific_status_code =
(uint16_t)g_controller_state.mode;
broadcast_manager.encode_message(
node_status_transfer_id++, msg);
node_status_time = current_time;
} else if (g_enumeration_active &&
std::abs(g_enumeration_velocity_rad_per_s) >
ENUMERATION_VELOCITY_THRESHOLD_RAD_PER_S) {
/*
Change rotation direction if the enumerated direction was
negative
*/
if (g_enumeration_velocity_rad_per_s < 0.0f) {
value = configuration.get_param_value_by_index(
PARAM_CONTROL_DIRECTION);
configuration.set_param_value_by_index(
PARAM_CONTROL_DIRECTION, value == 0.0f ? 1.0f : 0.0f);
}
uavcan::protocol::enumeration::Indication msg;
msg.parameter_name = "esc_index";
broadcast_manager.encode_message(
enumeration_indication_transfer_id++, msg);
/*
Reset enumeration velocity to avoid sending another message
straight away
*/
g_enumeration_active = false;
g_enumeration_velocity_rad_per_s = 0.0f;
}
}
/* Transmit broadcast CAN frames if available */
if (can_is_ready(0u) &&
broadcast_manager.transmit_frame(message_id, length, message)) {
can_tx(0u, message_id, length, message);
}
/*
Update the controller mode and setpoint, unless the motor is currently
stopping or enumeration is active.
*/
if (!g_controller_state.fault && got_setpoint &&
g_controller_state.mode != CONTROLLER_ABORTING &&
!g_enumeration_active) {
if (mode == CONTROLLER_VOLTAGE) {
g_controller_state.voltage_setpoint = setpoint;
g_controller_state.mode = CONTROLLER_VOLTAGE;
last_setpoint_update = current_time;
} else if ((g_controller_state.mode == CONTROLLER_STOPPED &&
motor_params.idle_speed_rad_per_s > 0.0f) ||
g_controller_state.mode == CONTROLLER_IDLING) {
/*
Setpoint was non-zero, but was not high enough to start
spinning and we're not already running -- so start idling, or
keep on idling.
*/
g_controller_state.mode = CONTROLLER_IDLING;
g_controller_state.voltage_setpoint = 0.0f;
last_setpoint_update = current_time;
}
} else if (g_controller_state.mode != CONTROLLER_STOPPED &&
g_controller_state.mode != CONTROLLER_STOPPING &&
g_controller_state.mode != CONTROLLER_ABORTING &&
current_time - last_setpoint_update > THROTTLE_TIMEOUT_MS) {
/*
Stop gracefully if the present command mode doesn't provide an
updated setpoint in the required period.
*/
g_controller_state.mode = CONTROLLER_STOPPING;
g_controller_state.voltage_setpoint = 0.0f;
}
/*
Update the Kv parameter with the latest estimate of phi -- we don't
really need to do this in the main loop, but it can't hurt.
*/
if (g_phi_v_s_per_rad > 0.0f &&
g_controller_state.mode == CONTROLLER_VOLTAGE) {
configuration.set_param_value_by_index(
PARAM_MOTOR_KV, to_rpm / g_phi_v_s_per_rad);
}
/* Stop enumeration if the deadline has expired */
if (enumeration_deadline < current_time) {
g_enumeration_active = false;
}
/*
Only restart into the bootloader if the acknowledgement message has
been sent, and we're otherwise unoccupied.
*/
if (g_controller_state.mode == CONTROLLER_STOPPED &&
broadcast_manager.is_tx_done() && can_is_ready(0u) &&
service_manager.is_tx_done() && can_is_ready(1u) &&
wants_bootloader_restart) {
g_controller_state.fault = true;
hal_restart();
}
}
}
/********************
Callback function for handling the incoming request_incremental_value CAN message
***********************/
void incoming_request_incremental_value(CAN_packet* p, unsigned char mob){
cli();
prepare_incremental_in_value_packet();
while(!can_tx(5, &incremental_in_value_packet));
sei();
}
/***** main loop *****/
int main(){
unsigned char i;
init_status = no_init;
volatile U8 led_toggle_counter = 0;
U8 reset_cause = MCUSR;
MCUSR = 0x00;
//red_LED_toggle();
//external crystal, no prescaler
CLKPR = 0x80; CLKPR = 0x00;
//prepare the incremental_in_value_packet
incremental_in_value_packet.id = 0x07;
incremental_in_value_packet.length = 6;
for (i = 0; i < incremental_in_value_packet.length; i++) {
incremental_in_value_packet.data[i] = 0;
}
//prepare the init_ack_async_packet
init_ack_sync_packet.id = 0x02;
init_ack_sync_packet.length = 1;
init_ack_sync_packet.data[0] = 0x14;
communication_init();
//EICRA = 0x0C; //INT1 pin triggers async interrupt on rising edge
//EICRB = 0xAA; //INT7-4 pins trigger sync interrupt on falling edge
//EIMSK = 0xF2; //INT7-4 and INT1 are enabled
//global interrupt enable
sei();
if (prepare_rx(1,0x01,0xFF,incoming_init)) red_LED_on(); //debug for incoming message
//TWBR = reset_cause;
update_DA_A(STOP_DA_VAL);
update_DA_B(STOP_DA_VAL);
update_DA_C(STOP_DA_VAL);
DA_LDAC_high();
DA_LDAC_low();
green_LED_on();
while(1){
if (init_status == init_2){
init_status = running;
prepare_rx(3, 0x05, 0xFF, incoming_stop);
prepare_rx(4, 0x06, 0xFF, incoming_DA_out_value);
prepare_rx(6, 0x0B, 0xFF, incoming_request_incremental_value);
prepare_rx(7, 0x0E, 0xFF, incoming_sync_command);
}
if (init_status == running) {
prepare_incremental_in_value_packet();
while(!can_tx(5, &incremental_in_value_packet)) {};
if ((++led_toggle_counter) == 0xFF) {
yellow_LED_toggle();
led_toggle_counter = 0;
}
}
}
return 0;
}
int main(void)
{
//USART init (mark)
USART0_Init(MYUBRR0);
fdevopen(uart_putc0, USART0_Receive);
printf("Hello World!\n");
//SPI_MasterInit();
//SPI_config for spi adc module ??? (mark)
DDR_SPI |= (1<<DD_MOSI) | (1<<DD_SCK) | (1<<DD_SS);
DDR_SPI &= ~(1<<DD_MISO);
PORT_SPI |= (1<<DD_SS);
SPCR = (1<<SPIE) | (1<<SPE) | (1<<MSTR);
//Can init with id 20. (mark)
can_init();
CAN_packet p;
p.id = ID_power_measure;
p.length = 4;
//Estop can message recv funcion setup -> id 1
prepare_rx(1, ID_e_stop, 0x7ff, receiveEStop);
//prepare_rx(2, MOTOR_STATUS_ID, 0x7ff, receiveMotorStatus);
//leds and e_stop relay
DDRD |= (1<<LED1) | (1<<LED2) | (1<<LED3);
DDRE |= (1<<E_STOP);
PORTD &= ~((1<<LED1) | (1<<LED2) | (1<<LED3));
PORTE &= ~(1<<E_STOP);
//ADC1 positive differential input, ADC0 negative differential input
//ADMUX = (1<<REFS0) | (1<<REFS1) | 0x10;
//Enable, start conversion, interrupt enable, prescaler 64 (8MHz/64 = 125KHz)
//ADCSRA |= (1<<ADEN) | (1<<ADSC) | (1<<ADIE) | (1<<ADPS2) | (1<<ADPS1);
TCCR1B |= (1<<WGM12) | (1<<CS12); //CTC mode, prescaler 256
TIMSK1 |= (1<<OCIE1A); //compare match A interrupt
OCR1A = 31249;
PORT_SPI &= ~(1<<DD_SS); //initate new reading
SPDR = 0x06; //single ended
spi_seq = 1;
spi_ch = 0;
sei();
uint32_t temp_voltage, temp_current_motor;
uint16_t temp_counter;
for (unsigned char i = 0; i < p.length; i++)
{
p.data[i] = i;
}
while(1)
{
if (timer_done == true)
{
timer_done = false;
//heartbeat (mark)
PORTD ^= (1<<LED1);
//saving sampled data (mark)
cli();
temp_voltage = voltage_ADC_sum;
temp_current_motor = current_motor_ADC_sum;
temp_counter = counter;
voltage_ADC_sum = 0;
current_motor_ADC_sum = 0;
counter = 0;
sei();
//Average and scale data (mark)
int voltage = temp_voltage*100/temp_counter/VOLTAGE_SCALE; //TODO chage to uint32_t ??
uint32_t currentM = ((uint32_t)(temp_current_motor/temp_counter) - CURRENT_MOTOR_ZERO + 5)/CURRENT_MOTOR_SCALE ;
//What is 455??? (mark)
if (voltage > 455)
{
PORTD |= (1<<LED3);
}
else
{
PORTD &= ~(1<<LED3);
}
//Preapere can message (mark)
p.data[0] = voltage>>8;
p.data[1] = voltage;
p.data[2] = currentM>>8;
p.data[3] = currentM;
//For debugging
printf("Average over %d samples:\n\r", temp_counter);
printf("Voltage: %d\n\r", voltage);
printf("Current motor: %lu\n\r", currentM);
//printf("Current without scaling: %lu\n\r", (uint32_t)(temp_current_motor/temp_counter));
//printf("calc: (%u/%d - %d + 5)/%d*100)\n\r", temp_current_motor, temp_counter, CURRENT_MOTOR_ZERO, CURRENT_MOTOR_SCALE);
//printf("Current one: %lu\n\r", current_motor_one_value);
//printf("CAn packet: %s\n", str);
can_tx(14, &p);
PORTD ^= (1<<LED1); //heartbeat (mark)
}
}
static uavcan_error_t uavcan_tx(uavcan_protocol_t *protocol, uint8_t *frame_data,
size_t length, uint8_t mailbox)
{
frame_data[length++] = protocol->tail_init.u8;
return can_tx(protocol->id.u32, length, frame_data, mailbox);
}
void canout()
{
can_tx(*addr,*dlc,data);
}
