mirror of https://github.com/ArduPilot/ardupilot
99 lines
2.8 KiB
C++
99 lines
2.8 KiB
C++
|
|
#include <AP_HAL/AP_HAL.h>
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <GCS_Console/GCS_Console.h>
|
|
#include "simplegcs.h"
|
|
|
|
void send_heartbeat(mavlink_channel_t chan) {
|
|
uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED
|
|
| MAV_MODE_FLAG_GUIDED_ENABLED
|
|
| MAV_MODE_FLAG_SAFETY_ARMED
|
|
| MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
|
uint8_t system_status = MAV_STATE_ACTIVE;
|
|
uint32_t custom_mode = 5 ; /* Loiter is mode 5. */
|
|
|
|
mavlink_msg_heartbeat_send(
|
|
chan,
|
|
MAV_TYPE_QUADROTOR,
|
|
MAV_AUTOPILOT_ARDUPILOTMEGA,
|
|
base_mode,
|
|
custom_mode,
|
|
system_status);
|
|
}
|
|
|
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
|
|
|
|
bool try_send_message(mavlink_channel_t chan, int msgid) {
|
|
|
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
|
|
|
switch (msgid) {
|
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
|
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
|
send_heartbeat(chan);
|
|
return true;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
|
|
|
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
|
CHECK_PAYLOAD_SIZE(STATUSTEXT);
|
|
|
|
char statustext[50] = { 0 };
|
|
if (len < 50) {
|
|
memcpy(statustext, text, len);
|
|
}
|
|
mavlink_msg_statustext_send(
|
|
chan,
|
|
4, /* MAV_SEVERITY_WARNING */
|
|
statustext);
|
|
return true;
|
|
}
|
|
// -----------------------------------------------------------------------
|
|
|
|
|
|
void simplegcs_update(mavlink_channel_t chan) {
|
|
mavlink_message_t msg;
|
|
mavlink_status_t status;
|
|
while(comm_get_available(chan)){
|
|
uint8_t c = comm_receive_ch(chan);
|
|
bool newmsg = mavlink_parse_char(chan, c, &msg, &status);
|
|
if (newmsg) {
|
|
handle_message(chan, &msg);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void handle_message(mavlink_channel_t chan, mavlink_message_t* msg) {
|
|
hal.console->printf("SimpleGCS Handle Message %d\r\n", msg->msgid);
|
|
switch (msg->msgid) {
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
|
{
|
|
char param_name[16] = "NOPARAMS";
|
|
|
|
/* Send a single parameter.*/
|
|
mavlink_msg_param_value_send(
|
|
chan,
|
|
param_name,
|
|
0.0f, /* param value */
|
|
MAVLINK_TYPE_FLOAT, /* param type */
|
|
1, /* _queued_parameter_count */
|
|
0); /* _queued_parameter_index */
|
|
break;
|
|
}
|
|
case MAVLINK_MSG_ID_DATA16:
|
|
gcs_console_handle_data16(msg);
|
|
break;
|
|
case MAVLINK_MSG_ID_DATA32:
|
|
gcs_console_handle_data32(msg);
|
|
break;
|
|
}
|
|
}
|
|
|