messaging.cc

#include "messaging.pb.h"

using namespace messaging;

std::string SerializeSettings(
    double angle_offset_x,
    double angle_offset_y,
    double angle_offset_z,
    double pid_x_factors_p,
    double pid_x_factors_i,
    double pid_x_factors_d,
    double pid_x_min_max_p,
    double pid_x_min_max_i,
    double pid_x_min_max_d,
    double pid_y_factors_p,
    double pid_y_factors_i,
    double pid_y_factors_d,
    double pid_y_min_max_p,
    double pid_y_min_max_i,
    double pid_y_min_max_d,
    double pid_z_factors_p,
    double pid_z_factors_i,
    double pid_z_factors_d
    double pid_z_min_max_p,
    double pid_z_min_max_i,
    double pid_z_min_max_d)
  Settings settngs;

  settings.mutable_angle_offset()->set_x(angle_offset_x);
  settings.mutable_angle_offset()->set_y(angle_offset_y);
  settings.mutable_angle_offset()->set_z(angle_offset_z);

  settings.mutable_pid_x()->mutable_factors()->set_p(pid_x_factors_p);
  settings.mutable_pid_x()->mutable_factors()->set_i(pid_x_factors_i);
  settings.mutable_pid_x()->mutable_factors()->set_d(pid_x_factors_d);

  settings.mutable_pid_x()->mutable_min_max()->set_p(pid_x_min_max_p);
  settings.mutable_pid_x()->mutable_min_max()->set_i(pid_x_min_max_i);
  settings.mutable_pid_x()->mutable_min_max()->set_d(pid_x_min_max_d);

  settings.mutable_pid_y()->mutable_factors()->set_p(pid_y_factors_p);
  settings.mutable_pid_y()->mutable_factors()->set_i(pid_y_factors_i);
  settings.mutable_pid_y()->mutable_factors()->set_d(pid_y_factors_d);

  settings.mutable_pid_y()->mutable_min_max()->set_p(pid_y_min_max_p);
  settings.mutable_pid_y()->mutable_min_max()->set_i(pid_y_min_max_i);
  settings.mutable_pid_y()->mutable_min_max()->set_d(pid_y_min_max_d);

  settings.mutable_pid_z()->mutable_factors()->set_p(pid_z_factors_p);
  settings.mutable_pid_z()->mutable_factors()->set_i(pid_z_factors_i);
  settings.mutable_pid_z()->mutable_factors()->set_d(pid_z_factors_d);

  settings.mutable_pid_z()->mutable_min_max()->set_p(pid_z_min_max_p);
  settings.mutable_pid_z()->mutable_min_max()->set_i(pid_z_min_max_i);
  settings.mutable_pid_z()->mutable_min_max()->set_d(pid_z_min_max_d);

  std::string output;
  if (settings.SerializeToString(&output)) {
    // All OK!
    return output;
  }
  // Fail.
  return std::string();
}

bool ParseSettings(
    const std::string& data,
    double* angle_offset_x,
    double* angle_offset_y,
    double* angle_offset_z,
    double* pid_x_factors_p,
    double* pid_x_factors_i,
    double* pid_x_factors_d,
    double* pid_x_min_max_p,
    double* pid_x_min_max_i,
    double* pid_x_min_max_d,
    double* pid_y_factors_p,
    double* pid_y_factors_i,
    double* pid_y_factors_d,
    double* pid_y_min_max_p,
    double* pid_y_min_max_i,
    double* pid_y_min_max_d,
    double* pid_z_factors_p,
    double* pid_z_factors_i,
    double* pid_z_factors_d
    double* pid_z_min_max_p,
    double* pid_z_min_max_i,
    double* pid_z_min_max_d)
  Settings settngs;
  if (!settings.ParseFromString(data)) {
    // Fail.
    return false;
  }

  *angle_offset_x = settings.angle_offset()->x();
  *angle_offset_y = settings.angle_offset()->y();
  *angle_offset_z = settings.angle_offset()->z();

  *pid_x_factors_p = settings.pid_x()->factors()->p();
  *pid_x_factors_i = settings.pid_x()->factors()->i();
  *pid_x_factors_d = settings.pid_x()->factors()->d();

  *pid_x_min_max_p = settings.pid_x()->min_max()->p();
  *pid_x_min_max_i = settings.pid_x()->min_max()->i();
  *pid_x_min_max_d = settings.pid_x()->min_max()->d();

  *pid_y_factors_p = settings.pid_y()->factors()->p();
  *pid_y_factors_i = settings.pid_y()->factors()->i();
  *pid_y_factors_d = settings.pid_y()->factors()->d();

  *pid_y_min_max_p = settings.pid_y()->min_max()->p();
  *pid_y_min_max_i = settings.pid_y()->min_max()->i();
  *pid_y_min_max_d = settings.pid_y()->min_max()->d();

  *pid_z_factors_p = settings.pid_z()->factors()->p();
  *pid_z_factors_i = settings.pid_z()->factors()->i();
  *pid_z_factors_d = settings.pid_z()->factors()->d();

  *pid_z_min_max_p = settings.pid_z()->min_max()->p();
  *pid_z_min_max_i = settings.pid_z()->min_max()->i();
  *pid_z_min_max_d = settings.pid_z()->min_max()->d();

  return true;
}

messaging.proto

package messaging;

// A set of three double values (x, y, z).
message Vector {
  required double x;
  required double y;
  required double z;
}

// A set of three double values (p, i, d).
message PIDValues {
  required double p;
  required double i;
  required double d;
}

// A set of pid data (factors and limit).
message PIDSettings {
  required PIDValues factors;
  required PIDValues min_max; 
}

// Message sended from the PC to the copter.
message Settings {
  required Vector angle_offset;
  required PIDSettings pid_x;
  required PIDSettings pid_y;
  required PIDSettings pid_z;
}

// Message sended from the Copter to the PC.
message Telemetry {
  required Vector torques;
  required Vector angular_velocity;
  required PIDValues pid_x;
  required PIDValues pid_y;
  required PIDValues pid_z;
  required double heading;
  required double control_heading;
  required double force;
  required double correction_x;
  required double correction_y;
}