This commit is contained in:
Chris Anderson 2012-04-02 12:45:20 -07:00
commit 07f44263ad
39 changed files with 2199 additions and 262 deletions

View File

@ -1067,11 +1067,6 @@ static void medium_loop()
if (g.log_bitmask & MASK_LOG_MOTORS)
Log_Write_Motors();
}
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs_data_stream_send(5,45);
break;
// This case controls the slow loop
@ -1163,7 +1158,7 @@ static void fifty_hz_loop()
// kick the GCS to process uplink data
gcs_update();
gcs_data_stream_send(45,1000);
gcs_data_stream_send();
#if FRAME_CONFIG == TRI_FRAME
// servo Yaw
@ -1218,10 +1213,6 @@ static void slow_loop()
// blink if we are armed
update_lights();
// send all requested output streams with rates requested
// between 3 and 5 Hz
gcs_data_stream_send(3,5);
if(g.radio_tuning > 0)
tuning();
@ -1262,7 +1253,6 @@ static void super_slow_loop()
}
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS
#ifdef USERHOOK_SUPERSLOWLOOP

View File

@ -76,16 +76,8 @@ public:
///
void send_text(gcs_severity severity, const prog_char_t *str) {}
// test if frequency within range requested for loop
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
}
// send streams which match frequency range
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void data_stream_send(void);
// set to true if this GCS link is active
bool initialised;
@ -118,12 +110,29 @@ public:
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void data_stream_send(void);
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum streams {STREAM_RAW_SENSORS,
STREAM_EXTENDED_STATUS,
STREAM_RC_CHANNELS,
STREAM_RAW_CONTROLLER,
STREAM_POSITION,
STREAM_EXTRA1,
STREAM_EXTRA2,
STREAM_EXTRA3,
STREAM_PARAMS,
NUM_STREAMS};
// see if we should send a stream now. Called at 50Hz
bool stream_trigger(enum streams stream_num);
private:
void handleMessage(mavlink_message_t * msg);
@ -168,7 +177,8 @@ private:
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
// data stream rates. The code assumes that
// streamRateRawSensors is the first
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
@ -177,6 +187,13 @@ private:
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
AP_Int16 streamRateParams;
// number of 50Hz ticks until we next send this stream
uint8_t stream_ticks[NUM_STREAMS];
// number of extra ticks to add to slow things down for the radio
uint8_t stream_slowdown;
};
#endif // __GCS_H

View File

@ -589,6 +589,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1),
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2),
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3),
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRateParams),
AP_GROUPEND
};
@ -653,11 +654,6 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
if (NULL != _queued_parameter) {
send_message(MSG_NEXT_PARAM);
}
if (!waypoint_receiving && !waypoint_sending) {
return;
}
@ -665,8 +661,8 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis();
if (waypoint_receiving &&
waypoint_request_i < (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500) {
waypoint_request_i <= (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
@ -682,19 +678,57 @@ GCS_MAVLINK::update(void)
}
}
void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
{
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
AP_Int16 *stream_rates = &streamRateRawSensors;
uint8_t rate = (uint8_t)stream_rates[stream_num].get();
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
if (rate == 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
}
void
GCS_MAVLINK::data_stream_send(void)
{
if (waypoint_receiving || waypoint_sending) {
// don't interfere with mission transfer
return;
}
if (_queued_parameter != NULL) {
if (streamRateParams.get() <= 0) {
streamRateParams.set(50);
}
if (stream_trigger(STREAM_PARAMS)) {
send_message(MSG_NEXT_PARAM);
}
// don't send anything else at the same time as parameters
return;
}
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_CURRENT_WAYPOINT);
@ -711,7 +745,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
}
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
#if HIL_MODE == HIL_MODE_ATTITUDE
send_message(MSG_LOCATION);
@ -719,35 +753,35 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
}
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
}
}
}
void
GCS_MAVLINK::send_message(enum ap_message id)
@ -1679,6 +1713,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#endif // HIL_MODE
case MAVLINK_MSG_ID_RADIO:
{
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space - slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
}
break;
}
} // end switch
} // end handle mavlink
@ -1803,12 +1861,12 @@ static void gcs_send_message(enum ap_message id)
/*
send data streams in the given rate range on both links
*/
static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
static void gcs_data_stream_send(void)
{
gcs0.data_stream_send(freqMin, freqMax);
gcs0.data_stream_send();
if (gcs3.initialised) {
gcs3.data_stream_send(freqMin, freqMax);
}
gcs3.data_stream_send();
}
}
/*

View File

@ -38,13 +38,9 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
read_radio();
gcs_data_stream_send(45, 1000);
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5, 45);
gcs_data_stream_send();
if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1, 5);
gcs_send_message(MSG_HEARTBEAT);
}

View File

@ -749,11 +749,8 @@ static void fast_loop()
// ------------------------------
set_servos();
// XXX is it appropriate to be doing the comms below on the fast loop?
gcs_update();
gcs_data_stream_send(45,1000);
gcs_data_stream_send();
}
static void medium_loop()
@ -854,10 +851,6 @@ Serial.println(tempaccel.z, DEC);
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs_data_stream_send(5,45);
break;
// This case controls the slow loop
@ -919,7 +912,6 @@ static void slow_loop()
update_events();
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
gcs_data_stream_send(3,5);
#if USB_MUX_PIN > 0
check_usb_mux();
@ -936,7 +928,6 @@ static void one_second_loop()
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
}
static void update_GPS(void)

View File

@ -76,16 +76,8 @@ public:
///
void send_text(gcs_severity severity, const prog_char_t *str) {}
// test if frequency within range requested for loop
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
}
// send streams which match frequency range
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void data_stream_send(void);
// set to true if this GCS link is active
bool initialised;
@ -118,12 +110,29 @@ public:
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void data_stream_send(void);
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum streams {STREAM_RAW_SENSORS,
STREAM_EXTENDED_STATUS,
STREAM_RC_CHANNELS,
STREAM_RAW_CONTROLLER,
STREAM_POSITION,
STREAM_EXTRA1,
STREAM_EXTRA2,
STREAM_EXTRA3,
STREAM_PARAMS,
NUM_STREAMS};
// see if we should send a stream now. Called at 50Hz
bool stream_trigger(enum streams stream_num);
private:
void handleMessage(mavlink_message_t * msg);
@ -168,7 +177,8 @@ private:
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
// data stream rates. The code assumes that
// streamRateRawSensors is the first
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
@ -177,6 +187,13 @@ private:
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
AP_Int16 streamRateParams;
// number of 50Hz ticks until we next send this stream
uint8_t stream_ticks[NUM_STREAMS];
// number of extra ticks to add to slow things down for the radio
uint8_t stream_slowdown;
};
#endif // __GCS_H

View File

@ -794,6 +794,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1),
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2),
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3),
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRateParams),
AP_GROUPEND
};
@ -857,11 +858,6 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
if (NULL != _queued_parameter) {
send_message(MSG_NEXT_PARAM);
}
if (!waypoint_receiving && !waypoint_sending) {
return;
}
@ -870,7 +866,7 @@ GCS_MAVLINK::update(void)
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500) {
tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
@ -886,64 +882,100 @@ GCS_MAVLINK::update(void)
}
}
void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
{
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
AP_Int16 *stream_rates = &streamRateRawSensors;
uint8_t rate = (uint8_t)stream_rates[stream_num].get();
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (rate == 0) {
return false;
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_FENCE_STATUS);
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
return true;
}
if (last_gps_satellites != g_gps->num_sats) {
// this message is mostly a huge waste of bandwidth,
// except it is the only message that gives the number
// of visible satellites. So only send it when that
// changes.
send_message(MSG_GPS_STATUS);
last_gps_satellites = g_gps->num_sats;
}
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
// sent with GPS read
send_message(MSG_LOCATION);
}
void
GCS_MAVLINK::data_stream_send(void)
{
if (waypoint_receiving || waypoint_sending) {
// don't interfere with mission transfer
return;
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
}
if (_queued_parameter != NULL) {
if (streamRateParams.get() <= 0) {
streamRateParams.set(50);
}
if (stream_trigger(STREAM_PARAMS)) {
send_message(MSG_NEXT_PARAM);
}
// don't send anything else at the same time as parameters
return;
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
}
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_FENCE_STATUS);
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (last_gps_satellites != g_gps->num_sats) {
// this message is mostly a huge waste of bandwidth,
// except it is the only message that gives the number
// of visible satellites. So only send it when that
// changes.
send_message(MSG_GPS_STATUS);
last_gps_satellites = g_gps->num_sats;
}
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
}
}
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
send_message(MSG_LOCATION);
}
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
}
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
}
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
}
}
@ -2032,6 +2064,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
{
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space - slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
}
break;
}
} // end switch
} // end handle mavlink
@ -2157,11 +2213,11 @@ static void gcs_send_message(enum ap_message id)
/*
send data streams in the given rate range on both links
*/
static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
static void gcs_data_stream_send(void)
{
gcs0.data_stream_send(freqMin, freqMax);
gcs0.data_stream_send();
if (gcs3.initialised) {
gcs3.data_stream_send(freqMin, freqMax);
gcs3.data_stream_send();
}
}

