flix

// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix

// MAVLink communication

#include <MAVLink.h>
#include "util.h"

extern float controlTime;
extern float voltage;
extern int voltagePin, rcRxPin;

int mavlinkSysId = 1;

Rate telemetrySlow(2);
Rate telemetryAttitude(20);
Rate telemetryAttitudeTarget(0);
Rate telemetryRC(10);
Rate telemetryMotors(10);
Rate telemetryIMU(15);
Rate telemetryTopic(10);

float mavlinkTime = NAN; // time of last received message
String mavlinkPrintBuffer;

void processMavlink() {
	sendMavlink();
	receiveMavlink();
}

void sendMavlink() {
	sendMavlinkPrint();

	mavlink_message_t msg;
	uint32_t time = t * 1000;

	if (telemetrySlow) {
		mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_FLIX,
			(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
			((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
			mode, MAV_STATE_STANDBY);
		sendMessage(&msg);
	}

	if (!valid(mavlinkTime)) return; // send only heartbeat until connected

	if (telemetrySlow) {
		mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
		sendMessage(&msg);
	}

	if (telemetrySlow && valid(voltage)) {
		uint16_t voltages[] = {(uint16_t)(voltage * 1000), UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
		uint16_t voltagesExt[] = {0, 0, 0, 0};
		float remaining = constrain(mapf(voltage, 3.4, 4.2, 0, 1), 0, 1);
		mavlink_msg_battery_status_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, MAV_BATTERY_FUNCTION_ALL,
			MAV_BATTERY_TYPE_LIPO, INT16_MAX, voltages, -1, -1, -1, remaining * 100, 0, MAV_BATTERY_CHARGE_STATE_OK, voltagesExt, 0, 0);
		sendMessage(&msg);
	}

	if (telemetryAttitude) {
		const float offset[] = {0, 0, 0, 0};
		mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
		sendMessage(&msg);
	}

	if (telemetryAttitudeTarget) {
		const float att[] = {attitudeTarget.w, attitudeTarget.x, -attitudeTarget.y, -attitudeTarget.z};
		Vector rt = attitudeTarget.invalid() ? ratesTarget : ratesExtra;
		mavlink_msg_attitude_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
			attitudeTarget.invalid() ? ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE : 0,
			att, rt.x, -rt.y, -rt.z, thrustTarget);
		sendMessage(&msg);
	}

	if (telemetryRC && channels[0]) { // 0 means no RC input
		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
		sendMessage(&msg);
	}

	if (telemetryMotors) {
		float controls[8];
		memcpy(controls, motors, sizeof(motors));
		mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
		sendMessage(&msg);
	}

	if (telemetryIMU) {
		mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
			acc.x / ONE_G * 1000, -acc.y / ONE_G * 1000, -acc.z / ONE_G * 1000, // convert to frd
			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
			0, 0, 0, 0);
		sendMessage(&msg);
	}

	if (telemetryTopic && logExposed != nullptr) {
		mavlink_msg_named_value_float_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			time, logExposed->name, logExposed->value.get());
		sendMessage(&msg);
	}
}

void sendMessage(const void *msg) {
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	int len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
	sendWiFi(buf, len);
}

static uint8_t mavlinkBatch[ESP_NOW_MAX_DATA_LEN_V2];
static int mavlinkBatchSize = 0;

void batchMessage(const void *msg) {
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	int len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);

	if (mavlinkBatchSize + len > sizeof(mavlinkBatch)) {
		sendWiFi(mavlinkBatch, mavlinkBatchSize);
		mavlinkBatchSize = 0;
	}
	memcpy(mavlinkBatch + mavlinkBatchSize, buf, len);
	mavlinkBatchSize += len;
}

void flushBatchMessages() {
	sendWiFi(mavlinkBatch, mavlinkBatchSize);
	mavlinkBatchSize = 0;
}

void receiveMavlink() {
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);

	// New packet, parse it
	mavlink_message_t msg;
	mavlink_status_t status;
	for (int i = 0; i < len; i++) {
		if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
			mavlinkTime = t;
			handleMavlink(&msg);
		}
	}
}

