Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MSP: use const structures and brace initialization where possible

Browse Source 
saves ane extra 130bytes
This commit is contained in:
yaapu 2021-09-07 10:06:18 +02:00 committed by Andrew Tridgell
parent 3bbf2c1f70
commit e1b12fbee3
4 changed files with 98 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
View File

@ -608,24 +608,23 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_comp_gps(sbuf_t *dst)
update_home_pos(home_state); update_home_pos(home_state);
// no need to apply yaw compensation, the DJI air unit will do it for us :-) // no need to apply yaw compensation, the DJI air unit will do it for us :-)
int32_t home_angle_deg = home_state.home_bearing_cd * 0.01; uint16_t angle_deg = home_state.home_bearing_cd * 0.01;
if (home_state.home_distance_m < 2) { if (home_state.home_distance_m < 2) {
//avoid fast rotating arrow at small distances //avoid fast rotating arrow at small distances
home_angle_deg = 0; angle_deg = 0;
} }
struct PACKED { const struct PACKED {
uint16_t dist_home_m; uint16_t dist_home_m;
uint16_t home_angle_deg; uint16_t home_angle_deg;
uint8_t toggle_gps; uint8_t toggle_gps;
} gps; } gps {
dist_home_m : uint16_t(constrain_int32(home_state.home_distance_m, 0, 0xFFFF)),
gps.dist_home_m = home_state.home_distance_m; home_angle_deg : angle_deg,
gps.home_angle_deg = home_angle_deg; toggle_gps : 1
gps.toggle_gps = 1; };
sbuf_write_data(dst, &gps, sizeof(gps)); sbuf_write_data(dst, &gps, sizeof(gps));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
} }
@ -819,15 +818,15 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_attitude(sbuf_t *dst)
AP_AHRS &ahrs = AP::ahrs(); AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore()); WITH_SEMAPHORE(ahrs.get_semaphore());
struct PACKED { const struct PACKED {
int16_t roll; int16_t roll;
int16_t pitch; int16_t pitch;
int16_t yaw; int16_t yaw;
} attitude; } attitude {
roll : int16_t(ahrs.roll_sensor * 0.1), // centidegress to decidegrees
attitude.roll = ahrs.roll_sensor * 0.1; // centidegress to decidegrees pitch : int16_t(ahrs.pitch_sensor * 0.1), // centidegress to decidegrees
attitude.pitch = ahrs.pitch_sensor * 0.1; // centidegress to decidegrees yaw : int16_t(ahrs.yaw_sensor * 0.01) // centidegress to degrees
attitude.yaw = ahrs.yaw_sensor * 0.01; // centidegress to degrees };
sbuf_write_data(dst, &attitude, sizeof(attitude)); sbuf_write_data(dst, &attitude, sizeof(attitude));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -838,13 +837,13 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst)
home_state_t home_state; home_state_t home_state;
update_home_pos(home_state); update_home_pos(home_state);
struct PACKED { const struct PACKED {
int32_t rel_altitude_cm; // relative altitude cm int32_t rel_altitude_cm; // relative altitude cm
int16_t vspeed_cms; // climb rate cm/s int16_t vspeed_cms; // climb rate cm/s
} altitude {}; } altitude {
rel_altitude_cm : home_state.rel_altitude_cm,
altitude.rel_altitude_cm = home_state.rel_altitude_cm; vspeed_cms : int16_t(get_vspeed_ms() * 100)
altitude.vspeed_cms = int16_t(get_vspeed_ms() * 100); };
sbuf_write_data(dst, &altitude, sizeof(altitude)); sbuf_write_data(dst, &altitude, sizeof(altitude));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -859,19 +858,19 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst)
battery_state_t battery_state; battery_state_t battery_state;
update_battery_state(battery_state); update_battery_state(battery_state);
struct PACKED { const struct PACKED {
uint8_t voltage_dv; uint8_t voltage_dv;
uint16_t mah; uint16_t mah;
uint16_t rssi; uint16_t rssi;
int16_t current_ca; int16_t current_ca;
uint16_t voltage_cv; uint16_t voltage_cv;
} battery; } battery {
voltage_dv : (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255), // battery voltage V to dV
battery.voltage_dv = constrain_int16(battery_state.batt_voltage_v * 10, 0, 255); // battery voltage V to dV mah : (uint16_t)constrain_int32(battery_state.batt_consumed_mah, 0, 0xFFFF), // milliamp hours drawn from battery
battery.mah = constrain_int32(battery_state.batt_consumed_mah, 0, 0xFFFF); // milliamp hours drawn from battery rssi : uint16_t(rssi->enabled() ? rssi->read_receiver_rssi() * 1023 : 0), // rssi 0-1 to 0-1023
battery.rssi = rssi->enabled() ? rssi->read_receiver_rssi() * 1023 : 0; // rssi 0-1 to 0-1023 current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A)
battery.current_ca = constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF); // current A to cA (0.01 steps, range is -320A to 320A) voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF) // battery voltage in 0.01V steps
battery.voltage_cv = constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF); // battery voltage in 0.01V steps };
sbuf_write_data(dst, &battery, sizeof(battery)); sbuf_write_data(dst, &battery, sizeof(battery));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -886,7 +885,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst
battery_state_t battery_state; battery_state_t battery_state;
update_battery_state(battery_state); update_battery_state(battery_state);
struct PACKED { const struct PACKED {
uint8_t cellcount; uint8_t cellcount;
uint16_t capacity_mah; uint16_t capacity_mah;
uint8_t voltage_dv; uint8_t voltage_dv;
@ -894,15 +893,15 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst
int16_t current_ca; int16_t current_ca;
uint8_t state; uint8_t state;
uint16_t voltage_cv; uint16_t voltage_cv;
} battery; } battery {
cellcount : (uint8_t)constrain_int16((msp->_cellcount > 0 ? msp->_cellcount : battery_state.batt_cellcount), 0, 255), // cell count 0 indicates battery not detected.
battery.cellcount = constrain_int16((msp->_cellcount > 0 ? msp->_cellcount : battery_state.batt_cellcount), 0, 255); // cell count 0 indicates battery not detected. capacity_mah : (uint16_t)battery_state.batt_capacity_mah, // in mAh
battery.capacity_mah = battery_state.batt_capacity_mah; // in mAh voltage_dv : (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255), // battery voltage V to dV
battery.voltage_dv = constrain_int16(battery_state.batt_voltage_v * 10, 0, 255); // battery voltage V to dV mah : (uint16_t)MIN(battery_state.batt_consumed_mah, 0xFFFF), // milliamp hours drawn from battery
battery.mah = MIN(battery_state.batt_consumed_mah, 0xFFFF); // milliamp hours drawn from battery current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A)
battery.current_ca = constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF); // current A to cA (0.01 steps, range is -320A to 320A) state : (uint8_t)battery_state.batt_state, // BATTERY: OK=0, CRITICAL=2
battery.state = battery_state.batt_state; // BATTERY: OK=0, CRITICAL=2 voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100, 0, 0x7FFF) // battery voltage in 0.01V steps
battery.voltage_cv = constrain_int32(battery_state.batt_voltage_v * 100, 0, 0x7FFF); // battery voltage in 0.01V steps };
sbuf_write_data(dst, &battery, sizeof(battery)); sbuf_write_data(dst, &battery, sizeof(battery));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -944,7 +943,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst)
const time_t time_sec = time_usec / 1000000; const time_t time_sec = time_usec / 1000000;
localtime_tm = *gmtime(&time_sec); localtime_tm = *gmtime(&time_sec);
} }
struct PACKED { const struct PACKED {
uint16_t year; uint16_t year;
uint8_t mon; uint8_t mon;
uint8_t mday; uint8_t mday;
@ -952,15 +951,15 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst)
uint8_t min; uint8_t min;
uint8_t sec; uint8_t sec;
uint16_t millis; uint16_t millis;
} rtc; } rtc {
year : uint16_t(localtime_tm.tm_year + 1900), // tm_year is relative to year 1900
rtc.year = localtime_tm.tm_year + 1900; // tm_year is relative to year 1900 mon : uint8_t(localtime_tm.tm_mon + 1), // MSP requires [1-12] months
rtc.mon = localtime_tm.tm_mon + 1; // MSP requires [1-12] months mday : uint8_t(localtime_tm.tm_mday),
rtc.mday = localtime_tm.tm_mday; hour : uint8_t(localtime_tm.tm_hour),
rtc.hour = localtime_tm.tm_hour; min : uint8_t(localtime_tm.tm_min),
rtc.min = localtime_tm.tm_min; sec : uint8_t(localtime_tm.tm_sec),
rtc.sec = localtime_tm.tm_sec; millis : uint16_t((time_usec / 1000U) % 1000U)
rtc.millis = (time_usec / 1000U) % 1000U; };
sbuf_write_data(dst, &rtc, sizeof(rtc)); sbuf_write_data(dst, &rtc, sizeof(rtc));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -975,18 +974,19 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst)
uint16_t values[16] = {}; uint16_t values[16] = {};
rc().get_radio_in(values, ARRAY_SIZE(values)); rc().get_radio_in(values, ARRAY_SIZE(values));
struct PACKED { const struct PACKED {
uint16_t a; uint16_t a;
uint16_t e; uint16_t e;
uint16_t r; uint16_t r;
uint16_t t; uint16_t t;
} rc; } rc {
// send only 4 channels, MSP order is AERT // send only 4 channels, MSP order is AERT
// note: rcmap channels start at 1 // note: rcmap channels start at 1
rc.a = values[rcmap->roll()-1]; // A a : values[rcmap->roll()-1], // A
rc.e = values[rcmap->pitch()-1]; // E e : values[rcmap->pitch()-1], // E
rc.r = values[rcmap->yaw()-1]; // R r : values[rcmap->yaw()-1], // R
rc.t = values[rcmap->throttle()-1]; // T t : values[rcmap->throttle()-1] // T
};
sbuf_write_data(dst, &rc, sizeof(rc)); sbuf_write_data(dst, &rc, sizeof(rc));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -1143,10 +1143,7 @@ void AP_MSP_Telem_Backend::msp_displayport_draw_screen()
void AP_MSP_Telem_Backend::msp_displayport_write_string(uint8_t col, uint8_t row, bool blink, const char *string) void AP_MSP_Telem_Backend::msp_displayport_write_string(uint8_t col, uint8_t row, bool blink, const char *string)
{ {
int len = strlen(string); const uint8_t len = strnlen(string, OSD_MSP_DISPLAYPORT_MAX_STRING_LENGTH);
if (len >= OSD_MSP_DISPLAYPORT_MAX_STRING_LENGTH) {
len = OSD_MSP_DISPLAYPORT_MAX_STRING_LENGTH;
}
struct PACKED { struct PACKED {
uint8_t sub_cmd; uint8_t sub_cmd;

33
libraries/AP_MSP/AP_MSP_Telem_DJI.cpp
View File

@ -84,15 +84,15 @@ uint32_t AP_MSP_Telem_DJI::get_osd_flight_mode_bitmask(void)
MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_api_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_api_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t proto; uint8_t proto;
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
} api_version; } api_version {
proto : MSP_PROTOCOL_VERSION,
api_version.proto = MSP_PROTOCOL_VERSION; major : API_VERSION_MAJOR,
api_version.major = API_VERSION_MAJOR; minor : API_VERSION_MINOR
api_version.minor = API_VERSION_MINOR; };
sbuf_write_data(dst, &api_version, sizeof(api_version)); sbuf_write_data(dst, &api_version, sizeof(api_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -100,15 +100,15 @@ MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_api_version(sbuf_t *dst)
MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
uint8_t patch; uint8_t patch;
} fc_version; } fc_version {
major : FC_VERSION_MAJOR,
fc_version.major = FC_VERSION_MAJOR; minor : FC_VERSION_MINOR,
fc_version.minor = FC_VERSION_MINOR; patch : FC_VERSION_PATCH_LEVEL
fc_version.patch = FC_VERSION_PATCH_LEVEL; };
sbuf_write_data(dst, &fc_version, sizeof(fc_version)); sbuf_write_data(dst, &fc_version, sizeof(fc_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -129,13 +129,14 @@ MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_esc_sensor_data(sbuf_t *dst)
int16_t highest_temperature = 0; int16_t highest_temperature = 0;
telem.get_highest_motor_temperature(highest_temperature); telem.get_highest_motor_temperature(highest_temperature);
struct PACKED { const struct PACKED {
uint8_t temp; uint8_t temp;
uint16_t rpm; uint16_t rpm;
} esc_sensor_data {}; } esc_sensor_data {
temp : uint8_t(highest_temperature * 0.01f), // deg, report max temperature
rpm : uint16_t(telem.get_average_motor_rpm() * 0.1f) // rpm, report average RPM across all motors
};
esc_sensor_data.temp = uint8_t(highest_temperature * 0.01f); // deg, report max temperature
esc_sensor_data.rpm = uint16_t(telem.get_average_motor_rpm() * 0.1f); // rpm, report average RPM across all motors
sbuf_write_data(dst, &esc_sensor_data, sizeof(esc_sensor_data)); sbuf_write_data(dst, &esc_sensor_data, sizeof(esc_sensor_data));
} else { } else {
return AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(dst); return AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(dst);

24
libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp
View File

@ -27,15 +27,15 @@ using namespace MSP;
MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_api_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_api_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t proto; uint8_t proto;
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
} api_version; } api_version {
proto : MSP_PROTOCOL_VERSION,
api_version.proto = MSP_PROTOCOL_VERSION; major : API_VERSION_MAJOR,
api_version.major = API_VERSION_MAJOR; minor : API_VERSION_MINOR
api_version.minor = API_VERSION_MINOR; };
sbuf_write_data(dst, &api_version, sizeof(api_version)); sbuf_write_data(dst, &api_version, sizeof(api_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -43,15 +43,15 @@ MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_api_version(sbuf_t *d
MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_fc_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_fc_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
uint8_t patch; uint8_t patch;
} fc_version; } fc_version {
major : FC_VERSION_MAJOR,
fc_version.major = FC_VERSION_MAJOR; minor : FC_VERSION_MINOR,
fc_version.minor = FC_VERSION_MINOR; patch : FC_VERSION_PATCH_LEVEL
fc_version.patch = FC_VERSION_PATCH_LEVEL; };
sbuf_write_data(dst, &fc_version, sizeof(fc_version)); sbuf_write_data(dst, &fc_version, sizeof(fc_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;

24
libraries/AP_MSP/AP_MSP_Telem_Generic.cpp
View File

@ -27,15 +27,15 @@ using namespace MSP;
MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_api_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_api_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t proto; uint8_t proto;
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
} api_version; } api_version {
proto : MSP_PROTOCOL_VERSION,
api_version.proto = MSP_PROTOCOL_VERSION; major : API_VERSION_MAJOR,
api_version.major = API_VERSION_MAJOR; minor : API_VERSION_MINOR
api_version.minor = API_VERSION_MINOR; };
sbuf_write_data(dst, &api_version, sizeof(api_version)); sbuf_write_data(dst, &api_version, sizeof(api_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
@ -43,15 +43,15 @@ MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_api_version(sbuf_t *dst)
MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_fc_version(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_fc_version(sbuf_t *dst)
{ {
struct { const struct {
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
uint8_t patch; uint8_t patch;
} fc_version; } fc_version {
major : FC_VERSION_MAJOR,
fc_version.major = FC_VERSION_MAJOR; minor : FC_VERSION_MINOR,
fc_version.minor = FC_VERSION_MINOR; patch : FC_VERSION_PATCH_LEVEL
fc_version.patch = FC_VERSION_PATCH_LEVEL; };
sbuf_write_data(dst, &fc_version, sizeof(fc_version)); sbuf_write_data(dst, &fc_version, sizeof(fc_version));
return MSP_RESULT_ACK; return MSP_RESULT_ACK;