View File

@ -41,11 +41,8 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
read_radio();
gcs_data_stream_send(45,1000);
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5,45);
gcs_data_stream_send();
if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1,5);
gcs_send_message(MSG_HEARTBEAT);
}
loopcount++;

View File

@ -0,0 +1,56 @@
/*
AP_Motors.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_Motors.h"
// parameters for the motor class
const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
AP_GROUPINFO("TB_RATIO", 0, AP_Motors, top_bottom_ratio), // not used
AP_GROUPEND
};
// Constructor
AP_Motors::AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz ) :
top_bottom_ratio(AP_MOTORS_TOP_BOTTOM_RATIO),
_rc(rc_out),
_rc_roll(rc_roll),
_rc_pitch(rc_pitch),
_rc_throttle(rc_throttle),
_rc_yaw(rc_yaw),
_speed_hz(speed_hz),
_armed(false),
_auto_armed(false),
_frame_orientation(0),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE)
{
uint8_t i;
// initialise motor map
if( APM_version == AP_MOTORS_APM1 ) {
set_motor_to_channel_map(APM1_MOTOR_TO_CHANNEL_MAP);
} else {
set_motor_to_channel_map(APM2_MOTOR_TO_CHANNEL_MAP);
}
// clear output arrays
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
motor_out[i] = 0;
}
};
// throttle_pass_through - passes throttle through to motors - dangerous but used for initialising ESCs
void AP_Motors::throttle_pass_through() {
if( armed() ) {
for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
_rc->OutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in);
}
}
}

View File

@ -0,0 +1,137 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AxisController.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef AP_MOTORS
#define AP_MOTORS
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
// offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays
#define AP_MOTORS_MOT_1 0
#define AP_MOTORS_MOT_2 1
#define AP_MOTORS_MOT_3 2
#define AP_MOTORS_MOT_4 3
#define AP_MOTORS_MOT_5 4
#define AP_MOTORS_MOT_6 5
#define AP_MOTORS_MOT_7 6
#define AP_MOTORS_MOT_8 7
#define APM1_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_7,CH_8,CH_10,CH_11
#define APM2_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8
#define AP_MOTORS_MAX_NUM_MOTORS 8
#define AP_MOTORS_DEFAULT_MIN_THROTTLE 130
#define AP_MOTORS_DEFAULT_MAX_THROTTLE 850
// APM board definitions
#define AP_MOTORS_APM1 1
#define AP_MOTORS_APM2 2
// frame definitions
#define AP_MOTORS_PLUS_FRAME 0
#define AP_MOTORS_X_FRAME 1
#define AP_MOTORS_V_FRAME 2
// motor update rates
#define AP_MOTORS_SPEED_DEFAULT 490
#define AP_MOTORS_SPEED_INSTANT_PWM 0
// top-bottom ratio (for Y6)
#define AP_MOTORS_TOP_BOTTOM_RATIO 1.0
/// @class AP_Motors
class AP_Motors {
public:
// Constructor
AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// init
virtual void Init() {};
// set mapping from motor number to RC channel
virtual void set_motor_to_channel_map( uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t mot_4, uint8_t mot_5, uint8_t mot_6, uint8_t mot_7, uint8_t mot_8 ) {
_motor_to_channel_map[AP_MOTORS_MOT_1] = mot_1;
_motor_to_channel_map[AP_MOTORS_MOT_2] = mot_2;
_motor_to_channel_map[AP_MOTORS_MOT_3] = mot_3;
_motor_to_channel_map[AP_MOTORS_MOT_4] = mot_4;
_motor_to_channel_map[AP_MOTORS_MOT_5] = mot_5;
_motor_to_channel_map[AP_MOTORS_MOT_6] = mot_6;
_motor_to_channel_map[AP_MOTORS_MOT_7] = mot_7;
_motor_to_channel_map[AP_MOTORS_MOT_8] = mot_8;
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
// set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation ) { _frame_orientation = new_orientation; };
// enable - starts allowing signals to be sent to motors
virtual void enable() {};
// arm, disarm or check status status of motors
virtual bool armed() { return _armed; };
virtual void armed(bool armed) { _armed = armed; };
// check or set status of auto_armed - controls whether autopilot can take control of throttle
// Note: this should probably be moved out of this class as it has little to do with the motors
virtual bool auto_armed() { return _auto_armed; };
virtual void auto_armed(bool armed) { _auto_armed = armed; };
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
virtual void set_min_throttle(uint16_t min_throttle) { _min_throttle = min_throttle; };
virtual void set_max_throttle(uint16_t max_throttle) { _max_throttle = max_throttle; };
// output - sends commands to the motors
virtual void output() { if( _armed && _auto_armed ) { output_armed(); }else{ output_disarmed(); } };
// output_min - sends minimum values out to the motors
virtual void output_min() {};
// get basic information about the platform
virtual uint8_t get_num_motors() { return 0; };
// motor test
virtual void output_test() {};
// throttle_pass_through - passes throttle through to motors - dangerous but required for initialising ESCs
virtual void throttle_pass_through();
// 1 if motor is enabled, 0 otherwise
AP_Int8 motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
// final output values sent to the motors. public (for now) so that they can be access for logging
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];
// power ratio of upper vs lower motors (only used by y6 and octa quad copters)
AP_Float top_bottom_ratio;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// output functions that should be overloaded by child classes
virtual void output_armed() {};
virtual void output_disarmed() {};
APM_RC_Class* _rc; // APM_RC class used to send updates to ESCs/Servos
RC_Channel* _rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw; // input in from users
uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS]; // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
uint16_t _speed_hz; // speed in hz to send updates to motors
bool _armed; // true if motors are armed
bool _auto_armed; // true is throttle is above zero, allows auto pilot to take control of throttle
uint8_t _frame_orientation; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2
int16_t _min_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
int16_t _max_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
};
#endif // AP_MOTORS

View File

@ -0,0 +1,370 @@
/*
AP_MotorsHeli.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsHeli.h"
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
AP_NESTEDGROUPINFO(AP_Motors, 0),
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, servo1_pos),
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, servo2_pos),
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, servo3_pos),
AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, roll_max),
AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, pitch_max),
AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, collective_min),
AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, collective_max),
AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, collective_mid),
AP_GROUPINFO("GYR_ENABLE", 9, AP_MotorsHeli, ext_gyro_enabled),
AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type), // changed from trunk
AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, ext_gyro_gain),
AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, servo_manual),
AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle), // changed from trunk
AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect), // changed from trunk
AP_GROUPEND
};
// init
void AP_MotorsHeli::Init()
{
// set update rate
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsHeli::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
// setup fast channels
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_3]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
}
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsHeli::enable()
{
// enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]); // swash servo 2
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw
_rc->enable_out(AP_MOTORS_HELI_EXT_GYRO); // for external gyro
}
// output_min - sends minimum values out to the motors
void AP_MotorsHeli::output_min()
{
// move swash to mid
move_swash(0,0,500,0);
}
// output_armed - sends commands to the motors
void AP_MotorsHeli::output_armed()
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if( servo_manual == 1 ) {
_rc_roll->servo_out = _rc_roll->control_in;
_rc_pitch->servo_out = _rc_pitch->control_in;
_rc_throttle->servo_out = _rc_throttle->control_in;
_rc_yaw->servo_out = _rc_yaw->control_in;
}
//static int counter = 0;
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety
_auto_armed = true;
}
// for helis - armed or disarmed we allow servos to move
output_armed();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_test()
{
int16_t i;
// Send minimum values to all motors
output_min();
// servo 1
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0);
delay(300);
}
// servo 2
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0);
delay(300);
}
// servo 3
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0);
delay(300);
}
// external gyro
if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
}
// servo 4
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0);
delay(300);
}
// Send minimum values to all motors
output_min();
}
// reset_swash - free up swash for maximum movements. Used for set-up
void AP_MotorsHeli::reset_swash()
{
// free up servo ranges
_servo_1->radio_min = 1000;
_servo_1->radio_max = 2000;
_servo_2->radio_min = 1000;
_servo_2->radio_max = 2000;
_servo_3->radio_min = 1000;
_servo_3->radio_max = 2000;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing
// roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
// pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
// set roll, pitch and throttle scaling
_roll_scaler = 1.0;
_pitch_scaler = 1.0;
_collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0;
// we must be in set-up mode so mark swash as uninitialised
_swash_initialised = false;
}
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{
// swash servo initialisation
_servo_1->set_range(0,1000);
_servo_2->set_range(0,1000);
_servo_3->set_range(0,1000);
_servo_4->set_angle(4500);
// ensure _coll values are reasonable
if( collective_min >= collective_max ) {
collective_min = 1000;
collective_max = 2000;
}
collective_mid = constrain(collective_mid, collective_min, collective_max);
// calculate throttle mid point
throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0;
// determine roll, pitch and throttle scaling
_roll_scaler = (float)roll_max/4500.0;
_pitch_scaler = (float)pitch_max/4500.0;
_collective_scalar = ((float)(collective_max-collective_min))/1000.0;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
// pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
// servo min/max values
_servo_1->radio_min = 1000;
_servo_1->radio_max = 2000;
_servo_2->radio_min = 1000;
_servo_2->radio_max = 2000;
_servo_3->radio_min = 1000;
_servo_3->radio_max = 2000;
// mark swash as initialised
_swash_initialised = true;
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out)
{
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
if( servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to free up the swash
if( _swash_initialised ) {
reset_swash();
}
coll_out_scaled = coll_out * _collective_scalar + _rc_throttle->radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
if( !_swash_initialised ) {
init_swash();
}
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
roll_out = roll_out * _roll_scaler;
roll_out = constrain(roll_out, (int16_t)-roll_max, (int16_t)roll_max);
pitch_out = pitch_out * _pitch_scaler;
pitch_out = constrain(pitch_out, (int16_t)-pitch_max, (int16_t)pitch_max);
// scale collective pitch
coll_out = constrain(coll_out, 0, 1000);
coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000;
// rudder feed forward based on collective
if( !ext_gyro_enabled ) {
yaw_offset = collective_yaw_effect * abs(coll_out_scaled - collective_mid);
}
}
// swashplate servos
_servo_1->servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1->radio_trim-1500);
_servo_2->servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2->radio_trim-1500);
if( swash_type == AP_MOTORS_HELI_SWASH_H1 ) {
_servo_1->servo_out += 500;
_servo_2->servo_out += 500;
}
_servo_3->servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3->radio_trim-1500);
_servo_4->servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out
_servo_1->calc_pwm();
_servo_2->calc_pwm();
_servo_3->calc_pwm();
_servo_4->calc_pwm();
// actually move the servos
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_out);
// to be compatible with other frame types
motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out;
motor_out[AP_MOTORS_MOT_2] = _servo_2->radio_out;
motor_out[AP_MOTORS_MOT_3] = _servo_3->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out;
// output gyro value
if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
}
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}

View File

@ -0,0 +1,140 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsHeli.h
/// @brief Motor control class for Traditional Heli
#ifndef AP_MOTORSHELI
#define AP_MOTORSHELI
#include <inttypes.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters
#define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos
#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3
// tail servo uses channel 7
#define AP_MOTORS_HELI_EXT_GYRO CH_7
// frame definitions
#define AP_MOTORS_HELI_SWASH_CCPM 0
#define AP_MOTORS_HELI_SWASH_H1 1
/// @class AP_MotorsHeli
class AP_MotorsHeli : public AP_Motors {
public:
/// Constructor
AP_MotorsHeli( uint8_t APM_version,
APM_RC_Class* rc_out,
RC_Channel* rc_roll,
RC_Channel* rc_pitch,
RC_Channel* rc_throttle,
RC_Channel* rc_yaw,
RC_Channel* swash_servo_1,
RC_Channel* swash_servo_2,
RC_Channel* swash_servo_3,
RC_Channel* yaw_servo,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_servo_1(swash_servo_1),
_servo_2(swash_servo_2),
_servo_3(swash_servo_3),
_servo_4(yaw_servo),
swash_type(AP_MOTORS_HELI_SWASH_CCPM),
servo1_pos (-60),
servo2_pos (60),
servo3_pos (180),
roll_max (4500),
pitch_max (4500),
collective_min (1250),
collective_max (1750),
collective_mid (1500),
ext_gyro_enabled (0),
ext_gyro_gain (1350),
phase_angle (0),
collective_yaw_effect (0),
servo_manual (0),
throttle_mid(0),
_roll_scaler(1),
_pitch_scaler(1),
_collective_scalar(1),
_swash_initialised(false)
{};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
// you must have setup_motors before calling this
virtual void set_update_rate( uint16_t speed_hz );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return 4; };
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
// reset_swash - free up swash for maximum movements. Used for set-up
virtual void reset_swash();
// init_swash - initialise the swash plate
virtual void init_swash();
// heli_move_swash - moves swash plate to attitude of parameters passed in
virtual void move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
//protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
RC_Channel *_servo_1;
RC_Channel *_servo_2;
RC_Channel *_servo_3;
RC_Channel *_servo_4;
AP_Int8 swash_type;
AP_Int16 servo1_pos;
AP_Int16 servo2_pos;
AP_Int16 servo3_pos;
AP_Int16 roll_max;
AP_Int16 pitch_max;
AP_Int16 collective_min;
AP_Int16 collective_max;
AP_Int16 collective_mid;
AP_Int16 ext_gyro_enabled;
AP_Int16 ext_gyro_gain;
AP_Int16 phase_angle;
AP_Int16 collective_yaw_effect;
AP_Int8 servo_manual; // used to trigger swash reset from mission planner
// internally used variables
int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000)
float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
float _pitch_scaler; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
float _collective_scalar; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
bool _swash_initialised; // true if swash has been initialised
};
#endif // AP_MOTORSHELI

View File

@ -0,0 +1,37 @@
/*
AP_MotorsHexa.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsHexa.h"
// setup_motors - configures the motors for a hexa
void AP_MotorsHexa::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 5);
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 3);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 6);
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 3);
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 1);
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 4);
}
}

View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsHexa.h
/// @brief Motor control class for Hexacopters
#ifndef AP_MOTORSHEXA
#define AP_MOTORSHEXA
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsHexa
class AP_MotorsHexa : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsHexa( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSHEXA

View File

@ -0,0 +1,330 @@
/*
AP_MotorsMatrix.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsMatrix.h"
// Init
void AP_MotorsMatrix::Init()
{
int8_t i;
// setup the motors
setup_motors();
// double check that opposite motor definitions are ok
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( opposite_motor[i] <= 0 || opposite_motor[i] >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[opposite_motor[i]] )
opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
{
uint32_t fast_channel_mask = 0;
int8_t i;
// record requested speed
_speed_hz = speed_hz;
// initialise instant_pwm booleans. they will be set again below
instant_pwm_force01 = false;
instant_pwm_force23 = false;
instant_pwm_force67 = false;
// check each enabled motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
// set-up fast channel mask
fast_channel_mask |= _BV(_motor_to_channel_map[i]); // add to fast channel map
// and instant pwm
if( _motor_to_channel_map[i] == 0 || _motor_to_channel_map[i] == 1 )
instant_pwm_force01 = true;
if( _motor_to_channel_map[i] == 2 || _motor_to_channel_map[i] == 3 )
instant_pwm_force23 = true;
if( _motor_to_channel_map[i] == 6 || _motor_to_channel_map[i] == 7 )
instant_pwm_force67 = true;
}
}
// enable fast channels
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(fast_channel_mask, _speed_hz);
}
}
// set frame orientation (normally + or X)
void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
{
// return if nothing has changed
if( new_orientation == _frame_orientation ) {
return;
}
// call parent
AP_Motors::set_frame_orientation( new_orientation );
// setup the motors
setup_motors();
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsMatrix::enable()
{
int8_t i;
// enable output channels
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
_rc->enable_out(_motor_to_channel_map[i]);
}
}
}
// output_min - sends minimum values out to the motors
void AP_MotorsMatrix::output_min()
{
int8_t i;
// fill the motor_out[] array for HIL use and send minimum value to each motor
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
}
}
// Force output if instant pwm
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
if( instant_pwm_force01 )
_rc->Force_Out0_Out1();
if( instant_pwm_force23 )
_rc->Force_Out2_Out3();
if( instant_pwm_force67 )
_rc->Force_Out6_Out7();
}
}
// output_armed - sends commands to the motors
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
if(_rc_throttle->servo_out > 0)
out_min = _rc_throttle->radio_min + _min_throttle;
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
// mix roll, pitch, yaw, throttle into output for each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_out +
_rc_roll->pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i] +
_rc_yaw->pwm_out*_yaw_factor[i];
}
// ensure motor is not below the minimum
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
}
// stability patch
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED && motor_out[i] > out_max ) {
motor_out[opposite_motor[i]] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
// ensure motors are not below the minimum
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = max(motor_out[i], out_min);
}
}
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0){
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
}
}
}
#endif
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
}
}
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
if( instant_pwm_force01 )
_rc->Force_Out0_Out1();
if( instant_pwm_force23 )
_rc->Force_Out2_Out3();
if( instant_pwm_force67 )
_rc->Force_Out6_Out7();
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety for auto pilot
_auto_armed = true;
}
// Send minimum values to all motors
output_min();
}
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_test()
{
int8_t min_order, max_order;
int8_t i,j;
// find min and max orders
min_order = test_order[0];
max_order = test_order[0];
for(i=1; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( test_order[i] < min_order )
min_order = test_order[i];
if( test_order[i] > max_order )
max_order = test_order[i];
}
// shut down all motors
output_min();
// first delay is longer
delay(4000);
// loop through all the possible orders spinning any motors that match that description
for( i=min_order; i<=max_order; i++ ) {
for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
if( motor_enabled[j] && test_order[j] == i ) {
// turn on this motor and wait 1/3rd of a second
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min);
delay(2000);
}
}
}
// shut down all motors
output_min();
}
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num, int8_t testing_order)
{
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// increment number of motors if this motor is being newly motor_enabled
if( !motor_enabled[motor_num] ) {
motor_enabled[motor_num] = true;
_num_motors++;
}
// set roll, pitch, thottle factors and opposite motor (for stability patch)
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
// set opposite motor after checking it's somewhat valid
if( opposite_motor_num == AP_MOTORS_MATRIX_MOTOR_UNDEFINED || (opposite_motor_num >=0 && opposite_motor_num < AP_MOTORS_MAX_NUM_MOTORS) ) {
opposite_motor[motor_num] = opposite_motor_num;
}else{
opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// set order that motor appears in test
if( testing_order == AP_MOTORS_MATRIX_ORDER_UNDEFINED ) {
test_order[motor_num] = motor_num;
}else{
test_order[motor_num] = testing_order;
}
}
}
// add_motor using just position and prop direction
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num, int8_t testing_order)
{
// call raw motor set-up method
add_motor_raw(
motor_num,
cos(radians(angle_degrees + 90)), // roll factor
cos(radians(angle_degrees)), // pitch factor
(float)direction, // yaw factor
opposite_motor_num,
testing_order);
}
// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
void AP_MotorsMatrix::remove_motor(int8_t motor_num)
{
int8_t i;
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num <= AP_MOTORS_MAX_NUM_MOTORS ) {
// if the motor was enabled decrement the number of motors
if( motor_enabled[motor_num] )
_num_motors--;
// disable the motor, set all factors to zero
motor_enabled[motor_num] = false;
_roll_factor[motor_num] = 0;
_pitch_factor[motor_num] = 0;
_yaw_factor[motor_num] = 0;
opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// if another motor has referred to this motor as it's opposite, remove that reference
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( opposite_motor[i] == motor_num )
opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
}
// remove_all_motors - removes all motor definitions
void AP_MotorsMatrix::remove_all_motors()
{
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
remove_motor(i);
}
_num_motors = 0;
}

View File

@ -0,0 +1,93 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsMatrix.h
/// @brief Motor control class for Matrixcopters
#ifndef AP_MOTORSMATRIX
#define AP_MOTORSMATRIX
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1
#define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1
#define AP_MOTORS_MATRIX_MOTOR_CW -1
#define AP_MOTORS_MATRIX_MOTOR_CCW 1
/// @class AP_MotorsMatrix
class AP_MotorsMatrix : public AP_Motors {
public:
/// Constructor
AP_MotorsMatrix( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
instant_pwm_force01(false),
instant_pwm_force23(false),
instant_pwm_force67(false),
_num_motors(0) {};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
// you must have setup_motors before calling this
virtual void set_update_rate( uint16_t speed_hz );
// set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return _num_motors; };
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
// add_motor using just position and prop direction
virtual void add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
// remove_motor
virtual void remove_motor(int8_t motor_num);
// remove_all_motors - removes all motor definitions
virtual void remove_all_motors();
// setup_motors - configures the motors for a given frame type - should be overwritten by child classes
virtual void setup_motors() { remove_all_motors(); };
// matrix
AP_Int16 angle[AP_MOTORS_MAX_NUM_MOTORS]; // angle in degrees from the front of the copter
AP_Int8 direction[AP_MOTORS_MAX_NUM_MOTORS]; // direction of rotation of the motor (-1 = clockwise, +1 = counter clockwise)
AP_Int8 opposite_motor[AP_MOTORS_MAX_NUM_MOTORS]; // motor number of the opposite motor
AP_Int8 test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence
// used for instant_pwm only
bool instant_pwm_force01;
bool instant_pwm_force23;
bool instant_pwm_force67;
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
// add_motor using raw roll, pitch, throttle and yaw factors
virtual void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
int8_t _num_motors; // not a very useful variable as you really need to check the motor_enabled array to see which motors are enabled
float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1)
};
#endif // AP_MOTORSMATRIX

View File

@ -0,0 +1,53 @@
/*
AP_MotorsOcta.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsOcta.h"
// setup_motors - configures the motors for a octa
void AP_MotorsOcta::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 3);
}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 8);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 5);
}else {
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 7);
add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 3);
}
}

View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsOcta.h
/// @brief Motor control class for Octacopters
#ifndef AP_MOTORSOCTA
#define AP_MOTORSOCTA
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta
class AP_MotorsOcta : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOcta( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSOCTA

View File

@ -0,0 +1,41 @@
/*
AP_MotorsOctaQuad.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsOctaQuad.h"
// setup_motors - configures the motors for a octa
void AP_MotorsOctaQuad::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 7);
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 4);
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 2);
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_6, 6);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_6, 6);
}
}

View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsOctaQuad.h
/// @brief Motor control class for OctaQuadcopters
#ifndef AP_MOTORSOCTAQUAD
#define AP_MOTORSOCTAQUAD
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta
class AP_MotorsOctaQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOctaQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSOCTAQUAD

View File

@ -0,0 +1,33 @@
/*
AP_MotorsQuad.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsQuad.h"
// setup_motors - configures the motors for a quad
void AP_MotorsQuad::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_3, 3);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_3, 2);
}
}

View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsQuad.h
/// @brief Motor control class for Quadcopters
#ifndef AP_MOTORSQUAD
#define AP_MOTORSQUAD
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsQuad
class AP_MotorsQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSQUAD

View File

@ -0,0 +1,182 @@
/*
AP_MotorsTri.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsTri.h"
// init
void AP_MotorsTri::Init()
{
// set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
// set update rate for the 3 motors (but not the servo on channel 7)
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
}
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsTri::enable()
{
// enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]);
_rc->enable_out(AP_MOTORS_CH_TRI_YAW);
}
// output_min - sends minimum values out to the motors
void AP_MotorsTri::output_min()
{
// fill the motor_out[] array for HIL use
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
// send minimum value to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_throttle->radio_trim);
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}
// output_armed - sends commands to the motors
void AP_MotorsTri::output_armed()
{
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
if(_rc_throttle->servo_out > 0)
out_min = _rc_throttle->radio_min + _min_throttle;
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
int roll_out = (float)_rc_roll->pwm_out * .866;
int pitch_out = _rc_pitch->pwm_out / 2;
//left front
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_out + roll_out + pitch_out;
//right front
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_out - roll_out + pitch_out;
// rear
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_out - _rc_pitch->pwm_out;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max){
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_1] = out_max;
}
if(motor_out[AP_MOTORS_MOT_2] > out_max){
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_2] = out_max;
}
if(motor_out[AP_MOTORS_MOT_4] > out_max){
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] = out_max;
}
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min);
motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0){
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
}
#endif
// send output to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
// also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value)
// note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner
_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out);
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsTri::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety
_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = AP_MOTORS_MOT_1; i < AP_MOTORS_MOT_4; i++){
motor_out[i] = _rc_throttle->radio_min;
}
// Send minimum values to all motors
output_min();
}
// output_disarmed - sends commands to the motors
void AP_MotorsTri::output_test()
{
// Send minimum values to all motors
output_min();
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
delay(4000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
}

View File

@ -0,0 +1,51 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsTri.h
/// @brief Motor control class for Tricopters
#ifndef AP_MOTORSTRI
#define AP_MOTORSTRI
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
// tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7
/// @class AP_MotorsTri
class AP_MotorsTri : public AP_Motors {
public:
/// Constructor
AP_MotorsTri( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void set_update_rate( uint16_t speed_hz );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return 4; }; // 3 motors + 1 tail servo
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
};
#endif // AP_MOTORSTRI

View File

@ -0,0 +1,26 @@
/*
AP_MotorsY6.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsY6.h"
// setup_motors - configures the motors for a hexa
void AP_MotorsY6::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// MultiWii set-up
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 3);
}

View File

@ -0,0 +1,32 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsY6.h
/// @brief Motor control class for Y6 frames
#ifndef AP_MOTORSY6
#define AP_MOTORSY6
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
#define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter
/// @class AP_MotorsY6
class AP_MotorsY6 : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsY6( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSY6

View File

@ -0,0 +1,114 @@
/*
Example of AP_Motors library.
Code by Randy Mackay. DIYDrones.com
*/
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#include <AP_MotorsTri.h>
#include <AP_MotorsQuad.h>
#include <AP_MotorsHexa.h>
#include <AP_MotorsY6.h>
#include <AP_MotorsOcta.h>
#include <AP_MotorsOctaQuad.h>
#include <AP_MotorsHeli.h>
#include <AP_MotorsMatrix.h>
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
Arduino_Mega_ISR_Registry isr_registry;
// uncomment one row below depending upon whether you have an APM1 or APM2
//APM_RC_APM1 APM_RC;
APM_RC_APM2 APM_RC;
RC_Channel rc1, rc2, rc3, rc4;
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHexa motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsY6 motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOcta motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOctaQuad motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHeli motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
// setup
void setup()
{
Serial.begin(115200);
Serial.println("AP_Motors library test ver 1.0");
RC_Channel::set_apm_rc(&APM_RC);
APM_RC.Init( &isr_registry ); // APM Radio initialization
// motor initialisation
motors.set_update_rate(490);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_min_throttle(130);
motors.set_max_throttle(850);
motors.Init(); // initialise motors
motors.enable();
motors.output_min();
delay(1000);
}
// loop
void loop()
{
int value;
// display help
Serial.println("Press 't' to test motors. Becareful they will spin!");
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
}
// get character from user
value = Serial.read();
// test motors
if( value == 't' || value == 'T' ) {
Serial.println("testing motors...");
motors.armed(true);
motors.output_test();
motors.armed(false);
Serial.println("finished test.");
}
}
// print motor output
void print_motor_output()
{
int8_t i;
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if( motors.motor_enabled[i] ) {
Serial.printf("\t%d %d",i,motors.motor_out[i]);
}
}
}

