Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Fix mavlink module
Oleg Kalachev2df8c608d54c
diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 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.1void 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);}}