aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix mavlink module

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-10 03:41:01 +03:00
parent 02aac609ab
commit 2df8c608d5
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flix/mavlink.ino
View File

@ -6,18 +6,18 @@
#include "mavlink/common/mavlink.h" #include "mavlink/common/mavlink.h"
#define SYSTEM_ID 1 #define SYSTEM_ID 1
#define PERIOD_SLOW 1000000 // us #define PERIOD_SLOW 1.0
#define PERIOD_FAST 100000 // us #define PERIOD_FAST 0.1
uint32_t lastSlow;
uint32_t lastFast;
void sendMavlink() void sendMavlink()
{ {
static float lastSlow = 0;
static float lastFast = 0;
mavlink_message_t msg; mavlink_message_t msg;
if (stepTime - lastSlow >= PERIOD_SLOW) { if (t - lastSlow >= PERIOD_SLOW) {
lastSlow = stepTime; lastSlow = t;
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, 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, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,
@ -33,21 +33,21 @@ void sendMavlink()
// sendMessage(&msg); // sendMessage(&msg);
} }
if (stepTime - lastFast >= PERIOD_FAST) { if (t - lastFast >= PERIOD_FAST) {
lastFast = stepTime; 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); // sendMessage(&msg);
const float zeroQuat[] = {0, 0, 0, 0}; const float zeroQuat[] = {0, 0, 0, 0};
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 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, // 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); 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[0] * 10000, controls[1] * 10000, controls[2] * 10000,
controls[3] * 10000, controls[4] * 10000, controls[5] * 10000, controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
UINT16_MAX, UINT16_MAX, 255); UINT16_MAX, UINT16_MAX, 255);
@ -55,7 +55,7 @@ void sendMavlink()
float actuator[32]; float actuator[32];
memcpy(motors, actuator, 4 * sizeof(float)); 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); sendMessage(&msg);
} }
} }