Ejemplo n.º 1
0
/*****************************************************************************
 * 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);
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 6
0
/*****************************************************************************
 * 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;
}
Ejemplo n.º 7
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();
    }
  }
}