Пример #1
0
/**
 * The structure of the entry point function for one of
 * the 3 'output' boards. The sensor board will differ slightly in
 * its running state.
 */
void main(void) {
    // initialise stuff
    proto_init();


    while (1) {
        switch (proto_state()) {
            case STARTUP:
                break;
            case CALIBRATING:
                break;
            case RUNNING: 
                if (proto_msg_buff_length()) {
                    msg = proto_msg_buff_pop();
                    if (msg.command == CMD_SET) {
                        set_steering(msg.data[0]);
                        proto_refresh();
                    } else {
                        proto_state_error();
                    }
                }
                break;
            default: // ERROR
                //broadcast ERROR signal
                break;
        }
    }
}
Пример #2
0
void ModeAcro::update()
{
    // get speed forward
    float speed, desired_steering;
    if (!attitude_control.get_forward_speed(speed)) {
        float desired_throttle;
        // convert pilot stick input into desired steering and throttle
        get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
        // no valid speed, just use the provided throttle
        g2.motors.set_throttle(desired_throttle);
    } else {
        float desired_speed;
        // convert pilot stick input into desired steering and speed
        get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
        calc_throttle(desired_speed, true);
    }

    float steering_out;

    // handle sailboats
    if (!is_zero(desired_steering)) {
        // steering input return control to user
        rover.sailboat_clear_tack();
    }
    if (g2.motors.has_sail() && rover.sailboat_tacking()) {
        // call heading controller during tacking
        steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
                                                                 g2.wp_nav.get_pivot_rate(),
                                                                 g2.motors.limit.steer_left,
                                                                 g2.motors.limit.steer_right,
                                                                 rover.G_Dt);
    } else {
        // convert pilot steering input to desired turn rate in radians/sec
        const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);

        // run steering turn rate controller and throttle controller
        steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
                                                              g2.motors.limit.steer_left,
                                                              g2.motors.limit.steer_right,
                                                              rover.G_Dt);
    }

    set_steering(steering_out * 4500.0f);
}
Пример #3
0
static void httpd_websocket_cb(struct altcp_pcb *pcb, uint8_t *data,
                         uint16_t data_len, uint8_t mode)
{
  if (data_len == 0 || mode != WS_BIN_MODE) return;

  uint8_t msgtype = data[0];
  uint8_t *payload = &data[1];
  data_len -= 1;
  int8_t *signed_data;
  pid_controller_index pid_index;
  int32_t *i32_data;
  uint8_t res;

  switch (msgtype)
  {
    case STEERING:
      // Parameters: velocity (int8_t), turn rate (int8_t)
      if (data_len != 2) break;
      signed_data = (int8_t *)payload;
      set_steering((FLT_TO_Q16(SPEED_CONTROL_FACTOR) * signed_data[1]) / 128,
        (FLT_TO_Q16(STEERING_FACTOR) * signed_data[0]) / 128);
      break;

    case REQ_ORIENTATION:
      if (data_len != 0) break;
      send_orientation(pcb);
      break;

    case REQ_SET_PID_PARAMS:
      // Parameters: pid index (uint8_t), P (q16/int32_t), I (q16), D (q16)
      if (data_len != 13) break;

      pid_index = (pid_controller_index)payload[0];
      i32_data = (int32_t *)(&payload[1]);
      if (pid_index < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
      {
        update_pid_controller(pid_index, i32_data[0], i32_data[1],
            i32_data[2]);
        res = RES_SET_PID_PARAMS_ACK;
        httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
      }

      break;

    case REQ_GET_PID_PARAMS:
      if (data_len != 1) break;
      if (payload[0] < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
      {
        send_pid_params(pcb, (pid_controller_index)payload[0]);
      }
      break;

    case REQ_LOAD_FLASH_CONFIG:
      if (data_len != 0) break;
      load_config();
      res = RES_LOAD_FLASH_CONFIG_DONE;
      httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
      break;

    case REQ_SAVE_CONFIG:
      if (data_len != 0) break;
      httpd_websocket_save_config(pcb);
      break;

    case REQ_CLEAR_CONFIG:
      if (data_len != 0) break;
      httpd_websocket_clear_config(pcb);
      break;
  }
}