Browse Source

Copter: respond to param list request after motor init

master
Randy Mackay 8 years ago
parent
commit
91d4b4777b
  1. 1
      ArduCopter/Copter.h
  2. 2
      ArduCopter/GCS_Mavlink.cpp
  3. 6
      ArduCopter/system.cpp

1
ArduCopter/Copter.h

@ -274,6 +274,7 @@ private: @@ -274,6 +274,7 @@ private:
uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position
uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable
uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors
uint8_t initialised_params : 1; // 25 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
};
uint32_t value;
} ap;

2
ArduCopter/GCS_Mavlink.cpp

@ -862,7 +862,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) @@ -862,7 +862,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// if we have not yet initialised (including allocating the motors
// object) we drop this request. That prevents the GCS from getting
// a confusing parameter count during bootup
if (!copter.ap.initialised) {
if (!copter.ap.initialised_params) {
break;
}

6
ArduCopter/system.cpp

@ -184,7 +184,11 @@ void Copter::init_ardupilot() @@ -184,7 +184,11 @@ void Copter::init_ardupilot()
// allocate the motors class
allocate_motors();
init_rc_out(); // sets up motors and output to escs
// sets up motors and output to escs
init_rc_out();
// motors initialised so parameters can be sent
ap.initialised_params = true;
// initialise which outputs Servo and Relay events can use
ServoRelayEvents.set_channel_mask(~motors->get_motor_mask());

Loading…
Cancel
Save