diff --git a/flix/cli.ino b/flix/cli.ino index 58dd54c..c2ca1f7 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -8,6 +8,7 @@ extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID; extern LowPassFilter ratesFilter; +extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; const char* motd = "\nWelcome to\n" diff --git a/flix/control.ino b/flix/control.ino index 56fe5bf..0eb03d8 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -50,6 +50,8 @@ Vector ratesTarget; Vector torqueTarget; float thrustTarget; +extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; + void control() { interpretRC(); failsafe(); diff --git a/flix/flix.ino b/flix/flix.ino index cc59ed4..3eccb26 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -18,11 +18,6 @@ #define RC_CHANNEL_ARMED 4 #define RC_CHANNEL_MODE 5 -#define MOTOR_REAR_LEFT 0 -#define MOTOR_REAR_RIGHT 1 -#define MOTOR_FRONT_RIGHT 2 -#define MOTOR_FRONT_LEFT 3 - #define ONE_G 9.80665 float t = NAN; // current step time, s diff --git a/flix/motors.ino b/flix/motors.ino index c7fdc5f..5e15506 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -14,6 +14,12 @@ #define PWM_MIN 0 #define PWM_MAX 1000000 / PWM_FREQUENCY +// Motors array indexes: +const int MOTOR_REAR_LEFT = 0; +const int MOTOR_REAR_RIGHT = 1; +const int MOTOR_FRONT_RIGHT = 2; +const int MOTOR_FRONT_LEFT = 3; + void setupMotors() { Serial.println("Setup Motors"); diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index d305865..509b574 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -150,6 +150,9 @@ void delay(uint32_t ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); } +bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; } +bool ledcWrite(uint8_t pin, uint32_t duty) { return true; } + unsigned long __micros; unsigned long __resetTime = 0; diff --git a/gazebo/flix.h b/gazebo/flix.h index c66f2d7..3bebf19 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -12,11 +12,6 @@ #define RC_CHANNELS 16 -#define MOTOR_REAR_LEFT 0 -#define MOTOR_FRONT_LEFT 3 -#define MOTOR_FRONT_RIGHT 2 -#define MOTOR_REAR_RIGHT 1 - #define WIFI_ENABLED 1 #define ONE_G 9.80665 @@ -43,6 +38,7 @@ void controlAttitude(); void controlRate(); void controlTorque(); void showTable(); +void sendMotors(); bool motorsActive(); void cliTestMotor(uint8_t n); String stringToken(char* str, const char* delim); @@ -61,7 +57,6 @@ inline Quaternion FLU2FRD(const Quaternion &q); void setLED(bool on) {}; void calibrateGyro() { printf("Skip gyro calibrating\n"); }; void calibrateAccel() { printf("Skip accel calibrating\n"); }; -void sendMotors() {}; void printIMUCal() { printf("cal: N/A\n"); }; void printIMUInfo() {}; Vector accBias, gyroBias, accScale(1, 1, 1); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index e83e578..605750b 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -20,6 +20,7 @@ #include "util.ino" #include "rc.ino" #include "time.ino" +#include "motors.ino" #include "estimate.ino" #include "control.ino" #include "log.ino"