View File

@ -12,11 +12,11 @@ extern "C" {
// MESSAGE LENGTHS AND CRCS
#ifndef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}
#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 213, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
#endif
#ifndef MAVLINK_MESSAGE_INFO

View File

@ -6,14 +6,14 @@ typedef struct __mavlink_radio_t
{
uint8_t rssi; ///< local signal strength
uint8_t remrssi; ///< remote signal strength
uint8_t txbuf; ///< how full the tx buffer is as a percentage
uint16_t rxerrors; ///< receive errors
uint16_t serrors; ///< serial errors
uint16_t fixed; ///< count of error corrected packets
uint16_t txbuf; ///< number of bytes available in transmit buffer
} mavlink_radio_t;
#define MAVLINK_MSG_ID_RADIO_LEN 10
#define MAVLINK_MSG_ID_166_LEN 10
#define MAVLINK_MSG_ID_RADIO_LEN 9
#define MAVLINK_MSG_ID_166_LEN 9
@ -22,10 +22,10 @@ typedef struct __mavlink_radio_t
6, \
{ { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_radio_t, rssi) }, \
{ "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_radio_t, remrssi) }, \
{ "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, rxerrors) }, \
{ "serrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_radio_t, serrors) }, \
{ "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_radio_t, fixed) }, \
{ "txbuf", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_radio_t, txbuf) }, \
{ "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_radio_t, txbuf) }, \
{ "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_radio_t, rxerrors) }, \
{ "serrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 5, offsetof(mavlink_radio_t, serrors) }, \
{ "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 7, offsetof(mavlink_radio_t, fixed) }, \
} \
}
@ -38,39 +38,39 @@ typedef struct __mavlink_radio_t
*
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf)
uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint16_t rxerrors, uint16_t serrors, uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint8_t(buf, 0, rssi);
_mav_put_uint8_t(buf, 1, remrssi);
_mav_put_uint16_t(buf, 2, rxerrors);
_mav_put_uint16_t(buf, 4, serrors);
_mav_put_uint16_t(buf, 6, fixed);
_mav_put_uint16_t(buf, 8, txbuf);
_mav_put_uint8_t(buf, 2, txbuf);
_mav_put_uint16_t(buf, 3, rxerrors);
_mav_put_uint16_t(buf, 5, serrors);
_mav_put_uint16_t(buf, 7, fixed);
memcpy(_MAV_PAYLOAD(msg), buf, 10);
memcpy(_MAV_PAYLOAD(msg), buf, 9);
#else
mavlink_radio_t packet;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
memcpy(_MAV_PAYLOAD(msg), &packet, 10);
memcpy(_MAV_PAYLOAD(msg), &packet, 9);
#endif
msg->msgid = MAVLINK_MSG_ID_RADIO;
return mavlink_finalize_message(msg, system_id, component_id, 10);
return mavlink_finalize_message(msg, system_id, component_id, 9);
}
/**
@ -81,40 +81,40 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone
* @param msg The MAVLink message to compress the data into
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t rssi,uint8_t remrssi,uint16_t rxerrors,uint16_t serrors,uint16_t fixed,uint16_t txbuf)
uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint16_t rxerrors,uint16_t serrors,uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint8_t(buf, 0, rssi);
_mav_put_uint8_t(buf, 1, remrssi);
_mav_put_uint16_t(buf, 2, rxerrors);
_mav_put_uint16_t(buf, 4, serrors);
_mav_put_uint16_t(buf, 6, fixed);
_mav_put_uint16_t(buf, 8, txbuf);
_mav_put_uint8_t(buf, 2, txbuf);
_mav_put_uint16_t(buf, 3, rxerrors);
_mav_put_uint16_t(buf, 5, serrors);
_mav_put_uint16_t(buf, 7, fixed);
memcpy(_MAV_PAYLOAD(msg), buf, 10);
memcpy(_MAV_PAYLOAD(msg), buf, 9);
#else
mavlink_radio_t packet;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
memcpy(_MAV_PAYLOAD(msg), &packet, 10);
memcpy(_MAV_PAYLOAD(msg), &packet, 9);
#endif
msg->msgid = MAVLINK_MSG_ID_RADIO;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10);
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9);
}
/**
@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co
*/
static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio)
{
return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->rxerrors, radio->serrors, radio->fixed, radio->txbuf);
return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->rxerrors, radio->serrors, radio->fixed);
}
/**
@ -136,35 +136,35 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo
*
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf)
static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint16_t rxerrors, uint16_t serrors, uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint8_t(buf, 0, rssi);
_mav_put_uint8_t(buf, 1, remrssi);
_mav_put_uint16_t(buf, 2, rxerrors);
_mav_put_uint16_t(buf, 4, serrors);
_mav_put_uint16_t(buf, 6, fixed);
_mav_put_uint16_t(buf, 8, txbuf);
_mav_put_uint8_t(buf, 2, txbuf);
_mav_put_uint16_t(buf, 3, rxerrors);
_mav_put_uint16_t(buf, 5, serrors);
_mav_put_uint16_t(buf, 7, fixed);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 10);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 9);
#else
mavlink_radio_t packet;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 10);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 9);
#endif
}
@ -193,6 +193,16 @@ static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg
return _MAV_RETURN_uint8_t(msg, 1);
}
/**
* @brief Get field txbuf from radio message
*
* @return how full the tx buffer is as a percentage
*/
static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 2);
}
/**
* @brief Get field rxerrors from radio message
*
@ -200,7 +210,7 @@ static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg
*/
static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 2);
return _MAV_RETURN_uint16_t(msg, 3);
}
/**
@ -210,7 +220,7 @@ static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* m
*/
static inline uint16_t mavlink_msg_radio_get_serrors(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 4);
return _MAV_RETURN_uint16_t(msg, 5);
}
/**
@ -220,17 +230,7 @@ static inline uint16_t mavlink_msg_radio_get_serrors(const mavlink_message_t* ms
*/
static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 6);
}
/**
* @brief Get field txbuf from radio message
*
* @return number of bytes available in transmit buffer
*/
static inline uint16_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 8);
return _MAV_RETURN_uint16_t(msg, 7);
}
/**
@ -244,11 +244,11 @@ static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlin
#if MAVLINK_NEED_BYTE_SWAP
radio->rssi = mavlink_msg_radio_get_rssi(msg);
radio->remrssi = mavlink_msg_radio_get_remrssi(msg);
radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg);
radio->serrors = mavlink_msg_radio_get_serrors(msg);
radio->fixed = mavlink_msg_radio_get_fixed(msg);
radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
#else
memcpy(radio, _MAV_PAYLOAD(msg), 10);
memcpy(radio, _MAV_PAYLOAD(msg), 9);
#endif
}

View File

@ -835,19 +835,19 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
mavlink_radio_t packet_in = {
5,
72,
17339,
17443,
17547,
17651,
139,
17391,
17495,
17599,
};
mavlink_radio_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.rssi = packet_in.rssi;
packet1.remrssi = packet_in.remrssi;
packet1.txbuf = packet_in.txbuf;
packet1.rxerrors = packet_in.rxerrors;
packet1.serrors = packet_in.serrors;
packet1.fixed = packet_in.fixed;
packet1.txbuf = packet_in.txbuf;
@ -857,12 +857,12 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
@ -875,7 +875,7 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}

View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sun Apr 1 16:31:09 2012"
#define MAVLINK_BUILD_DATE "Mon Apr 2 09:57:26 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sun Apr 1 16:31:09 2012"
#define MAVLINK_BUILD_DATE "Mon Apr 2 09:57:26 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

View File

@ -12,11 +12,11 @@ extern "C" {
// MESSAGE LENGTHS AND CRCS
#ifndef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 3}
#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 3}
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
#endif
#ifndef MAVLINK_MESSAGE_INFO

View File

@ -7,13 +7,13 @@ typedef struct __mavlink_radio_t
uint16_t rxerrors; ///< receive errors
uint16_t serrors; ///< serial errors
uint16_t fixed; ///< count of error corrected packets
uint16_t txbuf; ///< number of bytes available in transmit buffer
uint8_t rssi; ///< local signal strength
uint8_t remrssi; ///< remote signal strength
uint8_t txbuf; ///< how full the tx buffer is as a percentage
} mavlink_radio_t;
#define MAVLINK_MSG_ID_RADIO_LEN 10
#define MAVLINK_MSG_ID_166_LEN 10
#define MAVLINK_MSG_ID_RADIO_LEN 9
#define MAVLINK_MSG_ID_166_LEN 9
@ -23,9 +23,9 @@ typedef struct __mavlink_radio_t
{ { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \
{ "serrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, serrors) }, \
{ "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_radio_t, fixed) }, \
{ "txbuf", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \
{ "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, rssi) }, \
{ "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_radio_t, remrssi) }, \
{ "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, rssi) }, \
{ "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, remrssi) }, \
{ "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, txbuf) }, \
} \
}
@ -38,39 +38,39 @@ typedef struct __mavlink_radio_t
*
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf)
uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint16_t rxerrors, uint16_t serrors, uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint16_t(buf, 0, rxerrors);
_mav_put_uint16_t(buf, 2, serrors);
_mav_put_uint16_t(buf, 4, fixed);
_mav_put_uint16_t(buf, 6, txbuf);
_mav_put_uint8_t(buf, 8, rssi);
_mav_put_uint8_t(buf, 9, remrssi);
_mav_put_uint8_t(buf, 6, rssi);
_mav_put_uint8_t(buf, 7, remrssi);
_mav_put_uint8_t(buf, 8, txbuf);
memcpy(_MAV_PAYLOAD(msg), buf, 10);
memcpy(_MAV_PAYLOAD(msg), buf, 9);
#else
mavlink_radio_t packet;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
memcpy(_MAV_PAYLOAD(msg), &packet, 10);
memcpy(_MAV_PAYLOAD(msg), &packet, 9);
#endif
msg->msgid = MAVLINK_MSG_ID_RADIO;
return mavlink_finalize_message(msg, system_id, component_id, 10, 128);
return mavlink_finalize_message(msg, system_id, component_id, 9, 244);
}
/**
@ -81,40 +81,40 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone
* @param msg The MAVLink message to compress the data into
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t rssi,uint8_t remrssi,uint16_t rxerrors,uint16_t serrors,uint16_t fixed,uint16_t txbuf)
uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint16_t rxerrors,uint16_t serrors,uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint16_t(buf, 0, rxerrors);
_mav_put_uint16_t(buf, 2, serrors);
_mav_put_uint16_t(buf, 4, fixed);
_mav_put_uint16_t(buf, 6, txbuf);
_mav_put_uint8_t(buf, 8, rssi);
_mav_put_uint8_t(buf, 9, remrssi);
_mav_put_uint8_t(buf, 6, rssi);
_mav_put_uint8_t(buf, 7, remrssi);
_mav_put_uint8_t(buf, 8, txbuf);
memcpy(_MAV_PAYLOAD(msg), buf, 10);
memcpy(_MAV_PAYLOAD(msg), buf, 9);
#else
mavlink_radio_t packet;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
memcpy(_MAV_PAYLOAD(msg), &packet, 10);
memcpy(_MAV_PAYLOAD(msg), &packet, 9);
#endif
msg->msgid = MAVLINK_MSG_ID_RADIO;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 128);
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 244);
}
/**
@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co
*/
static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio)
{
return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->rxerrors, radio->serrors, radio->fixed, radio->txbuf);
return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->rxerrors, radio->serrors, radio->fixed);
}
/**
@ -136,35 +136,35 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo
*
* @param rssi local signal strength
* @param remrssi remote signal strength
* @param txbuf how full the tx buffer is as a percentage
* @param rxerrors receive errors
* @param serrors serial errors
* @param fixed count of error corrected packets
* @param txbuf number of bytes available in transmit buffer
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf)
static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint16_t rxerrors, uint16_t serrors, uint16_t fixed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[10];
char buf[9];
_mav_put_uint16_t(buf, 0, rxerrors);
_mav_put_uint16_t(buf, 2, serrors);
_mav_put_uint16_t(buf, 4, fixed);
_mav_put_uint16_t(buf, 6, txbuf);
_mav_put_uint8_t(buf, 8, rssi);
_mav_put_uint8_t(buf, 9, remrssi);
_mav_put_uint8_t(buf, 6, rssi);
_mav_put_uint8_t(buf, 7, remrssi);
_mav_put_uint8_t(buf, 8, txbuf);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 10, 128);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 9, 244);
#else
mavlink_radio_t packet;
packet.rxerrors = rxerrors;
packet.serrors = serrors;
packet.fixed = fixed;
packet.txbuf = txbuf;
packet.rssi = rssi;
packet.remrssi = remrssi;
packet.txbuf = txbuf;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 10, 128);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 9, 244);
#endif
}
@ -180,7 +180,7 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi,
*/
static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 8);
return _MAV_RETURN_uint8_t(msg, 6);
}
/**
@ -190,7 +190,17 @@ static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg)
*/
static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 9);
return _MAV_RETURN_uint8_t(msg, 7);
}
/**
* @brief Get field txbuf from radio message
*
* @return how full the tx buffer is as a percentage
*/
static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 8);
}
/**
@ -223,16 +233,6 @@ static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg)
return _MAV_RETURN_uint16_t(msg, 4);
}
/**
* @brief Get field txbuf from radio message
*
* @return number of bytes available in transmit buffer
*/
static inline uint16_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 6);
}
/**
* @brief Decode a radio message into a struct
*
@ -245,10 +245,10 @@ static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlin
radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg);
radio->serrors = mavlink_msg_radio_get_serrors(msg);
radio->fixed = mavlink_msg_radio_get_fixed(msg);
radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
radio->rssi = mavlink_msg_radio_get_rssi(msg);
radio->remrssi = mavlink_msg_radio_get_remrssi(msg);
radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
#else
memcpy(radio, _MAV_PAYLOAD(msg), 10);
memcpy(radio, _MAV_PAYLOAD(msg), 9);
#endif
}

View File

@ -836,18 +836,18 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
17235,
17339,
17443,
17547,
151,
218,
29,
96,
};
mavlink_radio_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.rxerrors = packet_in.rxerrors;
packet1.serrors = packet_in.serrors;
packet1.fixed = packet_in.fixed;
packet1.txbuf = packet_in.txbuf;
packet1.rssi = packet_in.rssi;
packet1.remrssi = packet_in.remrssi;
packet1.txbuf = packet_in.txbuf;
@ -857,12 +857,12 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
@ -875,7 +875,7 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf );
mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.rxerrors , packet1.serrors , packet1.fixed );
mavlink_msg_radio_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}

View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sun Apr 1 16:31:09 2012"
#define MAVLINK_BUILD_DATE "Mon Apr 2 09:57:26 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sun Apr 1 16:31:09 2012"
#define MAVLINK_BUILD_DATE "Mon Apr 2 09:57:26 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

View File

@ -260,10 +260,10 @@
<description>Status generated by radio</description>
<field type="uint8_t" name="rssi">local signal strength</field>
<field type="uint8_t" name="remrssi">remote signal strength</field>
<field type="uint8_t" name="txbuf">how full the tx buffer is as a percentage</field>
<field type="uint16_t" name="rxerrors">receive errors</field>
<field type="uint16_t" name="serrors">serial errors</field>
<field type="uint16_t" name="fixed">count of error corrected packets</field>
<field type="uint16_t" name="txbuf">number of bytes available in transmit buffer</field>
</message>
</messages>
</mavlink>

View File

@ -260,10 +260,10 @@
<description>Status generated by radio</description>
<field type="uint8_t" name="rssi">local signal strength</field>
<field type="uint8_t" name="remrssi">remote signal strength</field>
<field type="uint8_t" name="txbuf">how full the tx buffer is as a percentage</field>
<field type="uint16_t" name="rxerrors">receive errors</field>
<field type="uint16_t" name="serrors">serial errors</field>
<field type="uint16_t" name="fixed">count of error corrected packets</field>
<field type="uint16_t" name="txbuf">number of bytes available in transmit buffer</field>
</message>
</messages>
</mavlink>