Rover: fixed the build for MAVLink 1.0

I have no idea if it will actually work, but at least it builds
This commit is contained in:
Andrew Tridgell 2012-06-10 19:34:11 +10:00
parent 8f1121c980
commit fb1a4a2f2b
4 changed files with 12 additions and 16 deletions

View File

@ -48,15 +48,14 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_C:
base_mode = MAV_MODE_FLAG_LEARNING_ENABLED;
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_LEARNING_ENABLED;
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
@ -66,11 +65,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
break;
}
if (control_mode != MANUAL && control_mode != INITIALISING) {
// stabiliser of some form is enabled
base_mode |= MAV_MODE_FLAG_LEARNING_ENABLED;
}
#if ENABLE_STICK_MIXING==ENABLED
if (control_mode != INITIALISING) {
// all modes except INITIALISING have some form of manual

View File

@ -268,13 +268,21 @@ static bool verify_land()
{
}
static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
{
wp_radius = ground_speed * 150 / g.roll_limit.get();
//Serial.println(wp_radius, DEC);
}
static bool verify_nav_wp()
{
hold_course = -1;
update_crosstrack();
if(g.auto_wp_radius)
{ calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
{
calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);

View File

@ -102,12 +102,6 @@ static long wrap_180(long error)
return error;
}
static void calc_turn_radius() // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
{
wp_radius = ground_speed * 150 / g.roll_limit.get();
//Serial.println(wp_radius, DEC);
}
static void update_loiter()
{
float power;

View File

@ -209,7 +209,7 @@ static void init_ardupilot()
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_GROUND;
mavlink_system.type = MAV_TYPE_GROUND_ROVER;
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override