mirror of https://github.com/ArduPilot/ardupilot
99 lines
2.6 KiB
C++
99 lines
2.6 KiB
C++
|
|
#include <AP_HAL.h>
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <GCS_MAVLink.h>
|
|
#include <GCS_Console.h>
|
|
#include "simplegcs.h"
|
|
|
|
extern mavlink_channel_t downstream_channel;
|
|
|
|
static volatile uint8_t lock = 0;
|
|
|
|
void simplegcs_send_console_async(uint32_t machtnichts) {
|
|
if (lock) return;
|
|
lock = 1;
|
|
gcs_console_send(downstream_channel);
|
|
lock = 0;
|
|
}
|
|
|
|
void simplegcs_send_heartbeat_async(uint32_t us) {
|
|
uint32_t ms = us / 1000;
|
|
static uint32_t last_ms = 0;
|
|
if (ms - last_ms < 1000) return;
|
|
if (lock) return;
|
|
last_ms = ms;
|
|
lock = 1;
|
|
{
|
|
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(
|
|
downstream_channel,
|
|
MAV_TYPE_QUADROTOR,
|
|
MAV_AUTOPILOT_ARDUPILOTMEGA,
|
|
base_mode,
|
|
custom_mode,
|
|
system_status);
|
|
}
|
|
lock = 0;
|
|
}
|
|
|
|
void simplegcs_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. */
|
|
|
|
while(lock);
|
|
lock = 1;
|
|
mavlink_msg_heartbeat_send(
|
|
chan,
|
|
MAV_TYPE_QUADROTOR,
|
|
MAV_AUTOPILOT_ARDUPILOTMEGA,
|
|
base_mode,
|
|
custom_mode,
|
|
system_status);
|
|
lock = 0;
|
|
}
|
|
|
|
|
|
bool simplegcs_try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
|
|
|
|
uint16_t txspace = comm_get_txspace(chan);
|
|
if (payload_space < MAVLINK_MSG_ID_STATUSTEXT_LEN+MAVLINK_NUM_NON_PAYLOAD_BYTES) return false;
|
|
|
|
char statustext[50] = { 0 };
|
|
if (len < 50) {
|
|
memcpy(statustext, text, len);
|
|
}
|
|
while(lock);
|
|
lock = 1;
|
|
mavlink_msg_statustext_send(
|
|
chan,
|
|
1, /* SEVERITY_LOW */
|
|
statustext);
|
|
lock = 0;
|
|
return true;
|
|
}
|
|
// -----------------------------------------------------------------------
|
|
|
|
|
|
void simplegcs_update(mavlink_channel_t chan, simplegcs_handler_t handler) {
|
|
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) {
|
|
handler(chan, &msg);
|
|
}
|
|
}
|
|
}
|
|
|