add param for control

drone_control/workspace/DroneAvator/hako_module_drone_controller_impl.cpp

@@ -1,14 +1,16 @@
 #include "hako_module_drone_controller_impl.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
 
 const char* hako_module_drone_controller_impl_get_name(void)
 {
  return "DroneAvator";
 }
 
-static double Kp = 0.5;
-static double Ki = 0.01;
-static double Kd = 0.05;
-
+static double Kp = 10;
+static double Ki = 0.0;
+static double Kd = 0.0;
 
 static double integral = 0.0;
 static double prev_error = 0.0;
@@ -21,26 +23,51 @@ int hako_module_drone_controller_impl_is_operation_doing(void*)
 {
  return true;
 }
-int hako_module_drone_controller_impl_init(void* )
+
+int hako_module_drone_controller_impl_init(void* context)
 {
+ char cwd[1024];
+ if (getcwd(cwd, sizeof(cwd)) != NULL) {
+ printf("Current working dir: %s\n", cwd);
+ } else {
+ perror("getcwd() error");
+ return -1;
+ }
+
+ FILE* file = fopen("drone_control_params.txt", "r");
+ if (file == nullptr) {
+ perror("Failed to open params.txt");
+ return -1;
+ }
+
+ char line[256];
+ while (fgets(line, sizeof(line), file)) {
+ if (sscanf(line, "DroneAvator_Kp=%lf", &Kp) == 1) continue;
+ if (sscanf(line, "DroneAvator_Ki=%lf", &Ki) == 1) continue;
+ if (sscanf(line, "DroneAvator_Kd=%lf", &Kd) == 1) continue;
+ }
+
+ fclose(file);
+
  integral = 0.0;
  prev_error = 0.0;
  return 0;
 }
 
 mi_drone_control_out_t hako_module_drone_controller_impl_run(mi_drone_control_in_t *in)
 {
- const double mass = 0.1;
- const double gravity = 9.81;
  const double target_altitude = 10.0;
+
  double error = target_altitude + in->pos_z;
+
  integral += error * 0.003;
+
  double derivative = (error - prev_error) / 0.003;
-
- double output = Kp*error + (mass * gravity) + Ki*integral + Kd*derivative;
-
+ 
+ double output = Kp * error + Ki * integral + Kd * derivative;
+ 
  prev_error = error;
-
+ 
  mi_drone_control_out_t control_output;
  control_output.thrust = output;
  control_output.torque_x = 0.0;

hakoniwa/config/sample_control/drone_config_3.json

@@ -0,0 +1,92 @@
+{
+ "name": "DroneAvator",
+ "simulation": {
+ "lockstep": true,
+ "timeStep": 0.003,
+ "logOutputDirectory": ".",
+ "logOutput": {
+ "sensors": {
+ "acc": true,
+ "gyro": true,
+ "mag": true,
+ "baro": true,
+ "gps": true 
+ },
+ "mavlink": {
+ "hil_sensor": true,
+ "hil_gps": true,
+ "hil_actuator_controls": true
+ }
+ },
+ "mavlink_tx_period_msec": {
+ "hil_sensor": 3,
+ "hil_gps": 3
+ },
+ "location": {
+ "latitude": 47.641468,
+ "longitude": -122.140165,
+ "altitude": 121.321,
+ "magneticField": {
+ "intensity_nT": 53045.1,
+ "declination_deg": 15.306,
+ "inclination_deg": 68.984
+ } 
+ }
+ },
+ "components": {
+ "droneDynamics": {
+ "physicsEquation": "BodyFrame",
+ "collision_detection": true,
+ "manual_control": false,
+ "airFrictionCoefficient": [ 0.0001, 0.0 ],
+ "inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
+ "mass_kg": 0.1,
+ "body_size": [ 0.1, 0.1, 0.01 ],
+ "position_meter": [ 0, 15, 0 ],
+ "angle_degree": [ 0, 0, 0 ]
+ },
+ "rotor": {
+ "vendor": "None",
+ "Tr": 0.2,
+ "Kr": 8000,
+ "rpmMax": 8000
+ },
+ "thruster": {
+ "vendor": "None",
+ "rotorPositions": [
+ {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 },
+ {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 },
+ {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 },
+ {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 }
+ ],
+ "HoveringRpm": 4000,
+ "parameterB": 1.3e-10,
+ "parameterJr": 1.0e-10
+ },
+ "sensors": {
+ "acc": {
+ "sampleCount": 1,
+ "noise": 0.03
+ },
+ "gyro": {
+ "sampleCount": 1,
+ "noise": 0.01
+ },
+ "mag": {
+ "sampleCount": 1,
+ "noise": 0.03
+ },
+ "baro": {
+ "sampleCount": 1,
+ "noise": 0.01
+ },
+ "gps": {
+ "sampleCount": 1,
+ "noise": 0
+ }
+ }
+ },
+ "controller": {
+ "moduleDirectory": "../drone_control/cmake-build/workspace/DroneAvator"
+ }
+}

hakoniwa/drone_control_params.txt

@@ -0,0 +1,3 @@
+DroneAvator_Kp=0.5
+DroneAvator_Ki=0.0
+DroneAvator_Kd=0.0