void ControlInput::task(void){
control_receiver->execute();
update_status();
//If the control receiver is OK we'll get data and publish:
if(status == STATUS_OK){
//Get Raw controller values:
//HACK-ALERT: ROLL AND YAW ARE ONLY INVERTED DUE TO THE DX6 transmitter!
control_receiver->get_all_channels(raw_values);
raw_values.roll *= -1;
raw_values.yaw *= -1;
//convert to attitude:
control_socket.pitch = convert_joystick_to_degree(raw_values.pitch);
control_socket.roll = convert_joystick_to_degree(raw_values.roll);
control_socket.yaw = convert_joystick_to_degree(raw_values.yaw);
control_socket.throttle = convert_joystick_to_throttle(raw_values.throttle);
//Update switch and clicker:
aux_states[0] = raw_values.aux1;
aux_states[1] = raw_values.aux2;
if(aux_states[0] != last_aux_states[0]) handle_aux1(aux_states[0]);
if(aux_states[1] != last_aux_states[1]) handle_aux2(aux_states[1]);
last_aux_states[0] = aux_states[0];
last_aux_states[1] = aux_states[1];
//If we are in fixed altitude mode, we'll integrate the throttle stick
//to get a new altitude.
if(control_socket.control_mode == CONTROLMODE_FIXED_ALTITUDE){
process_altitude_setpoint();
}
control_socket.altitude = contrain(control_socket.altitude, 0, 300);
}else{
//THE CONTROL RECEIVER ISN'T OK!!
control_socket.reset();
}
control_socket.publish();
check_calibration();
check_arm();
if(debugging_stream) handle_debug_stream();
}
// Handle events from AFU
static void _handle_afu(struct psl *psl)
{
struct client *client;
uint64_t error;
uint8_t *buffer;
int reset_done;
size_t size;
reset_done = handle_aux2(psl->job, &(psl->parity_enabled),
&(psl->latency), &error);
if (error && !directed_mode_support(psl->mmio)) {
client = psl->client[0];
size = 1 + sizeof(uint64_t);
buffer = (uint8_t *) malloc(size);
buffer[0] = PSLSE_AFU_ERROR;
error = htonll(error);
memcpy((char *)&(buffer[1]), (char *)&error, sizeof(error));
if (put_bytes
(client->fd, size, buffer, psl->dbg_fp, psl->dbg_id,
0) < 0) {
client_drop(client, PSL_IDLE_CYCLES, CLIENT_NONE);
}
}
handle_mmio_ack(psl->mmio, psl->parity_enabled);
if (psl->cmd != NULL) {
if (reset_done)
psl->cmd->credits = psl->cmd->parms->credits;
handle_response(psl->cmd);
handle_buffer_write(psl->cmd);
handle_buffer_read(psl->cmd);
handle_buffer_data(psl->cmd, psl->parity_enabled);
handle_mem_write(psl->cmd);
handle_touch(psl->cmd);
handle_cmd(psl->cmd, psl->parity_enabled, psl->latency);
handle_interrupt(psl->cmd);
}
}
