设定：用cube Orange设定电源，并使用rp2040-zero读取电池的状态
已成功: 可以顺利使用mavlink读取电池的值
待测试：无法修改battery_status内的battery_remaining
#include <Arduino.h>
#include <MAVLink_cubepilot.h>
// Define serial port for communication with Cube Orange
#define CUBE_SERIAL Serial1
void setup() {
Serial.begin(57600); // Initialize serial for debugging
Serial1.setRX(13);
Serial1.setTX(12);
CUBE_SERIAL.begin(57600); // Initialize serial for communication with Cube Orange
}
void loop() {
// Check if data is available on Cube Orange serial port
if (CUBE_SERIAL.available()) {
mavlink_message_t msg;
mavlink_status_t status;
request_datastream();
// Parse incoming MAVLink messages
while (CUBE_SERIAL.available() > 0) {
uint8_t c = CUBE_SERIAL.read();
// Try to parse the message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
// Message successfully parsed, handle the message here
handle_mavlink_message(&msg);
}
}
}
}
void request_datastream() {
//Request Data from Pixhawk
uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255)
uint8_t _component_id = 2; // seems like it can be any # except the number of what Pixhawk sys_id is
uint8_t _target_system = 1; // Id # of Pixhawk (should be 1)
uint8_t _target_component = 0; // Target component, 0 = all (seems to work with 0 or 1
uint8_t _req_stream_id = MAV_DATA_STREAM_ALL;
uint16_t _req_message_rate = 0x0F; //number of times per second to request the data in hex
uint8_t _start_stop = 1; //1 = start, 0 = stop
// STREAMS that can be requested
/*
* Definitions are in common.h: enum MAV_DATA_STREAM and more importantly at:
https://mavlink.io/en/messages/common.html#MAV_DATA_STREAM
*
* MAV_DATA_STREAM_ALL=0, // Enable all data streams
* MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
* MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
* MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
* MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
* MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
* MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
* MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
* MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
* MAV_DATA_STREAM_ENUM_END=13,
*
* Data in PixHawk available in:
* - Battery, amperage and voltage (SYS_STATUS) in MAV_DATA_STREAM_EXTENDED_STATUS
* - Gyro info (IMU_SCALED) in MAV_DATA_STREAM_EXTRA1
*/
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_request_data_stream_pack(_system_id, _component_id, &msg, _target_system, _target_component, _req_stream_id, _req_message_rate, _start_stop);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes)
Serial1.write(buf, len); //Write data to serial port
}
// Function to handle received MAVLink messages
void handle_mavlink_message(mavlink_message_t *msg) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_BATTERY_STATUS:{
mavlink_battery_status_t battery_status;
mavlink_msg_battery_status_decode(msg, &battery_status);
// Read battery voltage of cell 1 (index 0 in voltages array)
float battery_voltage_cell1 = battery_status.voltages[0] / 1000.0f; // Convert millivolts to volts
Serial.print("battery remaining:");
Serial.println(battery_status.battery_remaining);
Serial.print("Battery Voltage (Cell 1): ");
Serial.print(battery_voltage_cell1);
Serial.println(" V");
battery_status.battery_remaining = 50;
mavlink_send_battery_status(battery_status);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
// Handle HEARTBEAT message
Serial.println("Received HEARTBEAT type:");
break;
}
// Add cases for other message types you want to handle
// case MAVLINK_MSG_ID_OTHER_MESSAGE:
// // Handle other message
// break;
default:
// Handle unknown message types or ignore them
break;
}
}
void mavlink_send_battery_status(mavlink_battery_status_t battery_status)
{
uint8_t system_id = 255; // id of computer which is sending the command (ground control software has id of 255)
uint8_t component_id = 0;
mavlink_message_t msg;
uint8_t id=battery_status.id;
uint8_t battery_function=battery_status.battery_function;
uint8_t type=battery_status.type;
int16_t temperature=battery_status.temperature;
uint16_t* voltages=battery_status.voltages;
int16_t current_battery=battery_status.current_battery;
int32_t current_consumed=battery_status.current_consumed;
int32_t energy_consumed=battery_status.energy_consumed;
int8_t battery_remaining=50;
int32_t time_remaining=battery_status.time_remaining;
uint8_t charge_state=battery_status.charge_state;
uint16_t* voltages_ext=battery_status.voltages_ext;
uint8_t mode=battery_status.mode;
uint32_t fault_bitmask=battery_status.fault_bitmask;
//Pack battery message
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_battery_status_pack(system_id,component_id,&msg,id,battery_function,type,temperature,voltages,current_battery,current_consumed,energy_consumed,battery_remaining,time_remaining,charge_state,voltages_ext,mode,fault_bitmask);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes)
Serial1.write(buf, len); //Write data to serial port
}
