mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
Chris Anderson
commit
07f44263ad
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,18 +882,56 @@ 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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
@ -915,35 +949,33 @@ 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
|
||||
send_message(MSG_LOCATION);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
|
||||
if (stream_trigger(STREAM_RC_CHANNELS)) {
|
||||
send_message(MSG_RADIO_OUT);
|
||||
send_message(MSG_RADIO_IN);
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
|
||||
if (stream_trigger(STREAM_EXTRA1)) {
|
||||
send_message(MSG_ATTITUDE);
|
||||
send_message(MSG_SIMSTATE);
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
|
||||
if (stream_trigger(STREAM_EXTRA2)) {
|
||||
send_message(MSG_VFR_HUD);
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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]);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
Loading…
Reference in New Issue