Copter: integrate draft RCMapper

This commit is contained in:
Randy Mackay 2013-03-25 11:48:06 +09:00 committed by Andrew Tridgell
parent e9594c8b80
commit 213eaa8db6
4 changed files with 11 additions and 4 deletions

View File

@ -103,6 +103,7 @@
#include <memcheck.h> // memory limit checker
#include <SITL.h> // software in the loop support
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper.h> // RC input mapping library
// AP_HAL to Arduino compatibility layer
#include "compat.h"
@ -405,6 +406,7 @@ static int8_t control_mode = STABILIZE;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
static uint8_t oldSwitchPosition;
static RCMapper rcmap;
// receiver RSSI
static uint8_t receiver_rssi;

View File

@ -200,6 +200,7 @@ public:
k_param_rc_9,
k_param_rc_12,
k_param_failsafe_gcs, // 198
k_param_rcmap,
//
// 200: flight modes

View File

@ -999,6 +999,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(motors, "MOT_", AP_Motors),
#endif
// @Group: RCMAP_
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
GOBJECT(rcmap, "RCMAP_", RCMapper),
AP_VAREND
};

View File

@ -133,12 +133,12 @@ static void read_radio()
ap_system.new_radio_frame = true;
uint16_t periods[8];
hal.rcin->read(periods,8);
g.rc_1.set_pwm(periods[0]);
g.rc_2.set_pwm(periods[1]);
g.rc_1.set_pwm(periods[rcmap.roll()]);
g.rc_2.set_pwm(periods[rcmap.pitch()]);
set_throttle_and_failsafe(periods[2]);
set_throttle_and_failsafe(periods[rcmap.throttle()]);
g.rc_4.set_pwm(periods[3]);
g.rc_4.set_pwm(periods[rcmap.yaw()]);
g.rc_5.set_pwm(periods[4]);
g.rc_6.set_pwm(periods[5]);
g.rc_7.set_pwm(periods[6]);