void handleMavlink(const void *_msg) {
	const mavlink_message_t& msg = *(mavlink_message_t *)_msg;

	if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
		mavlink_manual_control_t m;
		mavlink_msg_manual_control_decode(&msg, &m);
		if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast

		controlThrottle = m.z / 1000.0f;
		controlPitch = m.x / 1000.0f;
		controlRoll = m.y / 1000.0f;
		controlYaw = m.r / 1000.0f;
		controlMode = NAN;
		controlTime = t;
	}

	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
		mavlink_param_request_list_t m;
		mavlink_msg_param_request_list_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		mavlink_message_t msg;
		for (int i = 0; i < parametersCount(); i++) {
			mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
				getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
			sendMessage(&msg);
		}
	}

	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
		mavlink_param_request_read_t m;
		mavlink_msg_param_request_read_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
		float value = strlen(name) == 0 ? getParameter(m.param_index) : getParameter(name);
		if (m.param_index != -1) {
			memcpy(name, getParameterName(m.param_index), 16);
		}
		mavlink_message_t msg;
		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
		sendMessage(&msg);
	}

	if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
		mavlink_param_set_t m;
		mavlink_msg_param_set_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
		bool success = setParameter(name, m.param_value);
		if (!success) return;
		// send ack
		mavlink_message_t msg;
		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
		sendMessage(&msg);
	}

	if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
		mavlink_mission_request_list_t m;
		mavlink_msg_mission_request_list_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		mavlink_message_t msg;
		mavlink_msg_mission_count_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
		sendMessage(&msg);
	}

	if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
		mavlink_serial_control_t m;
		mavlink_msg_serial_control_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
		strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
		doCommand(data, true);
	}

	if (msg.msgid == MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) {
		if (mode != AUTO) return;

		mavlink_set_attitude_target_t m;
		mavlink_msg_set_attitude_target_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
			// Attitude control
			attitudeTarget.w = m.q[0];
			attitudeTarget.x = m.q[1];
			attitudeTarget.y = -m.q[2]; // convert to flu
			attitudeTarget.z = -m.q[3];
			ratesExtra.x = m.body_roll_rate;
			ratesExtra.y = -m.body_pitch_rate;
			ratesExtra.z = -m.body_yaw_rate;
		} else {
			// Rates control
			attitudeTarget.invalidate();
			ratesTarget.x = m.body_roll_rate;
			ratesTarget.y = -m.body_pitch_rate;
			ratesTarget.z = -m.body_yaw_rate;
		}

		thrustTarget = valid(m.thrust) ? m.thrust : thrustTarget;
		armed = m.thrust > 0;
	}

	if (msg.msgid == MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) {
		if (mode != AUTO) return;

		mavlink_set_actuator_control_target_t m;
		mavlink_msg_set_actuator_control_target_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		attitudeTarget.invalidate();
		ratesTarget.invalidate();
		torqueTarget.invalidate();
		memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
	}

	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_LIST) {
		const uint32_t qgcEpoch = 1262304000; // qgc accepts only timestamps after 2010-01-01
		mavlink_message_t response;
		mavlink_msg_log_entry_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
			0, 1, 0, qgcEpoch + t * 60, logLength); // put fake unique date to make qgc happy with saving logs
		sendMessage(&response);
	}

	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
		mavlink_log_request_data_t m;
		mavlink_msg_log_request_data_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		for (int i = 0; i < m.count; i += MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
			int chunkSize = min(MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN, (int)(m.count - i));
			mavlink_message_t response;
			uint8_t data[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN];
			readLog(data, m.ofs + i, chunkSize);
			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
				m.id, m.ofs + i, chunkSize, data);
			batchMessage(&response);
		}
		flushBatchMessages();
	}

	if (msg.msgid == MAVLINK_MSG_ID_TUNNEL) {
		mavlink_tunnel_t m;
		mavlink_msg_tunnel_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		if (m.payload_type == 789) { // 789 is led control
			for (int i = 0; i < m.payload_length; i += 4) {
				led[m.payload[i]] = RGB(m.payload[i + 1], m.payload[i + 2], m.payload[i + 3]);
			}
		}
	}

	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
		mavlink_command_long_t m;
		mavlink_msg_command_long_decode(&msg, &m);
		if (m.target_system && m.target_system != mavlinkSysId) return;

		int result = handleMavlinkCommand(&m);
		mavlink_message_t ack;
		mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, result, UINT8_MAX, 0, msg.sysid, msg.compid);
		sendMessage(&ack);
	}
}

int handleMavlinkCommand(const void *_m) {
	const mavlink_command_long_t& m = *(mavlink_command_long_t *)_m;

	if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
		mavlink_message_t response;
		mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
			MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
		sendMessage(&response);
		return MAV_RESULT_ACCEPTED;
	}

	if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_COMPONENT_METADATA) {
		mavlink_message_t response;
		mavlink_msg_component_metadata_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response, t * 1000,
		1453394476, // crc
		"https://quadcopter.dev/meta/1453394476/general.json");
		sendMessage(&response);
		return MAV_RESULT_ACCEPTED;
	}

	if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
		if (m.param1 == 1 && controlThrottle > 0.05) return MAV_RESULT_DENIED; // don't arm if throttle is not low
		armed = m.param1 == 1;
		return MAV_RESULT_ACCEPTED;
	}

	if (m.command == MAV_CMD_DO_SET_MODE) {
		if (m.param2 < 0 || m.param2 > AUTO) return MAV_RESULT_DENIED; // incorrect mode
		mode = m.param2;
		return MAV_RESULT_ACCEPTED;
	}

	if (m.command == MAV_CMD_DO_SET_SERVO) {
		int pin = m.param1;
		int pwm = m.param2;
		int frequency = m.param3 > 0 ? m.param3 : 50; // non-standard
		int resolution = m.param4 > 0 ? m.param4 : 10; // non-standard

		// Basic safety checks to avoid short circuits
		if (!digitalPinCanOutput(pin)) return MAV_RESULT_DENIED;
		if (pin == imuIntPin || pin == voltagePin || pin == rcRxPin) return MAV_RESULT_DENIED;
		if (imuBus == 0 && (pin == imuSckPin || pin == imuMisoPin || pin == imuMosiPin || pin == imuCsPin)) return MAV_RESULT_DENIED;
		if (imuBus == 1 && (pin == imuSdaPin || pin == imuSclPin)) return MAV_RESULT_DENIED;

		ledcDetach(pin);
		ledcAttach(pin, frequency, resolution);
		ledcChangeFrequency(pin, frequency, resolution);
		return ledcWrite(pin, pwm * frequency * (1 << resolution) / 1000000) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
	}

	return MAV_RESULT_UNSUPPORTED;
}

// Send shell output to GCS
void mavlinkPrint(const char* str) {
	mavlinkPrintBuffer += str;
}

void sendMavlinkPrint() {
	// Send mavlink print data in chunks
	const char *str = mavlinkPrintBuffer.c_str();
	for (int i = 0; i < strlen(str); i += MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN) {
		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
		strlcpy(data, str + i, sizeof(data));
		mavlink_message_t msg;
		mavlink_msg_serial_control_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
			SERIAL_CONTROL_DEV_SHELL,
			i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go
			0, 0, strlen(data), (uint8_t *)data, 0, 0);
		sendMessage(&msg);
	}
	mavlinkPrintBuffer.clear();
}