/*****************************************************************************
* Halts execution and cleans up all state (terminates threads, etc)
*****************************************************************************/
int
cleanup(thread_manager_t *manager, int priority) {
int i;
void *value;
// Send a terminate message to the dispatcher thread
for( i = 0; i < manager->num_threads * 10; i++ ) {
enqueue_block(&manager->queue, priority, NULL);
}
// Wait for the SPU_thread to complete execution
for( i = 0; i < manager->num_threads; i++ ) {
// Join PPU thread presumably the one that got the message
if( pthread_join(manager->threads[i], &value) ) {
perror( "Failed to join thread" );
exit(1);
}
if( pthread_join(manager->callBack_threads[i], &value) ) {
perror( "Failed to join thread" );
exit(1);
}
}
if( pthread_mutex_destroy(&manager->mutex) ) {
perror("Destroying thread mutex");
}
// Free the thread array
free( manager->threads );
return(0);
}
// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
bool DataFlash_MAVLink::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
{
if (!WritesOK()) {
return false;
}
if (!semaphore->take_nonblocking()) {
dropped++;
return false;
}
if (! WriteBlockCheckStartupMessages()) {
semaphore->give();
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
semaphore->give();
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == nullptr) {
_current_block = next_block();
if (_current_block == nullptr) {
// should not happen - there's a sanity check above
internal_error();
semaphore->give();
return false;
}
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = remaining_space_in_current_block();
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
//block full, mark it to be sent:
enqueue_block(_blocks_pending, _current_block);
_current_block = next_block();
}
}
semaphore->give();
return true;
}
void DataFlash_MAVLink::handle_retry(uint32_t seqno)
{
if (!_initialised || !_sending_to_client) {
return;
}
struct dm_block *victim = dequeue_seqno(_blocks_sent, seqno);
if (victim != NULL) {
_last_response_time = hal.scheduler->millis();
enqueue_block(_blocks_retry, victim);
}
}
/* Write a block of data at current offset */
bool DataFlash_MAVLink::WriteBlock(const void *pBuffer, uint16_t size)
{
if (!_initialised || !_sending_to_client || !_writes_enabled) {
return false;
}
if (! WriteBlockCheckPrefaceMessages()) {
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == NULL) {
_current_block = next_block();
if (_current_block == NULL) {
// should not happen - there's a sanity check above
internal_error();
return false;
}
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = remaining_space_in_current_block();
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
//block full, mark it to be sent:
enqueue_block(_blocks_pending, _current_block);
_current_block = next_block();
}
}
if (!_writing_preface_messages) {
// push_log_blocks();
}
return true;
}
/* while we "successfully" send log blocks from a queue, move them to
* the sent list. DO NOT call this for blocks already sent!
*/
bool DataFlash_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
{
uint8_t sent_count = 0;
while (queue.oldest != NULL) {
if (sent_count++ > _max_blocks_per_send_blocks) {
return false;
}
if (! send_log_block(*queue.oldest)) {
return false;
}
queue.sent_count++;
struct DataFlash_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
if (tmp != NULL) { // should never be NULL
enqueue_block(_blocks_sent, tmp);
} else {
internal_error();
}
}
return true;
}
/*****************************************************************************
* Adds a job to the priority queue
*****************************************************************************/
int
addjob( thread_manager_t *manager, int priority,
thread_manager_function_t job, void *arg,
thread_manager_function_t callback, void *udata ) {
task_data_t *tag = (task_data_t *)malloc(sizeof(task_data_t));
if( tag == NULL ) {
perror("Allocating RAM for SPU process entry");
return 1;
}
tag->job = job;
tag->arg = arg;
tag->priority = priority;
tag->callback = callback;
tag->tag = udata;
// Get wall-clock time for job enquement
clock_gettime(CLOCK_REALTIME, &tag->enqueued);
enqueue_block(&manager->queue, priority, tag);
return 0;
}
int main(void){
configure_nvic();
initialize_i2c(I2C_BASE_ADDRESS + read_i2c_address());
// Configure all of the hardware and internal state
initialize_motion_queue();
initialize_parser();
reset_parameters();
initialize_stepper_state();
set_microstepping(DEFAULT_MICROSTEPPING);
reset_hardware();
float_sync_line();
while(1){
if(parser.status == PARSER_ERR){
send_response(IMC_RSP_UNKNOWN,0);
initialize_parser();
continue;
}
if(parser.status == PARSER_NEW_EVENT){
switch(parser.packet_type){
case IMC_MSG_INITIALIZE:
initialize_motion_queue();
initialize_parser();
// Unlike out first initialization round, don't reset parameters
initialize_stepper_state();
float_sync_line();
response.init.slave_hw_ver = 0;
response.init.slave_fw_ver = 0;
response.init.queue_depth = MOTION_QUEUE_LENGTH;
send_response(IMC_RSP_OK,sizeof(rsp_initialize_t));
break;
case IMC_MSG_GETPARAM:
handle_get_parameter(&parser.packet.get_param, &response.param);
send_response(IMC_RSP_OK,sizeof(rsp_get_param_t));
break;
case IMC_MSG_SETPARAM:
handle_set_parameter(&parser.packet.set_param);
send_response(IMC_RSP_OK,0);
break;
case IMC_MSG_QUEUEMOVE:
{
int space = enqueue_block(&parser.packet.move);
send_response(space < 0 ? IMC_RSP_QUEUEFULL : IMC_RSP_OK,0);
// If we're adding moves in idle state, make sure that the sync interface is listening
if(st.state == STATE_IDLE)
enable_sync_interrupt();
}
break;
case IMC_MSG_STATUS:
response.status.queued_moves = queue_length();
if(st.state == STATE_ERROR){
response.status.status = parameters.error_low;
}else{
response.status.status = IMC_ERR_NONE;
}
send_response(IMC_RSP_OK,sizeof(rsp_status_t));
break;
case IMC_MSG_HOME:
send_response(IMC_RSP_OK,0);
enter_homing_routine();
break;
case IMC_MSG_QUICKSTOP:
send_response(IMC_RSP_ERROR,0);
break;
}
initialize_parser();
}
}
}