Implement auto mode for automatic flight

Use arm/disarm gestures
Add arm/disarm commands
Add ratesExtra variable for 
Rename interpretRC to interpretControls
Rename controlRate to controlRates
Remove USER mode
Add invalidate methods for vector and quaternion
Add valid/invalid method for vector and quaternion
Add valid/invalid function
Print armed in rc command
Pass auto mode to heartbeat
Use actuator_control_target for motors
This commit is contained in:
Oleg Kalachev
2025-07-29 18:02:09 +03:00
parent 2066d05a60
commit dfceb8a6b2
9 changed files with 160 additions and 88 deletions

View File

@@ -15,7 +15,8 @@
double t = NAN;
float dt;
float motors[4];
float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
float controlArmed = NAN, controlMode = NAN;
Vector acc;
Vector gyro;
Vector rates;
@@ -28,9 +29,9 @@ void computeLoopRate();
void applyGyro();
void applyAcc();
void control();
void interpretRC();
void interpretControls();
void controlAttitude();
void controlRate();
void controlRates();
void controlTorque();
const char* getModeName();
void sendMotors();
@@ -54,8 +55,8 @@ void mavlinkPrint(const char* str);
void sendMavlinkPrint();
inline Quaternion fluToFrd(const Quaternion &q);
void failsafe();
void armingFailsafe();
void rcLossFailsafe();
void autoFailsafe();
void descend();
int parametersCount();
const char *getParameterName(int index);