void dc_check_error_callbacks(void) {
const uint16_t external_voltage = dc_get_external_voltage();
const uint16_t stack_voltage = dc_get_stack_voltage();
// Under Voltage if external voltage is below minimum voltage (regardless
// of stack voltage), or if external voltage is zero and stack voltage is
// below minimum voltage
if((dc_message_tick_counter % 1000 == 0 && dc_enabled) &&
((external_voltage > DC_VOLTAGE_EPSILON &&
external_voltage < dc_minimum_voltage) ||
(external_voltage < DC_VOLTAGE_EPSILON &&
stack_voltage > DC_VOLTAGE_EPSILON &&
stack_voltage < dc_minimum_voltage))) {
UnderVoltageCallback uvs;
com_make_default_header(&uvs, com_info.uid, sizeof(UnderVoltageCallback), FID_UNDER_VOLTAGE);
uvs.voltage = external_voltage < DC_VOLTAGE_EPSILON ? stack_voltage : external_voltage;
send_blocking_with_timeout(&uvs,
sizeof(UnderVoltageCallback),
com_info.current);
led_on(LED_STD_RED);
// If there is no under voltage, we are currently enabled and the
// status flag is low: There is a short-circuit or over-temperature
// -> Emergency Shutdown
} else if(!PIO_Get(&pin_status_flag) &&
dc_enabled &&
dc_mode == DC_MODE_DRIVE_BRAKE) {
dc_emergency_shutdown_counter++;
// Wait for DC_MAX_EMERGENCY_SHUTDOWN ms until callback is emitted
if(dc_emergency_shutdown_counter >= DC_MAX_EMERGENCY_SHUTDOWN) {
EmergencyShutdownCallback ess;
com_make_default_header(&ess, com_info.uid, sizeof(EmergencyShutdownCallback), FID_EMERGENCY_SHUTDOWN);
send_blocking_with_timeout(&ess,
sizeof(EmergencyShutdownCallback),
com_info.current);
dc_disable();
dc_emergency_shutdown_counter = 0;
dc_led_error_reason |= DC_LED_ERROR_SHUTDOWN;
led_on(LED_STD_RED);
}
} else {
dc_emergency_shutdown_counter = 0;
if(!(dc_led_error_reason & DC_LED_ERROR_SHUTDOWN) &&
(dc_tick_counter % 1000 == 0)) {
led_off(LED_STD_RED);
}
}
}
void dc_velocity_reached_callback(void) {
VelocityReachedCallback vrs;
com_make_default_header(&vrs, com_info.uid, sizeof(VelocityReachedCallback), FID_VELOCITY_REACHED);
vrs.velocity = dc_velocity/DC_VELOCITY_MULTIPLIER;
send_blocking_with_timeout(&vrs,
sizeof(VelocityReachedCallback),
com_info.current);
}
bool chibi_add_enumerate_connected_request(void) {
__disable_irq();
if(chibi_buffer_size_recv != 0) {
__enable_irq();
return false;
}
com_make_default_header(chibi_buffer_recv, 0, sizeof(Enumerate), FID_CREATE_ENUMERATE_CONNECTED);
chibi_buffer_size_recv = sizeof(Enumerate);
__enable_irq();
return true;
}
void make_brick_enumerate(EnumerateCallback *ec) {
com_make_default_header(ec, com_info.uid, sizeof(EnumerateCallback), FID_ENUMERATE_CALLBACK);
memset(ec->uid, '\0', UID_STR_MAX_LENGTH);
uid_to_serial_number(com_info.uid, ec->uid);
strncpy(ec->connected_uid, NO_CONNECTED_UID_STR, NO_CONNECTED_UID_STR_LENGTH);
ec->position = '0';
ec->version_fw[0] = BRICK_FIRMWARE_VERSION_MAJOR;
ec->version_fw[1] = BRICK_FIRMWARE_VERSION_MINOR;
ec->version_fw[2] = BRICK_FIRMWARE_VERSION_REVISION;
ec->version_hw[0] = brick_hardware_version[0];
ec->version_hw[1] = brick_hardware_version[1];
ec->version_hw[2] = brick_hardware_version[2];
ec->device_identifier = BRICK_DEVICE_IDENTIFIER;
}
void dc_current_velocity_callback(void) {
// Only emit callback if velocity has changed since last callback
if(dc_last_callback_velocity == dc_velocity /*&& !encoder_enabled*/) {
return;
}
dc_last_callback_velocity = dc_velocity;
CurrentVelocityCallback cvs;
com_make_default_header(&cvs, com_info.uid, sizeof(CurrentVelocityCallback), FID_CURRENT_VELOCITY);
cvs.velocity = dc_velocity/DC_VELOCITY_MULTIPLIER;
send_blocking_with_timeout(&cvs,
sizeof(CurrentVelocityCallback),
com_info.current);
}
void make_bricklet_enumerate(EnumerateCallback *ec, const uint8_t bricklet) {
com_make_default_header(ec, bs[bricklet].uid, sizeof(EnumerateCallback), FID_ENUMERATE_CALLBACK);
memset(ec->uid, '\0', UID_STR_MAX_LENGTH);
uid_to_serial_number(bs[bricklet].uid, ec->uid);
memset(ec->connected_uid, '\0', UID_STR_MAX_LENGTH);
uid_to_serial_number(com_info.uid, ec->connected_uid);
ec->position = 'a' + bricklet;
ec->version_fw[0] = bs[bricklet].firmware_version[0];
ec->version_fw[1] = bs[bricklet].firmware_version[1];
ec->version_fw[2] = bs[bricklet].firmware_version[2];
ec->version_hw[0] = bs[bricklet].hardware_version[0];
ec->version_hw[1] = bs[bricklet].hardware_version[1];
ec->version_hw[2] = bs[bricklet].hardware_version[2];
ec->device_identifier = bs[bricklet].device_identifier;
}
void make_bricklet_enumerate(EnumerateCallback *ec, const uint8_t bricklet) {
com_make_default_header(ec, bs[bricklet].uid, sizeof(EnumerateCallback), FID_ENUMERATE_CALLBACK);
memset(ec->uid, '\0', UID_STR_MAX_LENGTH);
uid_to_serial_number(bs[bricklet].uid, ec->uid);
memset(ec->connected_uid, '\0', UID_STR_MAX_LENGTH);
uid_to_serial_number(com_info.uid, ec->connected_uid);
ec->position = 'a' + bricklet;
ec->version_fw[0] = bs[bricklet].firmware_version[0];
ec->version_fw[1] = bs[bricklet].firmware_version[1];
ec->version_fw[2] = bs[bricklet].firmware_version[2];
ec->version_hw[0] = bs[bricklet].hardware_version[0];
ec->version_hw[1] = bs[bricklet].hardware_version[1];
ec->version_hw[2] = bs[bricklet].hardware_version[2];
ec->device_identifier = bs[bricklet].device_identifier;
if(bricklet_attached[bricklet] == BRICKLET_INIT_CO_MCU) {
// A co mcu Bricklet does the initial enumeration itself
ec->device_identifier = 0;
}
}
void routing_table_create_stack(void) {
uint8_t stack_address = 0;
uint8_t tries = 0;
while(stack_address < SPI_ADDRESS_MAX) {
StackEnumerate se;
com_make_default_header(&se, 0, sizeof(StackEnumerate), FID_STACK_ENUMERATE);
uint32_t options = stack_address + 1;
if(spi_stack_send(&se, sizeof(StackEnumerate), &options) != 0) {
spi_stack_select(stack_address + 1);
tries = 0;
while(!spi_stack_master_transceive() && tries < 10) {
SLEEP_MS(50);
tries++;
}
if(tries == 10) {
break;
}
spi_stack_deselect();
spi_stack_buffer_size_recv = 0;
}
StackEnumerateReturn ser;
tries = 0;
while(tries < 10) {
SLEEP_MS(50);
spi_stack_select(stack_address + 1);
spi_stack_master_transceive();
spi_stack_deselect();
if(spi_stack_recv(&ser, sizeof(StackEnumerateReturn), NULL)) {
break;
}
tries++;
}
if(tries == 10) {
logspise("Did not receive answer for Stack Enumerate\n\r");
break;
}
stack_address++;
com_info.last_stack_address = stack_address;
for(uint8_t i = 0; i < STACK_ENUMERATE_MAX_UIDS; i++) {
if(ser.uids[i] != 0) {
if(routing_table_size >= ROUTING_TABLE_MAX_SIZE) {
break;
}
routing_table[routing_table_size].uid = ser.uids[i];
routing_table[routing_table_size].route_to.to = ROUTING_STACK;
routing_table[routing_table_size].route_to.option = stack_address;
routing_table_last_stack = routing_table_size;
routing_table_size++;
logspisi("New Stack participant (sa, uid): %d, %lu\n\r", stack_address, ser.uids[i]);
}
}
}
spi_stack_buffer_size_send = 0;
}
void make_period_callback(const uint8_t type) {
imu_period_counter[type] -= imu_period[type];
switch(type) {
case IMU_PERIOD_TYPE_ACC: {
AccelerationCallback ac;
com_make_default_header(&ac, com_info.uid, sizeof(AccelerationCallback), FID_ACCELERATION);
ac.x = sensor_data.acc_x;
ac.y = sensor_data.acc_y;
ac.z = sensor_data.acc_z;
send_blocking_with_timeout(&ac,
sizeof(AccelerationCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_MAG: {
MagneticFieldCallback mfc;
com_make_default_header(&mfc, com_info.uid, sizeof(MagneticFieldCallback), FID_MAGNETIC_FIELD);
mfc.x = sensor_data.mag_x;
mfc.y = sensor_data.mag_y;
mfc.z = sensor_data.mag_z;
send_blocking_with_timeout(&mfc,
sizeof(MagneticFieldCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_ANG: {
AngularVelocityCallback avc;
com_make_default_header(&avc, com_info.uid, sizeof(AngularVelocityCallback), FID_ANGULAR_VELOCITY);
avc.x = sensor_data.gyr_x;
avc.y = sensor_data.gyr_y;
avc.z = sensor_data.gyr_z;
send_blocking_with_timeout(&avc,
sizeof(AngularVelocityCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_TMP: {
TemperatureCallback tc;
com_make_default_header(&tc, com_info.uid, sizeof(AngularVelocityCallback), FID_TEMPERATURE);
tc.temperature = sensor_data.temperature;
send_blocking_with_timeout(&tc,
sizeof(TemperatureCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_ORI: {
OrientationCallback oc;
com_make_default_header(&oc, com_info.uid, sizeof(OrientationCallback), FID_ORIENTATION);
oc.roll = sensor_data.eul_roll;
oc.pitch = sensor_data.eul_pitch;
oc.heading = sensor_data.eul_heading;
send_blocking_with_timeout(&oc,
sizeof(OrientationCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_LIA: {
LinearAccelerationCallback lac;
com_make_default_header(&lac, com_info.uid, sizeof(LinearAccelerationCallback), FID_LINEAR_ACCELERATION);
lac.x = sensor_data.lia_x;
lac.y = sensor_data.lia_y;
lac.z = sensor_data.lia_z;
send_blocking_with_timeout(&lac,
sizeof(LinearAccelerationCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_GRV: {
GravityVectorCallback gvc;
com_make_default_header(&gvc, com_info.uid, sizeof(GravityVectorCallback), FID_GRAVITY_VECTOR);
gvc.x = sensor_data.grv_x;
gvc.y = sensor_data.grv_y;
gvc.z = sensor_data.grv_z;
send_blocking_with_timeout(&gvc,
sizeof(GravityVectorCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_QUA: {
QuaternionCallback qc;
com_make_default_header(&qc, com_info.uid, sizeof(QuaternionCallback), FID_QUATERNION);
qc.x = sensor_data.qua_x;
qc.y = sensor_data.qua_y;
qc.z = sensor_data.qua_z;
qc.w = sensor_data.qua_w;
send_blocking_with_timeout(&qc,
sizeof(QuaternionCallback),
com_info.current);
break;
}
case IMU_PERIOD_TYPE_ALL: {
AllDataCallback adc;
com_make_default_header(&adc, com_info.uid, sizeof(AllDataCallback), FID_ALL_DATA);
memcpy(&adc.acceleration, &sensor_data, sizeof(SensorData));
send_blocking_with_timeout(&adc,
sizeof(AllDataCallback),
com_info.current);
break;
}
}
}
