Fix mavlink module

okalachev · Dec 10, 2023 · 1b952f3 · 1b952f3
1 parent 21f6536
commit 1b952f3
Showing 1 changed file with 14 additions and 14 deletions.
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
@@ -7,18 +7,18 @@
 #include "mavlink/common/mavlink.h"
 
 #define SYSTEM_ID 1
-#define PERIOD_SLOW 1000000 // us
-#define PERIOD_FAST 100000 // us
-
-uint32_t lastSlow;
-uint32_t lastFast;
+#define PERIOD_SLOW 1.0
+#define PERIOD_FAST 0.1
 
 void sendMavlink()
 {
+ static float lastSlow = 0;
+ static float lastFast = 0;
+
  mavlink_message_t msg;
 
- if (stepTime - lastSlow >= PERIOD_SLOW) {
- lastSlow = stepTime;
+ if (t - lastSlow >= PERIOD_SLOW) {
+ lastSlow = t;
 
  mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,
  MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,
@@ -34,29 +34,29 @@ void sendMavlink()
  // sendMessage(&msg);
  }
 
- if (stepTime - lastFast >= PERIOD_FAST) {
- lastFast = stepTime;
+ if (t - lastFast >= PERIOD_FAST) {
+ lastFast = t;
 
- // mavlink_msg_attitude_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, NAN, NAN, NAN, rollRate, pitchRate, yawRate);
+ // mavlink_msg_attitude_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t / 1000, NAN, NAN, NAN, rollRate, pitchRate, yawRate);
  // sendMessage(&msg);
 
  const float zeroQuat[] = {0, 0, 0, 0};
 
  mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
- stepTime / 1000, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat);
+ t / 1000, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat);
  // mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
- // stepTime / 1000, attitudeTarget.w, attitudeTarget.x, attitudeTarget.y, attitudeTarget.z, rates.x, rates.y, rates.z, zeroQuat);
+ // t / 1000, attitudeTarget.w, attitudeTarget.x, attitudeTarget.y, attitudeTarget.z, rates.x, rates.y, rates.z, zeroQuat);
  sendMessage(&msg);
 
- mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 0,
+ mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t / 1000, 0,
  controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
  controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
  UINT16_MAX, UINT16_MAX, 255);
  sendMessage(&msg);
 
  float actuator[32];
  memcpy(motors, actuator, 4 * sizeof(float));
- mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 4, actuator);
+ mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, t / 1000, 4, actuator);
  sendMessage(&msg);
  }
 }