void geo_send_heartbeat( void )
{
// Heart Beat to Master
can_msg_t heartbeat_geo_msg; // Can Message
bool can_status = false;
heartbeat_geo_msg.msg_id = GEO_HEARTBEAT_ID; // Geo Heartbeat ID
heartbeat_geo_msg.frame_fields.is_29bit = 0;
heartbeat_geo_msg.frame_fields.data_len = 0;
can_status = CAN_tx(GEO_CNTL_CANBUS, &heartbeat_geo_msg, GEO_CNTL_CAN_TIMEOUT);
if( !can_status )
{
LOG_ERROR("ERROR!!! Geo controller Heartbeat message not sent!!");
LE.off(GEO_HB_LED);
}
else
{
LE.toggle(GEO_HB_LED);
}
if(bus_off_flag) {
CAN_reset_bus(GEO_CNTL_CANBUS);
bus_off_flag = false;
}
}
void sensor_send_heartbeat( void )
{
// Heart Beat to Master
can_msg_t heartbeat_sensor_msg; // Can Message
bool can_status = false;
heartbeat_sensor_msg.msg_id = GEO_HEARTBEAT_ID; // Sensor Heartbeat ID
heartbeat_sensor_msg.frame_fields.is_29bit = 0;
heartbeat_sensor_msg.frame_fields.data_len = 0;
can_status = CAN_tx(SENSOR_CNTL_CANBUS, &heartbeat_sensor_msg, SENSOR_CNTL_CAN_TIMEOUT);
if( !can_status )
{
LOG_ERROR("ERROR!!! Sensor controller Heartbeat message not sent!!");
LE.off(SENSOR_HB_LED);
}
else
{
LE.toggle(SENSOR_HB_LED);
}
if(bus_off_state) {
CAN_reset_bus(SENSOR_CNTL_CANBUS);
bus_off_state = false;
}
}
// Send Motor_IO heartbeat to master
void motor_io_send_heartbeat( void )
{
// Heart Beat to Master
can_msg_t heartbeat_motor_io_msg; // Can Message
bool can_status = false;
heartbeat_motor_io_msg.msg_id = MOTORIO_HEARTBEAT_ID; // MOTORIO Heartbeat ID
heartbeat_motor_io_msg.frame_fields.is_29bit = 0;
heartbeat_motor_io_msg.frame_fields.data_len = 0;
can_status = CAN_tx(MOTORIO_CNTL_CANBUS, &heartbeat_motor_io_msg, MOTORIO_CNTL_CAN_TIMEOUT);
if( !can_status )
{
LOG_ERROR("ERROR!!! MotorIO controller Heartbeat message not sent!!");
LE.off(MOTORIO_HB_LED);
}
else
{
LE.toggle(MOTORIO_HB_LED);
}
if(bus_off_state) {
CAN_reset_bus(MOTORIO_CNTL_CANBUS);
bus_off_state = false;
}
}
void geo_send_gps()
{
can_msg_t gps_msg;
if(a == GEO_DATA_TO_SEND)
{
gps_msg.msg_id = GEO_LOC_DATA_ID;
gps_msg.frame_fields.is_29bit = 0;
gps_msg.frame_fields.data_len = sizeof(gps_data_dec);
memcpy(&gps_msg.data.qword, &gps_data_dec, sizeof(gps_data_dec));
CAN_tx(GEO_CNTL_CANBUS, &gps_msg, GEO_CNTL_CAN_TIMEOUT);
// printf("\nlat: %f, long: %f \n", gps_data_dec.latitude, gps_data_dec.longitude);
/*
if( !can_status )
{
LOG_ERROR("ERROR!!! Geo controller CAN message: GPS data not sent!!");
LE.on(GEO_CAN_ERR_LED);
}
else
LE.off(GEO_CAN_ERR_LED);
*/
}
if(a == GEO_DO_NOT_SEND)
{
geo_location dummy_gps_data;
dummy_gps_data.latitude = 0.0;
dummy_gps_data.longitude = 0.0;
gps_msg.msg_id = GEO_LOC_DATA_ID;
gps_msg.frame_fields.is_29bit = 0;
gps_msg.frame_fields.data_len = sizeof(dummy_gps_data);
memcpy(&gps_msg.data.qword, &dummy_gps_data, sizeof(dummy_gps_data));
CAN_tx(GEO_CNTL_CANBUS, &gps_msg, GEO_CNTL_CAN_TIMEOUT);
/*
if( !can_status )
{
LOG_ERROR("ERROR!!! Geo controller CAN message: GPS data not sent!!");
LE.on(GEO_CAN_ERR_LED);
}
else
LE.off(GEO_CAN_ERR_LED);
*/
}
}
void geo_send_heading()
{
can_msg_t geo_msg;
geo_spd_angle geo_data;
if(a == GEO_DATA_TO_SEND)
{
static imu& imu_handle = IMUInterface; // Handle to singleton IMU object
geo_data.bearing = calculateBearing(current_checkpoint_data); // put bearing here
geo_data.heading = imu_handle.getHeading();
#if 1
if(geo_data.heading < HEADING_OFFSET2 && geo_data.heading >= 0)
{
geo_data.heading = 360 - HEADING_OFFSET2 + (geo_data.heading);
}
else
{
geo_data.heading = geo_data.heading - HEADING_OFFSET2;
}
#endif
geo_data.speed = speed_gps;
geo_data.distance = (uint16_t)calculateDistance(current_checkpoint_data);
}
if(a == GEO_DO_NOT_SEND)
{
geo_data.bearing = 0; // put bearing here
geo_data.heading = 0;
geo_data.speed = 0;
geo_data.distance = 0;
}
geo_msg.msg_id = GEO_SPEED_ANGLE_ID;
geo_msg.frame_fields.is_29bit = 0;
geo_msg.frame_fields.data_len = sizeof(geo_data);
memcpy(&geo_msg.data.qword,&geo_data, sizeof(geo_data));
// CAN_Tx() will only time out when TX queue is full and that will only
// happen when CAN Bus turns off too long for us to empty the TX queue
CAN_tx(GEO_CNTL_CANBUS, &geo_msg, GEO_CNTL_CAN_TIMEOUT);
/*
if( !can_status )
{
LOG_ERROR("ERROR!!! Geo controller CAN message: IMU data not sent!!");
LE.on(GEO_CAN_ERR_LED);
}
else
LE.off(GEO_CAN_ERR_LED);
*/
}
bool transmit_data(can_msg_t send_message){
send_message.frame_fields.is_29bit = 0;
send_message.frame_fields.data_len = 8; // Send 8 bytes
if(test_data(send_message) || SAME_DATA_TRANSMISSION) {
if(false == CAN_tx(SENSOR_CNTL_CANBUS, &send_message, 0)) {
LOG_ERROR("CAN_tx failed\n");
return false;
}
else
return true;
}
else
return false;
}
// Transmit data over CAN
bool transmit_data(can_msg_t transmit_msg){
transmit_msg.frame_fields.is_29bit = 0;
transmit_msg.frame_fields.data_len = 8;
if(test_data(transmit_msg)) {
if(false == CAN_tx(MOTORIO_CNTL_CANBUS, &transmit_msg, 0)) {
//SET_ERROR(ERROR_TX_FAILED);
LOG_ERROR("CAN_tx failed\n");
return false;
}
else
return true;
}
else
return false;
}
bool CAN_test(void)
{
uint32_t i = 0;
#define can_test_msg(msg, id, rxtrue) do { \
u0_dbg_printf("Send ID: 0x%08X\n", id); \
msg.msg_id = id; \
CAN_ASSERT(CAN_tx(can1, &msg, 0)); \
msg.msg_id = 0; \
CAN_ASSERT(rxtrue == CAN_rx(can1, &msg, 10)); \
if (rxtrue) CAN_ASSERT(id == msg.msg_id); \
} while(0)
u0_dbg_printf(" Test init()\n");
CAN_ASSERT(!CAN_init(can_max, 100, 0, 0, NULL, NULL));
CAN_ASSERT(CAN_init(can1, 100, 5, 5, CAN_test_bufoff_cb, CAN_test_bufovr_cb));
CAN_ASSERT(LPC_CAN1->MOD == can_mod_reset);
CAN_bypass_filter_accept_all_msgs();
CAN_ASSERT(g_can_rx_qs[0] != NULL);
CAN_ASSERT(g_can_tx_qs[0] != NULL);
CAN_ASSERT(LPC_CANAF->SFF_sa == 0);
CAN_ASSERT(LPC_CANAF->SFF_GRP_sa == 0);
CAN_ASSERT(LPC_CANAF->EFF_sa == 0);
CAN_ASSERT(LPC_CANAF->EFF_GRP_sa == 0);
CAN_ASSERT(LPC_CANAF->ENDofTable == 0);
CAN_reset_bus(can1);
CAN_ASSERT(LPC_CAN1->MOD == can_mod_selftest);
/* Create a message, and test tx with bad input */
uint32_t id = 0x100;
can_msg_t msg;
memset(&msg, 0, sizeof(msg));
msg.frame = 0;
msg.msg_id = id;
msg.frame_fields.is_29bit = 0;
msg.frame_fields.data_len = 8;
msg.data.qword = 0x1122334455667788;
CAN_ASSERT(!CAN_tx(can_max, &msg, 0)); // Invalid CAN
CAN_ASSERT(!CAN_rx(can1, NULL, 0)); // Invalid message pointer
/* Send msg and test receive */
u0_dbg_printf(" Test Tx/Rx\n");
can_test_msg(msg, 0x100, true);
can_test_msg(msg, 0x200, true);
can_test_msg(msg, 0x300, true);
can_test_msg(msg, 0x400, true);
can_test_msg(msg, 0x500, true);
const can_std_id_t slist[] = { CAN_gen_sid(can1, 0x100), CAN_gen_sid(can1, 0x110), // 2 entries
CAN_gen_sid(can1, 0x120), CAN_gen_sid(can1, 0x130) // 2 entries
};
const can_std_grp_id_t sglist[] = { {CAN_gen_sid(can1, 0x200), CAN_gen_sid(can1, 0x210)}, // Group 1
{CAN_gen_sid(can1, 0x220), CAN_gen_sid(can1, 0x230)} // Group 2
};
const can_ext_id_t elist[] = { CAN_gen_eid(can1, 0x7500), CAN_gen_eid(can1, 0x8500)};
const can_ext_grp_id_t eglist[] = { {CAN_gen_eid(can1, 0xA000), CAN_gen_eid(can1, 0xB000)} }; // Group 1
/* Test filter setup */
u0_dbg_printf(" Test filter setup\n");
CAN_setup_filter(slist, 4, sglist, 2, elist, 2, eglist, 1);
/* We use offset of zero if 2 FullCAN messages are added, otherwise 4 if none were added above */
const uint8_t offset = 4;
CAN_ASSERT(LPC_CANAF->SFF_sa == 4 - offset);
CAN_ASSERT(LPC_CANAF->SFF_GRP_sa == 12 - offset);
CAN_ASSERT(LPC_CANAF->EFF_sa == 20 - offset);
CAN_ASSERT(LPC_CANAF->EFF_GRP_sa == 28 - offset);
CAN_ASSERT(LPC_CANAF->ENDofTable == 36 - offset);
for ( i = 0; i < 10; i++) {
u0_dbg_printf("%2i: 0x%08X\n", i, (uint32_t) LPC_CANAF_RAM->mask[i]);
}
/* Send a message defined in filter */
u0_dbg_printf(" Test filter messages\n");
msg.frame = 0;
msg.frame_fields.is_29bit = 0;
msg.frame_fields.data_len = 8;
msg.data.qword = 0x1122334455667788;
/* Test reception of messages defined in the filter */
u0_dbg_printf(" Test message reception according to filter\n");
msg.frame_fields.is_29bit = 0;
can_test_msg(msg, 0x100, true); // standard id
can_test_msg(msg, 0x110, true); // standard id
can_test_msg(msg, 0x120, true); // standard id
can_test_msg(msg, 0x130, true); // standard id
can_test_msg(msg, 0x200, true); // Start of standard ID group
can_test_msg(msg, 0x210, true); // Last of standard ID group
can_test_msg(msg, 0x220, true); // Start of standard ID group
can_test_msg(msg, 0x230, true); // Last of standard ID group
msg.frame_fields.is_29bit = 1;
can_test_msg(msg, 0x7500, true); // extended id
can_test_msg(msg, 0x8500, true); // extended id
can_test_msg(msg, 0xA000, true); // extended id group start
can_test_msg(msg, 0xB000, true); // extended id group end
u0_dbg_printf(" Test messages that should not be received\n");
/* Send a message not defined in filter */
msg.frame_fields.is_29bit = 0;
can_test_msg(msg, 0x0FF, false);
can_test_msg(msg, 0x111, false);
can_test_msg(msg, 0x131, false);
can_test_msg(msg, 0x1FF, false);
can_test_msg(msg, 0x211, false);
can_test_msg(msg, 0x21f, false);
can_test_msg(msg, 0x231, false);
msg.frame_fields.is_29bit = 1;
can_test_msg(msg, 0x7501, false);
can_test_msg(msg, 0x8501, false);
can_test_msg(msg, 0xA000-1, false);
can_test_msg(msg, 0xB000+1, false);
/* Test FullCAN */
u0_dbg_printf(" Test FullCAN\n");
CAN_init(can1, 100, 5, 5, CAN_test_bufoff_cb, CAN_test_bufovr_cb);
CAN_reset_bus(can1);
id = 0x100;
CAN_ASSERT(0 == CAN_fullcan_get_num_entries());
CAN_ASSERT(CAN_fullcan_add_entry(can1, CAN_gen_sid(can1, id), CAN_gen_sid(can1, id+1)));
CAN_ASSERT(2 == CAN_fullcan_get_num_entries());
CAN_ASSERT(LPC_CANAF->SFF_sa == 4);
CAN_ASSERT(LPC_CANAF->SFF_GRP_sa == 4);
CAN_ASSERT(LPC_CANAF->EFF_sa == 4);
CAN_ASSERT(LPC_CANAF->EFF_GRP_sa == 4);
CAN_ASSERT(LPC_CANAF->ENDofTable == 4);
CAN_ASSERT(CAN_fullcan_add_entry(can1, CAN_gen_sid(can1, id+2), CAN_gen_sid(can1, id+3)));
CAN_ASSERT(4 == CAN_fullcan_get_num_entries());
CAN_ASSERT(LPC_CANAF->SFF_sa == 8);
for ( i = 0; i < 3; i++) {
u0_dbg_printf("%2i: 0x%08X\n", i, (uint32_t) LPC_CANAF_RAM->mask[i]);
}
can_fullcan_msg_t *fc1 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id));
can_fullcan_msg_t *fc2 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id+1));
can_fullcan_msg_t *fc3 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id+2));
can_fullcan_msg_t *fc4 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id+3));
CAN_ASSERT((LPC_CANAF_RAM_BASE + LPC_CANAF->SFF_sa) == (uint32_t)fc1);
CAN_ASSERT((LPC_CANAF_RAM_BASE + LPC_CANAF->SFF_sa + 1*sizeof(can_fullcan_msg_t)) == (uint32_t)fc2);
CAN_ASSERT((LPC_CANAF_RAM_BASE + LPC_CANAF->SFF_sa + 2*sizeof(can_fullcan_msg_t)) == (uint32_t)fc3);
CAN_ASSERT((LPC_CANAF_RAM_BASE + LPC_CANAF->SFF_sa + 3*sizeof(can_fullcan_msg_t)) == (uint32_t)fc4);
can_fullcan_msg_t fc_temp;
CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc1, &fc_temp));
CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc2, &fc_temp));
CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc3, &fc_temp));
CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc4, &fc_temp));
/* Send message, see if fullcan captures it */
msg.frame = 0;
msg.msg_id = id;
msg.frame_fields.is_29bit = 0;
msg.frame_fields.data_len = 8;
#define can_test_fullcan_msg(fc, msg_copy, id) \
do { \
msg.msg_id = id; \
CAN_ASSERT(CAN_tx(can1, &msg, 0)); \
CAN_ASSERT(!CAN_rx(can1, &msg, 10)); \
CAN_ASSERT(CAN_fullcan_read_msg_copy(fc, &msg_copy)); \
CAN_ASSERT(fc->msg_id == id) \
} while(0)
can_test_fullcan_msg(fc1, fc_temp, id+0); CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc2, &fc_temp));
can_test_fullcan_msg(fc2, fc_temp, id+1); CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc3, &fc_temp));
can_test_fullcan_msg(fc3, fc_temp, id+2); CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc4, &fc_temp));
can_test_fullcan_msg(fc4, fc_temp, id+3); CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc1, &fc_temp));
u0_dbg_printf(" \n--> All tests successful! <--\n");
return true;
}
bool power_up_sync_geo_controller( void )
{
can_msg_t geo_sync_msg;
can_fullcan_msg_t *geo_syncack_msg_ptr = NULL;
can_fullcan_msg_t geo_syncack_msg_copy;
uint8_t sync_miss_count = 0;
bool sync_ack = false;
bool status = false;
master_sync* sync_msg;
SoftTimer can_rx_timer(GEO_CNTL_SYNC_TIME);
geo_syncack_msg_ptr = CAN_fullcan_get_entry_ptr(can_id_sync_ack);
if( geo_syncack_msg_ptr == NULL )
{
LOG_ERROR("ERROR!!! Cannot get FullCan msg pointer for Geo controller power-up sync\n");
return false;
}
geo_sync_msg.msg_id = GEO_SYNC_ID;
geo_sync_msg.frame_fields.is_29bit = 0;
geo_sync_msg.frame_fields.data_len = 0; // No data
geo_sync_msg.data.qword = 0;
do
{
// Send sync message
status = CAN_tx(GEO_CNTL_CANBUS, &geo_sync_msg, GEO_CNTL_CAN_TIMEOUT);
if( !status )
{
LOG_ERROR("ERROR!!! Unable to send Geo controller sync message\n");
LE.on(GEO_CAN_ERR_LED);
}
else
LE.off(GEO_CAN_ERR_LED);
// No need to delay here
// XXX: Cannot use FreeRTOS functions until the OS runs
// vTaskDelayMs(GEO_CNTL_SYNC_TIME);
can_rx_timer.restart();
while( !can_rx_timer.expired() );
// Check for ack
status = CAN_fullcan_read_msg_copy( geo_syncack_msg_ptr, &geo_syncack_msg_copy );
// XXX: Since OS is not up, maybe you can use SoftTime (polling timer)
if( status )
{
// We have a new message. Check if Geo is acked
// XXX: Suggest "shared" structures rather than memcpy
//memcpy((void *)&sync_msg, (void *)&geo_syncack_msg_copy.data.qword, sizeof(sync_msg));
sync_msg = (master_sync *)&geo_syncack_msg_copy.data.bytes[0];
if( sync_msg->ack_geo )
sync_ack = true;
}
sync_miss_count++;
} while( ( sync_ack == false ) ); // try forever
#if 0
if( !sync_ack )
LOG_ERROR("ERROR!!! Sync timeout from Geo controller. Stalling!!!\n");
#endif
return sync_ack;
}
bool CAN_test(void)
{
#define CAN_TEST_RX 0
CAN_ASSERT(!CAN_init(can_inv, 100, 0, 0, NULL, NULL));
CAN_ASSERT(CAN_init(can1, 100, 0, 0, CAN_test_bufoff_cb, CAN_test_bufovr_cb));
CAN_ASSERT(LPC_CAN1->MOD == can_mod_reset);
CAN_ASSERT(g_can_rx_qs[0] != NULL);
CAN_ASSERT(g_can_tx_qs[0] != NULL);
CAN_ASSERT(LPC_CANAF->SFF_sa == 0);
CAN_ASSERT(LPC_CANAF->SFF_GRP_sa == 0);
CAN_ASSERT(LPC_CANAF->EFF_sa == 0);
CAN_ASSERT(LPC_CANAF->EFF_GRP_sa == 0);
CAN_ASSERT(LPC_CANAF->ENDofTable == 0);
CAN_reset_bus(can1);
CAN_ASSERT(LPC_CAN1->MOD == can_mod_normal);
/* Send 11-bit CANID message */
uint32_t id = 0x100;
can_msg_t msg;
msg.frame = 0;
msg.msg_id = id;
msg.frame_fields.is_29bit = 0;
msg.frame_fields.data_len = 7;
msg.data.qword = 0x1122334455667788;
CAN_ASSERT(!CAN_tx(can_inv, &msg, 0));
CAN_ASSERT(!CAN_rx(can1, &msg, 0));
/* Send msg and test receive */
CAN_ASSERT(CAN_tx(can1, &msg, 0));
#if CAN_TEST_RX
memset(&msg, 0, sizeof(msg));
CAN_ASSERT(CAN_rx(can1, &msg, 1000));
CAN_ASSERT(msg.msg_id = id);
CAN_ASSERT(!msg.frame_fields.is_29bit);
#endif
/* Test filters */
CAN_ASSERT(0 == CAN_fullcan_get_num_entries());
CAN_ASSERT(CAN_fullcan_add_entry(can1, CAN_gen_sid(can1, id), CAN_gen_sid(can1, id+1)));
CAN_ASSERT(2 == CAN_fullcan_get_num_entries());
CAN_ASSERT(4 == LPC_CANAF->SFF_sa);
can_fullcan_msg_t *fc1 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id));
can_fullcan_msg_t *fc2 = CAN_fullcan_get_entry_ptr(CAN_gen_sid(can1, id+1));
CAN_ASSERT(0 != fc1);
CAN_ASSERT(0 != fc2);
#if CAN_TEST_RX
/* Send message, see if fullcan captures it */
msg.frame = 0;
msg.msg_id = id;
msg.frame_fields.is_29bit = 0;
msg.frame_fields.data_len = 8;
can_fullcan_msg_t fc_temp;
CAN_ASSERT(!CAN_fullcan_read_msg_copy(fc1, &fc_temp));
CAN_ASSERT(CAN_tx(can1, &msg, 0));
CAN_ASSERT(CAN_fullcan_read_msg_copy(fc1, &fc_temp));
CAN_ASSERT(fc1->msg_id == id);
#endif
const can_std_id_t slist[] = { CAN_gen_sid(can1, 0x100), CAN_gen_sid(can1, 0x110), // 2 entries
CAN_gen_sid(can1, 0x120), CAN_gen_sid(can1, 0x130) // 2 entries
};
const can_std_grp_id_t sglist[] = { {CAN_gen_sid(can1, 0x150), CAN_gen_sid(can1, 0x200)}, // Group 1
{CAN_gen_sid(can2, 0x300), CAN_gen_sid(can2, 0x400)} // Group 2
};
const can_ext_id_t *elist = NULL; // Not used, so set it to NULL
const can_ext_grp_id_t eglist[] = { {CAN_gen_eid(can1, 0x3500), CAN_gen_eid(can1, 0x4500)} }; // Group 1
/* Test filter setup */
CAN_setup_filter(slist, 4, sglist, 2, elist, 0, eglist, 1);
CAN_ASSERT(2 == CAN_fullcan_get_num_entries());
CAN_ASSERT(LPC_CANAF->SFF_sa == 4);
CAN_ASSERT(LPC_CANAF->SFF_GRP_sa == 12);
CAN_ASSERT(LPC_CANAF->EFF_sa == 20);
CAN_ASSERT(LPC_CANAF->EFF_GRP_sa == 20);
CAN_ASSERT(LPC_CANAF->ENDofTable == 28);
/* Send a message defined in filter */
/* Send a message not defined in filter */
return true;
}
bool bluetooth_controller_sync()
{
bool can_init_stat = false;
bool sync_tx_status = false;
bool sync_ack_status = true;
bool status = false;
SoftTimer can_rx_timer(BT_SYNC_TIME);
//Initialize can bus for sync frame transmission
can_init_stat = CAN_init(can_controller, can_baud_kbps, can_rx_queue_size, can_tx_queue_size, bus_off_cb, data_ovr_cb);
PRINT("can init started\n");
master_sync *ref_obj;
if(!can_init_stat)
{
LOG_ERROR("Bluetooth controller CAN bus could not be initialiazed");
return can_init_stat;
}
else
PRINT("\ncan init successful");
const can_std_id_t slist[] = {CAN_gen_sid(can_controller, MASTER_SYNC_ACK_ID), CAN_gen_sid(can_controller, CHECKPOINT_REQ_ID), CAN_gen_sid(can_controller, GEO_LOC_DATA_ID), CAN_gen_sid(can_controller, 0xFFFF)};
CAN_setup_filter(slist, 4, NULL, 0, NULL, 0, NULL, 0);
//CAN_bypass_filter_accept_all_msgs();
CAN_reset_bus(can_controller);
tx_mssg.msg_id = BLUETOOTH_SYNC_ID;
tx_mssg.frame_fields.data_len = 0;
tx_mssg.frame_fields.is_29bit = 0;
tx_mssg.data.qword = 0;
do
{
PRINT("\n can tx status = %d",sync_tx_status);
sync_tx_status = CAN_tx(can_controller, &tx_mssg,BT_CNTRL_TIMEOUT_CAN);
PRINT("\ncan tx started");
PRINT("\n can tx status = %d",sync_tx_status);
if(sync_tx_status)
{
PRINT("\ncan tx successful no errors");
}
else
{
LOG_ERROR("Bluetooth can message cannot be sent");
PRINT("\ncan tx unsuccessfull");
}
can_rx_timer.restart();
while(!can_rx_timer.expired());
status = CAN_rx(can_controller,&master_rx_mssg, BT_CNTRL_TIMEOUT_CAN);
PRINT("\ncan rx started");
if(status)
{
ref_obj = (master_sync*)&(master_rx_mssg.data.bytes[0]);
PRINT("\ncan rx successful");
if((master_rx_mssg.msg_id == MASTER_SYNC_ACK_ID) && (ref_obj->ack_bluetooth == 1))
sync_ack_status = true;
}
}while(sync_ack_status == false);
PRINT("\nack received successful\n");
return sync_ack_status;
}
// PowerUp Sync function for Sensor Controller
bool powerup_sync_sensor_controller( void )
{
can_msg_t sensor_sync_msg;
uint8_t sync_miss_count = 0;
master_sync* sync_msg;
bool sync_ack = false;
bool status = false;
SoftTimer can_rx_timer(SENSOR_CNTL_SYNC_TIME);
sensor_sync_msg.msg_id = SENSOR_SYNC_ID;
sensor_sync_msg.frame_fields.is_29bit = 0;
sensor_sync_msg.frame_fields.data_len = 0; // No data
sensor_sync_msg.data.qword = 0;
do
{
// Send sync message
status = CAN_tx(SENSOR_CNTL_CANBUS, &sensor_sync_msg, SENSOR_CNTL_CAN_TIMEOUT);
if( !status )
{
LOG_ERROR("ERROR!!! Unable to send SENSOR controller sync message\n");
LE.on(SENSOR_CAN_ERR_LED);
}
else
LE.off(SENSOR_CAN_ERR_LED);
// No need to delay here
// XXX: Cannot use FreeRTOS functions until the OS runs
// vTaskDelayMs(SENSOR_CNTL_SYNC_TIME);
can_rx_timer.restart();
while( !can_rx_timer.expired() );
// Check for ack
status = CAN_rx(SENSOR_CNTL_CANBUS, &received_msg, 0);
// XXX: Since OS is not up, maybe you can use SoftTime (polling timer)
if( status )
{
// We have a new message. Check if Sensor is acked
// XXX: Suggest "shared" structures rather than memcpy
sync_msg = (master_sync *)&received_msg.data.bytes[0];
if( sync_msg->ack_geo )
sync_ack = true;
}
sync_miss_count++;
}while( ( sync_ack == false ) ); // try forever
#if 0
if( !sync_ack )
LOG_ERROR("ERROR!!! Sync timeout from Sensor controller. Stalling!!!\n");
#endif
return sync_ack;
}
