diff --git a/flix/mavlink.ino b/flix/mavlink.ino index c1d9d81..c4e910d 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -43,7 +43,7 @@ void sendMavlink() { lastFast = t; const float zeroQuat[] = {0, 0, 0, 0}; - Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system + Quaternion attitudeFRD = fluToFrd(attitude); // MAVLink uses FRD coordinate system mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat); sendMessage(&msg); @@ -206,7 +206,7 @@ void mavlinkPrint(const char* str) { } // Convert Forward-Left-Up to Forward-Right-Down quaternion -inline Quaternion FLU2FRD(const Quaternion &q) { +inline Quaternion fluToFrd(const Quaternion &q) { return Quaternion(q.w, q.x, -q.y, -q.z); } diff --git a/gazebo/flix.h b/gazebo/flix.h index 0d4c24d..d5b37eb 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -23,6 +23,7 @@ Vector rates; Quaternion attitude; // declarations +void step(); void computeLoopRate(); void applyGyro(); void applyAcc(); @@ -31,25 +32,35 @@ void interpretRC(); void controlAttitude(); void controlRate(); void controlTorque(); -void showTable(); +const char* getModeName(); void sendMotors(); bool motorsActive(); +void testMotor(uint8_t n); void print(const char* format, ...); void doCommand(String str, bool echo); -void cliTestMotor(uint8_t n); +void handleInput(); +void calibrateRC(); void normalizeRC(); void printRCCal(); +void dumpLog(); void processMavlink(); void sendMavlink(); void sendMessage(const void *msg); void receiveMavlink(); void handleMavlink(const void *_msg); void mavlinkPrint(const char* str); +inline Quaternion fluToFrd(const Quaternion &q); void failsafe(); void armingFailsafe(); void rcLossFailsafe(); void descend(); -inline Quaternion FLU2FRD(const Quaternion &q); +int parametersCount(); +const char *getParameterName(int index); +float getParameter(int index); +float getParameter(const char *name); +bool setParameter(const char *name, const float value); +void printParameters(); +void resetParameters(); // mocks void setLED(bool on) {}; diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index 5986343..bcfa4ef 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -17,17 +17,18 @@ #include "Arduino.h" #include "flix.h" + +#include "cli.ino" +#include "control.ino" +#include "estimate.ino" +#include "failsafe.ino" +#include "log.ino" +#include "lpf.h" +#include "mavlink.ino" +#include "motors.ino" +#include "parameters.ino" #include "rc.ino" #include "time.ino" -#include "motors.ino" -#include "estimate.ino" -#include "control.ino" -#include "log.ino" -#include "parameters.ino" -#include "cli.ino" -#include "mavlink.ino" -#include "failsafe.ino" -#include "lpf.h" using ignition::math::Vector3d; using namespace gazebo;