Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Fix mavlink module

Oleg Kalachev2df8c608d54c
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 3714e04..2b53824 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -6,18 +6,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,
@@ -33,21 +33,21 @@ 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);
@@ -55,7 +55,7 @@ void sendMavlink()
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);
}
}