void handle_clear_command_queue_order()
{
generate_response_start(RSP_APPLICATION_ERROR, 1);
if (CommandQueue::GetQueueBufferLength() == 0)
{
if (!allocate_command_queue_memory())
{
send_app_error_response(PARAM_APP_ERROR_TYPE_FIRMWARE_ERROR,
PMSG(MSG_ERR_INSUFFICIENT_MEMORY));
return;
}
}
CommandQueue::FlushQueuedCommands();
uint16_t remaining_slots;
uint16_t current_command_count;
uint16_t total_command_count;
CommandQueue::GetQueueInfo(remaining_slots, current_command_count, total_command_count);
generate_response_start(RSP_OK);
generate_response_data_add(remaining_slots);
generate_response_data_add(current_command_count);
generate_response_data_add(total_command_count);
generate_response_send();
}
//
// Top level enqueue command handler
//
void enqueue_command()
{
uint8_t *ptr = parameter_value;
uint8_t index = 0;
if (CommandQueue::GetQueueBufferLength() == 0)
{
if (!allocate_command_queue_memory())
{
send_app_error_response(PARAM_APP_ERROR_TYPE_FIRMWARE_ERROR,
PMSG(MSG_ERR_INSUFFICIENT_MEMORY));
return;
}
}
generate_response_start(RSP_ORDER_SPECIFIC_ERROR, QUEUE_ERROR_MSG_OFFSET);
while (ptr < parameter_value + parameter_length)
{
const uint8_t length = *ptr++;
const uint8_t cmd = ptr[0];
if (length < 1 || ptr + length > parameter_value + parameter_length
|| (cmd == QUEUE_COMMAND_ORDER_WRAPPER && length < 2))
{
generate_response_msg_addPGM(PMSG(ERR_MSG_INSUFFICENT_BYTES));
generate_response_msg_add_ascii_number(length);
send_enqueue_error(QUEUE_COMMAND_ERROR_TYPE_MALFORMED_BLOCK, index);
return;
}
uint8_t retval;
switch (cmd)
{
case QUEUE_COMMAND_LINEAR_MOVE:
retval = enqueue_linear_move_command(ptr+1, length-1);
break;
case QUEUE_COMMAND_MOVEMENT_CHECKPOINT:
retval = enqueue_move_checkpoint_command(ptr+1, length-1);
break;
case QUEUE_COMMAND_DELAY:
retval = enqueue_delay_command(ptr+1, length-1);
break;
case QUEUE_COMMAND_ORDER_WRAPPER:
switch (ptr[1])
{
case ORDER_SET_OUTPUT_SWITCH_STATE:
retval = enqueue_set_output_switch_state_command(ptr+2, length-2);
break;
case ORDER_SET_PWM_OUTPUT_STATE:
retval = enqueue_set_pwm_output_state_command(ptr+2, length-2);
break;
case ORDER_SET_OUTPUT_TONE:
retval = enqueue_set_output_tone_command(ptr+2, length-2);
break;
case ORDER_SET_HEATER_TARGET_TEMP:
retval = enqueue_set_heater_target_temperature_command(ptr+2, length-2);
break;
case ORDER_ENABLE_DISABLE_STEPPERS:
retval = enqueue_set_stepper_enable_state_command(ptr+2, length-2);
break;
case ORDER_ENABLE_DISABLE_ENDSTOPS:
retval = enqueue_set_endstop_enable_state_command(ptr+2, length-2);
break;
default:
generate_response_msg_addPGM(PMSG(ERR_MSG_QUEUE_ORDER_NOT_PERMITTED));
generate_response_msg_add_ascii_number(ptr[1]);
send_enqueue_error(QUEUE_COMMAND_ERROR_TYPE_ERROR_IN_COMMAND_BLOCK, index, PARAM_APP_ERROR_TYPE_UNKNOWN_ORDER);
return;
}
break;
default:
send_enqueue_error(QUEUE_COMMAND_ERROR_TYPE_UNKNOWN_COMMAND_BLOCK, index);
return;
}
if (retval != ENQUEUE_SUCCESS)
{
send_enqueue_error(QUEUE_COMMAND_ERROR_TYPE_ERROR_IN_COMMAND_BLOCK, index, retval);
return;
}
ptr += length;
index += 1;
}
uint16_t remaining_slots;
uint16_t current_command_count;
uint16_t total_command_count;
CommandQueue::GetQueueInfo(remaining_slots, current_command_count, total_command_count);
generate_response_start(RSP_OK);
generate_response_data_add(remaining_slots);
generate_response_data_add(current_command_count);
generate_response_data_add(total_command_count);
generate_response_send();
}