ring_elem SchurRing2::add(const ring_elem f, const ring_elem g) const
{
if (is_zero(f)) return g;
if (is_zero(g)) return f;
const schur_poly *f1 = f.schur_poly_val;
const schur_poly *g1 = g.schur_poly_val;
ring_elem resultRE;
schur_poly *result = new schur_poly;
resultRE.schur_poly_val = result;
schur_poly::iterator i = f1->begin();
schur_poly::iterator j = g1->begin();
schur_poly::iterator iend = f1->end();
schur_poly::iterator jend = g1->end();
bool done = false;
while (!done)
{
int cmp = compare_partitions(i.getMonomial(), j.getMonomial());
switch (cmp) {
case LT:
result->appendTerm(j.getCoefficient(), j.getMonomial());
++j;
if (j == jend)
{
result->append(i, iend);
done = true;
}
break;
case GT:
result->appendTerm(i.getCoefficient(), i.getMonomial());
++i;
if (i == iend)
{
result->append(j, jend);
done = true;
}
break;
case EQ:
ring_elem c = coefficientRing->add(i.getCoefficient(), j.getCoefficient());
if (!coefficientRing->is_zero(c))
result->appendTerm(c, i.getMonomial());
++j;
++i;
if (j == jend)
{
result->append(i, iend);
done = true;
}
else
{
if (i == iend)
{
result->append(j, jend);
done = true;
}
}
break;
}
}
return resultRE;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
_pos_delta_unit = target_vel/target_vel.length();
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
Vector3f track_error = curr_pos - target_pos;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = min(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cms * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
}
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Copter::ModeGuided::posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control->set_pos_target(inertial_nav.get_position());
pos_control->set_desired_velocity(Vector3f(0,0,0));
zero_throttle_and_relax_ac();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
guided_vel_target_cms.zero();
if (auto_yaw.mode() == AUTO_YAW_RATE) {
auto_yaw.set_rate(0.0f);
}
}
// calculate dt
float dt = pos_control->time_since_last_xy_update();
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance position target using velocity target
guided_pos_target_cm += guided_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control->set_pos_target(guided_pos_target_cm);
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
// run position controllers
pos_control->update_xy_controller(ekfNavVelGainScaler);
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
}
}
/*
update navigation for normal mission waypoints. Return true when the
waypoint is complete
*/
bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
steer_state.hold_course_cd = -1;
// depending on the pass by flag either go to waypoint in regular manner or
// fly past it for set distance along the line of waypoints
Location flex_next_WP_loc = next_WP_loc;
uint8_t cmd_passby = HIGHBYTE(cmd.p1); // distance in meters to pass beyond the wp
uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp
if (cmd_passby > 0) {
float dist = get_distance(prev_WP_loc, flex_next_WP_loc);
if (!is_zero(dist)) {
float factor = (dist + cmd_passby) / dist;
flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat) * (factor - 1.0f);
flex_next_WP_loc.lng = flex_next_WP_loc.lng + (flex_next_WP_loc.lng - prev_WP_loc.lng) * (factor - 1.0f);
}
}
if (auto_state.no_crosstrack) {
nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
} else {
nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc);
}
// see if the user has specified a maximum distance to waypoint
// If override with p3 - then this is not used as it will overfly badly
if (g.waypoint_max_radius > 0 &&
auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
// this is needed to ensure completion of the waypoint
if (cmd_passby == 0) {
prev_WP_loc = current_loc;
}
}
return false;
}
float acceptance_distance_m = 0; // default to: if overflown - let it fly up to the point
if (cmd_acceptance_distance > 0) {
// allow user to override acceptance radius
acceptance_distance_m = cmd_acceptance_distance;
} else if (cmd_passby == 0) {
acceptance_distance_m = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle);
} else {
}
if (auto_state.wp_distance <= acceptance_distance_m) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, flex_next_WP_loc));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, flex_next_WP_loc));
return true;
}
return false;
}
static void rx_data_eapol_key_2_of_4(struct wlantest *wt, const u8 *dst,
const u8 *src, const u8 *data, size_t len)
{
struct wlantest_bss *bss;
struct wlantest_sta *sta;
const struct ieee802_1x_hdr *eapol;
const struct wpa_eapol_key *hdr;
const u8 *key_data, *kck;
u16 key_info, key_data_len;
struct wpa_eapol_ie_parse ie;
wpa_printf(MSG_DEBUG, "EAPOL-Key 2/4 " MACSTR " -> " MACSTR,
MAC2STR(src), MAC2STR(dst));
bss = bss_get(wt, dst);
if (bss == NULL)
return;
sta = sta_get(bss, src);
if (sta == NULL)
return;
eapol = (const struct ieee802_1x_hdr *) data;
hdr = (const struct wpa_eapol_key *) (eapol + 1);
if (is_zero(hdr->key_nonce, WPA_NONCE_LEN)) {
add_note(wt, MSG_INFO, "EAPOL-Key 2/4 from " MACSTR
" used zero nonce", MAC2STR(src));
}
if (!is_zero(hdr->key_rsc, 8)) {
add_note(wt, MSG_INFO, "EAPOL-Key 2/4 from " MACSTR
" used non-zero Key RSC", MAC2STR(src));
}
os_memcpy(sta->snonce, hdr->key_nonce, WPA_NONCE_LEN);
key_info = WPA_GET_BE16(hdr->key_info);
key_data_len = WPA_GET_BE16(hdr->key_data_length);
derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK, data, len);
if (!sta->ptk_set && !sta->tptk_set) {
add_note(wt, MSG_DEBUG,
"No PTK known to process EAPOL-Key 2/4");
return;
}
kck = sta->ptk.kck;
if (sta->tptk_set) {
add_note(wt, MSG_DEBUG,
"Use TPTK for validation EAPOL-Key MIC");
kck = sta->tptk.kck;
}
if (check_mic(kck, key_info & WPA_KEY_INFO_TYPE_MASK, data, len) < 0) {
add_note(wt, MSG_INFO, "Mismatch in EAPOL-Key 2/4 MIC");
return;
}
add_note(wt, MSG_DEBUG, "Valid MIC found in EAPOL-Key 2/4");
key_data = (const u8 *) (hdr + 1);
if (wpa_supplicant_parse_ies(key_data, key_data_len, &ie) < 0) {
add_note(wt, MSG_INFO, "Failed to parse EAPOL-Key Key Data");
return;
}
if (ie.wpa_ie) {
wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - WPA IE",
ie.wpa_ie, ie.wpa_ie_len);
if (os_memcmp(ie.wpa_ie, sta->rsnie, ie.wpa_ie_len) != 0) {
struct ieee802_11_elems elems;
add_note(wt, MSG_INFO,
"Mismatch in WPA IE between EAPOL-Key 2/4 "
"and (Re)Association Request from " MACSTR,
MAC2STR(sta->addr));
wpa_hexdump(MSG_INFO, "WPA IE in EAPOL-Key",
ie.wpa_ie, ie.wpa_ie_len);
wpa_hexdump(MSG_INFO, "WPA IE in (Re)Association "
"Request",
sta->rsnie,
sta->rsnie[0] ? 2 + sta->rsnie[1] : 0);
/*
* The sniffer may have missed (Re)Association
* Request, so try to survive with the information from
* EAPOL-Key.
*/
os_memset(&elems, 0, sizeof(elems));
elems.wpa_ie = ie.wpa_ie + 2;
elems.wpa_ie_len = ie.wpa_ie_len - 2;
wpa_printf(MSG_DEBUG, "Update STA data based on WPA "
"IE in EAPOL-Key 2/4");
sta_update_assoc(sta, &elems);
}
}
if (ie.rsn_ie) {
wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - RSN IE",
ie.rsn_ie, ie.rsn_ie_len);
if (os_memcmp(ie.rsn_ie, sta->rsnie, ie.rsn_ie_len) != 0) {
struct ieee802_11_elems elems;
add_note(wt, MSG_INFO,
"Mismatch in RSN IE between EAPOL-Key 2/4 "
"and (Re)Association Request from " MACSTR,
MAC2STR(sta->addr));
wpa_hexdump(MSG_INFO, "RSN IE in EAPOL-Key",
ie.rsn_ie, ie.rsn_ie_len);
wpa_hexdump(MSG_INFO, "RSN IE in (Re)Association "
"Request",
sta->rsnie,
sta->rsnie[0] ? 2 + sta->rsnie[1] : 0);
/*
* The sniffer may have missed (Re)Association
* Request, so try to survive with the information from
* EAPOL-Key.
*/
os_memset(&elems, 0, sizeof(elems));
elems.rsn_ie = ie.rsn_ie + 2;
elems.rsn_ie_len = ie.rsn_ie_len - 2;
wpa_printf(MSG_DEBUG, "Update STA data based on RSN "
"IE in EAPOL-Key 2/4");
sta_update_assoc(sta, &elems);
}
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if its for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, NULL);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text(MAV_SEVERITY_INFO,"Command received: ");
switch(packet.command) {
case MAV_CMD_PREFLIGHT_CALIBRATION:
{
if (is_equal(packet.param1,1.0f)) {
tracker.ins.init_gyro();
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
if (is_equal(packet.param3,1.0f)) {
tracker.init_barometer();
// zero the altitude difference on next baro update
tracker.nav_status.need_altitude_calibration = true;
}
if (is_equal(packet.param4,1.0f)) {
// Cant trim radio
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
// start with gyro calibration
tracker.ins.init_gyro();
// reset ahrs gyro bias
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
}
if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
tracker.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (is_equal(packet.param1,1.0f)) {
tracker.arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (is_zero(packet.param1)) {
tracker.disarm_servos();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_GET_HOME_POSITION:
send_home(tracker.ahrs.get_home());
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
tracker.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL:
result = tracker.compass.handle_mag_cal_command(packet);
break;
default:
break;
}
mavlink_msg_command_ack_send(
chan,
packet.command,
result);
break;
}
// When mavproxy 'wp sethome'
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
waypoint_receiving = true;
waypoint_request_i = 0;
waypoint_request_last = 0;
send_message(MSG_NEXT_WAYPOINT);
waypoint_receiving = true;
}
break;
}
// XXX receive a WP from GCS and store in EEPROM if it is HOME
case MAVLINK_MSG_ID_MISSION_ITEM:
{
// decode
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
struct Location tell_command = {};
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude
break;
}
#ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
#ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{
tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
default:
result = MAV_MISSION_UNSUPPORTED_FRAME;
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
send_text(MAV_SEVERITY_INFO,"New HOME received");
waypoint_receiving = false;
}
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
tracker.tracking_manual_control(packet);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracker.tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracker.tracking_update_pressure(packet);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
break;
}
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
break;
} // end switch
} // end handle mavlink
// circle_run - runs the circle flight mode
// should be called at 100hz or more
void Sub::circle_run()
{
float target_yaw_rate = 0;
float target_climb_rate = 0;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.set_alt_target_to_current_alt();
return;
}
// process pilot inputs
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
circle_pilot_yaw_override = true;
}
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run circle controller
circle_nav.update();
// call attitude controller
if (circle_pilot_yaw_override) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
}else{
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
}
// run altitude controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
}
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void Copter::poshold_run()
{
float target_roll, target_pitch; // pilot's roll and pitch angle inputs
float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec
float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec
float takeoff_climb_rate = 0.0f; // takeoff induced climb rate
float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions
const Vector3f& vel = inertial_nav.get_velocity();
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
return;
}
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate (for alt-hold mode and take-off)
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// check for take-off
if (ap.land_complete && (takeoff_state.running || channel_throttle->get_control_in() > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER)
poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch);
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.roll_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode
poshold.brake_roll = 0; // initialise braking angle to zero
poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
// update braking time estimate
if (!poshold.braking_time_updated_roll) {
// check if brake angle is increasing
if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) {
poshold.brake_angle_max_roll = abs(poshold.brake_roll);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_roll = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_roll > 0) {
poshold.brake_timeout_roll--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the roll and pitch mode switch statements
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.roll = poshold.brake_roll + poshold.wind_comp_roll;
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined roll-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_roll > 0) {
poshold.controller_to_pilot_timer_roll--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll);
break;
}
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.pitch_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode
poshold.brake_pitch = 0; // initialise braking angle to zero
poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// update braking time estimate
if (!poshold.braking_time_updated_pitch) {
// check if brake angle is increasing
if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) {
poshold.brake_angle_max_pitch = abs(poshold.brake_pitch);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_pitch > 0) {
poshold.brake_timeout_pitch--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch;
// check for pilot input
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined pitch-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_pitch > 0) {
poshold.controller_to_pilot_timer_pitch--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch);
break;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
//
// switch into LOITER mode when both roll and pitch are ready
if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) {
poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
// at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore
poshold.loiter_reset_I = false;
// set delay to start of wind compensation estimate updates
poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) {
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
poshold.pitch_mode = poshold.roll_mode;
// handle combined roll+pitch mode
switch (poshold.roll_mode) {
case POSHOLD_BRAKE_TO_LOITER:
// reduce brake_to_loiter timer
if (poshold.brake_to_loiter_timer > 0) {
poshold.brake_to_loiter_timer--;
} else {
// progress to full loiter on next iteration
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
}
// calculate percentage mix of loiter and brake control
brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
// calculate brake_roll and pitch angles to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// calculate final roll and pitch output by mixing loiter and brake controls
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll());
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch());
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
}
}
break;
case POSHOLD_LOITER:
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// set roll angle based on loiter controller outputs
poshold.roll = wp_nav.get_roll();
poshold.pitch = wp_nav.get_pitch();
// update wind compensation estimate
poshold_update_wind_comp_estimate();
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
poshold.brake_pitch = 0;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
// if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) {
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
poshold.brake_roll = 0;
}
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
// update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}
/* zQRcondest -- returns an estimate of the 2-norm condition number of the
matrix factorised by QRfactor() or QRCPfactor()
-- note that as Q does not affect the 2-norm condition number,
it is not necessary to pass the diag, beta (or pivot) vectors
-- generates a lower bound on the true condition number
-- if the matrix is exactly singular, HUGE_VAL is returned
-- note that QRcondest() is likely to be more reliable for
matrices factored using QRCPfactor() */
double zQRcondest(ZMAT *QR)
{
STATIC ZVEC *y=ZVNULL;
Real norm, norm1, norm2, tmp1, tmp2;
complex sum, tmp;
int i, j, limit;
if ( QR == ZMNULL )
error(E_NULL,"zQRcondest");
limit = min(QR->m,QR->n);
for ( i = 0; i < limit; i++ )
/* if ( QR->me[i][i] == 0.0 ) */
if ( is_zero(QR->me[i][i]) )
return HUGE_VAL;
y = zv_resize(y,limit);
MEM_STAT_REG(y,TYPE_ZVEC);
/* use the trick for getting a unit vector y with ||R.y||_inf small
from the LU condition estimator */
for ( i = 0; i < limit; i++ )
{
sum.re = sum.im = 0.0;
for ( j = 0; j < i; j++ )
/* sum -= QR->me[j][i]*y->ve[j]; */
sum = zsub(sum,zmlt(QR->me[j][i],y->ve[j]));
/* sum -= (sum < 0.0) ? 1.0 : -1.0; */
norm1 = zabs(sum);
if ( norm1 == 0.0 )
sum.re = 1.0;
else
{
sum.re += sum.re / norm1;
sum.im += sum.im / norm1;
}
/* y->ve[i] = sum / QR->me[i][i]; */
y->ve[i] = zdiv(sum,QR->me[i][i]);
}
zUAmlt(QR,y,y);
/* now apply inverse power method to R*.R */
for ( i = 0; i < 3; i++ )
{
tmp1 = zv_norm2(y);
zv_mlt(zmake(1.0/tmp1,0.0),y,y);
zUAsolve(QR,y,y,0.0);
tmp2 = zv_norm2(y);
zv_mlt(zmake(1.0/tmp2,0.0),y,y);
zUsolve(QR,y,y,0.0);
}
/* now compute approximation for ||R^{-1}||_2 */
norm1 = sqrt(tmp1)*sqrt(tmp2);
/* now use complementary approach to compute approximation to ||R||_2 */
for ( i = limit-1; i >= 0; i-- )
{
sum.re = sum.im = 0.0;
for ( j = i+1; j < limit; j++ )
sum = zadd(sum,zmlt(QR->me[i][j],y->ve[j]));
if ( is_zero(QR->me[i][i]) )
return HUGE_VAL;
tmp = zdiv(sum,QR->me[i][i]);
if ( is_zero(tmp) )
{
y->ve[i].re = 1.0;
y->ve[i].im = 0.0;
}
else
{
norm = zabs(tmp);
y->ve[i].re = sum.re / norm;
y->ve[i].im = sum.im / norm;
}
/* y->ve[i] = (sum >= 0.0) ? 1.0 : -1.0; */
/* y->ve[i] = (QR->me[i][i] >= 0.0) ? y->ve[i] : - y->ve[i]; */
}
/* now apply power method to R*.R */
for ( i = 0; i < 3; i++ )
{
tmp1 = zv_norm2(y);
zv_mlt(zmake(1.0/tmp1,0.0),y,y);
zUmlt(QR,y,y);
tmp2 = zv_norm2(y);
zv_mlt(zmake(1.0/tmp2,0.0),y,y);
zUAmlt(QR,y,y);
}
norm2 = sqrt(tmp1)*sqrt(tmp2);
/* printf("QRcondest: norm1 = %g, norm2 = %g\n",norm1,norm2); */
#ifdef THREADSAFE
ZV_FREE(y);
#endif
return norm1*norm2;
}
bool is_unit(const ElementType& f) const {
return !is_zero(f);
}
void archivebits(double *x, UL q, UL n, UL totalbits, UL b, UL c, double hi, double lo,
const char *version_info, FILE *outfp, FILE *dupfp) {
FILE *archfp = NULL;
archfp = open_archive();
if (is_zero(x, n, 0, 0)) {
if (outfp != NULL && !ferror(outfp)) fprintf(outfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info);
if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info);
if (archfp != NULL) fprintf(archfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info);
if (archfp != NULL) close_archive(archfp, outfp, dupfp);
return;
}
if (outfp != NULL && !ferror(outfp)) fprintf(outfp, "M( " PRINTF_FMT_UL " )C", q);
if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, "M( " PRINTF_FMT_UL " )C", q);
if (archfp != NULL) fprintf(archfp, "M( " PRINTF_FMT_UL " )C", q);
balancedtostdrep(x, n, b, c, hi, lo, 0, 0);
static UL *hex = NULL;
static UL prior_hex = 0;
if (hex != NULL && totalbits/BITS_IN_LONG + 1 > prior_hex) {
free(hex);
hex = NULL;
prior_hex = totalbits/BITS_IN_LONG + 1;
}
if (hex == NULL && (hex = (UL *)calloc(totalbits/BITS_IN_LONG + 1, sizeof(UL))) == NULL) {
perror(program_name);
fprintf(stderr, "%s: cannot get memory for residue bits; calloc() errno %d\n", program_name, errno);
exit(errno);
}
int i = 0;
int j = 0;
do {
UL k = (long)(ceil((double)q*(j + 1)/n) - ceil((double)q*j/n));
if (k > totalbits) k = totalbits;
totalbits -= k;
int word = (long)x[j];
for (j++; k > 0; k--, i++) {
if (i % BITS_IN_LONG == 0)
hex[i/BITS_IN_LONG] = 0L;
hex[i/BITS_IN_LONG] |= ((word & 0x1) << (i % BITS_IN_LONG));
word >>= 1;
}
} while(totalbits > 0);
if (outfp != NULL && !ferror(outfp)) fputs(", 0x", outfp);
if (dupfp != NULL && !ferror(dupfp)) fputs(", 0x", dupfp);
if (archfp != NULL) fputs(", 0x", archfp);
static char bits_fmt[16] = "\0"; /* "%%0%ulx" -> "%08lx" or "%016lx" depending on sizeof(UL) */
if (bits_fmt[0] != '%')
sprintf(bits_fmt, "%%0" PRINTF_FMT_UL "%s", (UL)(BITS_IN_LONG/4), "lx"); /* 4 bits per hex 'digit' */
for (j = (i - 1)/BITS_IN_LONG; j >= 0; j--) {
if (outfp != NULL && !ferror(outfp)) fprintf(outfp, bits_fmt, hex[j]);
if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, bits_fmt, hex[j]);
if (archfp != NULL) fprintf(archfp, bits_fmt, hex[j]);
}
if (outfp != NULL && !ferror(outfp)) {
fprintf(outfp, ", n = " PRINTF_FMT_UL ", %s", n, version_info);
fprintf(outfp, "\n");
fflush(outfp);
}
if (dupfp != NULL && !ferror(dupfp))
fprintf(dupfp, ", n = " PRINTF_FMT_UL ", %s\n", n, version_info);
if (archfp != NULL) {
fprintf(archfp, ", n = " PRINTF_FMT_UL ", %s\n", n, version_info);
close_archive(archfp, outfp, dupfp);
}
return;
}
// verify_payload_place - returns true if placing has been completed
bool Copter::verify_payload_place()
{
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed
const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
const float current_throttle_level = motors.get_throttle();
const uint32_t now = AP_HAL::millis();
// if we discover we've landed then immediately release the load:
if (ap.land_complete) {
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start:
case PayloadPlaceStateType_Calibrating_Hover:
case PayloadPlaceStateType_Descending_Start:
case PayloadPlaceStateType_Descending:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed");
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
break;
case PayloadPlaceStateType_Releasing_Start:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Released:
case PayloadPlaceStateType_Ascending_Start:
case PayloadPlaceStateType_Ascending:
case PayloadPlaceStateType_Done:
break;
}
}
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
if (!wp_nav.reached_wp_destination()) {
return false;
}
// we're there; set loiter target
auto_payload_place_start(wp_nav.get_wp_destination());
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// fall through
case PayloadPlaceStateType_Calibrating_Hover_Start:
// hover for 1 second to get an idea of what our hover
// throttle looks like
debug("Calibrate start");
nav_payload_place.hover_start_timestamp = now;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
// fall through
case PayloadPlaceStateType_Calibrating_Hover: {
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
// still calibrating...
debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp);
return false;
}
// we have a valid calibration. Hopefully.
nav_payload_place.hover_throttle_level = current_throttle_level;
const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta));
nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
// fall through
};
case PayloadPlaceStateType_Descending_Start:
nav_payload_place.descend_start_timestamp = now;
nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
nav_payload_place.descend_throttle_level = 0;
nav_payload_place.state = PayloadPlaceStateType_Descending;
// fall through
case PayloadPlaceStateType_Descending:
// make sure we don't descend too far:
debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
if (!is_zero(nav_payload_place.descend_max) &&
nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) {
nav_payload_place.state = PayloadPlaceStateType_Ascending;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent");
return false; // we'll do any cleanups required next time through the loop
}
// see if we've been descending long enough to calibrate a descend-throttle-level:
if (is_zero(nav_payload_place.descend_throttle_level) &&
now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
nav_payload_place.descend_throttle_level = current_throttle_level;
}
// watch the throttle to determine whether the load has been placed
// debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level));
if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
(is_zero(nav_payload_place.descend_throttle_level) ||
current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
// throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
nav_payload_place.place_start_timestamp = 0;
return false;
}
if (nav_payload_place.place_start_timestamp == 0) {
// we've only just now hit the correct throttle level
nav_payload_place.place_start_timestamp = now;
return false;
} else if (now - nav_payload_place.place_start_timestamp < placed_time) {
// keep going down....
debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
// fall through
case PayloadPlaceStateType_Releasing_Start:
if (g2.gripper.valid()) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper");
g2.gripper.release();
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid");
}
nav_payload_place.state = PayloadPlaceStateType_Releasing;
// fall through
case PayloadPlaceStateType_Releasing:
if (g2.gripper.valid() && !g2.gripper.released()) {
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Released;
// fall through
case PayloadPlaceStateType_Released: {
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
}
// fall through
case PayloadPlaceStateType_Ascending_Start: {
Location_Class target_loc = inertial_nav.get_position();
target_loc.alt = nav_payload_place.descend_start_altitude;
auto_wp_start(target_loc);
nav_payload_place.state = PayloadPlaceStateType_Ascending;
}
//fall through
case PayloadPlaceStateType_Ascending:
if (!wp_nav.reached_wp_destination()) {
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Done;
// fall through
case PayloadPlaceStateType_Done:
return true;
default:
// this should never happen
// TO-DO: log an error
return true;
}
// should never get here
return true;
}
Expr lower_lerp(Expr zero_val, Expr one_val, Expr weight) {
Expr result;
internal_assert(zero_val.type() == one_val.type());
internal_assert(weight.type().is_uint() || weight.type().is_float());
Type result_type = zero_val.type();
Expr bias_value = make_zero(result_type);
Type computation_type = result_type;
if (zero_val.type().is_int()) {
computation_type = UInt(zero_val.type().bits(), zero_val.type().lanes());
bias_value = result_type.min();
}
// For signed integer types, just convert everything to unsigned
// and then back at the end to ensure proper rounding, etc.
// There is likely a better way to handle this.
if (result_type != computation_type) {
zero_val = Cast::make(computation_type, zero_val - bias_value);
one_val = Cast::make(computation_type, one_val - bias_value);
}
if (result_type.is_bool()) {
Expr half_weight;
if (weight.type().is_float())
half_weight = 0.5f;
else {
half_weight = weight.type().max() / 2;
}
result = select(weight > half_weight, one_val, zero_val);
} else {
Expr typed_weight;
Expr inverse_typed_weight;
if (weight.type().is_float()) {
typed_weight = weight;
if (computation_type.is_uint()) {
// TODO: Verify this reduces to efficient code or
// figure out a better way to express a multiply
// of unsigned 2^32-1 by a double promoted weight
if (computation_type.bits() == 32) {
typed_weight =
Cast::make(computation_type,
cast<double>(Expr(65535.0f)) * cast<double>(Expr(65537.0f)) *
Cast::make(Float(64, typed_weight.type().lanes()), typed_weight));
} else {
typed_weight =
Cast::make(computation_type,
computation_type.max() * typed_weight);
}
inverse_typed_weight = computation_type.max() - typed_weight;
} else {
inverse_typed_weight = 1.0f - typed_weight;
}
} else {
if (computation_type.is_float()) {
int weight_bits = weight.type().bits();
if (weight_bits == 32) {
// Should use ldexp, but can't make Expr from result
// that is double
typed_weight =
Cast::make(computation_type,
cast<double>(weight) / (pow(cast<double>(2), 32) - 1));
} else {
typed_weight =
Cast::make(computation_type,
weight / ((float)ldexp(1.0f, weight_bits) - 1));
}
inverse_typed_weight = 1.0f - typed_weight;
} else {
// This code rescales integer weights to the right number of bits.
// It takes advantage of (2^n - 1) == (2^(n/2) - 1)(2^(n/2) + 1)
// e.g. 65535 = 255 * 257. (Ditto for the 32-bit equivalent.)
// To recale a weight of m bits to be n bits, we need to do:
// scaled_weight = (weight / (2^m - 1)) * (2^n - 1)
// which power of two values for m and n, results in a series like
// so:
// (2^(m/2) + 1) * (2^(m/4) + 1) ... (2^(n*2) + 1)
// The loop below computes a scaling constant and either multiples
// or divides by the constant and relies on lowering and llvm to
// generate efficient code for the operation.
int bit_size_difference = weight.type().bits() - computation_type.bits();
if (bit_size_difference == 0) {
typed_weight = weight;
} else {
typed_weight = Cast::make(computation_type, weight);
int bits_left = ::abs(bit_size_difference);
int shift_amount = std::min(computation_type.bits(), weight.type().bits());
uint64_t scaling_factor = 1;
while (bits_left != 0) {
internal_assert(bits_left > 0);
scaling_factor = scaling_factor + (scaling_factor << shift_amount);
bits_left -= shift_amount;
shift_amount *= 2;
}
if (bit_size_difference < 0) {
typed_weight =
Cast::make(computation_type, weight) *
cast(computation_type, (int32_t)scaling_factor);
} else {
typed_weight =
Cast::make(computation_type,
weight / cast(weight.type(), (int32_t)scaling_factor));
}
}
inverse_typed_weight =
Cast::make(computation_type,
computation_type.max() - typed_weight);
}
}
if (computation_type.is_float()) {
result = zero_val * inverse_typed_weight +
one_val * typed_weight;
} else {
int32_t bits = computation_type.bits();
switch (bits) {
case 1:
result = select(typed_weight, one_val, zero_val);
break;
case 8:
case 16:
case 32: {
Expr zero_expand = Cast::make(UInt(2 * bits, computation_type.lanes()),
zero_val);
Expr one_expand = Cast::make(UInt(2 * bits, one_val.type().lanes()),
one_val);
Expr rounding = Cast::make(UInt(2 * bits), 1) << Cast::make(UInt(2 * bits), (bits - 1));
Expr divisor = Cast::make(UInt(2 * bits), 1) << Cast::make(UInt(2 * bits), bits);
Expr prod_sum = zero_expand * inverse_typed_weight +
one_expand * typed_weight + rounding;
Expr divided = ((prod_sum / divisor) + prod_sum) / divisor;
result = Cast::make(UInt(bits, computation_type.lanes()), divided);
break;
}
case 64:
// TODO: 64-bit lerp is not supported as current approach
// requires double-width multiply.
// There is an informative error message in IROperator.h.
internal_error << "Can't do a 64-bit lerp.\n";
break;
default:
break;
}
}
if (!is_zero(bias_value)) {
result = Cast::make(result_type, result) + bias_value;
}
}
return simplify(result);
}
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Sub::auto_wp_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
// call attitude controller
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
// TODO logic for terrain tracking target going below fence limit
// TODO implement waypoint radius individually for each waypoint based on cmd.p2
// TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter
failsafe_terrain_set_status(wp_nav.update_wpnav());
///////////////////////
// update xy outputs //
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
////////////////////////////
// update attitude output //
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain());
}
}
void AP_TECS::_update_pitch(void)
{
// Calculate Speed/Height Control Weighting
// This is used to determine how the pitch control prioritises speed and height control
// A weighting of 1 provides equal priority (this is the normal mode of operation)
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed
// rises above the demanded value, the pitch angle will be increased by the TECS controller.
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1));
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
logging.SKE_weighting = SKE_weighting;
float SPE_weighting = 2.0f - SKE_weighting;
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
logging.SKE_error = _SKE_dem - _SKE_est;
logging.SPE_error = _SPE_dem - _SPE_est;
// Calculate integrator state, constraining input if pitch limits are exceeded
float integSEB_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
integSEB_input = MIN(integSEB_input, _PITCHmaxf - _pitch_dem);
}
else if (_pitch_dem < _PITCHminf)
{
integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem);
}
float integSEB_delta = integSEB_input * _DT;
#if 0
if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) {
::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n",
_hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem),
SEB_dem, SEBdot_dem, SEB_error, SEBdot_error);
}
#endif
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
// During flare a different damping gain is used
float gainInv = (_TAS_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_dem * timeConstant();
float pitch_damp = _ptchDamp;
if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
pitch_damp = _landDamp;
} else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) {
pitch_damp = _land_pitch_damp;
}
temp += SEBdot_error * pitch_damp;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv;
}
float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp;
float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp;
float integSEB_range = integSEB_max - integSEB_min;
logging.SEB_delta = integSEB_delta;
// don't allow the integrator to rise by more than 20% of its full
// range in one step. This prevents single value glitches from
// causing massive integrator changes. See Issue#4066
integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f);
// integrate
_integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integSEB_state) / gainInv;
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
// Rate limit the pitch demand to comply with specified vertical
// acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr)
{
_pitch_dem = _last_pitch_dem + ptchRateIncr;
}
else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr)
{
_pitch_dem = _last_pitch_dem - ptchRateIncr;
}
// re-constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf);
_last_pitch_dem = _pitch_dem;
}
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
/* Distance from polar axis. */
const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]);
/* Compute longitude first, this can be done exactly. */
if (!is_zero(p))
llh[1] = atan2(ecef[1], ecef[0]);
else
llh[1] = 0;
/* If we are close to the pole then convergence is very slow, treat this is a
* special case. */
if (p < WGS84_A*1e-16) {
llh[0] = copysign(M_PI_2, ecef[2]);
llh[2] = fabs(ecef[2]) - WGS84_B;
return;
}
/* Caluclate some other constants as defined in the Fukushima paper. */
const double P = p / WGS84_A;
const double e_c = sqrt(1. - WGS84_E*WGS84_E);
const double Z = fabs(ecef[2]) * e_c / WGS84_A;
/* Initial values for S and C correspond to a zero height solution. */
double S = Z;
double C = e_c * P;
/* Neither S nor C can be negative on the first iteration so
* starting prev = -1 will not cause and early exit. */
double prev_C = -1;
double prev_S = -1;
double A_n, B_n, D_n, F_n;
/* Iterate a maximum of 10 times. This should be way more than enough for all
* sane inputs */
for (int i=0; i<10; i++)
{
/* Calculate some intermmediate variables used in the update step based on
* the current state. */
A_n = sqrt(S*S + C*C);
D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S;
F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C;
B_n = 1.5*WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C);
/* Update step. */
S = D_n*F_n - B_n*S;
C = F_n*F_n - B_n*C;
/* The original algorithm as presented in the paper by Fukushima has a
* problem with numerical stability. S and C can grow very large or small
* and over or underflow a double. In the paper this is acknowledged and
* the proposed resolution is to non-dimensionalise the equations for S and
* C. However, this does not completely solve the problem. The author caps
* the solution to only a couple of iterations and in this period over or
* underflow is unlikely but as we require a bit more precision and hence
* more iterations so this is still a concern for us.
*
* As the only thing that is important is the ratio T = S/C, my solution is
* to divide both S and C by either S or C. The scaling is chosen such that
* one of S or C is scaled to unity whilst the other is scaled to a value
* less than one. By dividing by the larger of S or C we ensure that we do
* not divide by zero as only one of S or C should ever be zero.
*
* This incurs an extra division each iteration which the author was
* explicityl trying to avoid and it may be that this solution is just
* reverting back to the method of iterating on T directly, perhaps this
* bears more thought?
*/
if (S > C) {
C = C / S;
S = 1;
} else {
S = S / C;
C = 1;
}
/* Check for convergence and exit early if we have converged. */
if (fabs(S - prev_S) < 1e-16 && fabs(C - prev_C) < 1e-16) {
break;
} else {
prev_S = S;
prev_C = C;
}
}
A_n = sqrt(S*S + C*C);
llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
}
static inline bool
is_one(const fixed value, const int accuracy=ACCURACY)
{
return is_zero(value - fixed(1), accuracy);
}
bool is_zero(std::complex<value_t> const &x, value_t tolerance = 100 * std::numeric_limits<value_t>::epsilon()) {
return is_zero(std::real(x), tolerance) && is_zero(std::imag(x), tolerance);
}
literalt float_utilst::relation(
const bvt &src1,
relt rel,
const bvt &src2)
{
if(rel==GT)
return relation(src2, LT, src1); // swapped
else if(rel==GE)
return relation(src2, LE, src1); // swapped
assert(rel==EQ || rel==LT || rel==LE);
// special cases: -0 and 0 are equal
literalt is_zero1=is_zero(src1);
literalt is_zero2=is_zero(src2);
literalt both_zero=prop.land(is_zero1, is_zero2);
// NaN compares to nothing
literalt is_NaN1=is_NaN(src1);
literalt is_NaN2=is_NaN(src2);
literalt NaN=prop.lor(is_NaN1, is_NaN2);
if(rel==LT || rel==LE)
{
literalt bitwise_equal=bv_utils.equal(src1, src2);
// signs different? trivial! Unless Zero.
literalt signs_different=
prop.lxor(sign_bit(src1), sign_bit(src2));
// as long as the signs match: compare like unsigned numbers
// this works due to the BIAS
literalt less_than1=bv_utils.unsigned_less_than(src1, src2);
// if both are negative (and not the same), need to turn around!
literalt less_than2=
prop.lxor(less_than1, prop.land(sign_bit(src1), sign_bit(src2)));
literalt less_than3=
prop.lselect(signs_different,
sign_bit(src1),
less_than2);
if(rel==LT)
{
bvt and_bv;
and_bv.push_back(less_than3);
and_bv.push_back(!bitwise_equal); // for the case of two negative numbers
and_bv.push_back(!both_zero);
and_bv.push_back(!NaN);
return prop.land(and_bv);
}
else if(rel==LE)
{
bvt or_bv;
or_bv.push_back(less_than3);
or_bv.push_back(both_zero);
or_bv.push_back(bitwise_equal);
return prop.land(prop.lor(or_bv), !NaN);
}
else
assert(false);
}
else if(rel==EQ)
{
literalt bitwise_equal=bv_utils.equal(src1, src2);
return prop.land(
prop.lor(bitwise_equal, both_zero),
!NaN);
}
else
assert(0);
// not reached
return const_literal(false);
}
// sends commands to the motors
void AP_MotorsCoax::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float thrust_out; //
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f*(float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) {
limit.roll_pitch = true;
}
}
actuator_allowed = 2.0f * (1.0f - rp_scale * rp_thrust_max);
if (fabsf(yaw_thrust) > actuator_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
limit.yaw = true;
}
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust));
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max;
}
// calculate the throttle setting for the lift fan
thrust_out = _throttle_avg_max + thr_adj;
if (fabsf(yaw_thrust) > thrust_out) {
yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out);
limit.yaw = true;
}
_thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust;
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f);
if (is_zero(thrust_out)) {
limit.roll_pitch = true;
}
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
_actuator_out[2] = -_actuator_out[0];
_actuator_out[3] = -_actuator_out[1];
}
/*
a special glide slope calculation for the landing approach
During the land approach use a linear glide slope to a point
projected through the landing point. We don't use the landing point
itself as that leads to discontinuities close to the landing point,
which can lead to erratic pitch control
*/
void Plane::setup_landing_glide_slope(void)
{
float total_distance = get_distance(prev_WP_loc, next_WP_loc);
// If someone mistakenly puts all 0's in their LAND command then total_distance
// will be calculated as 0 and cause a divide by 0 error below. Lets avoid that.
if (total_distance < 1) {
total_distance = 1;
}
// height we need to sink for this WP
float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;
// current ground speed
float groundspeed = ahrs.groundspeed();
if (groundspeed < 0.5f) {
groundspeed = 0.5f;
}
// calculate time to lose the needed altitude
float sink_time = total_distance / groundspeed;
if (sink_time < 0.5f) {
sink_time = 0.5f;
}
// find the sink rate needed for the target location
float sink_rate = sink_height / sink_time;
// the height we aim for is the one to give us the right flare point
float aim_height = aparm.land_flare_sec * sink_rate;
if (aim_height <= 0) {
aim_height = g.land_flare_alt;
}
// don't allow the aim height to be too far above LAND_FLARE_ALT
if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) {
aim_height = g.land_flare_alt*2;
}
// calculate slope to landing point
bool is_first_calc = is_zero(auto_state.land_slope);
auto_state.land_slope = (sink_height - aim_height) / total_distance;
if (is_first_calc) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope)));
}
// time before landing that we will flare
float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();
// distance to flare is based on ground speed, adjusted as we
// get closer. This takes into account the wind
float flare_distance = groundspeed * flare_time;
// don't allow the flare before half way along the final leg
if (flare_distance > total_distance/2) {
flare_distance = total_distance/2;
}
// project a point 500 meters past the landing point, passing
// through the landing point
const float land_projection = 500;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
// now calculate our aim point, which is before the landing
// point and above it
Location loc = next_WP_loc;
location_update(loc, land_bearing_cd*0.01f, -flare_distance);
loc.alt += aim_height*100;
// calculate point along that slope 500m ahead
location_update(loc, land_bearing_cd*0.01f, land_projection);
loc.alt -= auto_state.land_slope * land_projection * 100;
// setup the offset_cm for set_target_altitude_proportion()
target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
// calculate the proportion we are to the target
float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
// now setup the glide slope for landing
set_target_altitude_proportion(loc, 1.0f - land_proportion);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(loc, prev_WP_loc);
}
// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{
uint32_t tnow = AP_HAL::millis();
// initialize vertical speeds and acceleration
pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control.set_max_accel_z(g.pilot_accel_z);
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
last_pilot_heading = ahrs.yaw_sensor;
return;
}
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// get pilot desired lean angles
float target_roll, target_pitch;
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = degrees(target_roll);
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}
if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
// output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
// reset z targets to current values
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
} else { // hold z
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt);
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
pos_control.update_z_controller();
}
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}
/*
initialise the barometer object, loading backend drivers
*/
void AP_Baro::init(void)
{
// ensure that there isn't a previous ground temperature saved
if (!is_zero(_user_ground_temperature)) {
_user_ground_temperature.set_and_save(0.0f);
_user_ground_temperature.notify();
}
if (_hil_mode) {
drivers[0] = new AP_Baro_HIL(*this);
_num_drivers = 1;
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
ADD_BACKEND(new AP_Baro_SITL(*this));
return;
#endif
#if HAL_WITH_UAVCAN
bool added;
do {
added = _add_backend(AP_Baro_UAVCAN::probe(*this));
if (_num_drivers == BARO_MAX_DRIVERS || _num_sensors == BARO_MAX_INSTANCES) {
return;
}
} while (added);
#endif
#if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
#ifdef HAL_BARO_MS5611_I2C_BUS
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR))));
#endif
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
case AP_BoardConfig::PX4_BOARD_PHMINI:
case AP_BoardConfig::PX4_BOARD_AUAV21:
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_EXT_NAME))));
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
break;
case AP_BoardConfig::PX4_BOARD_PIXRACER:
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_INT_NAME))));
break;
case AP_BoardConfig::PX4_BOARD_AEROFC:
#ifdef HAL_BARO_MS5607_I2C_BUS
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5607));
#endif
break;
default:
break;
}
#elif HAL_BARO_DEFAULT == HAL_BARO_HIL
drivers[0] = new AP_Baro_HIL(*this);
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_BMP085
drivers[0] = new AP_Baro_BMP085(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR)));
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_I2C
ADD_BACKEND(AP_Baro_BMP280::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP280_BUS, HAL_BARO_BMP280_I2C_ADDR))));
#elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_SPI
ADD_BACKEND(AP_Baro_BMP280::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_BMP280_NAME))));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR))));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5607_I2C
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5607));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5637_I2C
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5637));
#elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT
drivers[0] = new AP_Baro_QFLIGHT(*this);
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_QURT
drivers[0] = new AP_Baro_QURT(*this);
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H
ADD_BACKEND(AP_Baro_LPS25H::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR))));
#endif
// can optionally have baro on I2C too
if (_ext_bus >= 0) {
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5837_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5837));
ADD_BACKEND(AP_Baro_KellerLD::probe(*this,
std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_KELLERLD_I2C_ADDR))));
#else
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR))));
#endif
}
if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
AP_BoardConfig::sensor_config_error("Baro: unable to initialise driver");
}
}
/// check_fence - returns the fence type that has been breached (if any)
/// curr_alt is the altitude above home in meters
uint8_t AC_Fence::check_fence(float curr_alt)
{
uint8_t ret = AC_FENCE_TYPE_NONE;
// return immediately if disabled
if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) {
return AC_FENCE_TYPE_NONE;
}
// check if pilot is attempting to recover manually
if (_manual_recovery_start_ms != 0) {
// we ignore any fence breaches during the manual recovery period which is about 10 seconds
if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
return AC_FENCE_TYPE_NONE;
} else {
// recovery period has passed so reset manual recovery time and continue with fence breach checks
_manual_recovery_start_ms = 0;
}
}
// altitude fence check
if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
// check if we are over the altitude fence
if( curr_alt >= _alt_max ) {
// record distance above breach
_alt_max_breach_distance = curr_alt - _alt_max;
// check for a new breach or a breach of the backup fence
if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) == 0 || (!is_zero(_alt_max_backup) && curr_alt >= _alt_max_backup)) {
// record that we have breached the upper limit
record_breach(AC_FENCE_TYPE_ALT_MAX);
ret |= AC_FENCE_TYPE_ALT_MAX;
// create a backup fence 20m higher up
_alt_max_backup = curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE;
}
}else{
// clear alt breach if present
if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
clear_breach(AC_FENCE_TYPE_ALT_MAX);
_alt_max_backup = 0.0f;
_alt_max_breach_distance = 0.0f;
}
}
}
// circle fence check
if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) != 0 ) {
// check if we are outside the fence
if (_home_distance >= _circle_radius) {
// record distance outside the fence
_circle_breach_distance = _home_distance - _circle_radius;
// check for a new breach or a breach of the backup fence
if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) == 0 || (!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) {
// record that we have breached the circular distance limit
record_breach(AC_FENCE_TYPE_CIRCLE);
ret |= AC_FENCE_TYPE_CIRCLE;
// create a backup fence 20m further out
_circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
}
}else{
// clear circle breach if present
if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) != 0) {
clear_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = 0.0f;
_circle_breach_distance = 0.0f;
}
}
}
// polygon fence check
if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) {
// check consistency of number of points
if (_boundary_num_points != _total) {
_boundary_loaded = false;
}
// load fence if necessary
if (!_boundary_loaded) {
load_polygon_from_eeprom();
} else if (_boundary_valid) {
// check if vehicle is outside the polygon fence
const Vector3f& position = _inav.get_position();
if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) {
// check if this is a new breach
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) {
// record that we have breached the polygon
record_breach(AC_FENCE_TYPE_POLYGON);
ret |= AC_FENCE_TYPE_POLYGON;
}
} else {
// clear breach if present
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
}
}
}
// return any new breaches that have occurred
return ret;
}
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
{
// calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED
// parameter and the value set by the EKF to observe optical flow limits
float gnd_speed_limit_cms = min(_loiter_speed_cms,ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = max(gnd_speed_limit_cms, 10.0f);
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// check loiter speed and avoid divide by zero
if(gnd_speed_limit_cms < WPNAV_LOITER_SPEED_MIN) {
gnd_speed_limit_cms = WPNAV_LOITER_SPEED_MIN;
}
_pos_control.set_speed_xy(gnd_speed_limit_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
// rotate pilot input to lat/lon frame
Vector2f desired_accel;
desired_accel.x = (_pilot_accel_fwd_cms*_ahrs.cos_yaw() - _pilot_accel_rgt_cms*_ahrs.sin_yaw());
desired_accel.y = (_pilot_accel_fwd_cms*_ahrs.sin_yaw() + _pilot_accel_rgt_cms*_ahrs.cos_yaw());
// calculate the difference
Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel);
// constrain and scale the desired acceleration
float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y);
float accel_change_max = _loiter_jerk_max_cmsss * nav_dt;
if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) {
des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total;
des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total;
}
// adjust the desired acceleration
_loiter_desired_accel += des_accel_diff;
// get pos_control's feed forward velocity
const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity();
Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);
// add pilot commanded acceleration
desired_vel.x += _loiter_desired_accel.x * nav_dt;
desired_vel.y += _loiter_desired_accel.y * nav_dt;
float desired_speed = desired_vel.length();
if (!is_zero(desired_speed)) {
Vector2f desired_vel_norm = desired_vel/desired_speed;
float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms;
if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) {
drag_speed_delta = min(drag_speed_delta,-_loiter_accel_min_cmss*nav_dt);
}
desired_speed = max(desired_speed+drag_speed_delta,0.0f);
desired_vel = desired_vel_norm*desired_speed;
}
// Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y));
if (horizSpdDem > gnd_speed_limit_cms) {
desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
static bool is_prop(expr type) {
while (is_pi(type)) {
type = binding_body(type);
}
return is_sort(type) && is_zero(sort_level(type));
}
DEVICE bool operator()(int idx) {
return is_zero(rays[idx].dir);
}
/*
calculate throttle demand - airspeed enabled case
*/
void AP_TECS::_update_throttle_with_airspeed(void)
{
// Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem;
// Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast;
_STEdotErrLast = STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_flags.underspeed)
{
_throttle_dem = 1.0f;
}
else
{
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
float throttle_damp = _thrDamp;
if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) {
throttle_damp = _land_throttle_damp;
}
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
{
if (!_flags.reached_speed_takeoff) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
}
_integTHR_state = integ_max;
}
else
{
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
}
// Sum the components.
_throttle_dem = _throttle_dem + _integTHR_state;
}
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
}
// *this *= 2
void PointGFp::mult2(std::vector<BigInt>& ws_bn)
{
if(is_zero())
return;
else if(m_coord_y.is_zero())
{
*this = PointGFp(m_curve); // setting myself to zero
return;
}
/*
http://hyperelliptic.org/EFD/g1p/auto-shortw-jacobian-3.html#doubling-dbl-1986-cc
*/
const BigInt& p = m_curve.get_p();
BigInt& y_2 = ws_bn[0];
BigInt& S = ws_bn[1];
BigInt& z4 = ws_bn[2];
BigInt& a_z4 = ws_bn[3];
BigInt& M = ws_bn[4];
BigInt& U = ws_bn[5];
BigInt& x = ws_bn[6];
BigInt& y = ws_bn[7];
BigInt& z = ws_bn[8];
curve_sqr(y_2, m_coord_y);
curve_mult(S, m_coord_x, y_2);
S <<= 2; // * 4
while(S >= p)
S -= p;
curve_sqr(z4, curve_sqr(m_coord_z));
curve_mult(a_z4, m_curve.get_a_rep(), z4);
M = curve_sqr(m_coord_x);
M *= 3;
M += a_z4;
while(M >= p)
M -= p;
curve_sqr(x, M);
x -= (S << 1);
while(x.is_negative())
x += p;
curve_sqr(U, y_2);
U <<= 3;
while(U >= p)
U -= p;
S -= x;
while(S.is_negative())
S += p;
curve_mult(y, M, S);
y -= U;
if(y.is_negative())
y += p;
curve_mult(z, m_coord_y, m_coord_z);
z <<= 1;
if(z >= p)
z -= p;
m_coord_x = x;
m_coord_y = y;
m_coord_z = z;
}
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_STATUSTEXT:
{
// ignore any statustext messages not from our GCS:
if (msg->sysid != rover.g.sysid_my_gcs) {
break;
}
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G', 'C', 'S', ':'};
memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
rover.DataFlash.Log_Write_Message(text);
break;
}
case MAVLINK_MSG_ID_COMMAND_INT: {
// decode packet
mavlink_command_int_t packet;
mavlink_msg_command_int_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
switch (packet.command) {
#if MOUNT == ENABLED
case MAV_CMD_DO_SET_ROI: {
// param1 : /* Region of interest mode (not used)*/
// param2 : /* MISSION index/ target ID (not used)*/
// param3 : /* ROI index (not used)*/
// param4 : /* empty */
// x : lat
// y : lon
// z : alt
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
break;
}
Location roi_loc;
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
}
#endif
default:
result = MAV_RESULT_UNSUPPORTED;
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
switch (packet.command) {
case MAV_CMD_START_RX_PAIR:
result = handle_rc_bind(packet);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
rover.set_mode(RTL);
result = MAV_RESULT_ACCEPTED;
break;
#if MOUNT == ENABLED
// Sets the region of interest (ROI) for the camera
case MAV_CMD_DO_SET_ROI:
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
#endif
#if CAMERA == ENABLED
case MAV_CMD_DO_DIGICAM_CONFIGURE:
rover.camera.configure(packet.param1,
packet.param2,
packet.param3,
packet.param4,
packet.param5,
packet.param6,
packet.param7);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_DIGICAM_CONTROL:
if (rover.camera.control(packet.param1,
packet.param2,
packet.param3,
packet.param4,
packet.param5,
packet.param6)) {
rover.log_picture();
}
result = MAV_RESULT_ACCEPTED;
break;
#endif // CAMERA == ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
#endif
break;
case MAV_CMD_MISSION_START:
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (hal.util->get_soft_armed()) {
result = MAV_RESULT_FAILED;
break;
}
if (is_equal(packet.param1, 1.0f)) {
rover.ins.init_gyro();
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3, 1.0f)) {
rover.init_barometer(false);
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param4, 1.0f)) {
rover.trim_radio();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5, 1.0f)) {
result = MAV_RESULT_ACCEPTED;
// start with gyro calibration
rover.ins.init_gyro();
// reset ahrs gyro bias
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
} else {
result = MAV_RESULT_FAILED;
}
rover.ins.acal_init();
rover.ins.get_acal()->start(this);
} else if (is_equal(packet.param5, 2.0f)) {
// start with gyro calibration
rover.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if (rover.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
}
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
{
uint8_t compassNumber = -1;
if (is_equal(packet.param1, 2.0f)) {
compassNumber = 0;
} else if (is_equal(packet.param1, 5.0f)) {
compassNumber = 1;
} else if (is_equal(packet.param1, 6.0f)) {
compassNumber = 2;
}
if (compassNumber != (uint8_t) -1) {
rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
rover.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_STABILIZE_DISARMED:
case MAV_MODE_STABILIZE_ARMED:
rover.set_mode(LEARNING);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_SERVO:
if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_RELAY:
if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1, 3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1, 1.0f)) {
// run pre_arm_checks and arm_checks and display failures
if (rover.arm_motors(AP_Arming::MAVLINK)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_zero(packet.param1)) {
if (rover.disarm_motors()) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_GET_HOME_POSITION:
if (rover.home_is_set != HOME_UNSET) {
send_home(rover.ahrs.get_home());
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1, 1.0f)) {
send_autopilot_version(FIRMWARE_VERSION);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_SET_HOME:
{
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1, 1.0f)) {
rover.init_home();
} else {
if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
// don't allow the 0,0 position
break;
}
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location new_home_loc {};
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
rover.ahrs.set_home(new_home_loc);
rover.home_is_set = HOME_SET_NOT_LOCKED;
rover.Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(new_home_loc);
result = MAV_RESULT_ACCEPTED;
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat * 1.0e-7f),
(double)(new_home_loc.lng * 1.0e-7f),
(uint32_t)(new_home_loc.alt * 0.01f));
}
break;
}
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL:
result = rover.compass.handle_mag_cal_command(packet);
break;
case MAV_CMD_NAV_SET_YAW_SPEED:
{
// param1 : yaw angle to adjust direction by in centidegress
// param2 : Speed - normalized to 0 .. 1
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
rover.guided_mode = Guided_Angle;
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
rover.guided_yaw_speed.turn_angle = packet.param1;
rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
rover.nav_set_yaw_speed();
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED;
if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
result = MAV_RESULT_ACCEPTED;
}
break;
default:
break;
}
mavlink_msg_command_ack_send_buf(
msg,
chan,
packet.command,
result);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
{
handle_mission_request_list(rover.mission, msg);
break;
}
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
handle_mission_request(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_ACK:
{
// not used
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// mark the firmware version in the tlog
send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
#endif
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
{
handle_mission_clear_all(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
handle_mission_set_current(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT:
{
handle_mission_count(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
handle_mission_write_partial_list(rover.mission, msg);
break;
}
// GCS has sent us a mission item, store to EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM:
{
if (handle_mission_item(msg, rover.mission)) {
rover.DataFlash.Log_Write_EntireMission(rover.mission);
}
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
{
if (handle_mission_item(msg, rover.mission)) {
rover.DataFlash.Log_Write_EntireMission(rover.mission);
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, &rover.DataFlash);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
break;
}
mavlink_rc_channels_override_t packet;
int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet);
v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
v[3] = packet.chan4_raw;
v[4] = packet.chan5_raw;
v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
hal.rcin->set_overrides(v, 8);
rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if (msg->sysid != rover.g.sysid_my_gcs) {
break;
}
rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
// prepare and send target position
if (!pos_ignore) {
Location loc = rover.current_loc;
if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
// rotate from body-frame to NE frame
float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
// add offset to current location
location_offset(loc, ne_x, ne_y);
} else if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED) {
// add offset to current location
location_offset(loc, packet.x, packet.y);
} else {
// MAV_FRAME_LOCAL_NED interpret as an offset from home
loc = rover.ahrs.get_home();
location_offset(loc, packet.x, packet.y);
}
rover.guided_WP = loc;
rover.rtl_complete = false;
rover.set_guided_WP();
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
// prepare and send target position
if (!pos_ignore) {
Location loc = rover.current_loc;
loc.lat = packet.lat_int;
loc.lng = packet.lon_int;
rover.guided_WP = loc;
rover.rtl_complete = false;
rover.set_guided_WP();
}
break;
}
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
case MAVLINK_MSG_ID_GPS_INPUT:
{
rover.gps.handle_msg(msg);
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE
#if CAMERA == ENABLED
// deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
{
break;
}
// deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
rover.camera.control_msg(msg);
rover.log_picture();
break;
}
#endif // CAMERA == ENABLED
#if MOUNT == ENABLED
// deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
rover.camera_mount.configure_msg(msg);
break;
}
// deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
rover.camera_mount.control_msg(msg);
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
break;
}
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
rover.in_log_download = true;
/* no break */
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
rover.in_log_download = false;
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, rover.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, rover.gps);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
rover.sonar.handle_msg(msg);
break;
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
rover.DataFlash.remote_log_block_status_msg(chan, msg);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
send_autopilot_version(FIRMWARE_VERSION);
break;
case MAVLINK_MSG_ID_LED_CONTROL:
// send message to Notify
AP_Notify::handle_led_control(msg);
break;
case MAVLINK_MSG_ID_PLAY_TUNE:
// send message to Notify
AP_Notify::handle_play_tune(msg);
break;
default:
handle_common_message(msg);
break;
} // end switch
} // end handle mavlink
